Fix a bug

TODO.txt

@@ -5,6 +5,9 @@ TODO:
 - test different tick lengths
 - air friction
 - test env functions with a single distinct point near camera
+- test/handle: collision where the velocity goes "away" from the collision (can
+  happen e.g. if one body is hit by a faster body OR by manual manipulation with
+  velocities)
 
 DONE:
 - zero sized joints should never collide (can be useful) <- NO because 0 size

programs/player.c

@@ -1,6 +1,3 @@
-#define S3L_SORT 1
-#define S3L_Z_BUFFER 0
-
 #include "helper.h"
 
 #define ROOM_SIZE 10000
@@ -11,7 +8,7 @@ TPE_Vec3 environmentDistance(TPE_Vec3 p, TPE_Unit maxD)
   TPE_ENV_START( TPE_envAABoxInside(p,TPE_vec3(0,ROOM_SIZE / 4,0),TPE_vec3(ROOM_SIZE,ROOM_SIZE / 2,ROOM_SIZE)),p )
   TPE_ENV_NEXT( TPE_envAABox(p,TPE_vec3(4000,160,4000),TPE_vec3(1000,160,1000)),p )
   TPE_ENV_NEXT( TPE_envAABox(p,TPE_vec3(4000,80,2500),TPE_vec3(1000,80,500)),p )
-  TPE_ENV_NEXT( TPE_envAABox(p,TPE_vec3(-500,270,4500),TPE_vec3(4000,270,250)),p )
+  TPE_ENV_NEXT( TPE_envAABox(p,TPE_vec3(-1000,270,4500),TPE_vec3(4000,270,250)),p )
   TPE_ENV_END
 }
 
@@ -76,6 +73,9 @@ tpe_world.bodies[1].elasticity = 512;
   {
     if (onGroundCount > 0)
       onGroundCount--;
+      
+    if (jumpCountdown > 0)
+      jumpCountdown--;
 
     helper_frameStart();
 
@@ -99,11 +99,10 @@ tpe_world.bodies[1].elasticity = 512;
     {
       if (sdl_keyboard[SDL_SCANCODE_SPACE] && jumpCountdown == 0 && onGroundCount)
       {
-        tpe_world.bodies[0].joints[0].velocity[1] += 80;
-        jumpCountdown = 3;
+        tpe_world.bodies[0].joints[0].velocity[1] = 80;
+        jumpCountdown = 8;
+        onGroundCount = 0;
       }
-      else if (jumpCountdown > 0)
-        jumpCountdown--;
 
 #define AAA 16
       if (sdl_keyboard[SDL_SCANCODE_UP] || sdl_keyboard[SDL_SCANCODE_W])
@@ -141,6 +140,8 @@ tpe_world.bodies[1].elasticity = 512;
     helper_draw3dCubeInside(TPE_vec3(0,ROOM_SIZE / 4,0),TPE_vec3(ROOM_SIZE,ROOM_SIZE / 2,ROOM_SIZE),TPE_vec3(0,0,0));
     helper_draw3dCube(TPE_vec3(4000,160,4000),TPE_vec3(2000,320,2000),TPE_vec3(0,0,0));
     helper_draw3dCube(TPE_vec3(4000,80,2500),TPE_vec3(2000,160,1000),TPE_vec3(0,0,0));
+
+    helper_draw3dCube(TPE_vec3(-1000,270,4500),TPE_vec3(8000,540,500),TPE_vec3(0,0,0));
     
 
 helper_draw3dSphere(

tinyphysicsengine.h

@@ -131,6 +131,13 @@ of 2. */
   #define TPE_COLLISION_RESOLUTION_MARGIN (TPE_FRACTIONS_PER_UNIT / 64)
 #endif
 
+#ifndef TPE_NONROTATING_COLLISION_RESOLVE_ATTEMPTS
+/** Number of times a collision of nonrotating bodies with environment will be
+  attempted to resolve. This probably won't have great performance implications
+  as complex collisions of this kind should be relatively rare. */
+  #define TPE_NONROTATING_COLLISION_RESOLVE_ATTEMPTS 3
+#endif
+
 #define TPE_PRINTF_VEC3(v) printf("[%d %d %d]",(v).x,(v).y,(v).z);
 
 typedef struct
@@ -665,7 +672,7 @@ void TPE_worldStep(TPE_World *world)
 
     TPE_Joint *joint = body->joints, *joint2;
 
-TPE_Vec3 origPos = body->joints[0].position;
+    TPE_Vec3 origPos = body->joints[0].position;
 
     for (uint16_t j = 0; j < body->jointCount; ++j) // apply velocities
     {
@@ -721,10 +728,20 @@ TPE_Vec3 origPos = body->joints[0].position;
     if (body->flags & TPE_BODY_FLAG_NONROTATING)
     {
       /* Non-rotating bodies may end up still colliding after environment coll 
-      resolvement (unlike rotatng bodies where each joint is ensured separately
-      to not collide). So if still in collision, we simply undo any shifts we've
-      done. This sadly results in jitter movement along walls sometimes, but it
-      should absolutely prevent any body escaping out of environment bounds. */
+      resolvement (unlike rotating bodies where each joint is ensured separately
+      to not collide). So if still in collision, we try a few more times. If not
+      successful, we simply undo any shifts we've done. This should absolutely
+      prevent any body escaping out of environment bounds. */
+ 
+      for (uint8_t i = 0; i < TPE_NONROTATING_COLLISION_RESOLVE_ATTEMPTS; ++i) 
+      {
+        if (!collided)
+          break;
+
+        collided =
+          TPE_bodyEnvironmentResolveCollision(body,world->environmentFunction);
+      }
+
       if (collided &&
         TPE_bodyEnvironmentCollide(body,world->environmentFunction))
         TPE_bodyMove(body,TPE_vec3Minus(origPos,body->joints[0].position));
@@ -1342,7 +1359,8 @@ uint8_t TPE_jointEnvironmentResolveCollision(TPE_Joint *joint, TPE_Unit elastici
           
         joint->position = TPE_vec3Plus(joint->position,shift);
   
-        toJoint = TPE_vec3Minus(joint->position,env(joint->position,TPE_JOINT_SIZE(*joint)));
+        toJoint = TPE_vec3Minus(joint->position,env(joint->position,
+          TPE_JOINT_SIZE(*joint)));
 
         len = TPE_LENGTH(toJoint); // still colliding?
 
@@ -1381,14 +1399,15 @@ uint8_t TPE_jointEnvironmentResolveCollision(TPE_Joint *joint, TPE_Unit elastici
         shift.z /= 2;
       }
     }
+
     if (success)
     {
       TPE_Vec3 vel = TPE_vec3(joint->velocity[0],joint->velocity[1],
         joint->velocity[2]);
 
-      vel = TPE_vec3Project(vel,shift);
+      vel = TPE_vec3Project(vel,shift); // parallel part of velocity
 
-      TPE_Vec3 vel2 = TPE_vec3Minus(
+      TPE_Vec3 vel2 = TPE_vec3Minus( // perpendicular part of velocity
         TPE_vec3(joint->velocity[0],joint->velocity[1],joint->velocity[2]),vel);
 
       vel2 = TPE_vec3Times(vel2,friction);