Handle start position

assets.h

@@ -32,77 +32,16 @@ LCR_MAP_BLOCK(LCR_BLOCK_CUBOID_FILL,10,1,1,0,0),
 LCR_MAP_BLOCK(LCR_BLOCK_RAMP,46,0,33,LCR_BLOCK_MATERIAL_CONCRETE,LCR_BLOCK_TRANSFORM_ROT_90),
 LCR_MAP_BLOCK(LCR_BLOCK_CUBOID_FILL,1,1,10,0,0),
 
-LCR_MAP_BLOCK(LCR_BLOCK_FULL,36,0,33,LCR_BLOCK_MATERIAL_CONCRETE,0),
+LCR_MAP_BLOCK(LCR_BLOCK_LEFT,36,0,33,LCR_BLOCK_MATERIAL_CONCRETE,0),
 LCR_MAP_BLOCK(LCR_BLOCK_CUBOID_FILL,10,1,10,0,0),
 
 LCR_MAP_BLOCK(LCR_BLOCK_FULL,36,0,43,LCR_BLOCK_MATERIAL_CONCRETE,0),
 LCR_MAP_BLOCK(LCR_BLOCK_CUBOID_FILL,10,4,1,0,0),
 
+LCR_MAP_BLOCK(LCR_BLOCK_FULL,32,0,32,0,0),
+LCR_MAP_BLOCK(LCR_BLOCK_START,32,1,32,0,LCR_BLOCK_TRANSFORM_ROT_180),
 
-/*    
-    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),
 
-    LCR_MAP_BLOCK(LCR_BLOCK_FULL_ACCEL,0,0,63,LCR_BLOCK_MATERIAL_CONCRETE,0),
-    LCR_MAP_BLOCK(LCR_BLOCK_CUBOID_HOLLOW,63,1,1,0,0),
-
-    LCR_MAP_BLOCK(LCR_BLOCK_FULL_ACCEL,0,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,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),
-    LCR_MAP_BLOCK(LCR_BLOCK_CUBOID_HOLLOW,4,4,4,0,0),
-
-    LCR_MAP_BLOCK(LCR_BLOCK_FULL,50,20,20,LCR_BLOCK_MATERIAL_GRASS,0),
-    LCR_MAP_BLOCK(LCR_BLOCK_CUBOID_HOLLOW,5,5,5,0,0),
-
-    LCR_MAP_BLOCK(LCR_BLOCK_FULL,10,10,10,LCR_BLOCK_MATERIAL_GRASS,0),
-    LCR_MAP_BLOCK(LCR_BLOCK_CUBOID_HOLLOW,3,3,3,0,0),
-
-    LCR_MAP_BLOCK(LCR_BLOCK_FULL,2,25,50,LCR_BLOCK_MATERIAL_GRASS,0),
-    LCR_MAP_BLOCK(LCR_BLOCK_CUBOID_HOLLOW,3,3,3,0,0),
-*/
-
-/*
-    LCR_MAP_BLOCK(LCR_BLOCK_FULL,0,0,0,LCR_BLOCK_MATERIAL_GRASS,0),
-    LCR_MAP_BLOCK(LCR_BLOCK_CUBOID_HOLLOW,40,1,1,0,0),
-
-    LCR_MAP_BLOCK(LCR_BLOCK_FULL,63,63,0,LCR_BLOCK_MATERIAL_GRASS,0),
-    LCR_MAP_BLOCK(LCR_BLOCK_CUBOID_HOLLOW,1,1,40,0,0),
-
-    LCR_MAP_BLOCK(LCR_BLOCK_FULL,0,15,0,LCR_BLOCK_MATERIAL_GRASS,0),
-    LCR_MAP_BLOCK(LCR_BLOCK_CUBOID_HOLLOW,7,4,8,0,0),
-*/
-/*
-    LCR_MAP_BLOCK(LCR_BLOCK_FULL,0,15,0,LCR_BLOCK_MATERIAL_GRASS,0),
-    LCR_MAP_BLOCK(LCR_BLOCK_CUBOID_HOLLOW,7,4,8,0,0),
-
-    LCR_MAP_BLOCK(LCR_BLOCK_FULL,32,32,32,LCR_BLOCK_MATERIAL_CONCRETE,0),
-    LCR_MAP_BLOCK(LCR_BLOCK_FULL,33,32,32,LCR_BLOCK_MATERIAL_GRASS,0),
-    LCR_MAP_BLOCK(LCR_BLOCK_RAMP_CURVED_WALL,34,32,32,LCR_BLOCK_MATERIAL_GRASS,0),
-    LCR_MAP_BLOCK(LCR_BLOCK_RAMP_CURVED_WALL,35,32,32,LCR_BLOCK_MATERIAL_CONCRETE,LCR_BLOCK_TRANSFORM_ROT_90),
-
-    LCR_MAP_BLOCK(LCR_BLOCK_FULL,63,35,32,LCR_BLOCK_MATERIAL_CONCRETE,0),
-    LCR_MAP_BLOCK(LCR_BLOCK_FULL,0,35,32,LCR_BLOCK_MATERIAL_ICE,0),
-    LCR_MAP_BLOCK(LCR_BLOCK_FULL,40,35,32,LCR_BLOCK_MATERIAL_DIRT,0),
-    LCR_MAP_BLOCK(LCR_BLOCK_FULL,41,35,32,LCR_BLOCK_MATERIAL_CONCRETE,0),
-    LCR_MAP_BLOCK(LCR_BLOCK_FULL,42,35,32,LCR_BLOCK_MATERIAL_CONCRETE,0),
-    LCR_MAP_BLOCK(LCR_BLOCK_FULL,43,35,32,LCR_BLOCK_MATERIAL_CONCRETE,0),
-    LCR_MAP_BLOCK(LCR_BLOCK_FULL,44,35,32,LCR_BLOCK_MATERIAL_CONCRETE,0),
-
-    LCR_MAP_BLOCK(LCR_BLOCK_BOTTOM_LEFT,34,32,33,LCR_BLOCK_MATERIAL_CONCRETE,LCR_BLOCK_TRANSFORM_ROT_90),
-
-    LCR_MAP_BLOCK(LCR_BLOCK_FULL,32,33,32,LCR_BLOCK_MATERIAL_CONCRETE,0),
-    LCR_MAP_BLOCK(LCR_BLOCK_FULL,33,33,32,LCR_BLOCK_MATERIAL_CONCRETE,0),
-    LCR_MAP_BLOCK(LCR_BLOCK_FULL,32,33,33,LCR_BLOCK_MATERIAL_CONCRETE,0),
-    LCR_MAP_BLOCK(LCR_BLOCK_FULL,33,33,33,LCR_BLOCK_MATERIAL_CONCRETE,0),
-    LCR_MAP_BLOCK(LCR_BLOCK_FULL,32,32,33,LCR_BLOCK_MATERIAL_CONCRETE,0),
-    LCR_MAP_BLOCK(LCR_BLOCK_FULL,33,32,33,LCR_BLOCK_MATERIAL_CONCRETE,0),
-*/
     LCR_MAP_TERMINATOR
   };
 

map.h

@@ -104,7 +104,7 @@ struct
 {
   uint16_t blockCount;
   uint8_t blocks[LCR_SETTING_MAP_MAX_BLOCKS * LCR_BLOCK_SIZE];
-  uint32_t startPos;
+  uint8_t startPos[4]; ///< Initial position and rotation.
 
   uint8_t environment;
 
@@ -227,6 +227,8 @@ uint8_t *LCR_getMapBlockAtCoordNumber(uint32_t coord)
 */
 uint8_t _LCR_mapAddBlock(const uint8_t block[LCR_BLOCK_SIZE])
 {
+  LCR_LOG2("adding map block");
+
   if (LCR_currentMap.blockCount >= LCR_SETTING_MAP_MAX_BLOCKS)
     return 0;
 
@@ -283,7 +285,11 @@ uint8_t _LCR_mapAddBlock(const uint8_t block[LCR_BLOCK_SIZE])
 */
 uint8_t LCR_mapLoad(const uint8_t *map)
 {
-  LCR_currentMap.startPos = 0;
+  LCR_LOG0("loading map")
+
+  for (int i = 0; i < 4; ++i)
+    LCR_currentMap.startPos[i] = 0;
+
   LCR_currentMap.blockCount = 0;
 
   if (map[0] != LCR_MAP_MAGIC_NUMBER1 || map[1] != LCR_MAP_MAGIC_NUMBER2)
@@ -348,6 +354,15 @@ uint8_t LCR_mapLoad(const uint8_t *map)
         break;
       }
 
+      case LCR_BLOCK_START:
+        LCR_mapBlockGetCoords(map,
+          LCR_currentMap.startPos,
+          LCR_currentMap.startPos + 1,
+          LCR_currentMap.startPos + 2);
+
+        LCR_currentMap.startPos[3] = LCR_mapBlockGetTransform(map) & 0x60;
+        break;
+
       default:
         if (!_LCR_mapAddBlock(map)) // normal block
           return 0;
@@ -355,16 +370,10 @@ uint8_t LCR_mapLoad(const uint8_t *map)
         break;
     }
     
-    map += 4;
+    map += LCR_BLOCK_SIZE;
   }
-
+  
-  // process and remove special blocks:
+  LCR_LOG2("map loaded")
-
-  // TODO
- 
-  // sort the blocks (for fast searching): 
-
-  // TODO
 
   return 1;
 }
@@ -447,8 +456,9 @@ void _LCR_addBlockShapeByte(uint8_t *bytes, uint8_t *byteCount,
   *byteCount += 1;
 }
 
-
+/**
-
+  Macro that transforms coordinates according to block transformation.
+*/
 #define LCR_TRANSFORM_COORDS(trans,cx,cy,cz,maxXZ,maxY)\
   if (trans & LCR_BLOCK_TRANSFORM_FLIP_H) cx = maxXZ - cx;\
   if (trans & 0x20) {         /* for both 90 and 270 */ \
@@ -460,8 +470,6 @@ void _LCR_addBlockShapeByte(uint8_t *bytes, uint8_t *byteCount,
   if (trans & LCR_BLOCK_TRANSFORM_FLIP_V) \
     cy = maxY - cy; 
 
-
-
 /**
   Gets a shape of given map block type as a 3D model composed of triangles. The
   model is returned as an array of byte triplets (triangles), with each byte
@@ -584,28 +592,7 @@ void LCR_mapGetBlockShape(uint8_t blockType, uint8_t transform,
 
       _LCR_decodeMapBlockCoords(bytes[i],&x,&y,&z);
 
-LCR_TRANSFORM_COORDS(transform,x,y,z,6,4)
+      LCR_TRANSFORM_COORDS(transform,x,y,z,6,4)
-
-/*     
-      if (transform & LCR_BLOCK_TRANSFORM_FLIP_H)
-        x = 6 - x; 
-
-      if (transform & 0x20) // for both 90 and 270
-      {
-        tmp = z;
-        z = x;
-        x = 6 - tmp;
-      }
-
-      if (transform & 0x40) // for both 180 and 270
-      {
-        x = 6 - x;
-        z = 6 - z;
-      }
-
-      if (transform & LCR_BLOCK_TRANSFORM_FLIP_V)
-        y = 4 - y; 
-*/
 
       bytes[i] = _LCR_encodeMapBlockCoords(x,y,z);
     }

racing.h

@@ -62,9 +62,8 @@ struct
                                  Lower bits record current collisions, higher
                                  bits the previous state (for averaging). */
 
-
+  TPE_Vec3 carPositions[2]; ///* Current and previous position in game units.
-  TPE_Vec3 carPositions[2]; ///* Current and previous position.
+  TPE_Vec3 carRotations[2]; ///* Current and previous rotation in game units.
-  TPE_Vec3 carRotations[2]; ///* Current and previous rotation.
 
 
 TPE_Vec3 carOKPositions[LCR_CAR_JOINTS];
@@ -322,6 +321,61 @@ void LCR_racingRestart(void)
 
   LCR_racing.carRotations[0] = TPE_vec3(0,0,0);
   LCR_racing.carRotations[1] = LCR_racing.carRotations[0];
+    
+TPE_bodyMoveTo(&(LCR_racing.carBody),
+  TPE_vec3(
+    (((TPE_Unit) LCR_currentMap.startPos[0]) - LCR_MAP_SIZE_BLOCKS / 2)
+      * LCR_PHYSICS_UNIT + LCR_PHYSICS_UNIT / 2,
+    (((TPE_Unit) LCR_currentMap.startPos[1]) - LCR_MAP_SIZE_BLOCKS / 2)
+      * LCR_PHYSICS_UNIT / 2 + LCR_PHYSICS_UNIT / 4,
+    (((TPE_Unit) LCR_currentMap.startPos[2]) - LCR_MAP_SIZE_BLOCKS / 2)
+      * LCR_PHYSICS_UNIT + LCR_PHYSICS_UNIT / 2));
+
+// TODO: allow also flipping the car upside down on start?
+if (LCR_currentMap.startPos[3])
+  TPE_bodyRotateByAxis(&(LCR_racing.carBody),
+    TPE_vec3(0,
+      LCR_currentMap.startPos[3] == LCR_BLOCK_TRANSFORM_ROT_90 ? 3 * TPE_F / 4 :
+        (LCR_currentMap.startPos[3] == LCR_BLOCK_TRANSFORM_ROT_180 ? TPE_F / 2 :
+          (TPE_F / 4)),0));
+
+
+/*
+const uint8_t *b = LCR_currentMap.blocks;
+
+for (int i = 0; i < LCR_currentMap.blockCount; ++i)
+{
+  if (*b == LCR_BLOCK_START)
+  {
+    uint8_t x, y, z;
+
+    LCR_LOG1("placing car to start position");
+     
+    LCR_mapBlockGetCoords(b,&x,&y,&z);
+
+    TPE_bodyMoveTo(&(LCR_racing.carBody),
+      TPE_vec3(
+        (((TPE_Unit) x) - LCR_MAP_SIZE_BLOCKS / 2) * LCR_PHYSICS_UNIT +
+          LCR_PHYSICS_UNIT / 2,
+        (((TPE_Unit) y) - LCR_MAP_SIZE_BLOCKS / 2) * LCR_PHYSICS_UNIT / 2 +
+          LCR_PHYSICS_UNIT / 4,
+        (((TPE_Unit) z) - LCR_MAP_SIZE_BLOCKS / 2) * LCR_PHYSICS_UNIT +
+          LCR_PHYSICS_UNIT / 2
+      ));
+
+    // resuse x:
+    x = LCR_mapBlockGetTransform(b) & 0x60;
+
+    if (x)
+      TPE_bodyRotateByAxis(&(LCR_racing.carBody),
+        TPE_vec3(0,x == LCR_BLOCK_TRANSFORM_ROT_90 ? TPE_F / 4 :
+          (x == LCR_BLOCK_TRANSFORM_ROT_180 ? TPE_F / 2 : (3 * TPE_F / 4)),0));
+  }
+ 
+  b += LCR_BLOCK_SIZE;
+}   
+*/
+
 
 
 for (int i = 0; i < LCR_CAR_JOINTS; ++i)
@@ -373,9 +427,6 @@ void LCR_racingInit(void)
     all map blocks. */
   LCR_racing.carBody.flags |= TPE_BODY_FLAG_NO_BSPHERE;
 
-TPE_bodyRotateByAxis(&LCR_racing.carBody,
-  TPE_vec3(TPE_F / 3,0,0));
-
   LCR_racingRestart();
 }
 
@@ -616,29 +667,29 @@ LCR_racing.carPositions[0] =
   LCR_racing.carRotations[0] = tmpVec;
 
 
-int carOK = 1; // TODO: for checking if car shape is fine, if we're inside a wall etc.
 
 
-  TPE_Unit roofAngle =
+  TPE_Unit angle =
-    TPE_vec3Dot(carUp,TPE_vec3Normalized(TPE_vec3Minus(LCR_racing.carBody.joints[4].position,
+    TPE_vec3Dot(carUp,TPE_vec3Normalized(TPE_vec3Minus(
-    LCR_racing.carBody.joints[0].position)));
+      LCR_racing.carBody.joints[4].position,
+      LCR_racing.carBody.joints[0].position)));
 
-  if (roofAngle < TPE_F / 4) // TODO: magic constant
+  if (angle < TPE_F / 4) // TODO: magic constant
   {
     LCR_LOG2("car roof low, applying anti force")
 
     tmpVec = TPE_vec3Times(carUp,LCR_PHYSICS_UNIT / 16); // TODO: 16 magic con.
 
-    if (roofAngle <= 0)  
+    if (angle <= 0)  
     {
       LCR_LOG1("car roof flipped over, fixing")
       LCR_racing.carBody.joints[4].position = wheelAverage;
-      roofAngle = 0;
+      angle = 0;
     }
     
-    roofAngle = TPE_F - 4 * roofAngle; // 4 comes from above TPE_F / 4
+    angle = TPE_F - 4 * angle; // 4 comes from above TPE_F / 4
 
-    tmpVec = TPE_vec3Times(tmpVec,roofAngle);
+    tmpVec = TPE_vec3Times(tmpVec,angle);
 
     // accelerate roof and wheels away from each other
     for (int i = 0; i < LCR_CAR_JOINTS; ++i)
@@ -650,76 +701,75 @@ int carOK = 1; // TODO: for checking if car shape is fine, if we're inside a wal
   }
 
 
+angle =
+  (TPE_vec3Dot(
+    TPE_vec3Minus(
+      LCR_racing.carBody.joints[1].position,
+      LCR_racing.carBody.joints[0].position),
+    TPE_vec3Minus(
+      LCR_racing.carBody.joints[2].position,
+      LCR_racing.carBody.joints[0].position)) +
+  TPE_vec3Dot(
+    TPE_vec3Minus(
+      LCR_racing.carBody.joints[0].position,
+      LCR_racing.carBody.joints[3].position),
+    TPE_vec3Minus(
+      LCR_racing.carBody.joints[2].position,
+      LCR_racing.carBody.joints[3].position)));
+
+int carOK = 1; // TODO: for checking if car shape is fine, if we're inside a wall etc.
+
+if (angle < 1900 || angle > 2350 || 
+  (LCR_racing.carBody.flags & TPE_BODY_FLAG_UNRESOLVED))
+  carOK = 0;
 
 
-if (1 /*carOK*/)
+if (carOK)
 {
+  LCR_racing.carNotOKCount -= LCR_racing.carNotOKCount ? 1 : 0;
+
   for (int i = 0; i < LCR_CAR_JOINTS; ++i)
     LCR_racing.carOKPositions[i] = LCR_racing.carBody.joints[i].position;
 }
 else
 {
-  LCR_racing.carNotOKCount++;
 
-//  if (LCR_racing.carNotOKCount > 4)
+  if (LCR_racing.carNotOKCount > 5)
   {
     LCR_LOG1("car not OK, fixing");
 
     for (int i = 0; i < LCR_CAR_JOINTS; ++i)
-{
+    {
-      LCR_racing.carBody.joints[i].position = LCR_racing.carOKPositions[i];
+      LCR_racing.carBody.joints[i].position = 
+        TPE_vec3Plus(
+          LCR_racing.carBody.joints[i].position,
+          LCR_racing.carOKPositions[i]);
+
+      LCR_racing.carBody.joints[i].position.x /= 2;
+      LCR_racing.carBody.joints[i].position.y /= 2;
+      LCR_racing.carBody.joints[i].position.z /= 2;
+
+
+
+
 //LCR_racing.carBody.joints[i].velocity[0] = 0;
 //LCR_racing.carBody.joints[i].velocity[1] = 0;
 //LCR_racing.carBody.joints[i].velocity[2] = 0;
-}
+    }
-  TPE_bodyReshape(&(LCR_racing.carBody),
+
-    LCR_racing.physicsWorld.environmentFunction);
+//  TPE_bodyReshape(&(LCR_racing.carBody),
+//    LCR_racing.physicsWorld.environmentFunction);
 
     // TODO
 
-    LCR_racing.carNotOKCount = 0;
+    // LCR_racing.carNotOKCount = 0;
   }
+  else
+    LCR_racing.carNotOKCount++;
 }
 
 
 
-
-#if 0
-  if (TPE_vec3Dot(carUp,TPE_vec3Minus(LCR_racing.carBody.joints[4].position,
-    LCR_racing.carBody.joints[0].position)) < 0)
-  {
-    /* if the car falls on its roof the center joint may flip to the other
-       side, here we fix it */
-    LCR_LOG1("car roof flipped over, fixing")
-
- //   LCR_racing.carBody.joints[4].position = TPE_vec3Plus(TPE_vec3Times(carUp,
- //     LCR_PHYSICS_UNIT / 8),LCR_racing.carBody.joints[4].position);
-
-//tmpVec = TPE_vec3Times(carUp,LCR_PHYSICS_UNIT / 8);
-
-/*
-for (int j = 0; j < 4; ++j)
-  LCR_racing.carBody.joints[j].position = TPE_vec3Minus(
-    LCR_racing.carBody.joints[j].position,tmpVec);
-*/
-
-/*
-LCR_racing.carBody.joints[4].position = TPE_vec3Plus(TPE_vec3Times(carUp,
-      LCR_GAME_UNIT / 4),LCR_racing.carBody.joints[4].position); 
-
-
-LCR_racing.carBody.joints[4].position =
-  TPE_vec3Plus(LCR_racing.carBody.joints[4],tmpVec);
-*/
-
-//  TPE_bodyReshape(&(LCR_racing.carBody), 
-//LCR_racing.physicsWorld.environmentFunction);
-
-
-  }
-#endif
-
-
   LCR_racing.tick++;
 
   LCR_LOG2("racing step end");

tinyphysicsengine.h

@@ -252,6 +252,9 @@ typedef struct
                                             energy. */
 #define TPE_BODY_FLAG_NO_BSPHERE 64    /**< Stops quick bounding sphere checks
                                             against environment. */
+#define TPE_BODY_FLAG_UNRESOLVED 128   /**< Set automatically for bodies whose
+                                            collision with environment failed
+                                            to be resolved. */
 
 /** Function used for defining static environment, working similarly to an SDF
   (signed distance function). The parameters are: 3D point P, max distance D.
@@ -1913,6 +1916,8 @@ uint8_t TPE_bodyEnvironmentResolveCollision(TPE_Body *body,
   TPE_Vec3 c;
   TPE_Unit d;
 
+  body->flags &= ~TPE_BODY_FLAG_UNRESOLVED;
+
   if (!(body->flags & TPE_BODY_FLAG_NO_BSPHERE))
   {
     TPE_bodyGetFastBSphere(body,&c,&d);
@@ -1938,6 +1943,9 @@ uint8_t TPE_bodyEnvironmentResolveCollision(TPE_Body *body,
     {
       collision = 1;
 
+      if (r == 2)
+        body->flags |= TPE_BODY_FLAG_UNRESOLVED;
+
       if (body->flags & TPE_BODY_FLAG_NONROTATING)
         _TPE_bodyNonrotatingJointCollided(body,i,previousPos,r == 1);
     }