From 6b5244a8e171e41f0e8a114cd65d64fde508d6d4 Mon Sep 17 00:00:00 2001
From: bjorn <mail@bjornbyt.es>
Date: Thu, 4 Apr 2024 21:41:12 -0700
Subject: [PATCH] Add Collider:get/setShapeOffset;

---
 src/api/l_physics_collider.c       | 27 ++++++++++++++++++++
 src/modules/physics/physics.h      |  2 ++
 src/modules/physics/physics_jolt.c | 40 ++++++++++++++++++++++++++++++
 3 files changed, 69 insertions(+)

diff --git a/src/api/l_physics_collider.c b/src/api/l_physics_collider.c
index d9ba26fd..234e0390 100644
--- a/src/api/l_physics_collider.c
+++ b/src/api/l_physics_collider.c
@@ -46,6 +46,31 @@ static int l_lovrColliderSetShape(lua_State* L) {
   return 0;
 }
 
+static int l_lovrColliderGetShapeOffset(lua_State* L) {
+  Collider* collider = luax_checktype(L, 1, Collider);
+  float position[3], orientation[4], angle, ax, ay, az;
+  lovrColliderGetShapeOffset(collider, position, orientation);
+  quat_getAngleAxis(orientation, &angle, &ax, &ay, &az);
+  lua_pushnumber(L, position[0]);
+  lua_pushnumber(L, position[1]);
+  lua_pushnumber(L, position[2]);
+  lua_pushnumber(L, angle);
+  lua_pushnumber(L, ax);
+  lua_pushnumber(L, ay);
+  lua_pushnumber(L, az);
+  return 7;
+}
+
+static int l_lovrColliderSetShapeOffset(lua_State* L) {
+  Collider* collider = luax_checktype(L, 1, Collider);
+  int index = 2;
+  float position[3], orientation[4];
+  index = luax_readvec3(L, index, position, NULL);
+  index = luax_readquat(L, index, orientation, NULL);
+  lovrColliderSetShapeOffset(collider, position, orientation);
+  return 0;
+}
+
 static int l_lovrColliderGetJoints(lua_State* L) {
   Collider* collider = luax_checktype(L, 1, Collider);
   size_t count;
@@ -561,6 +586,8 @@ const luaL_Reg lovrCollider[] = {
   { "getWorld", l_lovrColliderGetWorld },
   { "getShape", l_lovrColliderGetShape },
   { "setShape", l_lovrColliderSetShape },
+  { "getShapeOffset", l_lovrColliderGetShapeOffset },
+  { "setShapeOffset", l_lovrColliderSetShapeOffset },
   { "getJoints", l_lovrColliderGetJoints },
   { "getUserData", l_lovrColliderGetUserData },
   { "setUserData", l_lovrColliderSetUserData },
diff --git a/src/modules/physics/physics.h b/src/modules/physics/physics.h
index c40b8b9d..7e8d135b 100644
--- a/src/modules/physics/physics.h
+++ b/src/modules/physics/physics.h
@@ -95,6 +95,8 @@ World* lovrColliderGetWorld(Collider* collider);
 Collider* lovrColliderGetNext(Collider* collider);
 Shape* lovrColliderGetShape(Collider* collider);
 void lovrColliderSetShape(Collider* collider, Shape* shape);
+void lovrColliderGetShapeOffset(Collider* collider, float* position, float* orientation);
+void lovrColliderSetShapeOffset(Collider* collider, float* position, float* orientation);
 Joint** lovrColliderGetJoints(Collider* collider, size_t* count);
 const char* lovrColliderGetTag(Collider* collider);
 bool lovrColliderSetTag(Collider* collider, const char* tag);
diff --git a/src/modules/physics/physics_jolt.c b/src/modules/physics/physics_jolt.c
index 8e1b9c75..4ced7534 100644
--- a/src/modules/physics/physics_jolt.c
+++ b/src/modules/physics/physics_jolt.c
@@ -425,6 +425,46 @@ void lovrColliderSetShape(Collider* collider, Shape* shape) {
   }
 }
 
+void lovrColliderGetShapeOffset(Collider* collider, float* position, float* orientation) {
+  if (!collider->shape) {
+    vec3_set(position, 0.f, 0.f, 0.f);
+    quat_identity(orientation);
+    return;
+  }
+
+  const JPH_Shape* shape = JPH_BodyInterface_GetShape(collider->world->body_interface, collider->id);
+
+  if (JPH_Shape_GetSubType(shape) == JPH_ShapeSubType_RotatedTranslated) {
+    JPH_Vec3 jposition;
+    JPH_Quat jrotation;
+    JPH_RotatedTranslatedShape_GetPosition((JPH_RotatedTranslatedShape*) shape, &jposition);
+    JPH_RotatedTranslatedShape_GetRotation((JPH_RotatedTranslatedShape*) shape, &jrotation);
+    vec3_init(position, &jposition.x);
+    quat_init(orientation, &jrotation.x);
+  } else {
+    vec3_set(position, 0.f, 0.f, 0.f);
+    quat_identity(orientation);
+  }
+}
+
+void lovrColliderSetShapeOffset(Collider* collider, float* position, float* orientation) {
+  if (!collider->shape) {
+    return;
+  }
+
+  const JPH_Shape* shape = JPH_BodyInterface_GetShape(collider->world->body_interface, collider->id);
+
+  if (JPH_Shape_GetSubType(shape) == JPH_ShapeSubType_RotatedTranslated) {
+    JPH_Shape_Destroy((JPH_Shape*) shape);
+  }
+
+  JPH_Vec3 jposition = { position[0], position[1], position[2] };
+  JPH_Quat jrotation = { orientation[0], orientation[1], orientation[2], orientation[3] };
+  shape = (JPH_Shape*) JPH_RotatedTranslatedShape_Create(&jposition, &jrotation, collider->shape->shape);
+  bool updateMass = collider->shape->type == SHAPE_MESH || collider->shape->type == SHAPE_TERRAIN;
+  JPH_BodyInterface_SetShape(collider->world->body_interface, collider->id, shape, updateMass, JPH_Activation_Activate);
+}
+
 Joint** lovrColliderGetJoints(Collider* collider, size_t* count) {
   *count = collider->joints.length;
   return collider->joints.data;