ENH: Adds penalty based mesh to rigid CH

Adds penalty based mesh to rigid contact handling. Adds contact force vector to deformable body model

Base/Collision/CollisionData/imstkCollisionData.h

@@ -36,6 +36,12 @@ struct PositionDirectionCollisionData
     double penetrationDepth;
 };
 
+struct MeshToAnalyticalPointDepthCollisionData
+{
+    int nodeId;
+    Vec3d penetrationVector;
+};
+
 struct VertexTriangleCollisionData
 {
     size_t vertexIdA;
@@ -94,6 +100,8 @@ public:
     std::vector<VertexTriangleCollisionData> VTColData;    //!< Vertex Triangle collision data
     std::vector<TriangleVertexCollisionData> TVColData;    //!< Triangle Vertex collision data
     std::vector<EdgeEdgeCollisionData> EEColData;          //!< Edge Edge collision data
+
+    std::vector<MeshToAnalyticalPointDepthCollisionData> PDMeshAnaColData;
 };
 }
 

Base/Collision/CollisionHandling/imstkPenaltyMeshToRigidCH.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 "imstkPenaltyMeshToRigidCH.h"
+
+#include "imstkCollidingObject.h"
+#include "imstkVirtualCouplingObject.h"
+#include "imstkCollisionData.h"
+#include "imstkDeformableObject.h"
+
+#include <g3log/g3log.hpp>
+
+namespace imstk {
+
+void
+PenaltyMeshToRigidCH::computeContactForces()
+{
+    auto deformableObj = std::dynamic_pointer_cast<imstk::DeformableObject>(m_object);
+
+    if (deformableObj == nullptr)
+    {
+        LOG(WARNING) << "PenaltyMeshToRigidCH::computeContactForces error: "
+                     << m_object->getName() << " is not a deformable object. ";
+        return;
+    }
+
+    // Get current force vector
+    auto force = deformableObj->getContactForce();
+    auto velVector = deformableObj->getVelocities();
+
+    Vec3d nodalForce;
+    Vec3d velocityProjection;
+    int nodeDofID;
+    double depth;
+    // If collision data, append forces
+    if (!m_colData.PDMeshAnaColData.empty())
+    {
+        for (const auto& cd : m_colData.PDMeshAnaColData)
+        {
+            nodeDofID = 3 * cd.nodeId;
+            velocityProjection = Vec3d(velVector(nodeDofID), velVector(nodeDofID+1), velVector(nodeDofID+2));
+            velocityProjection = cd.penetrationVector.dot(velocityProjection) * cd.penetrationVector;
+            depth = cd.penetrationVector.norm();
+
+            nodalForce = -m_stiffness * cd.penetrationVector - m_damping * velocityProjection;
+
+            force(nodeDofID) = nodalForce.x();
+            force(nodeDofID + 1) = nodalForce.y();
+            force(nodeDofID + 2) = nodalForce.z();
+        }
+    }
+}
+
+}

Base/Collision/CollisionHandling/imstkPenaltyMeshToRigidCH.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 imstkPenaltyMeshToRigidCH_h
+#define imstkPenaltyMeshToRigidCH_h
+
+#include "imstkCollisionHandling.h"
+
+#include <memory>
+
+namespace imstk {
+
+class CollidingObject;
+class CollisionData;
+
+class PenaltyMeshToRigidCH : public CollisionHandling
+{
+public:
+
+    ///
+    /// \brief Constructor
+    ///
+    PenaltyMeshToRigidCH(const Side& side, const CollisionData& colData, std::shared_ptr<CollidingObject> obj) :
+        CollisionHandling(CollisionHandling::Type::Penalty, side, colData), m_object(obj)
+    {
+        m_stiffness = 1000;
+        m_damping = 0.5;
+    }
+
+    ///
+    /// \brief Destructor
+    ///
+    ~PenaltyMeshToRigidCH() = default;
+
+    ///
+    /// \brief Compute forces based on collision data
+    ///
+    void computeContactForces() override;
+
+private:
+    std::shared_ptr<CollidingObject> m_object; ///> The mesh object under collision
+
+    double m_stiffness; ///> Stiffness of contact
+    double m_damping; ///> Damping of the contact
+};
+}
+
+#endif // ifndef imstkPenaltyMeshToRigidCH_h

Base/Collision/CollisionHandling/imstkPenaltyRigidCH.h

@@ -53,7 +53,7 @@ public:
     ~PenaltyRigidCH() = default;
 
     ///
-    /// \brief Compute forces based on collision data (pure virtual)
+    /// \brief Compute forces based on collision data
     ///
     void computeContactForces() override;
 

Base/SceneElements/Objects/imstkDeformableBodyModel.cpp

@@ -109,6 +109,7 @@ DeformableBodyModel::initialize()
 
     m_Feff.resize(m_numDOF);
     m_Finternal.resize(m_numDOF);
+    m_Fcontact.resize(m_numDOF);
 }
 
 void

Base/SceneElements/Objects/imstkDeformableBodyModel.h

@@ -197,6 +197,14 @@ public:
     ///
     static void initializeEigenMatrixFromVegaMatrix(const vega::SparseMatrix& vegaMatrix, SparseMatrixd& eigenMatrix);
 
+    ///
+    /// \brief Get the contact force vector
+    ///
+    Vectord& getContactForce()
+    {
+        return m_Fcontact;
+    }
+
 protected:
     std::shared_ptr<InternalForceModel> m_internalForceModel;       ///> Mathematical model for intenal forces
     std::shared_ptr<TimeIntegrator>     m_timeIntegrator;           ///> Time integrator
@@ -214,8 +222,9 @@ protected:
 
     std::shared_ptr<NonLinearSystem> m_nonLinearSystem; ///> Nonlinear system resulting from TI and force model
 
-    Vectord m_Finternal;       ///> Vector of gravity forces
-    Vectord m_Feff;       ///> Vector of gravity forces
+    Vectord m_Finternal;    ///> Vector of gravity forces
+    Vectord m_Feff;         ///> Vector of effective forces
+    Vectord m_Fcontact;     ///> Vector of contact forces
 
     // External field forces
     Vectord m_gravityForce;   ///> Vector of gravity forces