Merge branch 'PBD-cloth-cutting' into 'master'

Pbd SurfaceMesh cutting using surface mesh and analytical geometries

See merge request !519

Examples/PBDCutting/CMakeLists.txt

+###########################################################################
+#
+# Copyright (c) Kitware, Inc.
+#
+#  Licensed under the Apache License, Version 2.0 (the "License");
+#  you may not use this file except in compliance with the License.
+#  You may obtain a copy of the License at
+#
+#      http://www.apache.org/licenses/LICENSE-2.0.txt
+#
+#  Unless required by applicable law or agreed to in writing, software
+#  distributed under the License is distributed on an "AS IS" BASIS,
+#  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+#  See the License for the specific language governing permissions and
+#  limitations under the License.
+#
+###########################################################################
+if(iMSTK_USE_OpenHaptics)
+
+  project(Example-PBDCutting)
+
+  #-----------------------------------------------------------------------------
+  # Create executable
+  #-----------------------------------------------------------------------------
+  imstk_add_executable(${PROJECT_NAME} PBDCuttingExample.cpp)
+
+  #-----------------------------------------------------------------------------
+  # Add the target to Examples folder
+  #-----------------------------------------------------------------------------
+  SET_TARGET_PROPERTIES (${PROJECT_NAME} PROPERTIES FOLDER Examples/Haptics)
+  
+  #-----------------------------------------------------------------------------
+  # Link libraries to executable
+  #-----------------------------------------------------------------------------
+  target_link_libraries(${PROJECT_NAME} SimulationManager apiUtilities Filtering)
+
+endif()

Examples/PBDCutting/PBDCuttingExample.cpp

+/*=========================================================================
+
+   Library: iMSTK
+
+   Copyright (c) Kitware, Inc. & Center for Modeling, Simulation,
+   & Imaging in Medicine, Rensselaer Polytechnic Institute.
+
+   Licensed under the Apache License, Version 2.0 (the "License");
+   you may not use this file except in compliance with the License.
+   You may obtain a copy of the License at
+
+      http://www.apache.org/licenses/LICENSE-2.0.txt
+
+   Unless required by applicable law or agreed to in writing, software
+   distributed under the License is distributed on an "AS IS" BASIS,
+   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+   See the License for the specific language governing permissions and
+   limitations under the License.
+
+=========================================================================*/
+
+#include "imstkCamera.h"
+#include "imstkHapticDeviceClient.h"
+#include "imstkHapticDeviceManager.h"
+#include "imstkKeyboardDeviceClient.h"
+#include "imstkKeyboardSceneControl.h"
+#include "imstkLight.h"
+#include "imstkLogger.h"
+#include "imstkMouseSceneControl.h"
+#include "imstkNew.h"
+#include "imstkPbdModel.h"
+#include "imstkPbdObject.h"
+#include "imstkPbdObjectCuttingPair.h"
+#include "imstkRenderMaterial.h"
+#include "imstkScene.h"
+#include "imstkSceneManager.h"
+#include "imstkSceneObjectController.h"
+#include "imstkSimulationManager.h"
+#include "imstkSurfaceMesh.h"
+#include "imstkVisualModel.h"
+#include "imstkVTKViewer.h"
+
+using namespace imstk;
+
+// Parameters to play with
+const double width  = 50.0;
+const double height = 50.0;
+const int    nRows  = 12;
+const int    nCols  = 12;
+
+///
+/// \brief Creates cloth geometry
+///
+static std::shared_ptr<SurfaceMesh>
+makeClothGeometry(const double width,
+                  const double height,
+                  const int    nRows,
+                  const int    nCols)
+{
+    imstkNew<SurfaceMesh> clothMesh("Cloth_SurfaceMesh");
+
+    imstkNew<VecDataArray<double, 3>> verticesPtr(nRows * nCols);
+    VecDataArray<double, 3>&          vertices = *verticesPtr.get();
+    const double                      dy       = width / static_cast<double>(nCols - 1);
+    const double                      dx       = height / static_cast<double>(nRows - 1);
+    for (int i = 0; i < nRows; i++)
+    {
+        for (int j = 0; j < nCols; j++)
+        {
+            vertices[i * nCols + j] = Vec3d(dx * static_cast<double>(i), 1.0, dy * static_cast<double>(j));
+        }
+    }
+
+    // Add connectivity data
+    imstkNew<VecDataArray<int, 3>> indicesPtr;
+    VecDataArray<int, 3>&          indices = *indicesPtr.get();
+    for (int i = 0; i < nRows - 1; i++)
+    {
+        for (int j = 0; j < nCols - 1; j++)
+        {
+            const int index1 = i * nCols + j;
+            const int index2 = index1 + nCols;
+            const int index3 = index1 + 1;
+            const int index4 = index2 + 1;
+
+            // Interleave [/][\] pattern
+            if (i % 2 ^ j % 2)
+            {
+                indices.push_back(Vec3i(index1, index2, index3));
+                indices.push_back(Vec3i(index4, index3, index2));
+            }
+            else
+            {
+                indices.push_back(Vec3i(index2, index4, index1));
+                indices.push_back(Vec3i(index4, index3, index1));
+            }
+        }
+    }
+
+    clothMesh->initialize(verticesPtr, indicesPtr);
+
+    return clothMesh;
+}
+
+///
+/// \brief Creates cloth object
+///
+static std::shared_ptr<PbdObject>
+makeClothObj(const std::string& name,
+             const double       width,
+             const double       height,
+             const int          nRows,
+             const int          nCols)
+{
+    imstkNew<PbdObject> clothObj(name);
+
+    // Setup the Geometry
+    std::shared_ptr<SurfaceMesh> clothMesh(makeClothGeometry(width, height, nRows, nCols));
+
+    // Setup the Parameters
+    imstkNew<PBDModelConfig> pbdParams;
+    pbdParams->enableConstraint(PbdConstraint::Type::Distance, 1.0e3);
+    pbdParams->enableConstraint(PbdConstraint::Type::Dihedral, 1.0e3);
+    pbdParams->m_fixedNodeIds     = { 0, static_cast<size_t>(nCols) - 1 };
+    pbdParams->m_uniformMassValue = width * height / ((double)nRows * (double)nCols);
+    pbdParams->m_gravity    = Vec3d(0.0, -9.8, 0.0);
+    pbdParams->m_defaultDt  = 0.005;
+    pbdParams->m_iterations = 5;
+
+    // Setup the Model
+    imstkNew<PbdModel> pbdModel;
+    pbdModel->setModelGeometry(clothMesh);
+    pbdModel->configure(pbdParams);
+
+    // Setup the VisualModel
+    imstkNew<RenderMaterial> material;
+    material->setBackFaceCulling(false);
+    material->setDisplayMode(RenderMaterial::DisplayMode::WireframeSurface);
+
+    imstkNew<VisualModel> visualModel(clothMesh);
+    visualModel->setRenderMaterial(material);
+
+    // Setup the Object
+    clothObj->addVisualModel(visualModel);
+    clothObj->setPhysicsGeometry(clothMesh);
+    clothObj->setCollidingGeometry(clothMesh);
+    clothObj->setDynamicalModel(pbdModel);
+
+    return clothObj;
+}
+
+///
+/// \brief This example demonstrates the concept of PBD cutting
+/// for haptic interaction. NOTE: Requires GeoMagic Touch device
+///
+int
+main()
+{
+    // Setup logger (write to file and stdout)
+    Logger::startLogger();
+
+    // Scene
+    imstkNew<Scene> scene("PBDCutting");
+
+    // Create a cutting plane object in the scene
+    std::shared_ptr<SurfaceMesh> cutGeom(makeClothGeometry(40, 40, 2, 2));
+    cutGeom->setTranslation(Vec3d(-10, -20, 0));
+    cutGeom->updatePostTransformData();
+    imstkNew<CollidingObject> cutObj("CuttingObject");
+    cutObj->setVisualGeometry(cutGeom);
+    cutObj->setCollidingGeometry(cutGeom);
+    cutObj->getVisualModel(0)->getRenderMaterial()->setDisplayMode(RenderMaterial::DisplayMode::WireframeSurface);
+    scene->addSceneObject(cutObj);
+
+    // Create a pbd cloth object in the scene
+    std::shared_ptr<PbdObject> clothObj = makeClothObj("Cloth", width, height, nRows, nCols);
+    scene->addSceneObject(clothObj);
+
+    // Add interaction pair for pbd cutting
+    imstkNew<PbdObjectCuttingPair> cuttingPair(clothObj, cutObj);
+
+    // Device Server
+    imstkNew<HapticDeviceManager>       server;
+    std::shared_ptr<HapticDeviceClient> client = server->makeDeviceClient();
+
+    // Create the virtual coupling object controller
+    imstkNew<SceneObjectController> controller(cutObj, client);
+    scene->addController(controller);
+
+    // Camera
+    scene->getActiveCamera()->setPosition(Vec3d(1.0, 1.0, 1.0) * 100.0);
+    scene->getActiveCamera()->setFocalPoint(Vec3d(0, -50, 0));
+
+    // Light
+    imstkNew<DirectionalLight> light("light");
+    light->setFocalPoint(Vec3d(5.0, -8.0, -5.0));
+    light->setIntensity(1.0);
+    scene->addLight(light);
+
+    //Run the simulation
+    {
+        // Setup a viewer to render in its own thread
+        imstkNew<VTKViewer> viewer("Viewer");
+        viewer->setActiveScene(scene);
+
+        // Setup a scene manager to advance the scene in its own thread
+        imstkNew<SceneManager> sceneManager("Scene Manager");
+        sceneManager->setActiveScene(scene);
+        sceneManager->setExecutionType(Module::ExecutionType::ADAPTIVE);
+        sceneManager->pause(); // Start simulation paused
+
+        imstkNew<SimulationManager> driver;
+        driver->addModule(server);
+        driver->addModule(viewer);
+        driver->addModule(sceneManager);
+        driver->setDesiredDt(0.001);
+
+        // Add mouse and keyboard controls to the viewer
+        {
+            imstkNew<MouseSceneControl> mouseControl(viewer->getMouseDevice());
+            mouseControl->setSceneManager(sceneManager);
+            viewer->addControl(mouseControl);
+
+            imstkNew<KeyboardSceneControl> keyControl(viewer->getKeyboardDevice());
+            keyControl->setSceneManager(sceneManager);
+            keyControl->setModuleDriver(driver);
+            viewer->addControl(keyControl);
+        }
+
+        // Queue keypress to be called after scene thread
+        queueConnect<KeyEvent>(viewer->getKeyboardDevice(), &KeyboardDeviceClient::keyPress, sceneManager, [&](KeyEvent* e)
+        {
+            // When i is pressed replace the PBD cloth with a cut one
+            if (e->m_key == 'i' && e->m_keyPressType == KEY_PRESS)
+            {
+                cuttingPair->apply();
+            }
+            });
+
+        driver->start();
+    }
+    return 0;
+}

Examples/PBDPicking/PBDPickingExample.cpp

@@ -158,7 +158,6 @@ main()
 
     // Scene
     imstkNew<Scene> scene("PBDPicking");
-    scene->getConfig()->writeTaskGraph = true;
 
     // Create the virtual coupling object controller
 
@@ -177,7 +176,7 @@ main()
     geomShaft->setOrientationAxis(Vec3d(0.0, 0.0, 1.0));
     geomShaft->setTranslation(Vec3d(0.0, 0.0, 10.0));
     imstkNew<CollidingObject> objShaft("ShaftObject");
-    objShaft->setVisualGeometry(geomShaft);
+    objShaft->setVisualGeometry(pivotSurfMesh);
     objShaft->setCollidingGeometry(geomShaft);
     scene->addSceneObject(objShaft);
 
@@ -187,7 +186,7 @@ main()
     geomUpperJaw->setRadius(2.0);
     geomUpperJaw->setOrientationAxis(Vec3d(0.0, 0.0, 1.0));
     imstkNew<CollidingObject> objUpperJaw("UpperJawObject");
-    objUpperJaw->setVisualGeometry(geomUpperJaw);
+    objUpperJaw->setVisualGeometry(upperSurfMesh);
     objUpperJaw->setCollidingGeometry(geomUpperJaw);
     scene->addSceneObject(objUpperJaw);
 
@@ -197,7 +196,7 @@ main()
     geomLowerJaw->setRadius(2.0);
     geomLowerJaw->setOrientationAxis(Vec3d(0.0, 0.0, 1.0));
     imstkNew<CollidingObject> objLowerJaw("LowerJawObject");
-    objLowerJaw->setVisualGeometry(geomLowerJaw);
+    objLowerJaw->setVisualGeometry(lowerSurfMesh);
     objLowerJaw->setCollidingGeometry(geomLowerJaw);
     scene->addSceneObject(objLowerJaw);
 

Examples/SDFHaptics/SDFHapticsExample.cpp

@@ -99,6 +99,7 @@ main()
         // Setup a scene manager to advance the scene in its own thread
         imstkNew<SceneManager> sceneManager("Scene Manager");
         sceneManager->setActiveScene(scene);
+        sceneManager->setExecutionType(Module::ExecutionType::ADAPTIVE);
 
         imstkNew<SimulationManager> driver;
         driver->addModule(viewer);

Source/Controllers/imstkLaparoscopicToolController.cpp

@@ -91,13 +91,19 @@ LaparoscopicToolController::update(const double dt)
 
     // TRS decompose and set upper/lower jaw geometries
     {
-        m_upperJaw->getVisualGeometry()->setTransform(m_controllerWorldTransform * m_upperJawLocalTransform * m_upperJawVisualTransform);
-        m_upperJaw->getCollidingGeometry()->setTransform(m_controllerWorldTransform * m_upperJawLocalTransform * m_upperJawCollidingTransform);
+        const Mat4d upperWorldTransform = m_controllerWorldTransform * m_upperJawLocalTransform;
+        m_upperJaw->getVisualGeometry()->setTransform(upperWorldTransform * m_upperJawVisualTransform);
+        m_upperJaw->getCollidingGeometry()->setTransform(upperWorldTransform * m_upperJawCollidingTransform);
     }
     {
-        m_lowerJaw->getVisualGeometry()->setTransform(m_controllerWorldTransform * m_lowerJawLocalTransform * m_upperJawVisualTransform);
-        m_lowerJaw->getCollidingGeometry()->setTransform(m_controllerWorldTransform * m_lowerJawLocalTransform * m_lowerJawCollidingTransform);
+        const Mat4d lowerWorldTransform = m_controllerWorldTransform * m_lowerJawLocalTransform;
+        m_lowerJaw->getVisualGeometry()->setTransform(lowerWorldTransform * m_upperJawVisualTransform);
+        m_lowerJaw->getCollidingGeometry()->setTransform(lowerWorldTransform * m_lowerJawCollidingTransform);
     }
+
+    m_shaft->getVisualGeometry()->postModified();
+    m_lowerJaw->getVisualGeometry()->postModified();
+    m_upperJaw->getVisualGeometry()->postModified();
 }
 
 void

Source/DynamicalModels/ObjectModels/imstkPbdModel.cpp

@@ -36,6 +36,9 @@
 #include "imstkTaskGraph.h"
 #include "imstkTetrahedralMesh.h"
 
+#include <map>
+#include <set>
+
 namespace imstk
 {
 PbdModel::PbdModel() : DynamicalModel(DynamicalModelType::PositionBasedDynamics),
@@ -520,18 +523,24 @@ PbdModel::initializeDihedralConstraints(const double stiffness)
                 rs.resize(static_cast<size_t>(it - rs.begin()));
                 if (rs.size() > 1)
                 {
-                    size_t      idx = (rs[0] == k) ? 1 : 0;
-                    const auto& tri = elements[rs[idx]];
+                    size_t      idx  = (rs[0] == k) ? 1 : 0;
+                    const auto& tri0 = elements[k];
+                    const auto& tri1 = elements[rs[idx]];
+                    size_t      idx0 = 0;
+                    size_t      idx1 = 0;
                     for (size_t i = 0; i < 3; ++i)
                     {
-                        if (tri[i] != tri[0] && tri[i] != tri[1])
+                        if (tri0[i] != i1 && tri0[i] != i2)
                         {
-                            idx = i;
-                            break;
+                            idx0 = tri0[i];
+                        }
+                        if (tri1[i] != i1 && tri1[i] != i2)
+                        {
+                            idx1 = tri1[i];
                         }
                     }
                     auto c = std::make_shared<PbdDihedralConstraint>();
-                    c->initConstraint(*m_initialState->getPositions(), tri[2], tri[idx], tri[0], tri[1], stiffness);
+                    c->initConstraint(*m_initialState->getPositions(), idx0, idx1, i1, i2, stiffness);
                     m_constraints->push_back(std::move(c));
                 }
             }
@@ -570,6 +579,199 @@ PbdModel::initializeConstantDensityConstraint(const double stiffness)
     return true;
 }
 
+void
+PbdModel::removeConstraints(std::shared_ptr<std::unordered_set<size_t>> vertices)
+{
+    // constraint removal predicate
+    auto removeConstraint =
+        [&](std::shared_ptr<PbdConstraint> constraint)
+        {
+            for (auto i : constraint->getVertexIds())
+            {
+                if (vertices->find(i) != vertices->end())
+                {
+                    return true;
+                }
+            }
+            return false;
+        };
+
+    // remove constraints from constraint-partition maps and constraint vectors.
+    m_constraints->erase(std::remove_if(m_constraints->begin(), m_constraints->end(), removeConstraint),
+                         m_constraints->end());
+    for (auto& pc : *m_partitionedConstraints)
+    {
+        pc.erase(std::remove_if(pc.begin(), pc.end(), removeConstraint), pc.end());
+    }
+}
+
+void
+PbdModel::addConstraints(std::shared_ptr<std::unordered_set<size_t>> vertices)
+{
+    // check if constraint type matches the mesh type
+    CHECK(m_mesh->getTypeName() == "SurfaceMesh")
+        << "Add element constraints does not support current mesh type.";
+
+    const auto&                      triMesh  = std::static_pointer_cast<SurfaceMesh>(m_mesh);
+    const auto                       nV       = triMesh->getNumVertices();
+    const auto                       elements = triMesh->getTriangleIndices();
+    std::vector<std::vector<size_t>> onering(nV);
+
+    // build onering
+    for (auto& vertOnering : onering)
+    {
+        vertOnering.reserve(10);
+    }
+    for (int k = 0; k < elements->size(); ++k)
+    {
+        auto& tri = (*elements)[k];
+        onering[tri[0]].push_back(k);
+        onering[tri[1]].push_back(k);
+        onering[tri[2]].push_back(k);
+    }
+    for (auto& vertOnering : onering)
+    {
+        std::sort(vertOnering.begin(), vertOnering.end());
+    }
+
+    // functions for adding constraints
+    auto addDistanceConstraint =
+        [&](size_t i1, size_t i2, double stiffness)
+        {
+            auto c = std::make_shared<PbdDistanceConstraint>();
+            c->initConstraint(*m_initialState->getPositions(), i1, i2, stiffness);
+            m_constraints->push_back(std::move(c));
+        };
+    auto addAreaConstraint =
+        [&](size_t k, double stiffness)
+        {
+            auto& tri = (*elements)[k];
+            auto  c   = std::make_shared<PbdAreaConstraint>();
+            c->initConstraint(*m_initialState->getPositions(), tri[0], tri[1], tri[2], stiffness);
+            m_constraints->push_back(std::move(c));
+        };
+    auto addDihedralConstraint =
+        [&](size_t t0, size_t t1, size_t i1, size_t i2, double stiffness)
+        {
+            const auto& tri0 = (*elements)[t0];
+            const auto& tri1 = (*elements)[t1];
+            size_t      idx0 = 0;
+            size_t      idx1 = 0;
+            for (size_t i = 0; i < 3; ++i)
+            {
+                if (tri0[i] != i1 && tri0[i] != i2)
+                {
+                    idx0 = tri0[i];
+                }
+                if (tri1[i] != i1 && tri1[i] != i2)
+                {
+                    idx1 = tri1[i];
+                }
+            }
+            auto c = std::make_shared<PbdDihedralConstraint>();
+            c->initConstraint(*m_initialState->getPositions(), idx0, idx1, i1, i2, stiffness);
+            m_constraints->push_back(std::move(c));
+        };
+
+    // count constraints to be added for pre-allocation
+    std::set<std::pair<size_t, size_t>>                            distanceSet;
+    std::unordered_set<size_t>                                     areaSet;
+    std::map<std::pair<size_t, size_t>, std::pair<size_t, size_t>> dihedralSet;
+    for (auto& constraint : m_parameters->m_regularConstraints)
+    {
+        switch (constraint.first)
+        {
+        case PbdConstraint::Type::Distance:
+            for (const auto& vertIdx : *vertices)
+            {
+                for (const auto& triIdx : onering[vertIdx])
+                {
+                    const auto& tri = (*elements)[triIdx];
+                    size_t      i1  = 0;
+                    size_t      i2  = 0;
+                    for (size_t i = 0; i < 3; i++)
+                    {
+                        if (tri[i] == vertIdx)
+                        {
+                            i1 = tri[(i + 1) % 3];
+                            i2 = tri[(i + 2) % 3];
+                            break;
+                        }
+                    }
+                    auto pair1 = std::make_pair(std::min(vertIdx, i1), std::max(vertIdx, i1));
+                    auto pair2 = std::make_pair(std::min(vertIdx, i2), std::max(vertIdx, i2));
+                    distanceSet.insert(pair1);
+                    distanceSet.insert(pair2);
+                }
+            }
+            break;
+        case PbdConstraint::Type::Area:
+            for (const auto& vertIdx : *vertices)
+            {
+                for (const auto& triIdx : onering[vertIdx])
+                {
+                    areaSet.insert(triIdx);
+                }
+            }
+            break;
+        case PbdConstraint::Type::Dihedral:
+            for (const auto& vertIdx : *vertices)
+            {
+                for (const auto& triIdx : onering[vertIdx])
+                {
+                    const auto& tri = (*elements)[triIdx];
+                    for (size_t i = 0; i < 3; i++)
+                    {
+                        size_t j  = (i + 1) % 3;
+                        size_t i0 = tri[i];
+                        size_t i1 = tri[j];
+                        if (i0 > i1)
+                        {
+                            std::swap(i0, i1);
+                        }
+                        auto&               r0 = onering[i0];
+                        auto&               r1 = onering[i1];
+                        std::vector<size_t> rs(2);
+                        auto                it = std::set_intersection(r0.begin(), r0.end(), r1.begin(), r1.end(), rs.begin());
+                        rs.resize(static_cast<size_t>(it - rs.begin()));
+                        if (rs.size() > 1)
+                        {
+                            dihedralSet[std::make_pair(i0, i1)] = std::make_pair(rs[0], rs[1]);
+                        }
+                    }
+                }
+            }
+            break;
+        default:
+            break;
+        }
+    }
+
+    // add constraints
+    m_constraints->reserve(m_constraints->size() + distanceSet.size() + areaSet.size() + dihedralSet.size());
+    for (auto& constraint : m_parameters->m_regularConstraints)
+    {
+        switch (constraint.first)
+        {
+        case PbdConstraint::Type::Distance:
+            for (auto& c : distanceSet)
+            {
+                addDistanceConstraint(c.first, c.second, constraint.second);
+            }
+        case PbdConstraint::Type::Area:
+            for (auto& c : areaSet)
+            {
+                addAreaConstraint(c, constraint.second);
+            }
+        case PbdConstraint::Type::Dihedral:
+            for (auto& c : dihedralSet)
+            {
+                addDihedralConstraint(c.second.first, c.second.second, c.first.first, c.first.second, constraint.second);
+            }
+        }
+    }
+}