diff --git a/Examples/FemurCut/FemurCutExample.cpp b/Examples/FemurCut/FemurCutExample.cpp
index be222f4dae581660728c1e7cbc6b329d1bd05d9e..704074a4c31904e5fe0640c72eb970583390275f 100644
--- a/Examples/FemurCut/FemurCutExample.cpp
+++ b/Examples/FemurCut/FemurCutExample.cpp
@@ -56,7 +56,7 @@ makeRigidObj(const std::string& name)
     rbdModel->getConfig()->m_maxNumIterations       = 8;
     rbdModel->getConfig()->m_velocityDamping        = 1.0;
     rbdModel->getConfig()->m_angularVelocityDamping = 1.0;
-    rbdModel->getConfig()->m_maxNumConstraints      = 20;
+    rbdModel->getConfig()->m_maxNumConstraints      = 40;
 
     // Create the first rbd, plane floor
     imstkNew<RigidObject2> rigidObj(name);
@@ -158,8 +158,8 @@ main()
 
         imstkNew<RigidObjectController> controller(rbdObj, hapticDeviceClient);
         {
-            controller->setLinearKd(1000.0 * 0.9);
-            controller->setLinearKs(100000.0 * 0.9);
+            controller->setLinearKd(1000.0);
+            controller->setLinearKs(100000.0);
             controller->setAngularKs(300000000.0);
             controller->setAngularKd(400000.0);
             controller->setForceScaling(0.001);
@@ -187,13 +187,13 @@ main()
             ghostMesh->setRotation(controller->getRotation());
             ghostMesh->updatePostTransformData();
             ghostMesh->postModified();
-        });
+            });
 
         imstkNew<SimulationManager> driver;
         driver->addModule(viewer);
         driver->addModule(sceneManager);
         driver->addModule(hapticManager);
-        driver->setDesiredDt(0.001); // Little over 1000ups
+        driver->setDesiredDt(0.001); // Exactly 1000ups
 
         {
             imstkNew<MouseSceneControl> mouseControl(viewer->getMouseDevice());
diff --git a/Source/Common/Parallel/imstkParallelFor.h b/Source/Common/Parallel/imstkParallelFor.h
index 3fa9b397cccfe374076bff5cf1133a5924696074..77b26c93e89a844f9e0c90d21f62234830ab2015 100644
--- a/Source/Common/Parallel/imstkParallelFor.h
+++ b/Source/Common/Parallel/imstkParallelFor.h
@@ -29,18 +29,32 @@ namespace ParallelUtils
 {
 ///
 /// \brief Execute a function in parallel over a range [beginIdx, endIdx) of indices
+/// \param start index
+/// \param end index
+/// \param function to execute that takes index
+/// \param if less than maxN falls back to normal for loop
 ///
 template<class IndexType, class Function>
 void
-parallelFor(const IndexType beginIdx, const IndexType endIdx, Function&& function)
+parallelFor(const IndexType beginIdx, const IndexType endIdx, Function&& function, const bool doParallel = true)
 {
-    tbb::parallel_for(tbb::blocked_range<IndexType>(beginIdx, endIdx),
-        [&](const tbb::blocked_range<IndexType>& r) {
-                for (IndexType i = r.begin(), iEnd = r.end(); i < iEnd; ++i)
-                {
-                    function(i);
-                }
-        });
+    if (doParallel)
+    {
+        tbb::parallel_for(tbb::blocked_range<IndexType>(beginIdx, endIdx),
+            [&](const tbb::blocked_range<IndexType>& r) {
+                    for (IndexType i = r.begin(), iEnd = r.end(); i < iEnd; ++i)
+                    {
+                        function(i);
+                    }
+            });
+    }
+    else
+    {
+        for (IndexType i = beginIdx; i < endIdx; i++)
+        {
+            function(i);
+        }
+    }
 }
 
 ///
@@ -48,9 +62,9 @@ parallelFor(const IndexType beginIdx, const IndexType endIdx, Function&& functio
 ///
 template<class IndexType, class Function>
 void
-parallelFor(const IndexType endIdx, Function&& function)
+parallelFor(const IndexType endIdx, Function&& function, const bool doParallel = true)
 {
-    parallelFor(IndexType(0), endIdx, std::forward<Function>(function));
+    parallelFor(IndexType(0), endIdx, std::forward<Function>(function), doParallel);
 }
 
 ///
diff --git a/Source/DynamicalModels/ObjectModels/imstkLevelSetModel.cpp b/Source/DynamicalModels/ObjectModels/imstkLevelSetModel.cpp
index 63bead3b526aef199c33492db94c71f9cbf48725..6baab4abac0e5c391415fc20fbe14102219676c8 100644
--- a/Source/DynamicalModels/ObjectModels/imstkLevelSetModel.cpp
+++ b/Source/DynamicalModels/ObjectModels/imstkLevelSetModel.cpp
@@ -125,70 +125,77 @@ LevelSetModel::evolve()
             return;
         }
 
-        // index, coordinates, force, forward/backward gradient magnitude, curvature
-        //std::vector<std::tuple<size_t, Vec3i, double, Vec2d, double>> nodeUpdates;
-        //nodeUpdates.reserve(m_nodesToUpdate.size());
+        // Setup a map of 0 based index -> image sparse index m_nodesToUpdate to parallelize
+        std::vector<size_t> baseIndexToImageIndex;
+        baseIndexToImageIndex.reserve(m_nodesToUpdate.size());
+        for (std::unordered_map<size_t, std::tuple<Vec3i, double>>::iterator iter = m_nodesToUpdate.begin(); iter != m_nodesToUpdate.end(); iter++)
+        {
+            baseIndexToImageIndex.push_back(iter->first);
+        }
 
         // Compute gradients
-        const double constantVel = m_config->m_constantVelocity;
+        const double                             constantVel = m_config->m_constantVelocity;
+        std::tuple<size_t, Vec3i, double, Vec2d> val;
         for (int j = 0; j < m_config->m_substeps; j++)
         {
-            noteUpdatePoolSize = 0;
-            for (std::unordered_map<size_t, std::tuple<Vec3i, double>>::iterator iter = m_nodesToUpdate.begin(); iter != m_nodesToUpdate.end(); iter++)
-            {
-                const size_t index  = iter->first;
-                const Vec3i& coords = std::get<0>(iter->second);
-                const double f      = std::get<1>(iter->second);
+            ParallelUtils::parallelFor(baseIndexToImageIndex.size(), [&](const size_t i)
+                {
+                    std::tuple<size_t, Vec3i, double, Vec2d, double>& outputVal = m_nodeUpdatePool[i];
+                    const size_t& index = std::get<0>(outputVal) = baseIndexToImageIndex[i];
 
-                // Gradients
-                const Vec3d gradPos = m_forwardGrad(Vec3d(coords[0], coords[1], coords[2]));
-                const Vec3d gradNeg = m_backwardGrad(Vec3d(coords[0], coords[1], coords[2]));
+                    std::tuple<Vec3i, double>& inputVal = m_nodesToUpdate[index];
 
-                Vec3d gradNegMax = gradNeg.cwiseMax(0.0);
-                Vec3d gradNegMin = gradNeg.cwiseMin(0.0);
-                Vec3d gradPosMax = gradPos.cwiseMax(0.0);
-                Vec3d gradPosMin = gradPos.cwiseMin(0.0);
+                    const Vec3i& coords    = std::get<1>(outputVal) = std::get<0>(inputVal);
+                    std::get<2>(outputVal) = std::get<1>(inputVal);
 
-                // Square them
-                gradNegMax = gradNegMax.cwiseProduct(gradNegMax);
-                gradNegMin = gradNegMin.cwiseProduct(gradNegMin);
-                gradPosMax = gradPosMax.cwiseProduct(gradPosMax);
-                gradPosMin = gradPosMin.cwiseProduct(gradPosMin);
+                    // Gradients
+                    const Vec3d gradPos = m_forwardGrad(Vec3d(coords[0], coords[1], coords[2]));
+                    const Vec3d gradNeg = m_backwardGrad(Vec3d(coords[0], coords[1], coords[2]));
 
-                const double posMag =
-                    gradNegMax[0] + gradNegMax[1] + gradNegMax[2] +
-                    gradPosMin[0] + gradPosMin[1] + gradPosMin[2];
+                    Vec3d gradNegMax = gradNeg.cwiseMax(0.0);
+                    Vec3d gradNegMin = gradNeg.cwiseMin(0.0);
+                    Vec3d gradPosMax = gradPos.cwiseMax(0.0);
+                    Vec3d gradPosMin = gradPos.cwiseMin(0.0);
 
-                const double negMag =
-                    gradNegMin[0] + gradNegMin[1] + gradNegMin[2] +
-                    gradPosMax[0] + gradPosMax[1] + gradPosMax[2];
+                    // Square them
+                    gradNegMax = gradNegMax.cwiseProduct(gradNegMax);
+                    gradNegMin = gradNegMin.cwiseProduct(gradNegMin);
+                    gradPosMax = gradPosMax.cwiseProduct(gradPosMax);
+                    gradPosMin = gradPosMin.cwiseProduct(gradPosMin);
 
-                // Curvature
-                //const double kappa = m_curvature(Vec3d(coords[0], coords[1], coords[2]));
+                    const double posMag =
+                        gradNegMax[0] + gradNegMax[1] + gradNegMax[2] +
+                        gradPosMin[0] + gradPosMin[1] + gradPosMin[2];
 
-                m_nodeUpdatePool[noteUpdatePoolSize++] = std::tuple<size_t, Vec3i, double, Vec2d, double>(index, coords, f, Vec2d(negMag, posMag), 0.0);
-            }
+                    const double negMag =
+                        gradNegMin[0] + gradNegMin[1] + gradNegMin[2] +
+                        gradPosMax[0] + gradPosMax[1] + gradPosMax[2];
 
-            // Update levelset
-            for (size_t i = 0; i < noteUpdatePoolSize; i++)
-            //ParallelUtils::parallelFor(noteUpdatePoolSize, [&](const size_t& i)
-            {
-                const size_t index = std::get<0>(m_nodeUpdatePool[i]);
-                const double vel   = std::get<2>(m_nodeUpdatePool[i]) + constantVel;
-                const Vec2d& g     = std::get<3>(m_nodeUpdatePool[i]);
-                //const double kappa = std::get<4>(nodeUpdates[i]);
+                    std::get<3>(outputVal) = Vec2d(negMag, posMag);
 
-                // If speed function positive use forward difference (posMag)
-                if (vel > 0.0)
-                {
-                    imgPtr[index] += dt * (vel * std::sqrt(g[0]) /*+ kappa * k*/);
-                }
-                // If speed function negative use backward difference (negMag)
-                else if (vel < 0.0)
+                    // Curvature
+                    //const double kappa = m_curvature(Vec3d(coords[0], coords[1], coords[2]));
+                }, baseIndexToImageIndex.size() > 50);
+
+            // Update levelset
+            ParallelUtils::parallelFor(baseIndexToImageIndex.size(), [&](const size_t& i)
                 {
-                    imgPtr[index] += dt * (vel * std::sqrt(g[1]) /*+ kappa * k*/);
-                }
-            }//);
+                    const size_t index = std::get<0>(m_nodeUpdatePool[i]);
+                    const double vel   = std::get<2>(m_nodeUpdatePool[i]) + constantVel;
+                    const Vec2d& g     = std::get<3>(m_nodeUpdatePool[i]);
+                    //const double kappa = std::get<4>(nodeUpdates[i]);
+
+                    // If speed function positive use forward difference (posMag)
+                    if (vel > 0.0)
+                    {
+                        imgPtr[index] += dt * (vel * std::sqrt(g[0]) /*+ kappa * k*/);
+                    }
+                    // If speed function negative use backward difference (negMag)
+                    else if (vel < 0.0)
+                    {
+                        imgPtr[index] += dt * (vel * std::sqrt(g[1]) /*+ kappa * k*/);
+                    }
+            }, noteUpdatePoolSize > m_maxVelocitiesParallel);
         }
 
         m_nodesToUpdate.clear();
diff --git a/Source/DynamicalModels/ObjectModels/imstkLevelSetModel.h b/Source/DynamicalModels/ObjectModels/imstkLevelSetModel.h
index 39a149f1f4707bedfc01383593b270df50743c35..cd0ee2eb99348a10580bd6dd7f56883762c3584b 100644
--- a/Source/DynamicalModels/ObjectModels/imstkLevelSetModel.h
+++ b/Source/DynamicalModels/ObjectModels/imstkLevelSetModel.h
@@ -121,6 +121,7 @@ protected:
     std::unordered_map<size_t, std::tuple<Vec3i, double>> m_nodesToUpdate;
     std::vector<std::tuple<size_t, Vec3i, double, Vec2d, double>> m_nodeUpdatePool;
     size_t noteUpdatePoolSize;
+    size_t m_maxVelocitiesParallel = 100;                      // In sparse mode, if surpass this value, switch to parallel
 
     std::shared_ptr<ImageData> m_gradientMagnitudes = nullptr; ///> Gradient magnitude field when using dense
     std::shared_ptr<ImageData> m_velocities = nullptr;
diff --git a/Source/DynamicalModels/ObjectModels/imstkRigidBodyModel2.cpp b/Source/DynamicalModels/ObjectModels/imstkRigidBodyModel2.cpp
index 5287559612b01f2c862bd80b7df13abc5220ea30..eb3bd0ae9f11f13dc7f6862e14478b754fa83a5e 100644
--- a/Source/DynamicalModels/ObjectModels/imstkRigidBodyModel2.cpp
+++ b/Source/DynamicalModels/ObjectModels/imstkRigidBodyModel2.cpp
@@ -172,20 +172,18 @@ RigidBodyModel2::computeTentativeVelocities()
     const Vec3d&            fG      = m_config->m_gravity;
 
     // Sum gravity to the forces
-    //ParallelUtils::parallelFor(forces.size(), [&forces, &fG](const size_t& i)
-    for (size_t i = 0; i < forces.size(); i++)
-    {
-        forces[i] += fG;
-    }    //);
+    ParallelUtils::parallelFor(forces.size(), [&forces, &fG](const int& i)
+            {
+                forces[i] += fG;
+        }, forces.size() > m_maxBodiesParallel);
 
     // Compute the desired velocites, later we will solve for the proper velocities,
     // adjusted for the constraints
-    //ParallelUtils::parallelFor(tentativeVelocities.size(), [&](const size_t& i)
-    for (size_t i = 0; i < tentativeAngularVelocities.size(); i++)
-    {
-        tentativeVelocities[i] += forces[i] * invMasses[i] * dt;
-        tentativeAngularVelocities[i] += invInteriaTensors[i] * torques[i] * dt;
-    }    //);
+    ParallelUtils::parallelFor(tentativeVelocities.size(), [&](const size_t& i)
+            {
+                tentativeVelocities[i] += forces[i] * invMasses[i] * dt;
+                tentativeAngularVelocities[i] += invInteriaTensors[i] * torques[i] * dt;
+        }, tentativeVelocities.size() > m_maxBodiesParallel);
 }
 
 void
@@ -362,36 +360,35 @@ RigidBodyModel2::integrate()
     StdVectorOfVec3d& forces  = getCurrentState()->getForces();
     StdVectorOfVec3d& torques = getCurrentState()->getTorques();
 
-    //ParallelUtils::parallelFor(positions.size(), [&](const size_t& i)
-    for (size_t i = 0; i < positions.size(); i++)
-    {
-        if (!isStatic[i])
-        {
-            velocities[i] += forces[i] * invMasses[i] * dt;
-            velocities[i] *= velocityDamping;
-            angularVelocities[i] += invInteriaTensors[i] * torques[i] * dt;
-            angularVelocities[i] *= angularVelocityDamping;
-            positions[i] += velocities[i] * dt;
+    ParallelUtils::parallelFor(positions.size(), [&](const size_t& i)
             {
-                const Quatd q = Quatd(0.0,
-                        angularVelocities[i][0],
-                        angularVelocities[i][1],
-                        angularVelocities[i][2]) * orientations[i];
-                orientations[i].x() += q.x() * dt;
-                orientations[i].y() += q.y() * dt;
-                orientations[i].z() += q.z() * dt;
-                orientations[i].w() += q.w() * dt;
-                orientations[i].normalize();
-            }
-        }
+                if (!isStatic[i])
+                {
+                    velocities[i] += forces[i] * invMasses[i] * dt;
+                    velocities[i] *= velocityDamping;
+                    angularVelocities[i] += invInteriaTensors[i] * torques[i] * dt;
+                    angularVelocities[i] *= angularVelocityDamping;
+                    positions[i] += velocities[i] * dt;
+                    {
+                        const Quatd q = Quatd(0.0,
+                            angularVelocities[i][0],
+                            angularVelocities[i][1],
+                            angularVelocities[i][2]) * orientations[i];
+                        orientations[i].x() += q.x() * dt;
+                        orientations[i].y() += q.y() * dt;
+                        orientations[i].z() += q.z() * dt;
+                        orientations[i].w() += q.w() * dt;
+                        orientations[i].normalize();
+                    }
+                }
 
-        // Reset
-        m_bodies[i]->m_prevForce = forces[i];
-        forces[i]  = Vec3d(0.0, 0.0, 0.0);
-        torques[i] = Vec3d(0.0, 0.0, 0.0);
-        tentativeVelocities[i] = velocities[i];
-        tentativeAngularVelocities[i] = angularVelocities[i];
-    }    //);
+                // Reset
+                m_bodies[i]->m_prevForce = forces[i];
+                forces[i]  = Vec3d(0.0, 0.0, 0.0);
+                torques[i] = Vec3d(0.0, 0.0, 0.0);
+                tentativeVelocities[i] = velocities[i];
+                tentativeAngularVelocities[i] = angularVelocities[i];
+        }, positions.size() > m_maxBodiesParallel);
 }
 
 void
diff --git a/Source/DynamicalModels/ObjectModels/imstkRigidBodyModel2.h b/Source/DynamicalModels/ObjectModels/imstkRigidBodyModel2.h
index dbe665f78fed0432e9a4db2fd1d3f6f22f3f0e84..fad63f7ea0f54f189ecb8a79b567ebb0c24d6943 100644
--- a/Source/DynamicalModels/ObjectModels/imstkRigidBodyModel2.h
+++ b/Source/DynamicalModels/ObjectModels/imstkRigidBodyModel2.h
@@ -149,9 +149,10 @@ protected:
     std::list<std::shared_ptr<RbdConstraint>>    m_constraints;
     std::vector<std::shared_ptr<RigidBody>>      m_bodies;
     std::unordered_map<RigidBody*, StorageIndex> m_locations;
-    bool m_modified = true;
+    bool   m_modified = true;
+    size_t m_maxBodiesParallel = 10; // After 10 bodies, parallel for's are used
 
-    Eigen::VectorXd F; // Reaction forces
+    Eigen::VectorXd F;               // Reaction forces
 };
 }
 }
\ No newline at end of file
diff --git a/Source/Geometry/Implicit/imstkImplicitFunctionFiniteDifferenceFunctor.h b/Source/Geometry/Implicit/imstkImplicitFunctionFiniteDifferenceFunctor.h
index a8fc60ce11284de242cf4d5d885ee91dc3578c5d..dd294b2b427004c7bd09d8ef502ffdab4dd26121 100644
--- a/Source/Geometry/Implicit/imstkImplicitFunctionFiniteDifferenceFunctor.h
+++ b/Source/Geometry/Implicit/imstkImplicitFunctionFiniteDifferenceFunctor.h
@@ -141,7 +141,7 @@ struct ImplicitFunctionBackwardGradient : public ImplicitFunctionGradient
 struct StructuredForwardGradient : public ImplicitFunctionGradient
 {
     public:
-        Vec3d operator()(const Vec3d& pos) const override
+        inline Vec3d operator()(const Vec3d& pos) const override
         {
             const SignedDistanceField& funcRef      = *static_cast<SignedDistanceField*>(m_func.get());
             const double               centralValue = funcRef.getFunctionValueCoord(Vec3i(static_cast<int>(pos[0]), static_cast<int>(pos[1]), static_cast<int>(pos[2])));
@@ -171,7 +171,7 @@ struct StructuredForwardGradient : public ImplicitFunctionGradient
 struct StructuredBackwardGradient : public ImplicitFunctionGradient
 {
     public:
-        Vec3d operator()(const Vec3d& pos) const override
+        inline Vec3d operator()(const Vec3d& pos) const override
         {
             const SignedDistanceField& funcRef      = *static_cast<SignedDistanceField*>(m_func.get());
             const double               centralValue = funcRef.getFunctionValueCoord(Vec3i(static_cast<int>(pos[0]), static_cast<int>(pos[1]), static_cast<int>(pos[2])));
diff --git a/Source/Geometry/Implicit/imstkSignedDistanceField.cpp b/Source/Geometry/Implicit/imstkSignedDistanceField.cpp
index 15da73db818bda36ae2eb76bfe7fe3fcb8872f42..e32bb168f8c6f094c11f1d3e5c56fb0dd2259adc 100644
--- a/Source/Geometry/Implicit/imstkSignedDistanceField.cpp
+++ b/Source/Geometry/Implicit/imstkSignedDistanceField.cpp
@@ -116,22 +116,6 @@ SignedDistanceField::getFunctionValue(const Vec3d& pos) const
     }
 }
 
-double
-SignedDistanceField::getFunctionValueCoord(const Vec3i& coord) const
-{
-    if (coord[0] < m_imageDataSdf->getDimensions()[0] && coord[0] > 0
-        && coord[1] < m_imageDataSdf->getDimensions()[1] && coord[1] > 0
-        && coord[2] < m_imageDataSdf->getDimensions()[2] && coord[2] > 0)
-    {
-        const Vec3i clampedCoord = coord.cwiseMin(m_imageDataSdf->getDimensions() - Vec3i(1, 1, 1)).cwiseMax(0);
-        return (*m_scalars)[m_imageDataSdf->getScalarIndex(clampedCoord)] * m_scale;
-    }
-    else
-    {
-        return std::numeric_limits<double>::min();
-    }
-}
-
 void
 SignedDistanceField::computeBoundingBox(Vec3d& min, Vec3d& max, const double paddingPercent)
 {
diff --git a/Source/Geometry/Implicit/imstkSignedDistanceField.h b/Source/Geometry/Implicit/imstkSignedDistanceField.h
index 1c05fe5f2bde9842b49e58c987909a2442815671..c8095de257fe821dc93a6d5369a7ae61de3b4ffb 100644
--- a/Source/Geometry/Implicit/imstkSignedDistanceField.h
+++ b/Source/Geometry/Implicit/imstkSignedDistanceField.h
@@ -21,12 +21,14 @@
 
 #pragma once
 
+#include "imstkDataArray.h"
+#include "imstkImageData.h"
 #include "imstkImplicitGeometry.h"
 
 namespace imstk
 {
-class ImageData;
-template<typename T> class DataArray;
+//class ImageData;
+//template<typename T> class DataArray;
 
 ///
 /// \class SignedDistanceField
@@ -60,12 +62,25 @@ public:
     ///
     /// \brief Returns signed distance to surface at pos, returns clamped/nearest if out of bounds
     ///
-    double getFunctionValue(const Vec3d& pos) const override;
+    double SignedDistanceField::getFunctionValue(const Vec3d& pos) const;
 
     ///
     /// \brief Returns signed distance to surface at coordinate
-    ///
-    double getFunctionValueCoord(const Vec3i& coord) const;
+    /// inlined for performance
+    ///
+    inline double SignedDistanceField::getFunctionValueCoord(const Vec3i& coord) const
+    {
+        if (coord[0] < m_imageDataSdf->getDimensions()[0] && coord[0] > 0
+            && coord[1] < m_imageDataSdf->getDimensions()[1] && coord[1] > 0
+            && coord[2] < m_imageDataSdf->getDimensions()[2] && coord[2] > 0)
+        {
+            return (*m_scalars)[m_imageDataSdf->getScalarIndex(coord)] * m_scale;
+        }
+        else
+        {
+            return std::numeric_limits<double>::max();
+        }
+    }
 
     ///
     /// \brief Returns the bounds of the field
diff --git a/Source/SimulationManager/imstkSimulationManager.cpp b/Source/SimulationManager/imstkSimulationManager.cpp
index faa0aed82ec611a5c63adf78a247338e898d73fb..76481bdb5bbdcb22fe058a48e467ff57061333e2 100644
--- a/Source/SimulationManager/imstkSimulationManager.cpp
+++ b/Source/SimulationManager/imstkSimulationManager.cpp
@@ -147,21 +147,22 @@ SimulationManager::start()
             accumulator += passedTime;
 
             // Compute number of steps we can take (total time previously took / desired time step)
-            m_numSteps = 0;
-            while (accumulator >= desiredDt_ms)
             {
-                m_numSteps++;
-                accumulator -= desiredDt_ms;
+                // Divided out and floor
+                m_numSteps = static_cast<int>(accumulator / desiredDt_ms);
+                // Set the remainder
+                accumulator = accumulator - m_numSteps * desiredDt_ms;
+                // Flatten out the remainder over our desired dt
+                m_dt = desiredDt_ms;
+                if (m_numSteps != 0)
+                {
+                    m_dt += accumulator / m_numSteps;
+                    accumulator = 0.0;
+                }
+                m_dt *= 0.001; // ms->s
             }
 
-            // Flatten out the leftover accumulation in our chosen dt
-            m_dt = desiredDt_ms;
-            if (m_numSteps != 0)
-            {
-                m_dt += accumulator / m_numSteps;
-                accumulator = 0.0;
-            }
-            m_dt *= 0.001; // ms->s
+            //printf("%d steps at %f\n", m_numSteps, m_dt);
 
             // Optional smoothening + loss here
 
diff --git a/Source/SimulationManager/imstkSimulationManager.h b/Source/SimulationManager/imstkSimulationManager.h
index a2e5953cff8f1668ad5519d2b1c0875eff297f1c..b2949b425e64667821bca2fbd63a654e36a40b69 100644
--- a/Source/SimulationManager/imstkSimulationManager.h
+++ b/Source/SimulationManager/imstkSimulationManager.h
@@ -53,7 +53,7 @@ public:
     };
 
 public:
-    ~SimulationManager() override= default;
+    virtual ~SimulationManager() override= default;
 
 public:
     ///
@@ -123,6 +123,6 @@ protected:
     ThreadingType m_threadType = ThreadingType::STL;
     double m_desiredDt = 0.003; // Desired timestep
     double m_dt       = 0.0;    // Actual timestep
-    double m_numSteps = 0;
+    int    m_numSteps = 0;
 };
 };
\ No newline at end of file