init some work on pbd collision

Base/Collision/imstkPbdInteractionPair.cpp

+#include "imstkPbdInteractionPair.h"
+#include "imstkSurfaceMesh.h"
+
+namespace imstk {
+
+bool isIntersect(const double& a, const double& b, const double& c, const double& d)
+{
+    if ( (a <= d && a >= c) || (c <= b && c >= a) ) return true;
+    return false;
+}
+
+bool testAABBvsAABB(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);
+}
+
+bool testLINEvsLINE(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  testAABBvsAABB(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);
+
+}
+
+bool testPOINTvsTRIANGLE(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 min_x, max_x, min_y, max_y, min_z, max_z;
+    min_x = std::min(x2, std::min(x3,x4));
+    max_x = std::max(x2, std::max(x3,x4));
+    min_y = std::min(y2, std::min(y3,y4));
+    max_y = std::max(y2, std::max(y3,y4));
+    min_z = std::min(z2, std::min(z3,z4));
+    max_z = std::max(z2, std::max(z3,z4));
+    return  testAABBvsAABB(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);
+}
+
+bool PbdInteractionPair::doBroadPhase()
+{
+    auto g1 = first->getCollidingGeometry();
+    auto g2 = second->getCollidingGeometry();
+    auto mesh1 = std::static_pointer_cast<Mesh>(g1);
+    auto mesh2 = std::static_pointer_cast<Mesh>(g2);
+    Vec3d min1, max1;
+    mesh1->computeBoundingBox(min1, max1);
+    Vec3d min2, max2;
+    mesh2->computeBoundingBox(min2, max2);
+    double prox1 = first->getProximity();
+    double prox2 = second->getProximity();
+    return  testAABBvsAABB(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);
+
+}
+
+void PbdInteractionPair::doNarrowPhase()
+{
+    auto g1 = first->getCollidingGeometry();
+    auto g2 = second->getCollidingGeometry();
+    auto mesh1 = std::static_pointer_cast<SurfaceMesh>(g1);
+    auto mesh2 = std::static_pointer_cast<SurfaceMesh>(g2);
+
+    double prox1 = first->getProximity();
+    double prox2 = second->getProximity();
+
+    // brute force, use BVH or spatial grid woulbe be much better
+    // point
+    for (int i = 0; i < mesh1->getNumVertices(); ++i) {
+        Vec3d& p = mesh1->getVertexPosition(i);
+        std::vector<SurfaceMesh::TriangleArray> elements = mesh2->getTrianglesVertices();
+        for (int j = 0; j < elements.size(); ++j) {
+            Vec3i& e = elements[j];
+            Vec3d& p0 = mesh2->getVertexPosition(e[0]);
+            Vec3d& p1 = mesh2->getVertexPosition(e[1]);
+            Vec3d& p2 = mesh2->getVertexPosition(e[2]);
+
+            if (testPOINTvsTRIANGLE(p[0],p[1],p[2],
+                                    p0[0],p0[1],p0[2],
+                                    p1[0],p1[1],p1[2],
+                                    p2[0],p2[1],p2[2], prox1, prox2))
+            {
+                PointTriangleConstraint* c = new PointTriangleConstraint;
+                c->initConstraint(first, i, second, e[0], e[1], e[2]);
+                m_collisionConstraints.push_back(c);
+            }
+        }
+    }
+    // edge
+    // since we don't have edge structure, the following is not good
+    int nV = mesh1->getNumVertices();
+    std::vector<std::vector<bool>> E(nV, std::vector<bool>(nV, 1));
+    std::vector<SurfaceMesh::TriangleArray> elements = mesh1->getTrianglesVertices();
+    for (int k = 0; k < elements.size(); ++k) {
+       Vec3i& tri = elements[k];
+       unsigned int i1;
+       unsigned int i2;
+       i1 = tri[0];
+       i2 = tri[1];
+       if (E[i1][i2] && E[i2][i1]) {
+           Vec3d& P = mesh1->getVertexPosition(i1);
+           Vec3d& Q = mesh1->getVertexPosition(i2);
+           std::vector<SurfaceMesh::TriangleArray> elements2 = mesh2->getTrianglesVertices();
+           for (int j = 0; j < elements2.size(); ++j) {
+               Vec3i& e = elements2[j];
+               Vec3d& p0 = mesh2->getVertexPosition(e[0]);
+               Vec3d& p1 = mesh2->getVertexPosition(e[1]);
+               Vec3d& p2 = mesh2->getVertexPosition(e[2]);
+               if (testLINEvsLINE(P[0],P[1],P[2],
+                                  Q[0],Q[1],Q[2],
+                                  p0[0],p0[1],p0[2],
+                                  p1[0],p1[1],p1[2], prox1, prox2))
+               {
+                   EdgeEdgeConstraint* c = new EdgeEdgeConstraint;
+                   c->initConstraint(first, i1, i2, second, e[0], e[1]);
+                   m_collisionConstraints.push_back(c);
+               }
+               if (testLINEvsLINE(P[0],P[1],P[2],
+                                  Q[0],Q[1],Q[2],
+                                  p1[0],p1[1],p1[2],
+                                  p2[0],p2[1],p2[2], prox1, prox2))
+               {
+                   EdgeEdgeConstraint* c = new EdgeEdgeConstraint;
+                   c->initConstraint(first, i1, i2, second, e[1], e[2]);
+                   m_collisionConstraints.push_back(c);
+               }
+               if (testLINEvsLINE(P[0],P[1],P[2],
+                                  Q[0],Q[1],Q[2],
+                                  p2[0],p2[1],p2[2],
+                                  p0[0],p0[1],p0[2], prox1, prox2))
+               {
+                   EdgeEdgeConstraint* c = new EdgeEdgeConstraint;
+                   c->initConstraint(first, i1, i2, second, e[2], e[0]);
+                   m_collisionConstraints.push_back(c);
+               }
+           }
+           E[i1][i2] = 0;
+       }
+
+       i1 = tri[1];
+       i2 = tri[2];
+       if (E[i1][i2] && E[i2][i1]) {
+           Vec3d& P = mesh1->getVertexPosition(i1);
+           Vec3d& Q = mesh1->getVertexPosition(i2);
+           std::vector<SurfaceMesh::TriangleArray> elements2 = mesh2->getTrianglesVertices();
+           for (int j = 0; j < elements2.size(); ++j) {
+               Vec3i& e = elements2[j];
+               Vec3d& p0 = mesh2->getVertexPosition(e[0]);
+               Vec3d& p1 = mesh2->getVertexPosition(e[1]);
+               Vec3d& p2 = mesh2->getVertexPosition(e[2]);
+               if (testLINEvsLINE(P[0],P[1],P[2],
+                                  Q[0],Q[1],Q[2],
+                                  p0[0],p0[1],p0[2],
+                                  p1[0],p1[1],p1[2], prox1, prox2))
+               {
+                   EdgeEdgeConstraint* c = new EdgeEdgeConstraint;
+                   c->initConstraint(first, i1, i2, second, e[0], e[1]);
+                   m_collisionConstraints.push_back(c);
+               }
+               if (testLINEvsLINE(P[0],P[1],P[2],
+                                  Q[0],Q[1],Q[2],
+                                  p1[0],p1[1],p1[2],
+                                  p2[0],p2[1],p2[2], prox1, prox2))
+               {
+                   EdgeEdgeConstraint* c = new EdgeEdgeConstraint;
+                   c->initConstraint(first, i1, i2, second, e[1], e[2]);
+                   m_collisionConstraints.push_back(c);
+               }
+               if (testLINEvsLINE(P[0],P[1],P[2],
+                                  Q[0],Q[1],Q[2],
+                                  p2[0],p2[1],p2[2],
+                                  p0[0],p0[1],p0[2], prox1, prox2))
+               {
+                   EdgeEdgeConstraint* c = new EdgeEdgeConstraint;
+                   c->initConstraint(first, i1, i2, second, e[2], e[0]);
+                   m_collisionConstraints.push_back(c);
+               }
+           }
+           E[i1][i2] = 0;
+       }
+       i1 = tri[2];
+       i2 = tri[0];
+       if (E[i1][i2] && E[i2][i1]) {
+           Vec3d& P = mesh1->getVertexPosition(i1);
+           Vec3d& Q = mesh1->getVertexPosition(i2);
+           std::vector<SurfaceMesh::TriangleArray> elements2 = mesh2->getTrianglesVertices();
+           for (int j = 0; j < elements2.size(); ++j) {
+               Vec3i& e = elements2[j];
+               Vec3d& p0 = mesh2->getVertexPosition(e[0]);
+               Vec3d& p1 = mesh2->getVertexPosition(e[1]);
+               Vec3d& p2 = mesh2->getVertexPosition(e[2]);
+               if (testLINEvsLINE(P[0],P[1],P[2],
+                                  Q[0],Q[1],Q[2],
+                                  p0[0],p0[1],p0[2],
+                                  p1[0],p1[1],p1[2], prox1, prox2))
+               {
+                   EdgeEdgeConstraint* c = new EdgeEdgeConstraint;
+                   c->initConstraint(first, i1, i2, second, e[0], e[1]);
+                   m_collisionConstraints.push_back(c);
+               }
+               if (testLINEvsLINE(P[0],P[1],P[2],
+                                  Q[0],Q[1],Q[2],
+                                  p1[0],p1[1],p1[2],
+                                  p2[0],p2[1],p2[2], prox1, prox2))
+               {
+                   EdgeEdgeConstraint* c = new EdgeEdgeConstraint;
+                   c->initConstraint(first, i1, i2, second, e[1], e[2]);
+                   m_collisionConstraints.push_back(c);
+               }
+               if (testLINEvsLINE(P[0],P[1],P[2],
+                                  Q[0],Q[1],Q[2],
+                                  p2[0],p2[1],p2[2],
+                                  p0[0],p0[1],p0[2], prox1, prox2))
+               {
+                   EdgeEdgeConstraint* c = new EdgeEdgeConstraint;
+                   c->initConstraint(first, i1, i2, second, e[2], e[0]);
+                   m_collisionConstraints.push_back(c);
+               }
+           }
+           E[i1][i2] = 0;
+       }
+    }
+}
+
+void PbdInteractionPair::doCollision()
+{
+    if (!m_collisionConstraints.empty()) {
+        int i = 0;
+        while (++i < maxIter){
+            for (int k = 0; k < m_collisionConstraints.size(); ++k) {
+                m_collisionConstraints[k]->solvePositionConstraint();
+            }
+        }
+    }
+}
+
+
+
+}

Base/Collision/imstkPbdInteractionPair.h

+#ifndef IMSTKPBDINTERACTIONPAIR_H
+#define IMSTKPBDINTERACTIONPAIR_H
+
+#include "imstkInteractionPair.h"
+
+#include "imstkPbdCollisionConstraint.h"
+#include "imstkPbdCollidingObject.h"
+
+namespace imstk {
+
+class PbdInteractionPair : public InteractionPair
+{
+private:
+    std::vector<CollisionConstraint*>    m_collisionConstraints;
+    std::shared_ptr<PbdCollidingObject> first;
+    std::shared_ptr<PbdCollidingObject> second;
+    unsigned int maxIter;
+public:
+    PbdInteractionPair(std::shared_ptr<CollidingObject> A,
+                       std::shared_ptr<CollidingObject> B,
+                       CollisionDetection::Type CDType,
+                       CollisionHandling::Type CHAType,
+                       CollisionHandling::Type CHBType)
+    {
+        first = std::static_pointer_cast<PbdCollidingObject>(A);
+        second = std::static_pointer_cast<PbdCollidingObject>(B);
+    }
+
+    inline void resetConstraints()
+    {
+        m_collisionConstraints.clear();
+    }
+
+    inline void setNumberOfInterations(const unsigned int& n) { maxIter = n; }
+
+    bool doBroadPhase();
+
+    void doNarrowPhase();
+
+    void doCollision();
+
+};
+
+}
+
+#endif // IMSTKPBDINTERACTIONPAIR_H

Base/Constraint/imstkPbdCollisionConstraint.cpp

+#include "imstkPbdCollidingObject.h"
+#include "imstkPbdCollisionConstraint.h"
+
+namespace imstk
+{
+
+void EdgeEdgeConstraint::initConstraint( PbdCollidingObject* model1, const unsigned int &pIdx1, const unsigned int &pIdx2,
+                                         PbdCollidingObject* model2, const unsigned int &pIdx3, const unsigned int &pIdx4)
+{
+    m_model1 = model1;
+    m_model2 = model2;
+    m_bodiesFirst[0] = pIdx1;
+    m_bodiesFirst[1] = pIdx2;
+    m_bodiesSecond[0] = pIdx3;
+    m_bodiesSecond[1] = pIdx4;
+
+}
+
+bool EdgeEdgeConstraint::solvePositionConstraint()
+{
+    const unsigned int i0 = m_bodiesFirst[0];
+    const unsigned int i1 = m_bodiesFirst[1];
+    const unsigned int i2 = m_bodiesSecond[0];
+    const unsigned int i3 = m_bodiesSecond[1];
+
+//    auto state1 = m_model1->getState();
+//    auto state2 = m_model2->getState();
+
+    Vec3d x0 = m_model1->getVertexPosition(i0);
+    Vec3d x1 = m_model1->getVertexPosition(i1);
+    Vec3d x2 = m_model2->getVertexPosition(i2);
+    Vec3d x3 = m_model2->getVertexPosition(i3);
+
+    double a = (x3-x2).dot(x1-x0);
+    double b = (x1-x0).dot(x1-x0);
+    double c = (x0-x2).dot(x1-x0);
+    double d = (x3-x2).dot(x3-x2);
+    double e = a;
+    double f = (x0-x2).dot(x3-x2);
+
+    double 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 {
+        printf("WARNING: det is null \n");
+    }
+
+    Vec3d P = x0 + t*(x1-x0);
+    Vec3d Q = x2 + s*(x3-x2);
+
+    Vec3d n = Q - P;
+    double l = n.norm();
+    n /= l;
+
+    const double 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 double im0 = m_model1->getInvMass(i0);
+    const double im1 = m_model1->getInvMass(i1);
+    const double im2 = m_model2->getInvMass(i2);
+    const double im3 = m_model2->getInvMass(i3);
+
+    double lambda = im0*grad0.squaredNorm() + im1*grad1.squaredNorm() + im2*grad2.squaredNorm() + im3*grad3.squaredNorm();
+
+    lambda = (l - dist)/lambda;
+
+    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();
+
+    m_model1->setVertexPosition(i0,x0);
+    m_model1->setVertexPosition(i1,x1);
+
+    m_model2->setVertexPosition(i2,x2);
+    m_model2->setVertexPosition(i3,x3);
+
+    return true;
+}
+
+void PointTriangleConstraint::initConstraint(PbdCollidingObject* model1, const unsigned int &pIdx1,
+                                             PbdCollidingObject* model2, const unsigned int &pIdx2, const unsigned int &pIdx3, const unsigned int &pIdx4)
+{
+    m_model1 = model1;
+    m_model2 = model2;
+    m_bodiesFirst[0] = pIdx1;
+    m_bodiesSecond[0] = pIdx2;
+    m_bodiesSecond[1] = pIdx3;
+    m_bodiesSecond[2] = pIdx4;
+}
+
+bool PointTriangleConstraint::solvePositionConstraint()
+{
+    const unsigned int i0 = m_bodiesFirst[0];
+    const unsigned int i1 = m_bodiesSecond[0];
+    const unsigned int i2 = m_bodiesSecond[1];
+    const unsigned int i3 = m_bodiesSecond[2];
+
+//    auto state1 = m_model1->getState();
+//    auto state2 = m_model2->getState();
+
+    Vec3d x0 = m_model1->getVertexPosition(i0);
+
+    Vec3d x1 = m_model2->getVertexPosition(i1);
+    Vec3d x2 = m_model2->getVertexPosition(i2);
+    Vec3d x3 = m_model2->getVertexPosition(i3);
+
+    Vec3d x12 = x2 - x1;
+    Vec3d x13 = x3 - x1;
+    Vec3d n = x12.cross(x13);
+    Vec3d x01 = x0 - x1;
+
+    double alpha = n.dot(x12.cross(x01))/ (n.dot(n));
+    double beta  = n.dot(x01.cross(x13))/ (n.dot(n));
+
+    if (alpha < 0 || beta < 0 || alpha + beta > 1 ) {
+//        printf("WARNING: barycentric coordinate out of bounds ! \n");
+        return false;
+    }
+
+    const double dist = m_model1->getProximity() + m_model2->getProximity();
+
+    n.normalize();
+
+    double l = x01.dot(n);
+
+    if (l > dist)
+        return false;
+
+    double gamma = 1.0 - alpha - beta;
+    Vec3d grad0 = n;
+    Vec3d grad1 = -alpha*n;
+    Vec3d grad2 = -beta*n;
+    Vec3d grad3 = -gamma*n;
+
+    const double im0 = m_model1->getInvMass(i0);
+
+    const double im1 = m_model2->getInvMass(i1);
+    const double im2 = m_model2->getInvMass(i2);
+    const double im3 = m_model2->getInvMass(i3);
+
+    double lambda = im0*grad0.squaredNorm() + im1*grad1.squaredNorm() + im2*grad2.squaredNorm() + im3*grad3.squaredNorm();
+
+    lambda = (l - dist)/lambda;
+
+    if (im0 > 0)
+        x0 += -im0*lambda*grad0*m_model1->getContactStiffness();
+    if (im1 > 0)
+        x1 += -im1*lambda*grad1*m_model2->getContactStiffness();
+    if (im2 > 0)
+        x2 += -im2*lambda*grad2*m_model2->getContactStiffness();
+    if (im3 > 0)
+        x3 += -im3*lambda*grad3*m_model2->getContactStiffness();
+
+    m_model1->setVertexPosition(i0,x0);
+
+    m_model2->setVertexPosition(i1,x1);
+    m_model2->setVertexPosition(i2,x2);
+    m_model2->setVertexPosition(i3,x3);
+
+    return true;
+}
+
+}

Base/Constraint/imstkPbdCollisionConstraint.h

+#ifndef IMSTKPBDCOLLISIONCONSTRAINT_H
+#define IMSTKPBDCOLLISIONCONSTRAINT_H
+
+#include <vector>
+
+namespace imstk
+{
+
+class PbdCollidingObject;
+
+class CollisionConstraint
+{
+public:
+    enum class Type
+    {
+       EdgeEdge,
+       PointTriangle
+    };
+    std::vector<unsigned int> m_bodiesFirst;        // index of points for the first object
+    std::vector<unsigned int> m_bodiesSecond;   // index of points for the second object
+    PbdCollidingObject* m_model1;
+    PbdCollidingObject* m_model2;
+public:
+    CollisionConstraint(const unsigned int& n1, const unsigned int& n2)
+    {
+        m_bodiesFirst.resize(n1);
+        m_bodiesSecond.resize(n2);
+    }
+    virtual bool solvePositionConstraint()
+    {
+        return true;
+    }
+};
+
+///
+/// \brief The EdgeEdgeConstraint class for edge-edge collision response
+///
+
+class EdgeEdgeConstraint : public CollisionConstraint
+{
+
+public:
+    EdgeEdgeConstraint() : CollisionConstraint(2,2) {}
+
+    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( PbdCollidingObject* model1, const unsigned int& pIdx1, const unsigned int& pIdx2,
+                         PbdCollidingObject* model2, const unsigned int& pIdx3, const unsigned int& pIdx4);
+
+    bool solvePositionConstraint();
+};
+
+///
+/// \brief The PointTriangleConstraint class for point-triangle collision response
+///
+
+
+class PointTriangleConstraint : public CollisionConstraint
+{
+
+public:
+    PointTriangleConstraint() : CollisionConstraint(1,3) {}
+
+    Type getType() const { return Type::PointTriangle; }
+    ///
+    /// \brief initialize constraint
+    /// \param pIdx1 index of the point from object1
+    /// \param pIdx2 first point of the triangle from object2
+    /// \param pIdx3 second point of the triangle from object2
+    /// \param pIdx4 third point of the triangle from object2
+    /// \return
+    ///
+    void initConstraint( PbdCollidingObject* model1, const unsigned int& pIdx1,
+                         PbdCollidingObject* model2, const unsigned int& pIdx2,  const unsigned int& pIdx3, const unsigned int& pIdx4);
+
+    bool solvePositionConstraint();
+};
+
+}
+
+
+#endif // IMSTKPBDCOLLISIONCONSTRAINT_H

Base/SceneElements/Objects/imstkPbdCollidingObject.cpp

+#include "imstkPbdCollidingObject.h"
+#include "imstkMesh.h"
+
+namespace imstk {
+
+void PbdCollidingObject::setCollidingGeometry(std::shared_ptr<Geometry> geometry)
+{
+    m_collidingGeometry = geometry;
+    auto mesh = std::static_pointer_cast<Mesh>(m_collidingGeometry);
+    m_invMass.resize(mesh->getNumVertices());
+}
+
+Vec3d PbdCollidingObject::getVertexPosition(const unsigned int &idx)
+{
+    auto mesh = std::static_pointer_cast<Mesh>(m_collidingGeometry);
+    return mesh->getVertexPosition(idx);
+}
+
+void PbdCollidingObject::setVertexPosition(const unsigned int &idx, const Vec3d &v)
+{
+    auto mesh = std::static_pointer_cast<Mesh>(m_collidingGeometry);
+    mesh->setVerticePosition(idx,v);
+}
+
+}

Base/SceneElements/Objects/imstkPbdCollidingObject.h

+#ifndef IMSTKPBDCOLLIDINGOBJECT_H
+#define IMSTKPBDCOLLIDINGOBJECT_H
+
+#include "imstkCollidingObject.h"
+
+namespace imstk {
+
+class PbdCollidingObject : public CollidingObject
+{
+protected:
+    double m_proximity;
+    double m_contactStiffness;
+    std::vector<double> m_invMass;
+public:
+    PbdCollidingObject(std::string name)
+        : CollidingObject(name)
+    {
+
+    }
+
+    ~ PbdCollidingObject() = default;
+
+    inline void setProximity(const double& prox)
+    {
+        m_proximity = prox;
+    }
+
+    inline double getProximity()
+    {
+        return m_proximity;
+    }
+
+    inline void setContactStiffness(const double& stiffness)
+    {
+        m_contactStiffness = stiffness;
+    }
+
+    inline double getContactStiffness()
+    {
+        return m_contactStiffness;
+    }
+
+    inline double getInvMass(const unsigned int& idx)
+    {
+        // should check bounds, but ignore for now due to bad design!
+        return m_invMass[idx];
+    }
+
+    inline void setUniformMass(const double& val)
+    {
+        if (val >= 0)
+        {
+            std::fill(m_invMass.begin(), m_invMass.end(), val);
+        }
+    }
+
+    inline void setFixedPoint(const unsigned int& idx)
+    {
+        // should check bounds, but ignore for now due to bad design!
+        m_invMass[idx] = 0;
+    }
+
+    void setCollidingGeometry(std::shared_ptr<Geometry> geometry);
+
+    Vec3d getVertexPosition(const unsigned int& idx);
+
+    void setVertexPosition(const unsigned int& idx, const Vec3d& v);
+};
+
+}
+
+#endif // IMSTKPBDCOLLIDINGOBJECT_H

Base/SceneElements/Objects/imstkPbdModel.cpp

@@ -17,6 +17,7 @@ bool PositionBasedModel::initConstraints(PbdConstraint::Type type)
             LOG(WARNING) << "FEM Tetrahedral constraint should come with tetrahedral mesh";
             return false;
         }
+        LOG(INFO) << "Creating FEM constraints";
         // ok, now create constraints
         auto tetMesh = static_cast<TetrahedralMesh*>(m_mesh);
         std::vector<TetrahedralMesh::TetraArray> elements = tetMesh->getTetrahedraVertices();
@@ -35,6 +36,7 @@ bool PositionBasedModel::initConstraints(PbdConstraint::Type type)
             LOG(WARNING) << "Volume constraint should come with volumetric mesh";
             return false;
         }
+        LOG(INFO) << "Creating Volume constraints";
         // ok, now create constraints
         auto tetMesh = static_cast<TetrahedralMesh*>(m_mesh);
         std::vector<TetrahedralMesh::TetraArray> elements = tetMesh->getTetrahedraVertices();
@@ -53,6 +55,7 @@ bool PositionBasedModel::initConstraints(PbdConstraint::Type type)
             LOG(WARNING) << "Area constraint should come with a triangular mesh";
             return false;
         }
+        LOG(INFO) << "Creating Area constraints";
         // ok, now create constraints
         auto triMesh = static_cast<SurfaceMesh*>(m_mesh);
         std::vector<SurfaceMesh::TriangleArray> elements = triMesh->getTrianglesVertices();
@@ -71,6 +74,7 @@ bool PositionBasedModel::initConstraints(PbdConstraint::Type type)
             LOG(WARNING) << "Dihedral constraint should come with a triangular mesh";
             return false;
         }
+        LOG(INFO) << "Creating Dihedral constraints";
         // ok, now create constraints
         auto triMesh = static_cast<SurfaceMesh*>(m_mesh);
         std::vector<SurfaceMesh::TriangleArray> elements = triMesh->getTrianglesVertices();
@@ -158,6 +162,7 @@ bool PositionBasedModel::initConstraints(PbdConstraint::Type type)
         break;
     }
     case PbdConstraint::Type::Distance : {
+        LOG(INFO) << "Creating Distance constraints";
         if (m_mesh->getType() == Geometry::Type::TetrahedralMesh) {
             auto tetMesh = static_cast<TetrahedralMesh*>(m_mesh);
             int nV = tetMesh->getNumVertices();

Base/SceneElements/Objects/imstkPbdObject.cpp

@@ -27,6 +27,16 @@ PbdObject::setPhysicsToCollidingMap(std::shared_ptr<GeometryMap> map)
     m_physicsToCollidingGeomMap = map;
 }
 
+std::shared_ptr<GeometryMap> PbdObject::getCollidingToPhysicsMap() const
+{
+    return m_collidingToPhysicsGeomMap;
+}
+
+void PbdObject::setCollidingToPhysicsMap(std::shared_ptr<GeometryMap> map)
+{
+    m_collidingToPhysicsGeomMap = map;
+}
+
 std::shared_ptr<GeometryMap>
 PbdObject::getPhysicsToVisualMap() const
 {

Base/SceneElements/Objects/imstkPbdObject.h

@@ -34,6 +34,8 @@ public:
     std::shared_ptr<GeometryMap> getPhysicsToCollidingMap() const;
     void setPhysicsToCollidingMap(std::shared_ptr<GeometryMap> map);
 
+    std::shared_ptr<GeometryMap> getCollidingToPhysicsMap() const;
+    void setCollidingToPhysicsMap(std::shared_ptr<GeometryMap> map);
     ///
     /// \brief Set/Get the Physics-to-Visual map
     ///
@@ -65,6 +67,7 @@ protected:
 
     //Maps
     std::shared_ptr<GeometryMap> m_physicsToCollidingGeomMap;   ///> Maps from Physics to collision geometry
+    std::shared_ptr<GeometryMap> m_collidingToPhysicsGeomMap;   ///> Maps from Physics to collision geometry
     std::shared_ptr<GeometryMap> m_physicsToVisualGeomMap;      ///> Maps from Physics to visual geometry
 
     size_t numDOF; ///> Number of degree of freedom of the body in the discretized model

Examples/Sandbox/main.cpp

@@ -72,6 +72,7 @@ void testSurfaceMeshOptimizer();
 void testDeformableBody();
 void testPbdVolume();
 void testPbdCloth();
+void testPbdCollision();
 int main()
 {
     std::cout << "****************\n"
@@ -95,7 +96,8 @@ int main()
     //testSurfaceMeshOptimizer();
 //    testDeformableBody();
 //    testPbdVolume();
-    testPbdCloth();
+//    testPbdCloth();
+    testPbdCollision();
     return 0;
 }