From b31970d85de2ae0ca8998c2743d6985e0604d65e Mon Sep 17 00:00:00 2001
From: bjorn <tie.372@gmail.com>
Date: Mon, 15 May 2017 23:22:25 -0600
Subject: [PATCH] Body:getLocalPoint; Body:getWorldPoint;

---
 src/api/types/body.c  | 28 ++++++++++++++++++++++++++++
 src/physics/physics.c | 16 ++++++++++++++++
 src/physics/physics.h |  2 ++
 3 files changed, 46 insertions(+)

diff --git a/src/api/types/body.c b/src/api/types/body.c
index 689b05b2..91ff9ceb 100644
--- a/src/api/types/body.c
+++ b/src/api/types/body.c
@@ -153,6 +153,32 @@ int l_lovrBodySetKinematic(lua_State* L) {
   return 0;
 }
 
+int l_lovrBodyGetLocalPoint(lua_State* L) {
+  Body* body = luax_checktype(L, 1, Body);
+  float wx = luaL_checknumber(L, 2);
+  float wy = luaL_checknumber(L, 3);
+  float wz = luaL_checknumber(L, 4);
+  float x, y, z;
+  lovrBodyGetLocalPoint(body, wx, wy, wz, &x, &y, &z);
+  lua_pushnumber(L, x);
+  lua_pushnumber(L, y);
+  lua_pushnumber(L, z);
+  return 3;
+}
+
+int l_lovrBodyGetWorldPoint(lua_State* L) {
+  Body* body = luax_checktype(L, 1, Body);
+  float x = luaL_checknumber(L, 2);
+  float y = luaL_checknumber(L, 3);
+  float z = luaL_checknumber(L, 4);
+  float wx, wy, wz;
+  lovrBodyGetWorldPoint(body, x, y, z, &wx, &wy, &wz);
+  lua_pushnumber(L, wx);
+  lua_pushnumber(L, wy);
+  lua_pushnumber(L, wz);
+  return 3;
+}
+
 const luaL_Reg lovrBody[] = {
   { "getPosition", l_lovrBodyGetPosition },
   { "setPosition", l_lovrBodySetPosition },
@@ -170,5 +196,7 @@ const luaL_Reg lovrBody[] = {
   { "applyTorque", l_lovrBodyApplyForce },
   { "isKinematic", l_lovrBodyIsKinematic },
   { "setKinematic", l_lovrBodySetKinematic },
+  { "getLocalPoint", l_lovrBodyGetLocalPoint },
+  { "getWorldPoint", l_lovrBodyGetWorldPoint },
   { NULL, NULL }
 };
diff --git a/src/physics/physics.c b/src/physics/physics.c
index 7a2549ff..4d30efe8 100644
--- a/src/physics/physics.c
+++ b/src/physics/physics.c
@@ -184,3 +184,19 @@ void lovrBodySetKinematic(Body* body, int kinematic) {
     dBodySetDynamic(body->id);
   }
 }
+
+void lovrBodyGetLocalPoint(Body* body, float wx, float wy, float wz, float* x, float* y, float* z) {
+  dReal local[3];
+  dBodyGetPosRelPoint(body->id, wx, wy, wz, local);
+  *x = local[0];
+  *y = local[1];
+  *z = local[2];
+}
+
+void lovrBodyGetWorldPoint(Body* body, float x, float y, float z, float* wx, float* wy, float* wz) {
+  dReal world[3];
+  dBodyGetRelPointPos(body->id, x, y, z, world);
+  *wx = world[0];
+  *wy = world[1];
+  *wz = world[2];
+}
diff --git a/src/physics/physics.h b/src/physics/physics.h
index bbd7168f..6c7d73ff 100644
--- a/src/physics/physics.h
+++ b/src/physics/physics.h
@@ -46,3 +46,5 @@ void lovrBodyApplyForceAtPosition(Body* body, float x, float y, float z, float c
 void lovrBodyApplyTorque(Body* body, float x, float y, float z);
 int lovrBodyIsKinematic(Body* body);
 void lovrBodySetKinematic(Body* body, int kinematic);
+void lovrBodyGetLocalPoint(Body* body, float wx, float wy, float wz, float* x, float* y, float* z);
+void lovrBodyGetWorldPoint(Body* body, float x, float y, float z, float* wx, float* wy, float* wz);