Try to handle car turtle

frontend_sdl.c

@@ -1,7 +1,7 @@
 #include <stdio.h>
 #include <SDL2/SDL.h>
 
-#define LCR_SETTING_LOG_LEVEL 0
+#define LCR_SETTING_LOG_LEVEL 2
 
 #include "game.h"
 #include "debug.h"

racing.h

@@ -15,6 +15,10 @@ typedef int32_t LCR_GameUnit;  ///< abstract game unit
 #define LCR_RACING_INPUT_LEFT  0x08
 
 #define LCR_PHYSICS_UNIT       2048 ///< length of map square for physics engine
+  
+
+#define TPE_RESHAPE_TENSION_LIMIT 10
+#define TPE_RESHAPE_ITERATIONS 5
 
 #include "map.h"
 #include "tinyphysicsengine.h"
@@ -35,16 +39,14 @@ typedef int32_t LCR_GameUnit;  ///< abstract game unit
 
 #define LCR_GRAVITY (LCR_PHYSICS_UNIT / 140)
 #define LCR_CAR_FORWARD_FRICTION (TPE_F / 9)
-#define LCR_CAR_TURN_FRICTION (4 * TPE_F / 4)
+#define LCR_CAR_TURN_FRICTION (TPE_F)
 #define LCR_CAR_ELASTICITY (TPE_F / 110)
 #define LCR_CAR_ACCELERATION (LCR_PHYSICS_UNIT / 16)
-#define LCR_CAR_TURN_SPEED (LCR_GAME_UNIT / 20)
-#define LCR_CAR_TURN_MAX (LCR_GAME_UNIT / 4)
+#define LCR_CAR_TURN_SPEED (LCR_GAME_UNIT / 18)
+#define LCR_CAR_TURN_MAX ((7 * LCR_GAME_UNIT) / 24)
 
 #endif
 
-
-
 #define LCR_CAR_JOINTS 5
 #define LCR_CAR_CONNECTIONS 10
 
@@ -64,6 +66,11 @@ struct
   TPE_Vec3 carPositions[2]; ///* Current and previous position.
   TPE_Vec3 carRotations[2]; ///* Current and previous rotation.
 
+
+TPE_Vec3 carOKPositions[LCR_CAR_JOINTS];
+uint8_t carNotOKCount;
+
+
   LCR_GameUnit wheelRotation;
   LCR_GameUnit wheelSteer;
 } LCR_racing;
@@ -315,9 +322,22 @@ void LCR_racingRestart(void)
 
   LCR_racing.carRotations[0] = TPE_vec3(0,0,0);
   LCR_racing.carRotations[1] = LCR_racing.carRotations[0];
+
+
+for (int i = 0; i < LCR_CAR_JOINTS; ++i)
+  LCR_racing.carOKPositions[i] = TPE_vec3(0,0,0);
+
+LCR_racing.carNotOKCount = 0;
+
+
+
   // TODO
 }
 
+
+
+
+
 /**
   Initializes the racing module, only call once.
 */
@@ -353,6 +373,9 @@ 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();
 }
 
@@ -558,29 +581,24 @@ void LCR_racingStep(unsigned int input)
   TPE_worldStep(&(LCR_racing.physicsWorld));
   LCR_LOG2("stepping physics engine done");
 
-  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_racing.carBody.joints[4].position = TPE_vec3Plus(TPE_vec3Times(carUp,
-      LCR_GAME_UNIT / 4),LCR_racing.carBody.joints[4].position);
-  }
 
   TPE_Vec3 tmpVec = LCR_racing.carPositions[0];
 
-  LCR_racing.carPositions[0] = // average position of 4 wheels to get car pos
-    _LCR_TPE_vec3DividePlain(
-      TPE_vec3TimesPlain(
-        TPE_vec3Plus(
+TPE_Vec3 wheelAverage =
+  _LCR_TPE_vec3DividePlain(
+    TPE_vec3Plus(
           TPE_vec3Plus(
             LCR_racing.carBody.joints[0].position,
             LCR_racing.carBody.joints[1].position),
           TPE_vec3Plus(
             LCR_racing.carBody.joints[2].position,
-            LCR_racing.carBody.joints[3].position)
-          ),LCR_GAME_UNIT),4 * LCR_PHYSICS_UNIT);
+            LCR_racing.carBody.joints[3].position)),4);
+
+LCR_racing.carPositions[0] =
+  _LCR_TPE_vec3DividePlain(
+      TPE_vec3TimesPlain(
+         wheelAverage,LCR_GAME_UNIT),
+         LCR_PHYSICS_UNIT);
 
   LCR_racing.carPositions[0] = // smooth the position
     TPE_vec3KeepWithinBox(LCR_racing.carPositions[1],LCR_racing.carPositions[0],
@@ -597,6 +615,111 @@ void LCR_racingStep(unsigned int input)
   LCR_racing.carRotations[1] = LCR_racing.carRotations[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_vec3Dot(carUp,TPE_vec3Normalized(TPE_vec3Minus(LCR_racing.carBody.joints[4].position,
+    LCR_racing.carBody.joints[0].position)));
+
+  if (roofAngle < 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)  
+    {
+      LCR_LOG1("car roof flipped over, fixing")
+      LCR_racing.carBody.joints[4].position = wheelAverage;
+      roofAngle = 0;
+    }
+    
+    roofAngle = TPE_F - 4 * roofAngle; // 4 comes from above TPE_F / 4
+
+    tmpVec = TPE_vec3Times(tmpVec,roofAngle);
+
+    // accelerate roof and wheels away from each other
+    for (int i = 0; i < LCR_CAR_JOINTS; ++i)
+    {
+      LCR_racing.carBody.joints[i].velocity[0] += (i == 4 ? 1 : -1) * tmpVec.x;
+      LCR_racing.carBody.joints[i].velocity[1] += (i == 4 ? 1 : -1) * tmpVec.y;
+      LCR_racing.carBody.joints[i].velocity[2] += (i == 4 ? 1 : -1) * tmpVec.z;
+    }
+  }
+
+
+
+
+if (1 /*carOK*/)
+{
+  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)
+  {
+    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].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);
+
+    // TODO
+
+    LCR_racing.carNotOKCount = 0;
+  }
+}
+
+
+
+
+#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");

renderer.h

@@ -941,8 +941,7 @@ void LCR_rendererDrawRect(int x, int y, unsigned int w, unsigned int h,
 
   unsigned long index = y * LCR_EFFECTIVE_RESOLUTION_X + x;
 
-if (0)
-//  if (dither)
+  if (dither)
   {
     uint8_t parity = (x % 2) == (y % 2);