Body:getMassData; Body:setMassData;

src/api/types/body.c

@@ -300,6 +300,53 @@ int l_lovrBodySetMass(lua_State* L) {
   return 0;
 }
 
+int l_lovrBodyGetMassData(lua_State* L) {
+  Body* body = luax_checktype(L, 1, Body);
+  float cx, cy, cz, mass;
+  float inertia[6];
+  lovrBodyGetMassData(body, &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 < 6; i++) {
+    lua_pushnumber(L, inertia[i]);
+    lua_rawseti(L, -2, i + 1);
+  }
+  return 5;
+}
+
+int l_lovrBodySetMassData(lua_State* L) {
+  Body* body = luax_checktype(L, 1, Body);
+  float cx = luaL_checknumber(L, 2);
+  float cy = luaL_checknumber(L, 3);
+  float cz = luaL_checknumber(L, 4);
+  float mass = luaL_checknumber(L, 5);
+  float inertia[6];
+  if (lua_istable(L, 6) && lua_objlen(L, 6) >= 6) {
+    for (int i = 0; i < 6; i++) {
+      lua_rawgeti(L, 6, i + 1);
+      if (!lua_isnumber(L, -1)) {
+        return luaL_argerror(L, 6, "Expected 6 numbers or a table with 6 numbers");
+      }
+
+      inertia[i] = lua_tonumber(L, -1);
+      lua_pop(L, 1);
+    }
+  } else {
+    for (int i = 6; i < 12; i++) {
+      if (lua_isnumber(L, i)) {
+        inertia[i] = lua_tonumber(L, i);
+      } else {
+        return luaL_argerror(L, i, "Expected 6 numbers or a table with 6 numbers");
+      }
+    }
+  }
+  lovrBodySetMassData(body, cx, cy, cz, mass, inertia);
+  return 0;
+}
+
 const luaL_Reg lovrBody[] = {
   { "getPosition", l_lovrBodyGetPosition },
   { "setPosition", l_lovrBodySetPosition },
@@ -332,5 +379,7 @@ const luaL_Reg lovrBody[] = {
   { "getWorld", l_lovrBodyGetWorld },
   { "getMass", l_lovrBodyGetMass },
   { "setMass", l_lovrBodySetMass },
+  { "getMassData", l_lovrBodyGetMassData },
+  { "setMassData", l_lovrBodySetMassData },
   { NULL, NULL }
 };

src/api/types/shapes.c

@@ -167,14 +167,14 @@ 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];
+  float inertia[6];
   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++) {
+  for (int i = 0; i < 6; i++) {
     lua_pushnumber(L, inertia[i]);
     lua_rawseti(L, -2, i + 1);
   }

src/physics/physics.c

@@ -277,6 +277,32 @@ void lovrBodySetMass(Body* body, float mass) {
   dBodySetMass(body->id, &m);
 }
 
+void lovrBodyGetMassData(Body* body, float* cx, float* cy, float* cz, float* mass, float inertia[6]) {
+  dMass m;
+  dBodyGetMass(body->id, &m);
+  *cx = m.c[0];
+  *cy = m.c[1];
+  *cz = 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 lovrBodySetMassData(Body* body, float cx, float cy, float cz, float mass, float inertia[]) {
+  dMass m;
+  dBodyGetMass(body->id, &m);
+  dMassSetParameters(&m, mass, cx, cy, cz, inertia[0], inertia[1], inertia[2], inertia[3], inertia[4], inertia[5]);
+  dBodySetMass(body->id, &m);
+}
+
 void lovrShapeDestroy(const Ref* ref) {
   Shape* shape = containerof(ref, Shape);
   dGeomDestroy(shape->id);
@@ -355,7 +381,7 @@ 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]) {
+void lovrShapeComputeMass(Shape* shape, float density, float* cx, float* cy, float* cz, float* mass, float inertia[6]) {
   dMass m;
   dMassSetZero(&m);
   switch (shape->type) {
@@ -395,15 +421,16 @@ void lovrShapeComputeMass(Shape* shape, float density, float* cx, float* cy, flo
   *cy = m.c[1];
   *cz = m.c[2];
   *mass = m.mass;
+
+  // Diagonal
   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[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];
-  inertia[6] = m.I[2];
-  inertia[7] = m.I[6];
-  inertia[8] = m.I[10];
 }
 
 SphereShape* lovrSphereShapeCreate(float radius) {

src/physics/physics.h

@@ -81,6 +81,8 @@ void lovrBodySetUserData(Body* body, void* data);
 World* lovrBodyGetWorld(Body* body);
 float lovrBodyGetMass(Body* body);
 void lovrBodySetMass(Body* body, float mass);
+void lovrBodyGetMassData(Body* body, float* cx, float* cy, float* cz, float* mass, float inertia[6]);
+void lovrBodySetMassData(Body* body, float cx, float cy, float cz, float mass, float inertia[6]);
 
 void lovrShapeDestroy(const Ref* ref);
 ShapeType lovrShapeGetType(Shape* shape);
@@ -98,7 +100,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]);
+void lovrShapeComputeMass(Shape* shape, float density, float* cx, float* cy, float* cz, float* mass, float inertia[6]);
 
 SphereShape* lovrSphereShapeCreate(float radius);
 float lovrSphereShapeGetRadius(SphereShape* sphere);