Add AABBs

programs/cubes.c

@@ -14,7 +14,7 @@ TPE_Vec3 cubePositions[6];
 TPE_Joint ballJoints[4];
 TPE_Connection ballConnections[3];
 
-uint8_t debugDrawOn = 1;
+uint8_t debugDrawOn = 0;
 
 void updateOrientPos(int i)
 {
@@ -35,6 +35,8 @@ int main(void)
 
   s3l_scene.camera.transform.translation.z -= ROOM_SIZE / 2;
   s3l_scene.camera.transform.translation.y += ROOM_SIZE / 3;
+  s3l_scene.camera.transform.translation.x -= ROOM_SIZE / 4;
+  s3l_scene.camera.transform.rotation.y = -1 * TPE_FRACTIONS_PER_UNIT / 16;
 
   for (int i = 0; i < 6; ++i)
     helper_addBox(CUBE_SIZE / 2,CUBE_SIZE / 2,CUBE_SIZE / 2,CUBE_SIZE / 4,100);
@@ -61,15 +63,13 @@ int main(void)
   ballJoints[1] = TPE_joint(TPE_vec3(0,ROOM_SIZE / 2 - 100,-600),0);
   ballJoints[2] = TPE_joint(TPE_vec3(0,ROOM_SIZE / 2 - 200,-1200),0);
   ballJoints[3] = TPE_joint(TPE_vec3(0,ROOM_SIZE / 2 - 300,-1800),400);
-
-ballJoints[3].velocity[1] = -30;
+  ballJoints[3].velocity[1] = -100;
 
   ballConnections[0].joint1 = 0; ballConnections[0].joint2 = 1;
   ballConnections[1].joint1 = 1; ballConnections[1].joint2 = 2;
   ballConnections[2].joint1 = 2; ballConnections[2].joint2 = 3;
 
-  TPE_bodyInit(&tpe_world.bodies[6],ballJoints,4,ballConnections,3,
-    1000);
+  TPE_bodyInit(&tpe_world.bodies[6],ballJoints,4,ballConnections,3,10000);
 
   tpe_world.bodyCount++;
     
@@ -95,16 +95,12 @@ ballJoints[3].velocity[1] = -30;
         }
     }
 
-tpe_world.bodies[6].joints[0].position = TPE_vec3(0,ROOM_SIZE / 2 - 10,0);
-tpe_world.bodies[6].joints[0].velocity[0] = 0;
-tpe_world.bodies[6].joints[0].velocity[1] = 0;
-tpe_world.bodies[6].joints[0].velocity[2] = 0;
+    TPE_jointPin(&tpe_world.bodies[6].joints[0],TPE_vec3(0,ROOM_SIZE / 2 - 10,0));
 
-tpe_world.bodies[6].deactivateCount = 0;
+    tpe_world.bodies[6].deactivateCount = 0;
 
     TPE_worldStep(&tpe_world);
 
-
     helper_set3dColor(100,100,100);
     helper_draw3dCubeInside(TPE_vec3(0,ROOM_SIZE / 4,0),TPE_vec3(ROOM_SIZE,ROOM_SIZE / 2,ROOM_SIZE),TPE_vec3(0,0,0));
 
@@ -121,9 +117,15 @@ tpe_world.bodies[6].deactivateCount = 0;
       helper_draw3dCube(cubePositions[i],TPE_vec3(CUBE_SIZE,CUBE_SIZE,CUBE_SIZE),cubeOrientations[i]);
     }
 
-tpe_world.bodies[6].joints[3].velocity[1] -= 5;
+    tpe_world.bodies[6].joints[3].velocity[1] -= 5;
+
+    helper_draw3dSphere(tpe_world.bodies[6].joints[3].position,TPE_vec3(400,400,400),TPE_vec3(0,0,0)  );
 
-helper_draw3dSphere(tpe_world.bodies[6].joints[3].position,TPE_vec3(400,400,400),TPE_vec3(0,0,0)  );
+    for (int i = 0; i < 3; ++i)
+      helper_drawLine3D(
+        tpe_world.bodies[6].joints[tpe_world.bodies[6].connections[i].joint1].position,
+        tpe_world.bodies[6].joints[tpe_world.bodies[6].connections[i].joint2].position,
+        255,0,0);
 
     if (debugDrawOn)
       helper_debugDraw();

programs/helper.h

@@ -8,6 +8,7 @@
 #include "../tinyphysicsengine.h"
 #include <SDL2/SDL.h>
 #include <math.h>
+#include <sys/time.h> // for measuring time
 
 #ifndef RES_X
   #define RES_X 640
@@ -226,20 +227,41 @@ S3L_Scene s3l_scene;
 
 S3L_Vec4 helper_cameraForw, helper_cameraRight, helper_cameraUp;
 
+unsigned long helper_getMicroSecs(void)
+{
+  struct timeval t;
+  gettimeofday(&t,NULL);
+  return 1000000 * t.tv_sec + t.tv_usec;
+}
+
+void _helper_bodyAdded(int joints, int conns, TPE_Unit mass)
+{
+  TPE_bodyInit(&tpe_bodies[tpe_world.bodyCount],
+    &tpe_joints[helper_jointsUsed],joints,
+    &tpe_connections[helper_connectionsUsed],conns,mass);
+ 
+  helper_jointsUsed += joints;
+  helper_connectionsUsed += conns;
+
+  tpe_world.bodyCount++;
+}
+
 void helper_addBox(TPE_Unit w, TPE_Unit h, TPE_Unit d, TPE_Unit jointSize, TPE_Unit mass)
 {
   TPE_makeBox(
     tpe_joints + helper_jointsUsed,
     tpe_connections + helper_connectionsUsed,w,h,d,jointSize);
 
-  TPE_bodyInit(&tpe_bodies[tpe_world.bodyCount],
-    &tpe_joints[helper_jointsUsed],8,
-    &tpe_connections[helper_connectionsUsed],16,mass);
- 
-  helper_jointsUsed += 8;
-  helper_connectionsUsed += 16;
+  _helper_bodyAdded(8,16,mass);
+}
 
-  tpe_world.bodyCount++;
+void helper_addTriangle(TPE_Unit s, TPE_Unit d, TPE_Unit mass)
+{
+  TPE_makeTriangle(
+    tpe_joints + helper_jointsUsed,
+    tpe_connections + helper_connectionsUsed,s,d);
+
+  _helper_bodyAdded(3,3,mass);
 }
 
 void helper_printCamera(void)
@@ -299,6 +321,39 @@ void sdl_drawPixel(int x, int y, uint8_t r, uint8_t g, uint8_t b)
   *pixel = r;
 }
 
+void helper_drawLine2D(int x1, int y1, int x2, int y2, uint8_t r, uint8_t g,
+  uint8_t b)
+{
+  // stupid algorithm
+
+  x2 -= x1;
+  y2 -= y1;
+
+  int max = (x2 * x2 > y2 * y2) ? x2 : y2;
+
+  if (max < 0)
+    max *= -1;
+
+  for (int i = 0; i < max; ++i)
+    sdl_drawPixel(x1 + (x2 * i) / max,y1 + (y2 * i) / max,r,g,b);
+}
+
+void helper_drawLine3D(TPE_Vec3 p1, TPE_Vec3 p2, uint8_t rr, uint8_t gg,
+  uint8_t bb)
+{
+  S3L_Vec4 a, b, c, d;
+
+  a.x = p1.x; a.y = p1.y; a.z = p1.z; a.w = 0;
+  b.x = p2.x; b.y = p2.y; b.z = p2.z; b.w = 0;
+
+  S3L_project3DPointToScreen(a,s3l_scene.camera,&c);
+  S3L_project3DPointToScreen(b,s3l_scene.camera,&d);
+  
+  if (c.x >= 0 && c.x < S3L_RESOLUTION_X && c.y >= 0 && c.y < S3L_RESOLUTION_Y && c.z > 0 &&
+      d.x >= 0 && d.x < S3L_RESOLUTION_X && d.y >= 0 && d.y < S3L_RESOLUTION_Y && d.z > 0)
+    helper_drawLine2D(c.x,c.y,d.x,d.y,rr,gg,bb);
+}
+
 void tpe_debugDrawPixel(uint16_t x, uint16_t y, uint8_t color)
 {
   if (x < S3L_RESOLUTION_X - 2 && y < S3L_RESOLUTION_Y - 2)
@@ -475,4 +530,5 @@ void helper_frameEnd(void)
 
 void helper_end(void)
 {
+  // TODO
 }

tinyphysicsengine.h

@@ -41,6 +41,8 @@ typedef int16_t TPE_UnitReduced;        ///< Like TPE_Unit but saving space
 
 #define TPE_JOINT_SIZE_MULTIPLIER 32
 
+#define TPE_INFINITY 2147483647
+
 #define TPE_JOINT_SIZE(joint) ((joint).sizeDivided * TPE_JOINT_SIZE_MULTIPLIER)
 
 #ifndef TPE_APPROXIMATE_LENGTH
@@ -248,8 +250,15 @@ uint8_t TPE_jointEnvironmentResolveCollision(TPE_Joint *joint, TPE_Unit elastici
 uint8_t TPE_bodyEnvironmentResolveCollision(TPE_Body *body, 
   TPE_ClosestPointFunction env);
 
+void TPE_bodyGetAABB(const TPE_Body *body, TPE_Vec3 *vMin, TPE_Vec3 *vMax);
+
+uint8_t TPE_checkOverlapAABB(TPE_Vec3 v1Min, TPE_Vec3 v1Max, TPE_Vec3 v2Min,
+  TPE_Vec3 v2Max);
+
 uint8_t TPE_bodiesResolveCollision(TPE_Body *b1, TPE_Body *b2);
 
+void TPE_jointPin(TPE_Joint *joint, TPE_Vec3 position);
+
 // -----------------------------------------------------------------------------
 // body generation functions:
 
@@ -602,11 +611,23 @@ void TPE_worldStep(TPE_World *world)
     }
 
     TPE_Connection *connection = body->connections;
+
+TPE_Vec3 aabbMin, aabbMax;
+
+TPE_bodyGetAABB(body,&aabbMin,&aabbMax);
  
     for (uint16_t j = 0; j < world->bodyCount; ++j)
     {
       if (j > i ||  (world->bodies[j].flags & TPE_BODY_FLAG_DEACTIVATED))
-        if (TPE_bodiesResolveCollision(body,world->bodies + j)) // TODO: nested if
+{
+
+TPE_Vec3 aabbMin2, aabbMax2;
+
+TPE_bodyGetAABB(&world->bodies[j],&aabbMin2,&aabbMax2);
+
+        if (
+TPE_checkOverlapAABB(aabbMin,aabbMax,aabbMin2,aabbMax2) &&
+TPE_bodiesResolveCollision(body,world->bodies + j)) // TODO: nested if
         {
           TPE_bodyActivate(body);
           body->deactivateCount = TPE_LIGHT_DEACTIVATION; 
@@ -614,6 +635,9 @@ void TPE_worldStep(TPE_World *world)
           TPE_bodyActivate(world->bodies + j);
           world->bodies[j].deactivateCount = TPE_LIGHT_DEACTIVATION; 
         }
+}
+
+
     }
  
     TPE_bodyEnvironmentResolveCollision(body,
@@ -1620,4 +1644,68 @@ TPE_Vec3 TPE_envHalfPlane(TPE_Vec3 point, TPE_Vec3 center, TPE_Vec3 normal)
     TPE_vec3Times(normal,tmp));
 }
 
+uint8_t TPE_checkOverlapAABB(TPE_Vec3 v1Min, TPE_Vec3 v1Max, TPE_Vec3 v2Min,
+  TPE_Vec3 v2Max)
+{
+  TPE_Unit dist;
+
+#define test(c) \
+  dist = v1Min.c + v1Max.c - v2Max.c - v2Min.c; \
+  if (dist < 0) dist *= -1; \
+  if (dist > v1Max.c - v1Min.c + v2Max.c - v2Min.c) return 0;
+
+  test(x)
+  test(y)
+  test(z)
+
+#undef test
+
+  return 1;
+}
+
+void TPE_bodyGetAABB(const TPE_Body *body, TPE_Vec3 *vMin, TPE_Vec3 *vMax)
+{
+  *vMin = body->joints[0].position;
+  *vMax = *vMin;
+
+  TPE_Unit js = TPE_JOINT_SIZE(body->joints[0]);
+
+  vMin->x -= js;
+  vMin->y -= js;
+  vMin->z -= js;
+
+  vMax->x += js;
+  vMax->y += js;
+  vMax->z += js;
+
+  for (uint16_t i = 1; i < body->jointCount; ++i)
+  {
+    TPE_Unit v;
+  
+    js = TPE_JOINT_SIZE(body->joints[i]);
+  
+#define test(c) \
+  v = body->joints[i].position.c - js; \
+  if (v < vMin->c) \
+    vMin->c = v; \
+  v += 2 * js; \
+  if (v > vMax->c) \
+    vMax->c = v;
+
+    test(x)
+    test(y)
+    test(z)
+
+#undef test
+  }
+}
+
+void TPE_jointPin(TPE_Joint *joint, TPE_Vec3 position)
+{
+  joint->position = position;
+  joint->velocity[0] = 0;
+  joint->velocity[1] = 0;
+  joint->velocity[2] = 0;
+}
+
 #endif // guard