ENH: Implement Virtual Coupling Collision Handling

Base/Collision/CollisionHandling/imstkCollisionHandling.cpp

@@ -22,6 +22,7 @@
 #include "imstkCollisionHandling.h"
 
 #include "imstkPenaltyCH.h"
+#include "imstkVirtualCouplingCH.h"
 
 #include <g3log/g3log.hpp>
 
@@ -35,18 +36,22 @@ CollisionHandling::make_collision_handling(const Type& type,
                                            std::shared_ptr<CollidingObject> objA,
                                            std::shared_ptr<CollidingObject> objB)
 {
+    if (type != Type::None &&
+        objA->getType() == SceneObject::Type::Visual)
+    {
+        LOG(WARNING) << "CollisionHandling::make_collision_handling error: "
+                     << "penalty collision handling only implemented for colliding objects.";
+        return nullptr;
+    }
+
     switch (type)
     {
     case Type::None:
         return nullptr;
     case Type::Penalty:
-        if (objA->getType() == SceneObject::Type::Visual)
-        {
-            LOG(WARNING) << "CollisionHandling::make_collision_handling error: "
-                         << "penalty collision handling only implemented for colliding objects.";
-            return nullptr;
-        }
         return std::make_shared<PenaltyCH>(side, colData, objA);
+    case Type::VirtualCoupling:
+        return std::make_shared<VirtualCouplingCH>(side, colData, objA );
     default:
         LOG(WARNING) << "CollisionHandling::make_collision_handling error: type not implemented.";
         return nullptr;

Base/Collision/CollisionHandling/imstkCollisionHandling.h

@@ -48,7 +48,8 @@ public:
     enum class Type
     {
         None,
-        Penalty
+        Penalty,
+        VirtualCoupling
     };
 
     ///

Base/Collision/CollisionHandling/imstkVirtualCouplingCH.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 "imstkVirtualCouplingCH.h"
+
+#include "imstkCollidingObject.h"
+#include "imstkCollisionData.h"
+
+#include <g3log/g3log.hpp>
+
+namespace imstk
+{
+
+void
+VirtualCouplingCH::computeContactForces()
+{
+    // Check if any collisions
+    const auto collidingObjPos = m_object->getCollidingGeometry()->getPosition();
+    if (m_colData.PDColData.empty())
+    {
+        // Set the visual object position same as the colliding object position
+        m_object->getVisualGeometry()->setPosition(collidingObjPos);
+        return;
+    }
+
+    // Aggregate collision data
+    Vec3d t = Vec3d::Zero();
+    for (const auto& cd : m_colData.PDColData)
+    {
+        t += cd.dirAtoB * cd.penetrationDepth;
+    }
+
+    // Update the visual object position
+    const auto visualObjPos = collidingObjPos + t;
+    m_object->getVisualGeometry()->setPosition(visualObjPos);
+
+    // Spring force
+    Vec3d  force = m_stiffness * (visualObjPos - collidingObjPos);
+
+    // Damping force
+    const double dt = 0.1; // Time step size to calculate the object velocity
+    force += m_initialStep ? Vec3d(0.0, 0.0, 0.0) : m_damping * (collidingObjPos - m_prevPos) / dt;
+
+    // Update object contact force
+    m_object->appendForce(force);
+
+    // Housekeeping
+    m_initialStep = false;
+    m_prevPos = collidingObjPos;
+}
+
+}// iMSTK
\ No newline at end of file

Base/Collision/CollisionHandling/imstkVirtualCouplingCH.h

+/*=========================================================================
+
+   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.
+
+=========================================================================*/
+
+#ifndef imstkVirtualCouplingCH_h
+#define imstkVirtualCouplingCH_h
+
+// imstk
+#include "imstkCollisionHandling.h"
+
+namespace imstk
+{
+
+class CollidingObject;
+class CollisionData;
+
+///
+/// \class VirtualCouplingCH
+///
+/// \brief Implements virtual coupling collision handling
+///
+class VirtualCouplingCH : public CollisionHandling
+{
+public:
+
+    ///
+    /// \brief Constructor
+    ///
+    VirtualCouplingCH(const Side& side,
+                      const CollisionData& colData,
+                      std::shared_ptr<CollidingObject> obj) :
+                      CollisionHandling(Type::VirtualCoupling, side, colData),
+                      m_object(obj) {}
+
+    VirtualCouplingCH() = delete;
+
+    ///
+    /// \brief Destructor
+    ///
+    ~VirtualCouplingCH() = default;
+
+    ///
+    /// \brief Compute forces based on collision data
+    ///
+    void computeContactForces() override;
+
+    ///
+    /// \brief Get/Set Stiffness coefficient
+    ///
+    inline double getStiffness() const { return m_stiffness; }
+    inline void setStiffness(const double stiffness) { m_stiffness = stiffness; }
+
+    ///
+    /// \brief Get/Set Damping coefficient
+    ///
+    inline double getDamping() const { return m_damping; }
+    inline void setDamping(const double damping) { m_damping = damping; }
+
+private:
+
+    std::shared_ptr<CollidingObject> m_object;  ///> colliding object
+    bool m_initialStep = true;                  ///> Number of times steps
+    Vec3d m_prevPos;                            ///> Previous position of the colliding object
+
+    double m_stiffness = 5e-01;                 ///> Stiffness coefficient use to compute spring force
+    double m_damping = 0.005;                   ///> Damping coefficient used to compute damping force
+
+};
+
+}
+
+#endif // ifndef imstkVirtualCouplingCH_h
\ No newline at end of file

Examples/Sandbox/main.cpp

@@ -74,6 +74,7 @@
 #include "imstkInteractionPair.h"
 #include "imstkMeshToPlaneCD.h"
 #include "imstkMeshToSphereCD.h"
+#include "imstkVirtualCouplingCH.h"
 
 // logger
 #include "g3log/g3log.hpp"
@@ -127,8 +128,8 @@ void testLapToolController();
 void testScreenShotUtility();
 void testDeformableBodyCollision();
 void liverToolInteraction();
-void testBoneDrilling();
 void testCapsule();
+void testVirtualCoupling();
 
 int main()
 {
@@ -169,9 +170,9 @@ int main()
     Test physics
     ------------------*/
     //testPbdVolume();
-    testPbdCloth();
+    //testPbdCloth();
     //testPbdCollision();
-    testDeformableBody();
+    //testDeformableBody();
     //testDeformableBodyCollision();
     //liverToolInteraction();
 
@@ -199,7 +200,7 @@ int main()
     ------------------*/
     //testScenesManagement();
     //testVectorPlotters();
-    //testBoneDrilling();
+    testVirtualCoupling();
 
 
     return 0;
@@ -2518,55 +2519,70 @@ void liverToolInteraction()
     sdk->startSimulation(true);
 }
 
-void testBoneDrilling()
+void testVirtualCoupling()
 {
     // SDK and Scene
     auto sdk = std::make_shared<imstk::SimulationManager>();
-    auto scene = sdk->createNewScene("BoneDrilling");
+    auto scene = sdk->createNewScene("VirtualCoupling");
 
-    //Read the cube hex mesh files (bunch of points in the space)
-    //std::string iFile = iMSTK_DATA_ROOT"/cube-hexMesh-60002/hexCube.msh";
-    std::string iFile = iMSTK_DATA_ROOT"/cube-hexMesh/hexCube.msh";
-    auto mesh = MeshIO::read(iFile);
-    if (!mesh)
-    {
-        LOG(WARNING) << "testBoneDrilling::Failed to read the MSH file: " << iFile;
-    }
-    auto volHexMesh = std::dynamic_pointer_cast<imstk::HexahedralMesh>(mesh);
-
-    // Create and add cube in the scene
-    auto visualObject = std::make_shared<imstk::VisualObject>("cube");
-    visualObject->setVisualGeometry(volHexMesh);
-    scene->addSceneObject(visualObject);
-
-    // Create and add burr-tool object in the scene
-    auto createAndAddVisualSceneObject =
-        [](std::shared_ptr<imstk::Scene> scene,
-        const std::string& fileName,
-        const std::string& objectName) ->
-        std::shared_ptr<imstk::SceneObject>
-    {
-        auto mesh = imstk::MeshIO::read(fileName);
-        auto SurfaceMesh = std::dynamic_pointer_cast<imstk::SurfaceMesh>(mesh);
-        SurfaceMesh->translate(Vec3d(0.2,0.2,0.2));
-        SurfaceMesh->rotate(Vec3d(1.0,0.0,0.0), 90);
-        // Create object and add to scene
-        auto meshObject = std::make_shared<imstk::VisualObject>("meshObject");
-        meshObject->setVisualGeometry(SurfaceMesh);
-        meshObject->setName(objectName);
-        scene->addSceneObject(meshObject);
-        return meshObject;
-    };
-   auto burrTool = createAndAddVisualSceneObject(scene,iMSTK_DATA_ROOT"/sceneObjects/burr-tool2.obj","burrTool");
+    // Create a plane in the scene
+    auto planeGeom = std::make_shared<imstk::Plane>(WORLD_ORIGIN, UP_VECTOR, 1.0);
+    planeGeom->scale(400);
+    planeGeom->translate(Vec3d(0., -50, 0.));
+    auto planeObj = std::make_shared<imstk::CollidingObject>("Plane");
+    planeObj->setVisualGeometry(planeGeom);
+    planeObj->setCollidingGeometry(planeGeom);
+    scene->addSceneObject(planeObj);
 
-    // Get spherical burr tool in the scene.
+    // Create the virtual coupling object controller
+#ifdef iMSTK_USE_OPENHAPTICS
 
-    // Set Camera configuration
+    // Device clients
+    auto client = std::make_shared<imstk::HDAPIDeviceClient>("Default Device");
+
+    // Device Server
+    auto server = std::make_shared<imstk::HDAPIDeviceServer>();
+    server->addDeviceClient(client);
+    sdk->addModule(server);
+
+    // Device tracker
+    auto deviceTracker = std::make_shared<imstk::DeviceTracker>(client);
+
+    // Create a virtual coupling object
+    auto visualGeom = std::make_shared<imstk::Sphere>(WORLD_ORIGIN, 20);
+    auto collidingGeom = std::make_shared<imstk::Sphere>(WORLD_ORIGIN, 20);
+    auto obj = std::make_shared<CollidingObject>("VirtualCouplingObject");
+    obj->setCollidingGeometry(collidingGeom);
+    obj->setVisualGeometry(visualGeom);
+
+    // Add virtual coupling object (with visual, colliding, and physics geometry) in the scene.
+    scene->addSceneObject(obj);
+
+    // Create and add virtual coupling object controller in the scene
+    auto objController = std::make_shared<imstk::SceneObjectController>(obj, deviceTracker);
+    scene->addObjectController(objController);
+
+    // Create a collision graph
+    auto graph = scene->getCollisionGraph();
+    auto pair = graph->addInteractionPair(planeObj, obj,
+                                          CollisionDetection::Type::UnidirectionalPlaneToSphere,
+                                          CollisionHandling::Type::None,
+                                          CollisionHandling::Type::VirtualCoupling);
+
+    // Customize collision handling algorithm
+    auto colHandlingAlgo = std::dynamic_pointer_cast<VirtualCouplingCH>(pair->getCollisionHandlingB());
+    colHandlingAlgo->setStiffness(5e-01);
+    colHandlingAlgo->setDamping(0.005);
+
+#endif
+
+    // Move Camera
     auto cam = scene->getCamera();
-    cam->setPosition(imstk::Vec3d(0.5, 0.5, 0.5));
+    cam->setPosition(imstk::Vec3d(200, 200, 200));
     cam->setFocalPoint(imstk::Vec3d(0, 0, 0));
 
     //Run
     sdk->setCurrentScene(scene);
     sdk->startSimulation(false);
-}
\ No newline at end of file
+
+}