From 35bab33e54b70e0e2ef07376a74466cba592fcb1 Mon Sep 17 00:00:00 2001
From: bjorn <tie.372@gmail.com>
Date: Mon, 15 May 2017 23:27:33 -0600
Subject: [PATCH] Body:getLinearVelocityFromLocalPoint;
 Body:getLinearVelocityFromWorldPoint;

---
 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 91ff9ceb..265a10eb 100644
--- a/src/api/types/body.c
+++ b/src/api/types/body.c
@@ -179,6 +179,32 @@ int l_lovrBodyGetWorldPoint(lua_State* L) {
   return 3;
 }
 
+int l_lovrBodyGetLinearVelocityFromLocalPoint(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 vx, vy, vz;
+  lovrBodyGetLinearVelocityFromLocalPoint(body, x, y, z, &vx, &vy, &vz);
+  lua_pushnumber(L, vx);
+  lua_pushnumber(L, vy);
+  lua_pushnumber(L, vz);
+  return 3;
+}
+
+int l_lovrBodyGetLinearVelocityFromWorldPoint(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 vx, vy, vz;
+  lovrBodyGetLinearVelocityFromWorldPoint(body, x, y, z, &vx, &vy, &vz);
+  lua_pushnumber(L, vx);
+  lua_pushnumber(L, vy);
+  lua_pushnumber(L, vz);
+  return 3;
+}
+
 const luaL_Reg lovrBody[] = {
   { "getPosition", l_lovrBodyGetPosition },
   { "setPosition", l_lovrBodySetPosition },
@@ -198,5 +224,7 @@ const luaL_Reg lovrBody[] = {
   { "setKinematic", l_lovrBodySetKinematic },
   { "getLocalPoint", l_lovrBodyGetLocalPoint },
   { "getWorldPoint", l_lovrBodyGetWorldPoint },
+  { "getLinearVelocityFromLocalPoint", l_lovrBodyGetLinearVelocityFromLocalPoint },
+  { "getLinearVelocityFromWorldPoint", l_lovrBodyGetLinearVelocityFromWorldPoint },
   { NULL, NULL }
 };
diff --git a/src/physics/physics.c b/src/physics/physics.c
index 4d30efe8..d3b72eb6 100644
--- a/src/physics/physics.c
+++ b/src/physics/physics.c
@@ -200,3 +200,19 @@ void lovrBodyGetWorldPoint(Body* body, float x, float y, float z, float* wx, flo
   *wy = world[1];
   *wz = world[2];
 }
+
+void lovrBodyGetLinearVelocityFromLocalPoint(Body* body, float x, float y, float z, float* vx, float* vy, float* vz) {
+  dReal velocity[3];
+  dBodyGetRelPointVel(body->id, x, y, z, velocity);
+  *vx = velocity[0];
+  *vy = velocity[1];
+  *vz = velocity[2];
+}
+
+void lovrBodyGetLinearVelocityFromWorldPoint(Body* body, float wx, float wy, float wz, float* vx, float* vy, float* vz) {
+  dReal velocity[3];
+  dBodyGetPointVel(body->id, wx, wy, wz, velocity);
+  *vx = velocity[0];
+  *vy = velocity[1];
+  *vz = velocity[2];
+}
diff --git a/src/physics/physics.h b/src/physics/physics.h
index 6c7d73ff..47b5de38 100644
--- a/src/physics/physics.h
+++ b/src/physics/physics.h
@@ -48,3 +48,5 @@ 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);
+void lovrBodyGetLinearVelocityFromLocalPoint(Body* body, float x, float y, float z, float* vx, float* vy, float* vz);
+void lovrBodyGetLinearVelocityFromWorldPoint(Body* body, float wx, float wy, float wz, float* vx, float* vy, float* vz);