diff --git a/src/api/types/shapes.c b/src/api/types/shapes.c index b581ec8e..a59bb2fe 100644 --- a/src/api/types/shapes.c +++ b/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 } }; diff --git a/src/physics/physics.c b/src/physics/physics.c index 37b3f7c8..64f63ba1 100644 --- a/src/physics/physics.c +++ b/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; diff --git a/src/physics/physics.h b/src/physics/physics.h index e18cc313..c79a788d 100644 --- a/src/physics/physics.h +++ b/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);