From 4a3c9b2873b95a58ac9d447ca39f321d695f7ee1 Mon Sep 17 00:00:00 2001
From: Miloslav Ciz <drummyfish@disroot.org>
Date: Mon, 30 Sep 2024 02:28:27 +0200
Subject: [PATCH] Continue antiglitch

---
 racing.h | 29 ++++++++++++++++++-----------
 1 file changed, 18 insertions(+), 11 deletions(-)

diff --git a/racing.h b/racing.h
index 65d6e07..dce1623 100644
--- a/racing.h
+++ b/racing.h
@@ -583,6 +583,8 @@ void LCR_racingStep(unsigned int input)
     (!(input & LCR_RACING_INPUT_RIGHT)))
     LCR_racing.wheelSteer /= 2;
 
+
+
   if ((LCR_racing.wheelCollisions & 0x0f) != 0x0f) // EXPERIMENTAL: don't apply gravity with all wheels on ground
     TPE_bodyApplyGravity(&(LCR_racing.carBody),LCR_GRAVITY);
 
@@ -671,28 +673,33 @@ if ((LCR_racing.carBody.flags & TPE_BODY_FLAG_UNRESOLVED) ||
 
     for (int i = 0; i < LCR_CAR_JOINTS; ++i)
     {
-      // iterate towards previous OK position
+      if (LCR_racing.carNotOKCount < 30) // TODO: const
+      {
+        // for a while try to smoothly iterate towards previous OK position 
+        LCR_racing.carBody.joints[i].position = 
+          TPE_vec3Plus(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].position.x /= 2;
+        LCR_racing.carBody.joints[i].position.y /= 2;
+        LCR_racing.carBody.joints[i].position.z /= 2;
+      }
+      else // hard set the pos (iteration may be infinite due to sim.)
+        LCR_racing.carBody.joints[i].position = LCR_racing.carOKPositions[i];
 
       for (int j = 0; j < 3; ++j) // lower speed a bit
         LCR_racing.carBody.joints[i].velocity[j] = 
           (7 * ((int) LCR_racing.carBody.joints[i].velocity[j])) / 8;
     }
   }
-  else
-    LCR_racing.carNotOKCount++;
+
+  LCR_racing.carNotOKCount += LCR_racing.carNotOKCount < 32 ? 1 : 0;
 }
 else
 {
   // car OK
-  LCR_racing.carNotOKCount -= LCR_racing.carNotOKCount ? 1 : 0;
+//  LCR_racing.carNotOKCount -= LCR_racing.carNotOKCount ? 1 : 0;
+  LCR_racing.carNotOKCount = 0;
 
   for (int i = 0; i < LCR_CAR_JOINTS; ++i)
     LCR_racing.carOKPositions[i] = LCR_racing.carBody.joints[i].position;