Continue world

test_sdl.c

@@ -341,8 +341,6 @@ void updateBodies()
   {
     BodyModelExtra *e = &(extra[i]);
 
-    TPE_bodyStep(&(bodies[i]));
-    
     S3L_Mat4 m;
 
     TPE_bodyGetTransformMatrix(&(bodies[i]),m);
@@ -406,19 +404,22 @@ TPE_bodySetOrientation(&(bodies[1].body),quat);
 */
 
 int collided = 0;
+int time;
 
   while (running)
   {
 
-int time = SDL_GetTicks();
+time = SDL_GetTicks();
 
-bodies[0].velocity.y -= 4;
+    TPE_worldApplyGravityDown(&world,4);
 
     for (uint32_t i = 0; i < PIXELS_SIZE; ++i)
       pixels[i] = 0;
 
     S3L_newFrame();
 
+    TPE_worldStepBodies(&world);
+
     updateBodies();
 
     S3L_drawScene(scene);
@@ -455,6 +456,9 @@ qqq = TPE_bodyGetOrientation(&(bodies[0].body));
 TPE_PRINTF_VEC4(qqq)
 printf("\n");
 */
+TPE_worldResolveCollisionNaive(&world);
+
+#if 0
     TPE_Unit collDepth = TPE_bodyCollides(&(bodies[1]),&(bodies[0]),&p,&n);
 
     if (collDepth)
@@ -465,12 +469,12 @@ printf("\n");
 TPE_resolveCollision(&(bodies[1]),&(bodies[0]), 
   p,n,collDepth,300);
 
-/*
 printf("\nkin. energy: %d\n",
   TPE_bodyGetKineticEnergy(&bodies[0].body) +
   TPE_bodyGetKineticEnergy(&bodies[1].body));
-*/
 }
+*/
+
 
 collided++;
 
@@ -505,6 +509,8 @@ TPE_vec3Add
       project3DPointToScreen(p2,scene.camera,&scr);
       draw2DPoint(scr.x,scr.y,255,255,255);
     }
+#endif
+
 
     SDL_UpdateTexture(textureSDL,NULL,pixels,S3L_RESOLUTION_X * sizeof(uint32_t));
 

tinyphysicsengine.h

@@ -299,6 +299,9 @@ typedef struct
 } TPE_World;
 
 TPE_worldInit(TPE_World *world);
+TPE_worldStepBodies(TPE_World *world);
+TPE_worldApplyGravityDown(TPE_World *world, TPE_Unit g);
+TPE_worldResolveCollisionNaive(TPE_World *world);
 
 /** Multiplies two quaternions which can be seen as chaining two rotations
   represented by them. This is not commutative (a*b != b*a)! Rotations a is
@@ -2132,4 +2135,48 @@ TPE_worldInit(TPE_World *world)
   world->bodyCount = 0;
 }
 
+TPE_worldStepBodies(TPE_World *world)
+{
+  for (uint16_t i = 0; i < world->bodyCount; ++i)
+    TPE_bodyStep(&(world->bodies[i]));
+}
+
+TPE_worldApplyGravityDown(TPE_World *world, TPE_Unit g)
+{
+  TPE_Body *b = world->bodies;
+
+  for (uint16_t i = 0; i < world->bodyCount; ++i)
+  {
+    if (b->mass != TPE_INFINITY)
+      b->velocity.y -= g;
+
+    b++;
+  }
+}
+
+TPE_worldResolveCollisionNaive(TPE_World *world)
+{
+  TPE_Body *b1 = world->bodies;
+
+  for (uint16_t i = 0; i < world->bodyCount - 1; ++i)
+  {
+    TPE_Body *b2 = &(world->bodies[i + 1]);
+
+    TPE_Vec4 p, n;
+
+    for (uint16_t j = i + 1; j < world->bodyCount; ++j)
+    {
+      if (b1->mass == TPE_INFINITY && b2->mass == TPE_INFINITY)
+        continue;
+
+      TPE_Unit d = TPE_bodyCollides(b1,b2,&p,&n);
+
+      if (d)
+        TPE_resolveCollision(b1,b2,p,n,d,300);
+    }
+
+    b1++;
+  }
+}
+
 #endif // guard