From 2737113a8450dee0d13ccca1c3938944f3dba4ae Mon Sep 17 00:00:00 2001
From: Diren D Bharwani <diren.bharwani@gmail.com>
Date: Wed, 22 Mar 2023 22:25:11 +0800
Subject: [PATCH] forces are cleared after first iteration

---
 .../src/Physics/System/SHPhysicsSystem.cpp    | 23 ++++++++-----------
 1 file changed, 10 insertions(+), 13 deletions(-)

diff --git a/SHADE_Engine/src/Physics/System/SHPhysicsSystem.cpp b/SHADE_Engine/src/Physics/System/SHPhysicsSystem.cpp
index 5b737728..49097807 100644
--- a/SHADE_Engine/src/Physics/System/SHPhysicsSystem.cpp
+++ b/SHADE_Engine/src/Physics/System/SHPhysicsSystem.cpp
@@ -269,9 +269,7 @@ namespace SHADE
 
     double accumulatedTime  = 0.0f;
 
-    bool  terminate         = true;
-    int   iterationCounter  = simInfo.maxSteps;
-
+    int iterationCounter = simInfo.maxSteps;
     do
     {
       accumulatedTime += simInfo.timeStep;
@@ -280,21 +278,22 @@ namespace SHADE
       raycastInfo.ray.position  = ghostBody.position;
       raycastInfo.ray.direction = SHVec3::Normalise(ghostBody.linearVelocity);
 
-      terminate = !Raycast(raycastInfo).empty() || iterationCounter == 0;
-      if (terminate)
+      if (!Raycast(raycastInfo).empty())
         return;
 
       while (accumulatedTime > fixedDT)
       {
         simulateBody(ghostBody, simInfo);
         accumulatedTime -= fixedDT;
-      }
-        
 
-      if (!simInfo.continuousForce)
-      {
-        simInfo.force   = SHVec3::Zero;
-        simInfo.torque  = SHVec3::Zero;
+        if (!simInfo.continuousForce)
+        {
+          simInfo.force   = SHVec3::Zero;
+          simInfo.torque  = SHVec3::Zero;
+        }
+
+        if (--iterationCounter == 0)
+          return;
       }
 
       if (output.positions)
@@ -303,8 +302,6 @@ namespace SHADE
       if (output.orientations)
         output.orientations->emplace_back(ghostBody.orientation);
 
-      --iterationCounter;
-
     } while (true);
   }