Shape:getMass/Inertia/Center/MassData;

bjornbytes · Apr 30, 2024 · 68aaba9 · 68aaba9
1 parent ce217dd
commit 68aaba9
Show file tree

Hide file tree

Showing 4 changed files with 75 additions and 77 deletions.
diff --git a/src/api/l_physics_shapes.c b/src/api/l_physics_shapes.c
@@ -272,23 +272,50 @@ static int l_lovrShapeSetDensity(lua_State* L) {
   return 0;
 }
 
-static int l_lovrShapeGetMass(lua_State* L) {
+static int l_lovrShapeGetMassData(lua_State* L) {
   Shape* shape = luax_checkshape(L, 1);
-  float density = luax_checkfloat(L, 2);
-  float centerOfMass[3], mass, inertia[6];
-  lovrShapeGetMass(shape, density, centerOfMass, &mass, inertia);
-  lua_pushnumber(L, centerOfMass[0]);
-  lua_pushnumber(L, centerOfMass[1]);
-  lua_pushnumber(L, centerOfMass[2]);
+  float mass, inertia[9], center[3];
+  lovrShapeGetMassData(shape, &mass, inertia, center);
   lua_pushnumber(L, mass);
-  lua_newtable(L);
-  for (int i = 0; i < 6; i++) {
+  lua_createtable(L, 9, 0);
+  for (int i = 0; i < 9; i++) {
     lua_pushnumber(L, inertia[i]);
     lua_rawseti(L, -2, i + 1);
   }
+  lua_pushnumber(L, center[0]);
+  lua_pushnumber(L, center[1]);
+  lua_pushnumber(L, center[2]);
   return 5;
 }
 
+static int l_lovrShapeGetMass(lua_State* L) {
+  Shape* shape = luax_checkshape(L, 1);
+  float mass;
+  lovrShapeGetMassData(shape, &mass, NULL, NULL);
+  lua_pushnumber(L, mass);
+  return 1;
+}
+
+static int l_lovrShapeGetInertia(lua_State* L) {
+  Shape* shape = luax_checkshape(L, 1);
+  float inertia[9];
+  lovrShapeGetMassData(shape, NULL, inertia, NULL);
+  for (int i = 0; i < 9; i++) {
+    lua_pushnumber(L, inertia[i]);
+  }
+  return 9;
+}
+
+static int l_lovrShapeGetCenter(lua_State* L) {
+  Shape* shape = luax_checkshape(L, 1);
+  float center[3];
+  lovrShapeGetMassData(shape, NULL, NULL, center);
+  lua_pushnumber(L, center[0]);
+  lua_pushnumber(L, center[1]);
+  lua_pushnumber(L, center[2]);
+  return 3;
+}
+
 static int l_lovrShapeGetAABB(lua_State* L) {
   Shape* shape = luax_checkshape(L, 1);
   float position[3], orientation[4], aabb[6];
@@ -313,7 +340,10 @@ static int l_lovrShapeGetAABB(lua_State* L) {
   { "getVolume", l_lovrShapeGetVolume }, \
   { "getDensity", l_lovrShapeGetDensity }, \
   { "setDensity", l_lovrShapeSetDensity }, \
+  { "getMassData", l_lovrShapeGetMassData }, \
   { "getMass", l_lovrShapeGetMass }, \
+  { "getInertia", l_lovrShapeGetInertia }, \
+  { "getCenter", l_lovrShapeGetCenter }, \
   { "getAABB", l_lovrShapeGetAABB }
 
 static int l_lovrBoxShapeGetDimensions(lua_State* L) {

diff --git a/src/modules/physics/physics.h b/src/modules/physics/physics.h
@@ -178,7 +178,7 @@ ShapeType lovrShapeGetType(Shape* shape);
 float lovrShapeGetVolume(Shape* shape);
 float lovrShapeGetDensity(Shape* shape);
 void lovrShapeSetDensity(Shape* shape, float density);
-void lovrShapeGetMass(Shape* shape, float density, float centerOfMass[3], float* mass, float inertia[6]);
+void lovrShapeGetMassData(Shape* shape, float* mass, float inertia[9], float center[3]);
 void lovrShapeGetAABB(Shape* shape, float position[3], float orientation[4], float aabb[6]);
 
 BoxShape* lovrBoxShapeCreate(float dimensions[3]);

diff --git a/src/modules/physics/physics_jolt.c b/src/modules/physics/physics_jolt.c
@@ -963,21 +963,50 @@ float lovrShapeGetVolume(Shape* shape) {
 }
 
 float lovrShapeGetDensity(Shape* shape) {
-  if (shape->type == SHAPE_MESH || shape->type == SHAPE_TERRAIN) {
+  if (shape->type == SHAPE_MESH || shape->type == SHAPE_TERRAIN || shape->type == SHAPE_COMPOUND) {
     return 0.f;
   } else {
     return JPH_ConvexShape_GetDensity((JPH_ConvexShape*) shape->handle);
   }
 }
 
 void lovrShapeSetDensity(Shape* shape, float density) {
-  if (shape->type != SHAPE_MESH && shape->type != SHAPE_TERRAIN) {
+  if (shape->type != SHAPE_MESH && shape->type != SHAPE_TERRAIN && shape->type != SHAPE_COMPOUND) {
     JPH_ConvexShape_SetDensity((JPH_ConvexShape*) shape->handle, density);
   }
 }
 
-void lovrShapeGetMass(Shape* shape, float density, float centerOfMass[3], float* mass, float inertia[6]) {
-  //
+void lovrShapeGetMassData(Shape* shape, float* mass, float inertia[9], float center[3]) {
+  if (shape->type == SHAPE_MESH || shape->type == SHAPE_TERRAIN) {
+    if (mass) *mass = 0.f;
+    if (inertia) memset(inertia, 0, 9 * sizeof(float));
+    if (center) vec3_set(center, 0.f, 0.f, 0.f);
+  }
+
+  JPH_MassProperties properties;
+  JPH_Shape_GetMassProperties(shape->handle, &properties);
+
+  if (mass) *mass = properties.mass;
+
+  if (inertia) {
+    inertia[0] = properties.inertia.m11;
+    inertia[1] = properties.inertia.m12;
+    inertia[2] = properties.inertia.m13;
+    inertia[3] = properties.inertia.m21;
+    inertia[4] = properties.inertia.m22;
+    inertia[5] = properties.inertia.m23;
+    inertia[6] = properties.inertia.m31;
+    inertia[7] = properties.inertia.m32;
+    inertia[8] = properties.inertia.m33;
+  }
+
+  if (center) {
+    JPH_Vec3 centerOfMass;
+    JPH_Shape_GetCenterOfMass(shape->handle, &centerOfMass);
+    center[0] = centerOfMass.x;
+    center[1] = centerOfMass.y;
+    center[2] = centerOfMass.z;
+  }
 }
 
 void lovrShapeGetAABB(Shape* shape, float position[3], float orientation[4], float aabb[6]) {

diff --git a/src/modules/physics/physics_ode.c b/src/modules/physics/physics_ode.c
@@ -859,69 +859,8 @@ Collider* lovrShapeGetCollider(Shape* shape) {
   return shape->collider;
 }
 
-void lovrShapeGetMass(Shape* shape, float density, float centerOfMass[3], float* mass, float inertia[6]) {
-  dMass m;
-  dMassSetZero(&m);
-  switch (shape->type) {
-    case SHAPE_SPHERE: {
-      dMassSetSphere(&m, density, dGeomSphereGetRadius(shape->id));
-      break;
-    }
-
-    case SHAPE_BOX: {
-      dReal lengths[4];
-      dGeomBoxGetLengths(shape->id, lengths);
-      dMassSetBox(&m, density, lengths[0], lengths[1], lengths[2]);
-      break;
-    }
-
-    case SHAPE_CAPSULE: {
-      dReal radius, length;
-      dGeomCapsuleGetParams(shape->id, &radius, &length);
-      dMassSetCapsule(&m, density, 3, radius, length);
-      break;
-    }
-
-    case SHAPE_CYLINDER: {
-      dReal radius, length;
-      dGeomCylinderGetParams(shape->id, &radius, &length);
-      dMassSetCylinder(&m, density, 3, radius, length);
-      break;
-    }
-
-    case SHAPE_MESH: {
-      dMassSetTrimesh(&m, density, shape->id);
-      dGeomSetPosition(shape->id, -m.c[0], -m.c[1], -m.c[2]);
-      dMassTranslate(&m, -m.c[0], -m.c[1], -m.c[2]);
-      break;
-    }
-
-    case SHAPE_TERRAIN: {
-      break;
-    }
-
-    default: break;
-  }
-
-  const dReal* position = dGeomGetOffsetPosition(shape->id);
-  dMassTranslate(&m, position[0], position[1], position[2]);
-  const dReal* rotation = dGeomGetOffsetRotation(shape->id);
-  dMassRotate(&m, rotation);
-
-  centerOfMass[0] = m.c[0];
-  centerOfMass[1] = m.c[1];
-  centerOfMass[2] = m.c[2];
-  *mass = m.mass;
-
-  // Diagonal
-  inertia[0] = m.I[0];
-  inertia[1] = m.I[5];
-  inertia[2] = m.I[10];
-
-  // Lower triangular
-  inertia[3] = m.I[4];
-  inertia[4] = m.I[8];
-  inertia[5] = m.I[9];
+void lovrShapeGetMassData(Shape* shape, float* mass, float inertia[9], float center[3]) {
+  //
 }
 
 void lovrShapeGetAABB(Shape* shape, float position[3], float orientation[4], float aabb[6]) {