ENH: RigidBodyController force smoothening, Lazy VisualModels for LevelSet-RigidInteractionExample

Examples/Interactions/LevelSet-Rigid/LevelSet-RigidInteractionExample.cpp

@@ -56,13 +56,14 @@ using namespace imstk::expiremental;
 /// \brief Creates a level set obj (poly rendering)
 ///
 std::shared_ptr<LevelSetDeformableObject>
-makeLevelsetObj(const std::string& name, std::shared_ptr<LocalMarchingCubes> isoExtract)
+makeLevelsetObj(const std::string& name, std::shared_ptr<LocalMarchingCubes> isoExtract, std::unordered_set<int>& chunksGenerated)
 {
     imstkNew<LevelSetDeformableObject> levelsetObj(name);
 
     std::shared_ptr<ImageData> initLvlsetImage = MeshIO::read<ImageData>(iMSTK_DATA_ROOT "/legs/femurBone_SDF.nii")->cast(IMSTK_DOUBLE);
     const Vec3d&               currSpacing     = initLvlsetImage->getSpacing();
-    initLvlsetImage->setSpacing(currSpacing * 0.001); // note: Anisotropic scaling invalidates SDF
+    // Note: Anistropic scaling would invalidate the SDF
+    initLvlsetImage->setSpacing(currSpacing * 0.001);
     initLvlsetImage->setOrigin(Vec3d(0.0, 0.8, 1.5));
 
     // Setup the Parameters
@@ -76,27 +77,35 @@ makeLevelsetObj(const std::string& name, std::shared_ptr<LocalMarchingCubes> iso
     model->setModelGeometry(initLvlsetImage);
     model->configure(lvlSetConfig);
 
+    // Too many chunks and you'll hit memory constraints quickly
+    // Too little chunks and the updates for a chunk will take too long
+    // The chunks must divide the image dimensions (image dim-1 must be divisible by # chunks)
     isoExtract->setInputImage(initLvlsetImage);
     isoExtract->setIsoValue(0.0);
-    isoExtract->setNumberOfChunks(Vec3i(16, 9, 9)); // Image dim-1 must be divisible by # chunks
+    isoExtract->setNumberOfChunks(Vec3i(32, 9, 9));
     isoExtract->update();
 
     srand(time(NULL));
 
-    for (int i = 0; i < 9 * 9 * 16; i++)
+    const Vec3i& numChunks = isoExtract->getNumberOfChunks();
+    for (int i = 0; i < numChunks[0] * numChunks[1] * numChunks[2]; i++)
     {
-        imstkNew<VisualModel>    surfMeshModel(isoExtract->getOutput(i));
-        imstkNew<RenderMaterial> material;
-        material->setDisplayMode(RenderMaterial::DisplayMode::Surface);
-        material->setLineWidth(4.0);
-        //material->setPointSize(0.001);
-        const double r = (rand() % 500) / 500.0;
-        const double g = (rand() % 500) / 500.0;
-        const double b = (rand() % 500) / 500.0;
-        material->setColor(Color(r, g, b));
-        material->setEdgeColor(Color::Color::Orange);
-        surfMeshModel->setRenderMaterial(material);
-        levelsetObj->addVisualModel(surfMeshModel);
+        auto surfMesh = std::dynamic_pointer_cast<SurfaceMesh>(isoExtract->getOutput(i));
+        if (surfMesh->getNumVertices() > 0 && chunksGenerated.count(i) == 0)
+        {
+            imstkNew<VisualModel>    surfMeshModel(isoExtract->getOutput(i));
+            imstkNew<RenderMaterial> material;
+            material->setDisplayMode(RenderMaterial::DisplayMode::Surface);
+            material->setLineWidth(4.0);
+            const double r = (rand() % 500) / 500.0;
+            const double g = (rand() % 500) / 500.0;
+            const double b = (rand() % 500) / 500.0;
+            material->setColor(Color(r, g, b));
+            material->setEdgeColor(Color::Color::Orange);
+            surfMeshModel->setRenderMaterial(material);
+            levelsetObj->addVisualModel(surfMeshModel);
+            chunksGenerated.insert(i);
+        }
     }
 
     // Setup the Object
@@ -124,7 +133,7 @@ makeRigidObj(const std::string& name)
     imstkNew<RigidObject2> rigidObj("Cube");
 
     {
-        auto toolMesh = MeshIO::read<SurfaceMesh>(iMSTK_DATA_ROOT "/Surgical Instruments/Scalpel/Scalpel_Blade10_Hull.stl");
+        auto toolMesh = MeshIO::read<SurfaceMesh>("C:/Users/Andx_/Desktop/testMesh.stl");
         toolMesh->rotate(Vec3d(0.0, 1.0, 0.0), 3.14, Geometry::TransformType::ApplyToData);
         toolMesh->rotate(Vec3d(1.0, 0.0, 0.0), -1.57, Geometry::TransformType::ApplyToData);
         toolMesh->scale(Vec3d(0.07, 0.07, 0.07), Geometry::TransformType::ApplyToData);
@@ -154,7 +163,9 @@ makeRigidObj(const std::string& name)
 }
 
 ///
-/// \brief This example demonstrates evolution of a levelset
+/// \brief This example demonstrates cutting a femur bone with a tool
+/// Some of the example parameters may need to be tweaked for differing
+/// systems
 ///
 int
 main()
@@ -165,7 +176,8 @@ main()
     imstkNew<Scene> scene("FemurCut");
 
     std::shared_ptr<LocalMarchingCubes>       isoExtract = std::make_shared<LocalMarchingCubes>();
-    std::shared_ptr<LevelSetDeformableObject> lvlSetObj  = makeLevelsetObj("LevelSetObj", isoExtract);
+    std::unordered_set<int>                   chunksGenerated; // Lazy generation of chunks
+    std::shared_ptr<LevelSetDeformableObject> lvlSetObj = makeLevelsetObj("LevelSetObj", isoExtract, chunksGenerated);
     scene->addSceneObject(lvlSetObj);
 
     std::shared_ptr<RigidObject2> rbdObj = makeRigidObj("RigidObj");
@@ -186,9 +198,9 @@ main()
     colHandlerA->setStiffness(0.0); // inelastic collision
     auto colHandlerB = std::dynamic_pointer_cast<LevelSetCH>(interaction->getCollisionHandlingB());
     colHandlerB->setLevelSetVelocityScaling(0.1);
-    colHandlerB->setKernel(5, 1.0);
+    colHandlerB->setKernel(3, 1.0);
     //colHandlerB->setLevelSetVelocityScaling(0.0); // Can't push the levelset
-    colHandlerB->setUseProportionalVelocity(false);
+    colHandlerB->setUseProportionalVelocity(true);
     scene->getCollisionGraph()->addInteraction(interaction);
 
     // Light (white)
@@ -247,7 +259,30 @@ main()
         });
         connect<Event>(viewer, EventType::PreUpdate, [&](Event*)
         {
+            // Update any chunks that contain a voxel which was set modified
             isoExtract->update();
+
+            // Create meshes for chunks if they now contain vertices (and weren't already generated)
+            const Vec3i& numChunks = isoExtract->getNumberOfChunks();
+            for (int i = 0; i < numChunks[0] * numChunks[1] * numChunks[2]; i++)
+            {
+                auto surfMesh = std::dynamic_pointer_cast<SurfaceMesh>(isoExtract->getOutput(i));
+                if (surfMesh->getNumVertices() > 0 && chunksGenerated.count(i) == 0)
+                {
+                    imstkNew<VisualModel> surfMeshModel(isoExtract->getOutput(i));
+                    imstkNew<RenderMaterial> material;
+                    material->setDisplayMode(RenderMaterial::DisplayMode::Surface);
+                    material->setLineWidth(4.0);
+                    const double r = (rand() % 500) / 500.0;
+                    const double g = (rand() % 500) / 500.0;
+                    const double b = (rand() % 500) / 500.0;
+                    material->setColor(Color(r, g, b));
+                    material->setEdgeColor(Color::Color::Orange);
+                    surfMeshModel->setRenderMaterial(material);
+                    lvlSetObj->addVisualModel(surfMeshModel);
+                    chunksGenerated.insert(i);
+                }
+            }
         });
         connect<Event>(sceneManager, EventType::PostUpdate, [&](Event*)
         {

Source/Controllers/imstkRigidObjectController.cpp

@@ -132,9 +132,32 @@ RigidObjectController::applyForces()
         // Apply force back to device
         if (m_rigidObject != nullptr && m_useSpring)
         {
-            // Render only the spring force (not the other forces the body has)
-            const Vec3d force = fS * m_forceScaling;
-            m_deviceClient->setForce(-force);
+            const Vec3d force = -fS * m_forceScaling;
+            if (m_forceSmoothening)
+            {
+                m_forces.push_back(force);
+                if (m_forces.size() > m_smoothingKernelSize)
+                {
+                    m_forces.pop_front();
+                }
+
+                Vec3d avgForce = Vec3d(0.0, 0.0, 0.0);
+                int   count    = 0;
+                for (auto i : m_forces)
+                {
+                    avgForce += i;
+                    count++;
+                }
+                avgForce /= count;
+
+                // Render only the spring force (not the other forces the body has)
+                m_deviceClient->setForce(avgForce);
+            }
+            else
+            {
+                // Render only the spring force (not the other forces the body has)
+                m_deviceClient->setForce(force);
+            }
         }
     }
     else

Source/Controllers/imstkRigidObjectController.h

@@ -91,6 +91,20 @@ public:
     bool getUseSpring() const { return m_useSpring; }
     void setUseSpring(const bool useSpring) { m_useSpring = useSpring; }
 
+    ///
+    /// \brief Set/Get whether to use force smoothening
+    /// Force smoothening averages the force used on the device over kernel size
+    ///
+    bool getUseForceSmoothening() const { return m_forceSmoothening; }
+    void setUseForceSmoothening(const bool useForceSmoothening) { m_forceSmoothening = useForceSmoothening; }
+
+    ///
+    /// \brief Set/Get the kernel size
+    /// \todo: Vary with dt as performance of program will effect the size/number of samples
+    ///
+    int getSmoothingKernelSize() const { return m_smoothingKernelSize; }
+    void setSmoothingKernelSize(const int kernelSize) { m_smoothingKernelSize = kernelSize; }
+
 public:
     ///
     /// \brief Update controlled scene object using latest tracking information
@@ -104,6 +118,7 @@ public:
 
 protected:
     std::shared_ptr<RigidObject2> m_rigidObject;
+    std::deque<Vec3d> m_forces;
 
     double m_linearKd  = 10000.0;                                ///> Damping coefficient, linear
     double m_angularKd = 300.0;                                  ///> Damping coefficient, rotational
@@ -115,6 +130,9 @@ protected:
 
     double m_forceScaling = 0.0000075;
     bool   m_useSpring    = true; ///> Controller has ability to toggle to from springs
+
+    bool m_forceSmoothening    = true;
+    int  m_smoothingKernelSize = 25;
 };
 }
 }

Source/DynamicalModels/ObjectModels/imstkLevelSetModel.cpp

@@ -43,7 +43,7 @@ LevelSetModel::LevelSetModel()
     };
 
     // By default the level set defines a function for evolving the distances, this can be removed in subclasses
-    m_evolveQuantitiesNodes.push_back(std::make_shared<TaskNode>(std::bind(&LevelSetModel::evolveDistanceField, this), "Evolve Distances"));
+    m_evolveQuantitiesNodes.push_back(std::make_shared<TaskNode>(std::bind(&LevelSetModel::evolve, this), "Evolve Distances"));
     m_taskGraph->addNode(m_evolveQuantitiesNodes.back());
     /// are all quantities evolved the same but with different force functions?
 }
@@ -109,14 +109,12 @@ LevelSetModel::configure(std::shared_ptr<LevelSetModelConfig> config)
 }
 
 void
-LevelSetModel::evolveDistanceField()
+LevelSetModel::evolve()
 {
     auto         sdf       = std::dynamic_pointer_cast<SignedDistanceField>(m_mesh);
     auto         imageData = std::dynamic_pointer_cast<ImageData>(sdf->getImage());
     double*      imgPtr    = static_cast<double*>(imageData->getVoidPointer());
     const Vec3i& dim       = imageData->getDimensions();
-    //const Vec3d& spacing   = imageData->getSpacing();
-    //const Vec3d& origin    = imageData->getOrigin();
     const double dt = m_config->m_dt;
     //const double k  = m_config->m_k;
 

Source/DynamicalModels/ObjectModels/imstkLevelSetModel.h

@@ -83,7 +83,7 @@ public:
     ///
     void configure(std::shared_ptr<LevelSetModelConfig> config);
 
-    virtual void evolveDistanceField();
+    virtual void evolve();
 
     void addImpulse(const Vec3i& coord, double f);
     void setImpulse(const Vec3i& coord, double f);

Source/MeshIO/imstkMeshIO.cpp

@@ -28,6 +28,7 @@
 #include "imstkVegaMeshIO.h"
 #include "imstkVTKMeshIO.h"
 
+#include <cctype>
 #include <sys/stat.h>
 #include <unordered_map>