Merge branch 'refactorPhysicsModule' into 'master'

Refactor physics module

**Summary**:  
*  Modifies the Dynamical objects realted classes to generalize for any problem state. This allows for rigid bodies, deformable bodies using FEM and pbd under one dynamicalObject base class.
* Adds common interface for pbd solver and FE solvers to allow simpler code in SceneManager
* Refactors all the PBD classes for bugs, style, warnings and efficiency.
* Misc. modifications including rearranging folders, example codes, renaming classes and clearing some warnings.

See merge request !103

Base/Collision/CollisionDetection/imstkCollisionDetectionUtils.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 imstkCollisionDetectionUtils_h
+#define imstkCollisionDetectionUtils_h
+
+#include <algorithm>
+
+namespace imstk
+{
+
+///
+/// \brief
+///
+inline bool
+isIntersect(const double& a, const double& b, const double& c, const double& d)
+{
+    return ((a <= d && a >= c) || (c <= b && c >= a)) ? true : false;
+}
+
+///
+/// \brief Check if two AABBs are intersecting
+///
+inline bool
+testAABBToAABB(const double& min1_x, const double& max1_x,
+               const double& min1_y, const double& max1_y,
+               const double& min1_z, const double& max1_z,
+               const double& min2_x, const double& max2_x,
+               const double& min2_y, const double& max2_y,
+               const double& min2_z, const double& max2_z)
+{
+    return (isIntersect(min1_x, max1_x, min2_x, max2_x) &&
+            isIntersect(min1_y, max1_y, min2_y, max2_y) &&
+            isIntersect(min1_z, max1_z, min2_z, max2_z));
+}
+
+///
+/// \brief Check if two lines are intersecting with AABB intersection test
+///
+inline bool
+testLineToLineAABB(const double& x1, const double& y1, const double& z1,
+                   const double& x2, const double& y2, const double& z2,
+                   const double& x3, const double& y3, const double& z3,
+                   const double& x4, const double& y4, const double& z4,
+                   const double& prox1, const double& prox2)
+{
+    double min1_x, max1_x, min1_y, max1_y, min1_z, max1_z;
+
+    if (x1 < x2)
+    {
+        min1_x = x1;
+        max1_x = x2;
+    }
+    else
+    {
+        min1_x = x2;
+        max1_x = x1;
+    }
+
+    if (y1 < y2)
+    {
+        min1_y = y1;
+        max1_y = y2;
+    }
+    else
+    {
+        min1_y = y2;
+        max1_y = y1;
+    }
+
+    if (z1 < z2)
+    {
+        min1_z = z1;
+        max1_z = z2;
+    }
+    else
+    {
+        min1_z = z2;
+        max1_z = z1;
+    }
+
+    double min2_x, max2_x, min2_y, max2_y, min2_z, max2_z;
+
+    if (x3 < x4)
+    {
+        min2_x = x3;
+        max2_x = x4;
+    }
+    else
+    {
+        min2_x = x4;
+        max2_x = x3;
+    }
+
+    if (y3 < y4)
+    {
+        min2_y = y3;
+        max2_y = y4;
+    }
+    else
+    {
+        min2_y = y4;
+        max2_y = y3;
+    }
+
+    if (z3 < z4)
+    {
+        min2_z = z3;
+        max2_z = z4;
+    }
+    else
+    {
+        min2_z = z4;
+        max2_z = z3;
+    }
+
+    return testAABBToAABB(min1_x - prox1, max1_x + prox1, min1_y - prox1, max1_y + prox1,
+                          min1_z - prox1, max1_z + prox1, min2_x - prox2, max2_x + prox2,
+                          min2_y - prox2, max2_y + prox2, min2_z - prox2, max2_z + prox2);
+}
+
+///
+/// \brief Check if triangle and point are intersecting with AABB test
+///
+inline bool
+testPointToTriAABB(const double& x1, const double& y1, const double& z1,
+                   const double& x2, const double& y2, const double& z2,
+                   const double& x3, const double& y3, const double& z3,
+                   const double& x4, const double& y4, const double& z4,
+                   const double& prox1, const double& prox2)
+{
+    auto min_x = std::min(x2, std::min(x3, x4));
+    auto max_x = std::max(x2, std::max(x3, x4));
+    auto min_y = std::min(y2, std::min(y3, y4));
+    auto max_y = std::max(y2, std::max(y3, y4));
+    auto min_z = std::min(z2, std::min(z3, z4));
+    auto max_z = std::max(z2, std::max(z3, z4));
+
+    return testAABBToAABB(x1 - prox1, x1 + prox1, y1 - prox1, y1 + prox1,
+                          z1 - prox1, z1 + prox1, min_x - prox2, max_x + prox2,
+                          min_y - prox2, max_y + prox2, min_z - prox2, max_z + prox2);
+}
+
+}
+
+#endif // ifndef imstkCollisionDetectionUtils_h

Base/Collision/CollisionDetection/imstkMeshToMeshCD.cpp

@@ -35,8 +35,8 @@ MeshToMeshCD::MeshToMeshCD(std::shared_ptr<SurfaceMesh> meshA,
     m_meshA(meshA),
     m_meshB(meshB)
 {
-    m_modelA = std::make_shared<DeformModel>(meshA->getVerticesPositions(), meshA->getTrianglesVertices());
-    m_modelB = std::make_shared<DeformModel>(meshB->getVerticesPositions(), meshB->getTrianglesVertices());
+    m_modelA = std::make_shared<DeformModel>(meshA->getVertexPositions(), meshA->getTrianglesVertices());
+    m_modelB = std::make_shared<DeformModel>(meshB->getVertexPositions(), meshB->getTrianglesVertices());
 
     // Setup Callbacks
     m_modelA->SetEECallBack(MeshToMeshCD::EECallback, this);
@@ -52,8 +52,8 @@ void
 MeshToMeshCD::computeCollisionData()
 {
     // Update model
-    m_modelA->UpdateVert(m_meshA->getVerticesPositions());
-    m_modelB->UpdateVert(m_meshB->getVerticesPositions());
+    m_modelA->UpdateVert(m_meshA->getVertexPositions());
+    m_modelB->UpdateVert(m_meshB->getVertexPositions());
     m_modelB->UpdateBoxes();
     m_modelB->UpdateBoxes();
 

Base/Collision/CollisionHandling/imstkPenaltyMeshToRigidCH.cpp

@@ -27,12 +27,13 @@
 
 #include <g3log/g3log.hpp>
 
-namespace imstk {
+namespace imstk 
+{
 
 void
 PenaltyMeshToRigidCH::computeContactForces()
 {
-    auto deformableObj = std::dynamic_pointer_cast<imstk::DeformableObject>(m_object);
+    auto deformableObj = std::dynamic_pointer_cast<DeformableObject>(m_object);
 
     if (deformableObj == nullptr)
     {

Base/Collision/imstkInteractionPair.h

@@ -17,7 +17,7 @@
    See the License for the specific language governing permissions and
    limitations under the License.
 
-   =========================================================================*/
+=========================================================================*/
 
 #ifndef imstkInteractionPair_h
 #define imstkInteractionPair_h

Base/Collision/imstkPbdInteractionPair.h

-#ifndef IMSTKPBDINTERACTIONPAIR_H
-#define IMSTKPBDINTERACTIONPAIR_H
+/*=========================================================================
 
-#include "imstkInteractionPair.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.
+
+=========================================================================*/
 
-#include "imstkPbdCollisionConstraint.h"
+#ifndef imstkPbdInteractionPair_h
+#define imstkPbdInteractionPair_h
+
+#include "imstkInteractionPair.h"
+#include "imstkPbdEdgeEdgeCollisionConstraint.h"
+#include "imstkPbdPointTriCollisionConstraint.h"
 #include "imstkPbdObject.h"
 
 namespace imstk
@@ -21,11 +42,10 @@ public:
     /// \brief Constructor
     ///
     PbdInteractionPair(std::shared_ptr<PbdObject> A, std::shared_ptr<PbdObject> B):
-        first(A), second(B)
-    {}
+        first(A), second(B) {}
 
     ///
-    /// \brief
+    /// \brief Clear the collisions from previous step
     ///
     inline void resetConstraints()
     {
@@ -41,22 +61,22 @@ public:
     }
 
     ///
-    /// \brief
+    /// \brief Broad phase collision detection using AABB
     ///
-    bool doBroadPhase();
+    bool doBroadPhaseCollision();
 
     ///
-    /// \brief
+    /// \brief Narrow phase collision detection
     ///
-    void doNarrowPhase();
+    void doNarrowPhaseCollision();
 
     ///
-    /// \brief
+    /// \brief Resolves the collision by solving pbd collision constraints
     ///
-    void doCollision();
+    void resolveCollision();
 
 private:
-    std::vector<CollisionConstraint*> m_collisionConstraints;
+    std::vector<std::shared_ptr<PbdCollisionConstraint>> m_collisionConstraints;
     std::shared_ptr<PbdObject> first;
     std::shared_ptr<PbdObject> second;
     unsigned int maxIter;
@@ -64,4 +84,4 @@ private:
 
 }
 
-#endif // IMSTKPBDINTERACTIONPAIR_H
+#endif // imstkPbdInteractionPair_h

Base/Constraint/CMakeLists.txt

@@ -2,11 +2,12 @@
 # Create target
 #-----------------------------------------------------------------------------
 include(imstkAddLibrary)
-imstk_add_library( Constraint
+imstk_add_library( Constraints
   DEPENDS
     Core
     Geometry
     SceneElements
+    DynamicalModels
   )
 
 #-----------------------------------------------------------------------------

Base/Constraint/PbdConstraints/imstkPbdAreaConstraint.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 "imstkPbdAreaConstraint.h"
+#include "imstkPbdModel.h"
+
+namespace  imstk
+{
+
+void
+PbdAreaConstraint::initConstraint(PbdModel &model, const size_t& pIdx1,
+                                  const size_t& pIdx2, const size_t& pIdx3,
+                                  const double k)
+{
+    m_vertexIds[0] = pIdx1;
+    m_vertexIds[1] = pIdx2;
+    m_vertexIds[2] = pIdx3;
+
+    m_stiffness = k;
+
+    auto state = model.getInitialState();
+
+    const Vec3d &p0 = state->getVertexPosition(pIdx1);
+    const Vec3d &p1 = state->getVertexPosition(pIdx2);
+    const Vec3d &p2 = state->getVertexPosition(pIdx3);
+
+    m_restArea = 0.5*(p1 - p0).cross(p2 - p0).norm();
+}
+
+bool
+PbdAreaConstraint::solvePositionConstraint(PbdModel& model)
+{
+    const auto i1 = m_vertexIds[0];
+    const auto i2 = m_vertexIds[1];
+    const auto i3 = m_vertexIds[2];
+
+    auto state = model.getCurrentState();
+
+    Vec3d &p0 = state->getVertexPosition(i1);
+    Vec3d &p1 = state->getVertexPosition(i2);
+    Vec3d &p2 = state->getVertexPosition(i3);
+
+    const auto im0 = model.getInvMass(i1);
+    const auto im1 = model.getInvMass(i2);
+    const auto im2 = model.getInvMass(i3);
+
+    const auto e1 = p0 - p1;
+    const auto e2 = p1 - p2;
+    const auto e3 = p2 - p0;
+
+    auto n = e1.cross(e2);
+    const auto A = 0.5*n.norm();
+
+    if (A < m_epsilon)
+    {
+        return false;
+    }
+
+    n /= 2 * A;
+
+    const auto grad0 = e2.cross(n);
+    const auto grad1 = e3.cross(n);
+    const auto grad2 = e1.cross(n);
+
+    auto lambda = im0*grad0.squaredNorm() + im1*grad1.squaredNorm() + im2*grad2.squaredNorm();
+
+    lambda = (A - m_restArea) / lambda * m_stiffness;
+
+    if (im0 > 0)
+    {
+        p0 += -im0*lambda*grad0;
+    }
+
+    if (im1 > 0)
+    {
+        p1 += -im1*lambda*grad1;
+    }
+
+    if (im2 > 0)
+    {
+        p2 += -im2*lambda*grad2;
+    }
+
+    return true;
+}
+
+
+} // imstk
\ No newline at end of file

Base/Constraint/PbdConstraints/imstkPbdAreaConstraint.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 imstkPbdAreaConstraint_h
+#define imstkPbdAreaConstraint_h
+
+#include "imstkPbdConstraint.h"
+
+namespace imstk
+{
+
+////
+/// \class AreaConstraint
+///
+/// \brief Area constraint for triangular face
+///
+class PbdAreaConstraint : public PbdConstraint
+{
+public:
+    ///
+    /// \brief Constructor
+    ///
+    PbdAreaConstraint() : PbdConstraint() { m_vertexIds.resize(3); }
+
+    ///
+    /// \brief Returns PBD constraint of type Type::Area
+    ///
+    Type getType() const { return Type::Area; }
+
+    ///
+    /// \brief Initializes the area constraint
+    ///
+    void initConstraint(PbdModel& model, const size_t& pIdx1,
+                        const size_t& pIdx2, const size_t& pIdx3,
+                        const double k = 2.5);
+
+    ///
+    /// \brief Solves the area constraint
+    ///
+    bool solvePositionConstraint(PbdModel &model);
+
+public:
+    double m_restArea; ///> Area at the rest position
+    double m_stiffness; ///> Stiffness of the area constraint
+};
+
+} // imstk
+
+#endif // imstkPbdAreaConstraint_h
\ No newline at end of file

Base/Constraint/PbdConstraints/imstkPbdCollisionConstraint.cpp

+#include "imstkPbdCollisionConstraint.h"
+#include "g3log/g3log.hpp"
+
+namespace imstk
+{
+
+PbdCollisionConstraint::PbdCollisionConstraint(const unsigned int& n1, const unsigned int& n2)
+{
+    m_bodiesFirst.resize(n1);
+    m_bodiesSecond.resize(n2);
+}
+
+} // imstk
\ No newline at end of file

Base/Constraint/PbdConstraints/imstkPbdCollisionConstraint.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 imstkPbdCollisionConstraint_h
+#define imstkPbdCollisionConstraint_h
+
+#include "imstkPbdModel.h"
+
+#include <vector>
+
+namespace imstk
+{
+
+class PbdModel;
+
+///
+/// \class PbdCollisionConstraint
+///
+/// \brief
+///
+class PbdCollisionConstraint
+{
+public:
+    enum class Type
+    {
+        EdgeEdge,
+        PointTriangle
+    };
+
+    ///
+    /// \brief
+    ///
+    PbdCollisionConstraint(const unsigned int& n1, const unsigned int& n2);
+
+    ///
+    /// \brief
+    ///
+    virtual bool solvePositionConstraint() = 0;
+
+public:
+    std::vector<size_t> m_bodiesFirst;    ///> index of points for the first object
+    std::vector<size_t> m_bodiesSecond;   ///> index of points for the second object
+
+    std::shared_ptr<PbdModel> m_model1;
+    std::shared_ptr<PbdModel> m_model2;
+};
+
+}
+
+#endif // imstkPbdCollisionConstraint_h
\ No newline at end of file

Base/Constraint/PbdConstraints/imstkPbdConstraint.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 imstkPbdConstraint_h
+#define imstkPbdConstraint_h
+
+#include "imstkMath.h"
+
+namespace imstk
+{
+
+class PbdModel;
+
+///
+/// \brief Base Constraint class for Position based dynamics constraints
+///
+class PbdConstraint
+{
+public:
+    ///
+    /// \brief Type of the PBD constraint
+    ///
+    enum class Type
+    {
+        Distance,
+        Dihedral,
+        Area,
+        Volume,
+        FEMTet,
+        FEMHex,
+        none
+    };
+
+    ///
+    /// \brief Constructor
+    ///
+    PbdConstraint() = default;
+
+    ///
+    /// \brief abstract interface to know the type of constraint
+    /// \return particular type
+    ///
+    virtual Type getType() const = 0;
+
+    ///
+    /// \brief compute delta position from the constraint function
+    /// \param model \class PbdModel
+    /// \return true if succeeded
+    ///
+    virtual bool solvePositionConstraint(PbdModel& model) = 0;
+
+    ///
+    /// \brief Set the tolerance used for pbd constraints
+    ///
+    void setTolerance(const double eps) { m_epsilon = eps; }
+
+public:
+    std::vector<size_t> m_vertexIds; ///> index of points for the constraint
+    double m_epsilon = 1.0e6;         ///> Tolerance used for the costraints
+};
+}
+
+#endif // imstkPbdConstraint_h
\ No newline at end of file

Base/Constraint/PbdConstraints/imstkPbdDihedralConstraint.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 "imstkPbdDihedralConstraint.h"
+#include "imstkPbdModel.h"
+
+namespace  imstk
+{
+
+void
+PbdDihedralConstraint::initConstraint(PbdModel &model,
+                                      const size_t& pIdx1, const size_t& pIdx2,
+                                      const size_t& pIdx3, const size_t& pIdx4,
+                                      const double k)
+{
+    m_vertexIds[0] = pIdx1;
+    m_vertexIds[1] = pIdx2;
+    m_vertexIds[2] = pIdx3;
+    m_vertexIds[3] = pIdx4;
+
+    m_stiffness = k;
+    auto state = model.getInitialState();
+
+    const Vec3d &p0 = state->getVertexPosition(pIdx1);
+    const Vec3d &p1 = state->getVertexPosition(pIdx2);
+    const Vec3d &p2 = state->getVertexPosition(pIdx3);
+    const Vec3d &p3 = state->getVertexPosition(pIdx4);
+
+    const Vec3d n1 = (p2 - p0).cross(p3 - p0).normalized();
+    const Vec3d n2 = (p3 - p1).cross(p2 - p1).normalized();
+
+    m_restAngle = atan2(n1.cross(n2).dot(p3 - p2), (p3 - p2).norm()*n1.dot(n2));
+}
+
+bool
+PbdDihedralConstraint::solvePositionConstraint(PbdModel& model)
+{
+    const auto i1 = m_vertexIds[0];
+    const auto i2 = m_vertexIds[1];
+    const auto i3 = m_vertexIds[2];
+    const auto i4 = m_vertexIds[3];
+
+    auto state = model.getCurrentState();
+
+    Vec3d &p0 = state->getVertexPosition(i1);
+    Vec3d &p1 = state->getVertexPosition(i2);
+    Vec3d &p2 = state->getVertexPosition(i3);
+    Vec3d &p3 = state->getVertexPosition(i4);
+
+    const auto im0 = model.getInvMass(i1);
+    const auto im1 = model.getInvMass(i2);
+    const auto im2 = model.getInvMass(i3);
+    const auto im3 = model.getInvMass(i4);
+
+    if (im0 == 0.0 && im1 == 0.0)
+    {
+        return false;
+    }
+
+    const auto e = p3 - p2;
+    const auto e1 = p3 - p0;
+    const auto e2 = p0 - p2;
+    const auto e3 = p3 - p1;
+    const auto e4 = p1 - p2;
+    // To accelerate, all normal (area) vectors and edge length should be precomputed in parallel
+    auto n1 = e1.cross(e);
+    auto n2 = e.cross(e3);
+    const auto A1 = n1.norm();
+    const auto A2 = n2.norm();
+    n1 /= A1;
+    n2 /= A2;
+
+    const double l = e.norm();
+    if (l < m_epsilon)
+    {
+        return false;
+    }
+
+    const auto grad0 = -(l / A1)*n1;
+    const auto grad1 = -(l / A2)*n2;
+    const auto grad2 = (e.dot(e1) / (A1*l))*n1 + (e.dot(e3) / (A2*l))*n2;
+    const auto grad3 = (e.dot(e2) / (A1*l))*n1 + (e.dot(e4) / (A2*l))*n2;
+
+    auto lambda = im0*grad0.squaredNorm() +
+                im1*grad1.squaredNorm() +
+                im2*grad2.squaredNorm() +
+                im3*grad3.squaredNorm();
+
+    // huge difference if use acos instead of atan2
+    lambda = (atan2(n1.cross(n2).dot(e), l*n1.dot(n2)) - m_restAngle) / lambda * m_stiffness;
+
+    if (im0 > 0)
+    {
+        p0 += -im0*lambda*grad0;
+    }
+
+    if (im1 > 0)
+    {
+        p1 += -im1*lambda*grad1;
+    }
+
+    if (im2 > 0)
+    {
+        p2 += -im2*lambda*grad2;
+    }
+
+    if (im3 > 0)
+    {
+        p3 += -im3*lambda*grad3;
+    }
+
+    return true;
+}
+
+} // imstk
\ No newline at end of file

Base/Constraint/PbdConstraints/imstkPbdDihedralConstraint.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 imstkPbdDihedralConstraint_h
+#define imstkPbdDihedralConstraint_h
+
+#include "imstkPbdConstraint.h"
+
+namespace imstk
+{
+
+///
+/// \class PbdDihedralConstraint
+///
+/// \brief Angular constraint between two triangular faces
+///
+class PbdDihedralConstraint : public PbdConstraint
+{
+public:
+    ///
+    /// \brief Constructor
+    ///
+    PbdDihedralConstraint() : PbdConstraint()  { m_vertexIds.resize(4); }
+
+    ///
+    /// \brief Returns PBD constraint of type Type::Dihedral
+    ///
+    Type getType() const { return Type::Dihedral; }
+
+    ///
+    /// \brief initConstraint
+    ///        p3
+    ///       / | \
+                ///      /  |  \
+                ///     p0  |  p1
+    ///      \  |  /
+    ///       \ | /
+    ///         p2
+    /// \param model
+    /// \param pIdx1 index of p0
+    /// \param pIdx2 index of p1
+    /// \param pIdx3 index of p2
+    /// \param pIdx4 index of p3
+    /// \param k stiffness
+    ///
+    void initConstraint(PbdModel &model,
+                        const size_t& pIdx1, const size_t& pIdx2,
+                        const size_t& pIdx3, const size_t& pIdx4,
+                        const double k);
+
+    ///
+    /// \brief Solves the dihedral angular constraint
+    ///
+    bool solvePositionConstraint(PbdModel &model) override;
+
+public:
+    double m_restAngle; ///> Rest angle
+    double m_stiffness; ///> Angular stiffness
+};
+
+} //imstk
+
+#endif // imstkPbdDihedralConstraint_h
\ No newline at end of file

Base/Constraint/PbdConstraints/imstkPbdDistanceConstraint.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 "imstkPbdDistanceConstraint.h"
+#include "imstkPbdModel.h"
+
+namespace  imstk
+{
+
+void
+PbdDistanceConstraint::initConstraint(PbdModel& model, const size_t& pIdx1,
+                                      const size_t& pIdx2, const double k)
+{
+    m_vertexIds[0] = pIdx1;
+    m_vertexIds[1] = pIdx2;
+    m_stiffness = k;
+
+    auto state = model.getInitialState();
+    const Vec3d &p1 = state->getVertexPosition(pIdx1);
+    const Vec3d &p2 = state->getVertexPosition(pIdx2);
+
+    m_restLength = (p1 - p2).norm();
+}
+
+bool
+PbdDistanceConstraint::solvePositionConstraint(PbdModel &model)
+{
+    const auto i1 = m_vertexIds[0];
+    const auto i2 = m_vertexIds[1];
+
+    auto state = model.getCurrentState();
+
+    Vec3d &p0 = state->getVertexPosition(i1);
+    Vec3d &p1 = state->getVertexPosition(i2);
+
+    const auto im1 = model.getInvMass(i1);
+    const auto im2 = model.getInvMass(i2);
+
+    const auto wsum = im1 + im2;
+
+    if (wsum == 0.0)
+    {
+        return false;
+    }
+
+    Vec3d n = p1 - p0;
+    const auto len = n.norm();
+    n /= len;
+
+    auto gradC = n*m_stiffness*(len - m_restLength) / wsum;
+
+    if (im1 > 0)
+    {
+        p0 += im1*gradC;
+    }
+
+    if (im2 > 0)
+    {
+        p1 += -im2*gradC;
+    }
+    return true;
+}
+
+} // imstk
\ No newline at end of file

Base/Constraint/PbdConstraints/imstkPbdDistanceConstraint.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 imstkPbdDistanceConstraint_h
+#define imstkPbdDistanceConstraint_h
+
+#include "imstkPbdConstraint.h"
+
+namespace imstk
+{
+
+///
+/// \class PbdDistanceConstraint
+///
+/// \brief Distance constraints between two nodal points
+///
+class PbdDistanceConstraint : public PbdConstraint
+{
+public:
+    ///
+    /// \brief Constructor
+    ///
+    PbdDistanceConstraint() : PbdConstraint()  { m_vertexIds.resize(2); }
+
+    ///
+    /// \brief Returns PBD constraint of type Type::Distance
+    ///
+    Type getType() const { return Type::Distance; }
+
+    ///
+    /// \brief Initializes the distance constraint
+    ///
+    void initConstraint(PbdModel& model, const size_t& pIdx1,
+                        const size_t& pIdx2, const double k = 1e-1);
+
+    ///
+    /// \brief Solves the Distance constraint
+    ///
+    bool solvePositionConstraint(PbdModel &model) override;
+
+public:
+    double m_restLength; ///> Rest length between the nodes
+    double m_stiffness;  ///> Stiffness of the constaint
+};
+
+} // imstk
+
+#endif // imstkPbdDistanceConstraint_h
\ No newline at end of file

Base/Constraint/PbdConstraints/imstkPbdEdgeEdgeCollisionConstraint.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 "imstkPbdEdgeEdgeCollisionConstraint.h"
+#include "g3log/g3log.hpp"
+
+namespace imstk
+{
+
+void
+PbdEdgeEdgeConstraint::initConstraint(std::shared_ptr<PbdModel> model1,
+                                    const size_t& pIdx1, const size_t& pIdx2,
+                                    std::shared_ptr<PbdModel> model2,
+                                    const size_t& pIdx3, const size_t& pIdx4)
+{
+    m_model1 = model1;
+    m_model2 = model2;
+    m_bodiesFirst[0] = pIdx1;
+    m_bodiesFirst[1] = pIdx2;
+    m_bodiesSecond[0] = pIdx3;
+    m_bodiesSecond[1] = pIdx4;
+}
+
+bool
+PbdEdgeEdgeConstraint::solvePositionConstraint()
+{
+    const auto i0 = m_bodiesFirst[0];
+    const auto i1 = m_bodiesFirst[1];
+    const auto i2 = m_bodiesSecond[0];
+    const auto i3 = m_bodiesSecond[1];
+
+    auto state1 = m_model1->getCurrentState();
+    auto state2 = m_model2->getCurrentState();
+
+    Vec3d& x0 = state1->getVertexPosition(i0);
+    Vec3d& x1 = state1->getVertexPosition(i1);
+    Vec3d& x2 = state2->getVertexPosition(i2);
+    Vec3d& x3 = state2->getVertexPosition(i3);
+
+    auto a = (x3-x2).dot(x1-x0);
+    auto b = (x1 - x0).dot(x1 - x0);
+    auto c = (x0 - x2).dot(x1 - x0);
+    auto d = (x3 - x2).dot(x3 - x2);
+    auto e = a;
+    auto f = (x0 - x2).dot(x3 - x2);
+
+    auto det = a*e - d*b;
+    double s = 0.5;
+    double t = 0.5;
+    if ( fabs(det) > 1e-12 )
+    {
+        s = (c*e - b*f)/det;
+        t = (c*d - a*f)/det;
+        if (s < 0 || s > 1.0 || t < 0 || t > 1.0)
+        {
+            return false;
+        }
+    }
+    else
+    {
+        //LOG(WARNING) << "det is null";
+    }
+
+    Vec3d P = x0 + t*(x1-x0);
+    Vec3d Q = x2 + s*(x3-x2);
+
+    Vec3d n = Q - P;
+    auto l = n.norm();
+    n /= l;
+
+    const auto dist = m_model1->getProximity() + m_model2->getProximity();
+
+    if (l > dist)
+    {
+        return false;
+    }
+
+    Vec3d grad0 = -(1-t)*n;
+    Vec3d grad1 = -(t)*n;
+    Vec3d grad2 = (1-s)*n;
+    Vec3d grad3 = (s)*n;
+
+    const auto im0 = m_model1->getInvMass(i0);
+    const auto im1 = m_model1->getInvMass(i1);
+    const auto im2 = m_model2->getInvMass(i2);
+    const auto im3 = m_model2->getInvMass(i3);
+
+    auto lambda = im0*grad0.squaredNorm() +
+                    im1*grad1.squaredNorm() +
+                    im2*grad2.squaredNorm() +
+                    im3*grad3.squaredNorm();
+
+    lambda = (l - dist)/lambda;
+
+//    LOG(INFO) << "Lambda:" << lambda <<" Normal:" << n[0] <<" " << n[1] <<" "<<n[2];
+
+    if (im0 > 0)
+    {
+        x0 += -im0*lambda*grad0*m_model1->getContactStiffness();
+    }
+
+    if (im1 > 0)
+    {
+        x1 += -im1*lambda*grad1*m_model1->getContactStiffness();
+    }
+
+    if (im2 > 0)
+    {
+        x2 += -im2*lambda*grad2*m_model2->getContactStiffness();
+    }
+
+    if (im3 > 0)
+    {
+        x3 += -im3*lambda*grad3*m_model2->getContactStiffness();
+    }
+
+    return true;
+}
+
+} // imstk

Base/Constraint/PbdConstraints/imstkPbdEdgeEdgeCollisionConstraint.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 imstkPbdEdgeEdgeCollConstraint_h
+#define imstkPbdEdgeEdgeCollConstraint_h
+
+#include "imstkPbdCollisionConstraint.h"
+
+namespace imstk
+{
+
+///
+/// \brief The PbdEdgeEdgeConstraint class for edge-edge collision response
+///
+class PbdEdgeEdgeConstraint : public PbdCollisionConstraint
+{
+
+public:
+    PbdEdgeEdgeConstraint() : PbdCollisionConstraint(2,2) {}
+
+    ///
+    /// \brief Get the type of pbd constraint
+    ///
+    Type getType() const { return Type::EdgeEdge; }
+
+    ///
+    /// \brief initialize constraint
+    /// \param pIdx1 first point of the edge from object1
+    /// \param pIdx2 second point of the edge from object1
+    /// \param pIdx3 first point of the edge from object2
+    /// \param pIdx4 second point of the edge from object2
+    /// \return  true if succeeded
+    ///
+    void initConstraint(std::shared_ptr<PbdModel> model1,
+                        const size_t& pIdx1, const size_t& pIdx2,
+                        std::shared_ptr<PbdModel> model2,
+                        const size_t& pIdx3, const size_t& pIdx4);
+
+    ///
+    /// \brief Solve edge-edge collision constraint
+    ///
+    bool solvePositionConstraint();
+};
+
+}
+
+#endif // imstkPbdEdgeEdgeCollConstraint_h
\ No newline at end of file

Base/Constraint/PbdConstraints/imstkPbdFEHexConstraint.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 imstkPbdFeHexConstraint_h
+#define imstkPbdFeHexConstraint_h
+
+#include "imstkPbdFEMConstraint.h"
+
+namespace imstk
+{
+
+///
+/// \class FEMHexConstraint
+///
+/// \brief The FEMHexConstraint class class for constraint as the elastic energy
+/// computed by linear shape functions with hexahedral mesh.
+///
+class PbdFEMHexConstraint : public  PbdFEMConstraint
+{
+public:
+    ///
+    /// \brief Constructor
+    ///
+    explicit PbdFEMHexConstraint(MaterialType mtype = MaterialType::StVK) :
+        PbdFEMConstraint(8, mtype) {}
+
+    ///
+    /// \brief Get the type of FEM constraint
+    ///
+    Type getType() const { return Type::FEMHex; }
+
+    ///
+    /// \brief Initializes the FEM hexahedral element constraint
+    ///
+    bool initConstraint(PbdModel& model, const unsigned int& pIdx1,
+        const unsigned int& pIdx2, const unsigned int& pIdx3,
+        const unsigned int& pIdx4, const unsigned int& pIdx5,
+        const unsigned int& pIdx6, const unsigned int& pIdx7,
+        const unsigned int& pIdx8);
+
+    ///
+    /// \brief Solves the FEM hexahedral element constraint
+    ///
+    bool solvePositionConstraint(PbdModel &model) override;
+};
+
+} // imstk
+
+#endif // imstkPbdFeHexConstraint_h
\ No newline at end of file

Base/Constraint/PbdConstraints/imstkPbdFEMConstraint.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 imstkPbdFEMConstraint_h
+#define imstkPbdFEMConstraint_h
+
+#include "imstkPbdConstraint.h"
+
+namespace imstk
+{
+
+///
+/// \class PbdFEMConstraint
+///
+/// \brief The PbdFEMConstraint class for constraint as the elastic energy
+/// computed by linear shape functions with tetrahedral mesh.
+/// We provide several model for elastic energy including:
+/// Linear, Co-rotation, St Venant-Kirchhof and NeoHookean
+///
+class PbdFEMConstraint : public PbdConstraint
+{
+public:
+    // Material type
+    enum class MaterialType
+    {
+        Linear,
+        Corotation,
+        StVK,
+        NeoHookean
+    };
+
+    ///
+    /// \brief Constructor
+    ///
+    explicit PbdFEMConstraint(const unsigned int cardinality, MaterialType mtype = MaterialType::StVK) :
+        PbdConstraint(), m_material(mtype)
+    {
+        m_vertexIds.resize(cardinality);
+    }
+
+public:
+    double m_elementVolume;     ///> Volume of the element
+    MaterialType m_material;    ///> Material type
+    Mat3d m_invRestMat;         ///>
+};
+
+}
+
+#endif // imstkPbdFEMConstraint_h
\ No newline at end of file