Start block collisions

assets.h

@@ -25,7 +25,14 @@ static const uint8_t map1[] =
     10,
     10,
     0,
-    
+
+
+LCR_MAP_BLOCK(LCR_BLOCK_FULL_ACCEL,36,0,32,LCR_BLOCK_MATERIAL_CONCRETE,0),
+LCR_MAP_BLOCK(LCR_BLOCK_FULL_ACCEL,20,0,32,LCR_BLOCK_MATERIAL_CONCRETE,0),
+LCR_MAP_BLOCK(LCR_BLOCK_FULL_ACCEL,32,0,37,LCR_BLOCK_MATERIAL_CONCRETE,0),
+LCR_MAP_BLOCK(LCR_BLOCK_FULL_ACCEL,32,0,20,LCR_BLOCK_MATERIAL_CONCRETE,0),
+
+/*    
     LCR_MAP_BLOCK(LCR_BLOCK_FULL_ACCEL,0,0,0,LCR_BLOCK_MATERIAL_CONCRETE,0),
     LCR_MAP_BLOCK(LCR_BLOCK_CUBOID_HOLLOW,63,1,1,0,0),
 
@@ -37,6 +44,7 @@ static const uint8_t map1[] =
 
     LCR_MAP_BLOCK(LCR_BLOCK_FULL_ACCEL,63,0,0,LCR_BLOCK_MATERIAL_CONCRETE,0),
     LCR_MAP_BLOCK(LCR_BLOCK_CUBOID_HOLLOW,1,1,63,0,0),
+*/
 
 /*
     LCR_MAP_BLOCK(LCR_BLOCK_FULL_ACCEL,32,32,32,LCR_BLOCK_MATERIAL_CONCRETE,0),

debug.h

@@ -14,7 +14,7 @@ void LCR_debugPrintCurrentMap()
 
     LCR_mapBlockGetCoords(LCR_currentMap.blocks + i * 4,&x,&y,&z);
 
-    printf("  block %d: type %d, coord %d (%d %d %d)\n",i,
+    printf("  block %d: type %x, coord %d (%d %d %d)\n",i,
       LCR_currentMap.blocks[i * 4],LCR_mapBlockGetCoordNumber(
       LCR_currentMap.blocks + i * 4),x,y,z);
   }

map.h

@@ -163,6 +163,13 @@ uint32_t LCR_mapBlockGetCoordNumber(const uint8_t block[LCR_BLOCK_SIZE])
     ((((uint32_t) block[3]) & 0x3) << 16);
 }
 
+uint32_t LCR_mapBlockCoordsToCoordNumber(uint8_t x, uint8_t y, uint8_t z)
+{
+  uint8_t b[LCR_BLOCK_SIZE];
+  LCR_makeMapBlock(0,x,y,z,0,0,b);
+  return LCR_mapBlockGetCoordNumber(b);
+}
+
 uint8_t *LCR_getMapBlockAtCoordNumber(uint32_t coord)
 {
   // binary search the block:
@@ -339,12 +346,44 @@ uint8_t LCR_mapLoad(const uint8_t *map)
 }
 
 /**
-  Gets a pointer to a map block of the currently loaded map at given
+  Same as LCR_mapGetBlockAt, but allows to specify start and end block of the
-  coordinates. If there is no block at given coordinates, 0 is returned.
+  of the search to make it faster.
 */
-const uint8_t *LCR_mapGetBlockAt(uint8_t x, uint8_t y, uint8_t z)
+int LCR_mapGetBlockAtFast(uint8_t x, uint8_t y, uint8_t z,
+  int start, int end)
 {
-  return 0;
+  // binary search (the blocks are sorted)
+
+  uint32_t n = LCR_mapBlockCoordsToCoordNumber(x,y,z);
+
+  while (start <= end)
+  {
+    int m = (start + end) / 2;
+ 
+    uint32_t n2 = LCR_mapBlockGetCoordNumber(
+      LCR_currentMap.blocks + m * LCR_BLOCK_SIZE);
+
+    if (n2 < n)
+      start = m + 1;
+    else if (n2 > n)
+      end = m - 1;
+    else
+      return m;
+  }
+
+  return -1;
+}
+
+/**
+  Gets an index to a map block of the currently loaded map at given
+  coordinates. If there is no block at given coordinates, -1 is returned.
+*/
+int LCR_mapGetBlockAt(uint8_t x, uint8_t y, uint8_t z)
+{
+  if (LCR_currentMap.blockCount == 0)
+    return -1;
+
+  return LCR_mapGetBlockAtFast(x,y,z,0,LCR_currentMap.blockCount - 1);
 }
 
 uint8_t _LCR_encodeMapBlockCoords(uint8_t x, uint8_t y, uint8_t z)

racing.h

@@ -68,12 +68,103 @@ TPE_Vec3 _LCR_TPE_vec3DividePlain(TPE_Vec3 v, TPE_Unit d)
   return v;
 }
 
+TPE_Vec3 _LCR_racingBlockEnvFunc(TPE_Vec3 point, const uint8_t *block)
+{
+  uint8_t bx, by, bz;
+  LCR_mapBlockGetCoords(block,&bx,&by,&bz);
+
+  TPE_Vec3 center = TPE_vec3(
+    (((int) bx) - LCR_MAP_SIZE_BLOCKS / 2) * LCR_PHYSICS_UNIT
+      + LCR_PHYSICS_UNIT / 2,
+    (((int) by) - LCR_MAP_SIZE_BLOCKS / 2) * (LCR_PHYSICS_UNIT / 2)
+      + LCR_PHYSICS_UNIT / 4,
+    (((int) bz) - LCR_MAP_SIZE_BLOCKS / 2) * LCR_PHYSICS_UNIT
+      + LCR_PHYSICS_UNIT / 2);
+
+  return TPE_envAABox(point,center,TPE_vec3(LCR_PHYSICS_UNIT / 2,
+    LCR_PHYSICS_UNIT / 4,LCR_PHYSICS_UNIT / 2));
+}
+
 TPE_Vec3 _LCR_racingEnvironmentFunction(TPE_Vec3 point, TPE_Unit maxDist)
 {
-  return TPE_envAABoxInside(point,TPE_vec3(0,0,0),TPE_vec3(
+  // start with the map outside walls:
+  TPE_ENV_START(TPE_envAABoxInside(point,TPE_vec3(0,0,0),TPE_vec3(
       LCR_PHYSICS_UNIT * LCR_MAP_SIZE_BLOCKS,
       (LCR_PHYSICS_UNIT * LCR_MAP_SIZE_BLOCKS) / 2,
-      LCR_PHYSICS_UNIT * LCR_MAP_SIZE_BLOCKS));
+      LCR_PHYSICS_UNIT * LCR_MAP_SIZE_BLOCKS)),point)
+
+  const uint8_t *block = LCR_currentMap.blocks;
+
+  if (maxDist <= LCR_PHYSICS_UNIT / 4) // considering half of square height
+  {
+    /* Here we only check the 8 closest blocks => relatively fast. */
+
+    TPE_Vec3 pointShifted = TPE_vec3Plus(point,TPE_vec3(
+        (LCR_MAP_SIZE_BLOCKS / 2) * LCR_PHYSICS_UNIT,
+        (LCR_MAP_SIZE_BLOCKS / 4) * LCR_PHYSICS_UNIT,
+        (LCR_MAP_SIZE_BLOCKS / 2) * LCR_PHYSICS_UNIT));
+
+    uint8_t coords[6]; // x_low, x_high, y_low, y_high, z_low, z_high
+
+    coords[0] = (pointShifted.x / LCR_PHYSICS_UNIT);
+    coords[1] = (pointShifted.x % LCR_PHYSICS_UNIT < LCR_PHYSICS_UNIT / 2);
+
+    coords[2] = (pointShifted.y / (LCR_PHYSICS_UNIT / 2));
+    coords[3] =
+      (pointShifted.y % (LCR_PHYSICS_UNIT / 2) < LCR_PHYSICS_UNIT / 4);
+
+    coords[4] = (pointShifted.z / LCR_PHYSICS_UNIT);
+    coords[5] = (pointShifted.z % LCR_PHYSICS_UNIT < LCR_PHYSICS_UNIT / 2);
+
+    for (int i = 0; i < 6; i += 2)
+      if (coords[i + 1])
+      {
+        coords[i + 1] = coords[i];
+        coords[i] = coords[i] > 0 ? coords[i] - 1 : 0;
+      }
+      else
+        coords[i + 1] = coords[i] < 63 ? coords[i] + 1 : 63;
+
+    int start = 0, end = LCR_currentMap.blockCount - 1;
+
+    for (uint8_t i = 0; i < 8; ++i)
+    {
+      /* Black magic: here we make it so that we the lowest coord numbers
+        (0,0,0), then the highest (1,1,1), then second lowest (1,0,0), then
+        second highest (0,1,1) etc. This way we are narrowing the range (start,
+        end) for the binary search. */
+
+      int blockNum = LCR_mapGetBlockAtFast(
+        coords[0] + ((i ^ (i >> 1)) & 0x01),
+        coords[2] + ((i ^ (i >> 2)) & 0x01),
+        coords[4] + (i & 0x01),start,end);
+
+      if (blockNum >= 0) // is there a block at the coords?
+      {
+        TPE_ENV_NEXT(_LCR_racingBlockEnvFunc(point, // check it
+          LCR_currentMap.blocks + blockNum * LCR_BLOCK_SIZE),point)
+
+        // narrow the search range:
+        if (i % 2 == 0 && blockNum > start)
+          start = blockNum;
+
+        if (i % 2 && blockNum < end)
+          end = blockNum;
+      }
+    }
+  }
+  else
+  {
+printf("oof\n");
+    /* Full check of all map blocks, slow, shouldn't happen often! */
+    for (int j = 0; j < LCR_currentMap.blockCount; ++j)
+    {
+      TPE_ENV_NEXT(_LCR_racingBlockEnvFunc(point,block),point)
+      block += LCR_BLOCK_SIZE;
+    }
+  }
+
+  TPE_ENV_END
 }
 
 LCR_GameUnit LCR_racingGetCarSpeed(void)
@@ -164,6 +255,11 @@ void LCR_racingInit(void)
   LCR_racing.carBody.elasticity = LCR_CAR_ELASTICITY;
   LCR_racing.carBody.flags |= TPE_BODY_FLAG_ALWAYS_ACTIVE;
 
+  /* We disable bounding sphere checks because that would lead to calling env.
+    function with large min. distance which would lead to slow iteration over
+    all map blocks. */
+  LCR_racing.carBody.flags |= TPE_BODY_FLAG_NO_BSPHERE;
+
   LCR_racingRestart();
 }
 

tinyphysicsengine.h

@@ -250,6 +250,8 @@ typedef struct
                                             performance. */
 #define TPE_BODY_FLAG_ALWAYS_ACTIVE 32 /**< Will never deactivate due to low
                                             energy. */
+#define TPE_BODY_FLAG_NO_BSPHERE 64    /**< Stops quick bounding sphere checks
+                                            against environment. */
 
 /** Function used for defining static environment, working similarly to an SDF
   (signed distance function). The parameters are: 3D point P, max distance D.
@@ -1911,10 +1913,13 @@ uint8_t TPE_bodyEnvironmentResolveCollision(TPE_Body *body,
   TPE_Vec3 c;
   TPE_Unit d;
 
-  TPE_bodyGetFastBSphere(body,&c,&d);
+  if (!(body->flags & TPE_BODY_FLAG_NO_BSPHERE))
+  {
+    TPE_bodyGetFastBSphere(body,&c,&d);
 
-  if (TPE_DISTANCE(c,env(c,d)) > d)
+    if (TPE_DISTANCE(c,env(c,d)) > d)
-    return 0;
+      return 0;
+  }
 
   // now test the full body collision: