From b481b432641ecf2bc802a375110af70e1e2c0ce0 Mon Sep 17 00:00:00 2001
From: Martin Felis <martin@fysx.org>
Date: Thu, 23 Jun 2022 22:37:40 +0200
Subject: [PATCH] Number of solver steps now editable in debug gui

---
 3rdparty/rbdl/include/rbdl/SimpleMath/SimpleMath.h | 4 ----
 include/rbdlsim.h                                  | 2 +-
 src/main.cc                                        | 2 +-
 src/rbdlsim.cc                                     | 3 +--
 src/simulator.cc                                   | 5 ++++-
 src/vissim.cc                                      | 4 ++--
 6 files changed, 9 insertions(+), 11 deletions(-)

diff --git a/3rdparty/rbdl/include/rbdl/SimpleMath/SimpleMath.h b/3rdparty/rbdl/include/rbdl/SimpleMath/SimpleMath.h
index ba4b56c..075a5c2 100644
--- a/3rdparty/rbdl/include/rbdl/SimpleMath/SimpleMath.h
+++ b/3rdparty/rbdl/include/rbdl/SimpleMath/SimpleMath.h
@@ -819,9 +819,6 @@ struct Storage {
 
   inline size_t cols() const { return NumCols; }
 
-#ifdef NDEBUG
-  void resize(int UNUSED(num_rows), int UNUSED(num_cols)) {}
-#else
   void resize(int num_rows, int num_cols) {
   // Resizing of fixed size matrices not allowed
       if (num_rows != NumRows || num_cols != NumCols) {
@@ -831,7 +828,6 @@ struct Storage {
       }
       assert (num_rows == NumRows && num_cols == NumCols);
   }
-#endif
 
   inline ScalarType& coeff(int row_index, int col_index) {
 //    assert (row_index >= 0 && row_index <= NumRows);
diff --git a/include/rbdlsim.h b/include/rbdlsim.h
index d9425eb..92fa0a0 100644
--- a/include/rbdlsim.h
+++ b/include/rbdlsim.h
@@ -86,7 +86,7 @@ struct World {
   void calcUnconstrainedVelUpdate(double dt);
   void updateCollisionShapes();
   void detectCollisions();
-  void resolveCollisions(double dt);
+  void resolveCollisions(double dt, int num_iter);
   bool integrateWorld(double dt);
 };
 
diff --git a/src/main.cc b/src/main.cc
index 745e895..b0cddf5 100644
--- a/src/main.cc
+++ b/src/main.cc
@@ -26,7 +26,7 @@ void simplesim() {
   do {
     world.updateCollisionShapes();
     world.detectCollisions();
-    world.resolveCollisions(dt);
+    world.resolveCollisions(dt, 20);
     world.integrateWorld(dt);
   } while (world.mSimTime < 10.01);
 }
diff --git a/src/rbdlsim.cc b/src/rbdlsim.cc
index 863063e..572306a 100644
--- a/src/rbdlsim.cc
+++ b/src/rbdlsim.cc
@@ -796,14 +796,13 @@ void World::detectCollisions() {
   }
 }
 
-void World::resolveCollisions(double dt) {
+void World::resolveCollisions(double dt, int num_iter) {
   ZoneScoped;
 
   for (CollisionInfo& cinfo : mContactPoints) {
     PrepareConstraintImpulse(dt, cinfo.mBodyA, cinfo.mBodyB, cinfo);
   }
 
-  int num_iter = 20;
   for (int i = 0; i < num_iter; i++) {
     for (CollisionInfo& cinfo : mContactPoints) {
       CalcFrictionImpulse(cinfo.mBodyA, cinfo.mBodyB, cinfo);
diff --git a/src/simulator.cc b/src/simulator.cc
index 4469984..4c78956 100644
--- a/src/simulator.cc
+++ b/src/simulator.cc
@@ -32,6 +32,7 @@ static double sSimTimeAccumulator = 0.;
 static double sSimTimeStep = 1.0e-2;
 static sthstry* sStateHistory = nullptr;
 static int sStateHistoryCurrent = 0;
+static int sSolverNumIter = 20;
 
 void simulator_init() {
   gLog("Initializing Simulator");
@@ -153,6 +154,8 @@ void simulator_gui() {
   sSimTime = sSimTimeStep * sStateHistoryCurrent;
   ImGui::Text("Time: %f", sSimTime);
 
+	ImGui::SliderInt("Solver Steps", &sSolverNumIter, 0, 100);
+
   ImGui::Text("Ground Plane");
   Vector3f ground_pos = sGroundShape.pos;
   ImGui::DragFloat3("Position", ground_pos.data(), 0.1f, -5.0f, 5.0f);
@@ -205,7 +208,7 @@ void simulator_step(double dt) {
   sWorld.updateCollisionShapes();
   sWorld.detectCollisions();
 
-  sWorld.resolveCollisions(dt);
+  sWorld.resolveCollisions(dt, sSolverNumIter);
   sWorld.integrateWorld(dt);
 
   {
diff --git a/src/vissim.cc b/src/vissim.cc
index f73d01d..e8b6b31 100644
--- a/src/vissim.cc
+++ b/src/vissim.cc
@@ -1009,8 +1009,8 @@ int main() {
         ImGui::End();
       }
 
-      gHullScene.draw(gRndrCmds);
-      //      SimulatorSceneRender(gRndrCmds);
+      //gHullScene.draw(gRndrCmds);
+      SimulatorSceneRender(gRndrCmds);
       srndr_render(gRndr, gView, gRndrCmds);
       ImSRenderWidget(gRndr, gView);
     }