From f3df7aa11426943445f35a22805ae0659e0fdf56 Mon Sep 17 00:00:00 2001
From: bjorn <tie.372@gmail.com>
Date: Wed, 24 May 2017 17:40:02 -0700
Subject: [PATCH] World:raycast;

---
 src/api/types/world.c | 28 ++++++++++++++++++++++++++++
 src/physics/physics.c | 28 ++++++++++++++++++++++++++++
 src/physics/physics.h |  7 +++++++
 3 files changed, 63 insertions(+)

diff --git a/src/api/types/world.c b/src/api/types/world.c
index 5644ddb6..af7f6ddf 100644
--- a/src/api/types/world.c
+++ b/src/api/types/world.c
@@ -22,6 +22,19 @@ static int nextOverlap(lua_State* L) {
   }
 }
 
+static void raycastCallback(Shape* shape, float x, float y, float z, float nx, float ny, float nz, void* userdata) {
+  lua_State* L = userdata;
+  luaL_checktype(L, -1, LUA_TFUNCTION);
+  luax_pushshape(L, shape);
+  lua_pushnumber(L, x);
+  lua_pushnumber(L, y);
+  lua_pushnumber(L, z);
+  lua_pushnumber(L, nx);
+  lua_pushnumber(L, ny);
+  lua_pushnumber(L, nz);
+  lua_call(L, 7, 0);
+}
+
 int l_lovrWorldNewCollider(lua_State* L) {
   World* world = luax_checktype(L, 1, World);
   Collider* collider = lovrColliderCreate(world);
@@ -173,6 +186,20 @@ int l_lovrWorldSetSleepingAllowed(lua_State* L) {
   return 0;
 }
 
+int l_lovrWorldRaycast(lua_State* L) {
+  World* world = luax_checktype(L, 1, World);
+  float x1 = luaL_checknumber(L, 2);
+  float y1 = luaL_checknumber(L, 3);
+  float z1 = luaL_checknumber(L, 4);
+  float x2 = luaL_checknumber(L, 5);
+  float y2 = luaL_checknumber(L, 6);
+  float z2 = luaL_checknumber(L, 7);
+  luaL_checktype(L, 8, LUA_TFUNCTION);
+  lua_settop(L, 8);
+  lovrWorldRaycast(world, x1, y1, z1, x2, y2, z2, raycastCallback, L);
+  return 0;
+}
+
 const luaL_Reg lovrWorld[] = {
   { "newCollider", l_lovrWorldNewCollider },
   { "newBoxCollider", l_lovrWorldNewBoxCollider },
@@ -192,5 +219,6 @@ const luaL_Reg lovrWorld[] = {
   { "setAngularDamping", l_lovrWorldSetAngularDamping },
   { "isSleepingAllowed", l_lovrWorldIsSleepingAllowed },
   { "setSleepingAllowed", l_lovrWorldSetSleepingAllowed },
+  { "raycast", l_lovrWorldRaycast },
   { NULL, NULL }
 };
diff --git a/src/physics/physics.c b/src/physics/physics.c
index 1ad8accc..b8a7737b 100644
--- a/src/physics/physics.c
+++ b/src/physics/physics.c
@@ -12,6 +12,22 @@ static void customNearCallback(void* data, dGeomID shapeA, dGeomID shapeB) {
   vec_push(&world->overlaps, dGeomGetData(shapeB));
 }
 
+static void raycastCallback(void* data, dGeomID a, dGeomID b) {
+  RaycastCallback callback = ((RaycastData*) data)->callback;
+  void* userdata = ((RaycastData*) data)->userdata;
+  Shape* shape = dGeomGetData(b);
+
+  if (!shape) {
+    return;
+  }
+
+  dContact contact;
+  if (dCollide(a, b, MAX_CONTACTS, &contact.geom, sizeof(dContact))) {
+    dContactGeom g = contact.geom;
+    callback(shape, g.pos[0], g.pos[1], g.pos[2], g.normal[0], g.normal[1], g.normal[2], userdata);
+  }
+}
+
 void lovrPhysicsInit() {
   dInitODE();
 
@@ -165,6 +181,18 @@ void lovrWorldSetSleepingAllowed(World* world, int allowed) {
   dWorldSetAutoDisableFlag(world->id, allowed);
 }
 
+void lovrWorldRaycast(World* world, float x1, float y1, float z1, float x2, float y2, float z2, RaycastCallback callback, void* userdata) {
+  RaycastData data = { .callback = callback, .userdata = userdata };
+  float dx = x2 - x1;
+  float dy = y2 - y1;
+  float dz = z2 - z1;
+  float length = sqrt(dx * dx + dy * dy + dz * dz);
+  dGeomID ray = dCreateRay(world->space, length);
+  dGeomRaySet(ray, x1, y1, z1, dx, dy, dz);
+  dSpaceCollide2(ray, (dGeomID) world->space, &data, raycastCallback);
+  dGeomDestroy(ray);
+}
+
 Collider* lovrColliderCreate(World* world) {
   if (!world) {
     error("No world specified");
diff --git a/src/physics/physics.h b/src/physics/physics.h
index a2fe39fe..50d7ad41 100644
--- a/src/physics/physics.h
+++ b/src/physics/physics.h
@@ -60,6 +60,12 @@ typedef struct {
 typedef Joint BallJoint;
 
 typedef void (*CollisionResolver)(World* world, void* userdata);
+typedef void (*RaycastCallback)(Shape* shape, float x, float y, float z, float nx, float ny, float nz, void* userdata);
+
+typedef struct {
+  RaycastCallback callback;
+  void* userdata;
+} RaycastData;
 
 void lovrPhysicsInit();
 void lovrPhysicsDestroy();
@@ -79,6 +85,7 @@ void lovrWorldGetAngularDamping(World* world, float* damping, float* threshold);
 void lovrWorldSetAngularDamping(World* world, float damping, float threshold);
 int lovrWorldIsSleepingAllowed(World* world);
 void lovrWorldSetSleepingAllowed(World* world, int allowed);
+void lovrWorldRaycast(World* world, float x1, float y1, float z1, float x2, float y2, float z2, RaycastCallback callback, void* userdata);
 
 Collider* lovrColliderCreate();
 void lovrColliderDestroy(const Ref* ref);