Shape:computeMass;

I think

src/api/types/shapes.c

@@ -81,9 +81,9 @@ int l_lovrShapeGetPosition(lua_State* L) {
 
 int l_lovrShapeSetPosition(lua_State* L) {
   Shape* shape = luax_checktypeof(L, 1, Shape);
-  float x = luaL_checknumber(L, 1);
-  float y = luaL_checknumber(L, 2);
-  float z = luaL_checknumber(L, 3);
+  float x = luaL_checknumber(L, 2);
+  float y = luaL_checknumber(L, 3);
+  float z = luaL_checknumber(L, 4);
   lovrShapeSetPosition(shape, x, y, z);
   return 0;
 }
@@ -101,10 +101,10 @@ int l_lovrShapeGetOrientation(lua_State* L) {
 
 int l_lovrShapeSetOrientation(lua_State* L) {
   Shape* shape = luax_checktypeof(L, 1, Shape);
-  float angle = luaL_checknumber(L, 1);
-  float x = luaL_checknumber(L, 2);
-  float y = luaL_checknumber(L, 3);
-  float z = luaL_checknumber(L, 4);
+  float angle = luaL_checknumber(L, 2);
+  float x = luaL_checknumber(L, 3);
+  float y = luaL_checknumber(L, 4);
+  float z = luaL_checknumber(L, 5);
   lovrShapeSetOrientation(shape, angle, x, y, z);
   return 0;
 }
@@ -163,6 +163,24 @@ int l_lovrShapeSetMask(lua_State* L) {
   return 0;
 }
 
+int l_lovrShapeComputeMass(lua_State* L) {
+  Shape* shape = luax_checktypeof(L, 1, Shape);
+  float density = luaL_checknumber(L, 2);
+  float cx, cy, cz, mass;
+  float inertia[9];
+  lovrShapeComputeMass(shape, density, &cx, &cy, &cz, &mass, inertia);
+  lua_pushnumber(L, cx);
+  lua_pushnumber(L, cy);
+  lua_pushnumber(L, cz);
+  lua_pushnumber(L, mass);
+  lua_newtable(L);
+  for (int i = 0; i < 9; i++) {
+    lua_pushnumber(L, inertia[i]);
+    lua_rawseti(L, -2, i + 1);
+  }
+  return 5;
+}
+
 const luaL_Reg lovrShape[] = {
   { "getType", l_lovrShapeGetType },
   { "getBody", l_lovrShapeGetBody },
@@ -179,6 +197,7 @@ const luaL_Reg lovrShape[] = {
   { "setCategory", l_lovrShapeSetCategory },
   { "getMask", l_lovrShapeGetMask },
   { "setMask", l_lovrShapeSetMask },
+  { "computeMass", l_lovrShapeComputeMass },
   { NULL, NULL }
 };
 

src/physics/physics.c

@@ -342,6 +342,57 @@ void lovrShapeSetMask(Shape* shape, uint32_t mask) {
   dGeomSetCollideBits(shape->id, mask);
 }
 
+void lovrShapeComputeMass(Shape* shape, float density, float* cx, float* cy, float* cz, float* mass, float inertia[9]) {
+  dMass m;
+  dMassSetZero(&m);
+  switch (shape->type) {
+    case SHAPE_SPHERE: {
+      dMassSetSphere(&m, density, dGeomSphereGetRadius(shape->id));
+      break;
+    }
+
+    case SHAPE_BOX: {
+      dReal lengths[3];
+      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;
+    }
+  }
+
+  const dReal* position = dGeomGetOffsetPosition(shape->id);
+  dMassTranslate(&m, position[0], position[1], position[2]);
+  const dReal* rotation = dGeomGetOffsetRotation(shape->id);
+  dMassRotate(&m, rotation);
+
+  *cx = m.c[0];
+  *cy = m.c[1];
+  *cz = m.c[2];
+  *mass = m.mass;
+  inertia[0] = m.I[0];
+  inertia[1] = m.I[4];
+  inertia[2] = m.I[8];
+  inertia[3] = m.I[1];
+  inertia[4] = m.I[5];
+  inertia[5] = m.I[9];
+  inertia[6] = m.I[2];
+  inertia[7] = m.I[6];
+  inertia[8] = m.I[10];
+}
+
 SphereShape* lovrSphereShapeCreate(float radius) {
   SphereShape* sphere = lovrAlloc(sizeof(SphereShape), lovrShapeDestroy);
   if (!sphere) return NULL;

src/physics/physics.h

@@ -96,6 +96,7 @@ uint32_t lovrShapeGetCategory(Shape* shape);
 void lovrShapeSetCategory(Shape* shape, uint32_t category);
 uint32_t lovrShapeGetMask(Shape* shape);
 void lovrShapeSetMask(Shape* shape, uint32_t mask);
+void lovrShapeComputeMass(Shape* shape, float density, float* cx, float* cy, float* cz, float* mass, float inertia[9]);
 
 SphereShape* lovrSphereShapeCreate(float radius);
 float lovrSphereShapeGetRadius(SphereShape* sphere);