Number of solver steps now editable in debug gui

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);

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);
 };
 

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);
 }

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);

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);
 
   {

src/vissim.cc

@@ -1009,8 +1009,8 @@ int main() {
         ImGui::End();
       }
 
-      gHullScene.draw(gRndrCmds);
+      //gHullScene.draw(gRndrCmds);
-      //      SimulatorSceneRender(gRndrCmds);
+      SimulatorSceneRender(gRndrCmds);
       srndr_render(gRndr, gView, gRndrCmds);
       ImSRenderWidget(gRndr, gView);
     }