From 6b866b8833c52ff65bddf20606ce68b8a585d6f2 Mon Sep 17 00:00:00 2001
From: Thien Nguyen <thien.nguyen@kitware.com>
Date: Fri, 29 Jul 2016 17:56:58 -0400
Subject: [PATCH] init some work on pbd collision
---
Base/Collision/imstkPbdInteractionPair.cpp | 266 ++++++++++++++++++
Base/Collision/imstkPbdInteractionPair.h | 46 +++
.../imstkPbdCollisionConstraint.cpp | 183 ++++++++++++
Base/Constraint/imstkPbdCollisionConstraint.h | 89 ++++++
.../Objects/imstkPbdCollidingObject.cpp | 25 ++
.../Objects/imstkPbdCollidingObject.h | 72 +++++
Base/SceneElements/Objects/imstkPbdModel.cpp | 5 +
Base/SceneElements/Objects/imstkPbdObject.cpp | 10 +
Base/SceneElements/Objects/imstkPbdObject.h | 3 +
Examples/Sandbox/main.cpp | 4 +-
10 files changed, 702 insertions(+), 1 deletion(-)
create mode 100644 Base/Collision/imstkPbdInteractionPair.cpp
create mode 100644 Base/Collision/imstkPbdInteractionPair.h
create mode 100644 Base/Constraint/imstkPbdCollisionConstraint.cpp
create mode 100644 Base/Constraint/imstkPbdCollisionConstraint.h
create mode 100644 Base/SceneElements/Objects/imstkPbdCollidingObject.cpp
create mode 100644 Base/SceneElements/Objects/imstkPbdCollidingObject.h
diff --git a/Base/Collision/imstkPbdInteractionPair.cpp b/Base/Collision/imstkPbdInteractionPair.cpp
new file mode 100644
index 00000000..c91e7c6d
--- /dev/null
+++ b/Base/Collision/imstkPbdInteractionPair.cpp
@@ -0,0 +1,266 @@
+#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();
+ }
+ }
+ }
+}
+
+
+
+}
diff --git a/Base/Collision/imstkPbdInteractionPair.h b/Base/Collision/imstkPbdInteractionPair.h
new file mode 100644
index 00000000..dca3ea9f
--- /dev/null
+++ b/Base/Collision/imstkPbdInteractionPair.h
@@ -0,0 +1,46 @@
+#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
diff --git a/Base/Constraint/imstkPbdCollisionConstraint.cpp b/Base/Constraint/imstkPbdCollisionConstraint.cpp
new file mode 100644
index 00000000..26f01914
--- /dev/null
+++ b/Base/Constraint/imstkPbdCollisionConstraint.cpp
@@ -0,0 +1,183 @@
+#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;
+}
+
+}
diff --git a/Base/Constraint/imstkPbdCollisionConstraint.h b/Base/Constraint/imstkPbdCollisionConstraint.h
new file mode 100644
index 00000000..fcde00d7
--- /dev/null
+++ b/Base/Constraint/imstkPbdCollisionConstraint.h
@@ -0,0 +1,89 @@
+#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
diff --git a/Base/SceneElements/Objects/imstkPbdCollidingObject.cpp b/Base/SceneElements/Objects/imstkPbdCollidingObject.cpp
new file mode 100644
index 00000000..53370a36
--- /dev/null
+++ b/Base/SceneElements/Objects/imstkPbdCollidingObject.cpp
@@ -0,0 +1,25 @@
+#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);
+}
+
+}
diff --git a/Base/SceneElements/Objects/imstkPbdCollidingObject.h b/Base/SceneElements/Objects/imstkPbdCollidingObject.h
new file mode 100644
index 00000000..7fd2fe71
--- /dev/null
+++ b/Base/SceneElements/Objects/imstkPbdCollidingObject.h
@@ -0,0 +1,72 @@
+#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
diff --git a/Base/SceneElements/Objects/imstkPbdModel.cpp b/Base/SceneElements/Objects/imstkPbdModel.cpp
index 1e4b1db8..ab82aab8 100644
--- a/Base/SceneElements/Objects/imstkPbdModel.cpp
+++ b/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();
diff --git a/Base/SceneElements/Objects/imstkPbdObject.cpp b/Base/SceneElements/Objects/imstkPbdObject.cpp
index d35b226b..829d893a 100644
--- a/Base/SceneElements/Objects/imstkPbdObject.cpp
+++ b/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
{
diff --git a/Base/SceneElements/Objects/imstkPbdObject.h b/Base/SceneElements/Objects/imstkPbdObject.h
index 5e5577b9..43f96d1c 100644
--- a/Base/SceneElements/Objects/imstkPbdObject.h
+++ b/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
diff --git a/Examples/Sandbox/main.cpp b/Examples/Sandbox/main.cpp
index 0d76006d..fbca62db 100644
--- a/Examples/Sandbox/main.cpp
+++ b/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;
}
--
GitLab