From 29f4878dbca175db7d03d9875d9918ccbc9fc89e Mon Sep 17 00:00:00 2001 From: bjorn Date: Tue, 16 May 2017 19:13:38 -0600 Subject: [PATCH] Body:getMassData; Body:setMassData; --- src/api/types/body.c | 49 ++++++++++++++++++++++++++++++++++++++++++ src/api/types/shapes.c | 4 ++-- src/physics/physics.c | 43 +++++++++++++++++++++++++++++------- src/physics/physics.h | 4 +++- 4 files changed, 89 insertions(+), 11 deletions(-) diff --git a/src/api/types/body.c b/src/api/types/body.c index 6346f4ea..fd42b942 100644 --- a/src/api/types/body.c +++ b/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 } }; diff --git a/src/api/types/shapes.c b/src/api/types/shapes.c index a59bb2fe..afa5de3c 100644 --- a/src/api/types/shapes.c +++ b/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); } diff --git a/src/physics/physics.c b/src/physics/physics.c index ad8d9dc6..1a0fbeb8 100644 --- a/src/physics/physics.c +++ b/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) { diff --git a/src/physics/physics.h b/src/physics/physics.h index c83a128b..0cd9a777 100644 --- a/src/physics/physics.h +++ b/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);