BUG: Fix issue in MeshToMeshBruteForce, handles extra edge case, bary moved...

BUG: Fix issue in MeshToMeshBruteForce, handles extra edge case, bary moved and swapped with eigen vecs, comments added

Examples/PBD/PBDFluids/PBDFluidsExample.cpp

@@ -38,6 +38,8 @@
 #include "imstkVisualModel.h"
 #include "imstkVTKViewer.h"
 
+#include "imstkPlane.h"
+
 using namespace imstk;
 
 // mesh file name
@@ -215,9 +217,9 @@ main()
         scene->addSceneObject(fluidObj);
 
         imstkNew<CollidingObject>    floorObj("Floor");
-        std::shared_ptr<SurfaceMesh> floorMesh = createCollidingSurfaceMesh();
-        floorObj->setVisualGeometry(floorMesh);
-        floorObj->setCollidingGeometry(floorMesh);
+        std::shared_ptr<SurfaceMesh> floorGeom = createCollidingSurfaceMesh();
+        floorObj->setVisualGeometry(floorGeom);
+        floorObj->setCollidingGeometry(floorGeom);
         scene->addSceneObject(floorObj);
 
         // Collisions

Source/CollisionDetection/CollisionDetection/imstkCollisionUtils.h

@@ -23,7 +23,6 @@
 
 #include "imstkMath.h"
 #include "imstkTypes.h"
-#include "imstkGeometryUtilities.h"
 
 #include <algorithm>
 #include <array>
@@ -776,13 +775,15 @@ testSphereToTriangle(const Vec3d& spherePt, const double sphereRadius,
     }
 }
 
+///
+/// \brief Tests if a point is inside a tetrahedron
+///
 inline bool
-testPointInsideTet(const std::array<Vec3d, 4>& inputTetVerts, const Vec3d& p)
+testPointToTetrahedron(const std::array<Vec3d, 4>& inputTetVerts, const Vec3d& p)
 {
-    std::array<double, 4> bCoord;
-    GeometryUtils::computePointBarycentricCoordinates(inputTetVerts, p, bCoord);
+    const Vec4d bCoord = baryCentric(p, inputTetVerts[0], inputTetVerts[1], inputTetVerts[2], inputTetVerts[3]);
 
-    constexpr const double eps = VERY_SMALL_EPSILON;
+    constexpr const double eps = IMSTK_DOUBLE_EPS;
     if (bCoord[0] >= -eps
         && bCoord[1] >= -eps
         && bCoord[2] >= -eps
@@ -795,8 +796,7 @@ testPointInsideTet(const std::array<Vec3d, 4>& inputTetVerts, const Vec3d& p)
 }
 
 ///
-/// \brief Tests if the segment intersects any of the triangle faces of the tet
-/// \todo: Could be faster with SAT directly applied here
+/// \brief Tests if the segment intersects any of the triangle faces of the tet (GJK)
 ///
 inline bool
 testTetToSegment(
@@ -812,8 +812,8 @@ testTetToSegment(
         }
     }
 
-    // test if both points lie inside the tetrahedron
-    if (testPointInsideTet(inputTetVerts, x1) && testPointInsideTet(inputTetVerts, x2))
+    // If either point lies inside the tetrahedron (handles completely inside case)
+    if (testPointToTetrahedron(inputTetVerts, x1) || testPointToTetrahedron(inputTetVerts, x2))
     {
         return true;
     }
@@ -862,6 +862,45 @@ testTetToSegment(
     return firstFound;
 }
 
+///
+/// \brief ray OBB intersection with intersection point
+/// \param ray origin
+/// \param ray direction
+/// \param box to world
+/// \param world to box
+/// \param half length/extents
+/// \param intersection points (first component gives entry point, second gives exit)
+///
+inline bool
+testRayToObb(const Vec3d& rayOrigin, const Vec3d& rayDir,
+             const Mat4d& worldToBox, Vec3d extents,
+             Vec2d& iPt)
+{
+    // convert from world to box space
+    const Vec3d rd = (worldToBox * Vec4d(rayDir[0], rayDir[1], rayDir[2], 0.0)).head<3>();
+    const Vec3d ro = (worldToBox * Vec4d(rayOrigin[0], rayOrigin[1], rayOrigin[2], 1.0)).head<3>();
+
+    // ray-box intersection in box space
+    const Vec3d m = Vec3d(1.0, 1.0, 1.0).cwiseQuotient(rd);
+    const Vec3d s = Vec3d((rd[0] < 0.0) ? 1.0 : -1.0,
+        (rd[1] < 0.0) ? 1.0 : -1.0,
+        (rd[2] < 0.0) ? 1.0 : -1.0);
+    const Vec3d t1 = m.cwiseProduct(-ro + s.cwiseProduct(extents));
+    const Vec3d t2 = m.cwiseProduct(-ro - s.cwiseProduct(extents));
+
+    const double tN = std::max(std::max(t1[0], t1[1]), t1[2]);
+    const double tF = std::min(std::min(t2[0], t2[1]), t2[2]);
+
+    // Does not enter
+    if (tN > tF || tF < 0.0)
+    {
+        return false;
+    }
+    iPt = Vec2d(tN, tF); // Parameterized along the ray
+
+    return true;
+}
+
 ///
 /// \brief Compute closest distance from a point to a segment x1-x2
 ///

Source/CollisionDetection/CollisionDetection/imstkMeshToMeshBruteForceCD.cpp

@@ -20,12 +20,10 @@
 =========================================================================*/
 
 #include "imstkMeshToMeshBruteForceCD.h"
-#include "imstkCollisionData.h"
 #include "imstkCollisionUtils.h"
 #include "imstkLineMesh.h"
 #include "imstkSurfaceMesh.h"
 
-#include <stdint.h>
 #include <unordered_set>
 
 namespace imstk
@@ -233,15 +231,15 @@ polySignedDist(const Vec3d& pos, const SurfMeshData& surfMeshData,
         const Vec3d& x2   = surfMeshData.vertices[cell[1]];
         const Vec3d& x3   = surfMeshData.vertices[cell[2]];
 
-        int          caseType;
-        const Vec3d  closestPtOnTri = CollisionUtils::closestPointOnTriangle(pos, x1, x2, x3, caseType);
+        int          ptOnTriangleCaseType;
+        const Vec3d  closestPtOnTri = CollisionUtils::closestPointOnTriangle(pos, x1, x2, x3, ptOnTriangleCaseType);
         const double sqrDist = (closestPtOnTri - pos).squaredNorm();
         if (sqrDist < minSqrDist)
         {
             minSqrDist      = sqrDist;
             closestPt       = closestPtOnTri;
             closestCell     = j;
-            closestCellCase = caseType;
+            closestCellCase = ptOnTriangleCaseType;
         }
     }
 
@@ -321,6 +319,7 @@ MeshToMeshBruteForceCD::computeCollisionDataAB(
     // Broad phase collision
     if (doBroadPhaseCollisionCheck(geomA, geomB))
     {
+        auto pointSet = std::dynamic_pointer_cast<PointSet>(geomA);
         auto surfMesh = std::dynamic_pointer_cast<SurfaceMesh>(geomB);
         surfMesh->computeTrianglesNormals();
         surfMesh->computeVertexNeighborTriangles();
@@ -328,9 +327,9 @@ MeshToMeshBruteForceCD::computeCollisionDataAB(
         // Narrow phase
         if (m_generateVertexTriangleContacts)
         {
-            if (m_vertexInside.size() < surfMesh->getNumVertices())
+            if (m_vertexInside.size() < pointSet->getNumVertices())
             {
-                m_vertexInside = std::vector<bool>(surfMesh->getNumVertices(), false);
+                m_vertexInside = std::vector<bool>(pointSet->getNumVertices(), false);
             }
             else
             {
@@ -389,8 +388,8 @@ MeshToMeshBruteForceCD::vertexToTriangleTest(
                 elemB.idCount  = 1;
                 elemB.cellType = IMSTK_VERTEX;
 
-                elementsA.safeAppend(elemA);
-                elementsB.safeAppend(elemB);
+                elementsA.unsafeAppend(elemA);
+                elementsB.unsafeAppend(elemB);
                 m_vertexInside[i] = true;
             }
             else if (caseType == 1)
@@ -406,8 +405,8 @@ MeshToMeshBruteForceCD::vertexToTriangleTest(
                 elemB.idCount  = 2;
                 elemB.cellType = IMSTK_EDGE;
 
-                elementsA.safeAppend(elemA);
-                elementsB.safeAppend(elemB);
+                elementsA.unsafeAppend(elemA);
+                elementsB.unsafeAppend(elemB);
                 m_vertexInside[i] = true;
             }
             else if (caseType == 2)
@@ -424,8 +423,8 @@ MeshToMeshBruteForceCD::vertexToTriangleTest(
                 elemB.idCount  = 3;
                 elemB.cellType = IMSTK_TRIANGLE;
 
-                elementsA.safeAppend(elemA);
-                elementsB.safeAppend(elemB);
+                elementsA.unsafeAppend(elemA);
+                elementsB.unsafeAppend(elemB);
                 m_vertexInside[i] = true;
             }
         }
@@ -514,8 +513,8 @@ MeshToMeshBruteForceCD::lineMeshEdgeToTriangleTest(
                 elemB.idCount  = 2;
                 elemB.cellType = IMSTK_EDGE;
 
-                elementsA.safeAppend(elemA);
-                elementsB.safeAppend(elemB);
+                elementsA.unsafeAppend(elemA);
+                elementsB.unsafeAppend(elemB);
             }
         }
     }
@@ -618,8 +617,8 @@ MeshToMeshBruteForceCD::surfMeshEdgeToTriangleTest(
                             elemB.idCount  = 2;
                             elemB.cellType = IMSTK_EDGE;
 
-                            elementsA.safeAppend(elemA);
-                            elementsB.safeAppend(elemB);
+                            elementsA.unsafeAppend(elemA);
+                            elementsB.unsafeAppend(elemB);
 
                             hashedEdges.insert(edgePair);
                         }
@@ -650,6 +649,12 @@ MeshToMeshBruteForceCD::doBroadPhaseCollisionCheck(
     Vec3d min2, max2;
     mesh2->computeBoundingBox(min2, max2);
 
+    // Padding here helps with thin vs thin geometry
+    min1 -= m_padding;
+    max1 += m_padding;
+    min2 -= m_padding;
+    max2 += m_padding;
+
     return CollisionUtils::testAABBToAABB(
         min1[0], max1[0],
         min1[1], max1[1],

Source/CollisionDetection/CollisionDetection/imstkMeshToMeshBruteForceCD.h

@@ -78,6 +78,12 @@ public:
     ///
     void setGenerateVertexTriangleContacts(bool genVertexTriangleContacts) { m_generateVertexTriangleContacts = genVertexTriangleContacts; }
 
+    ///
+    /// \brief Set padding to the broad phase
+    ///
+    void setPadding(const Vec3d& padding) { m_padding = padding; }
+    const Vec3d& getPadding() const { return m_padding; }
+
 protected:
     ///
     /// \brief Compute collision data for AB simulatenously
@@ -119,5 +125,6 @@ private:
     bool m_generateVertexTriangleContacts = true;
 
     std::vector<bool> m_vertexInside;
+    Vec3d m_padding = Vec3d(0.001, 0.001, 0.001);
 };
 }
\ No newline at end of file

Source/CollisionDetection/CollisionDetection/imstkTetraToPointSetCD.cpp

@@ -68,8 +68,7 @@ TetraToPointSetCD::computeCollisionDataAB(
                 Vec3d vPos = verticesMeshB[vertexIdB];
 
                 // Now compute if the point is actually within the tet
-                TetrahedralMesh::WeightsArray bCoord; // Barycentric coordinates of the vertex in tetrahedron
-                tetMesh->computeBarycentricWeights(tetIdA, vPos, bCoord);
+                const Vec4d bCoord = tetMesh->computeBarycentricWeights(tetIdA, vPos);
                 if (bCoord[0] >= -eps
                     && bCoord[1] >= -eps
                     && bCoord[2] >= -eps
@@ -131,8 +130,7 @@ TetraToPointSetCD::computeCollisionDataA(
                 Vec3d vPos = verticesMeshB[vertexIdB];
 
                 // Now compute if the point is actually within the tet
-                TetrahedralMesh::WeightsArray bCoord; // Barycentric coordinates of the vertex in tetrahedron
-                tetMesh->computeBarycentricWeights(tetIdA, vPos, bCoord);
+                const Vec4d bCoord = tetMesh->computeBarycentricWeights(tetIdA, vPos);
                 if (bCoord[0] >= -eps
                     && bCoord[1] >= -eps
                     && bCoord[2] >= -eps
@@ -188,8 +186,7 @@ TetraToPointSetCD::computeCollisionDataB(
                 Vec3d vPos = verticesMeshB[vertexIdB];
 
                 // Now compute if the point is actually within the tet
-                TetrahedralMesh::WeightsArray bCoord; // Barycentric coordinates of the vertex in tetrahedron
-                tetMesh->computeBarycentricWeights(tetIdA, vPos, bCoord);
+                const Vec4d bCoord = tetMesh->computeBarycentricWeights(tetIdA, vPos);
                 if (bCoord[0] >= -eps
                     && bCoord[1] >= -eps
                     && bCoord[2] >= -eps

Source/Common/imstkMath.h

@@ -267,7 +267,7 @@ baryCentric(const Vec2d& p, const Vec2d& a, const Vec2d& b, const Vec2d& c)
 }
 
 ///
-/// \brief Compute bary centric coordinates (u,v,w) given triangle in 3d space (and point p on triangle)
+/// \brief Compute bary centric coordinates (u,v,w) of point p, given 3 points in 3d space (a,b,c)
 ///
 static Vec3d
 baryCentric(const Vec3d& p, const Vec3d& a, const Vec3d& b, const Vec3d& c)
@@ -287,6 +287,31 @@ baryCentric(const Vec3d& p, const Vec3d& a, const Vec3d& b, const Vec3d& c)
     return Vec3d(u, v, w);
 }
 
+///
+/// \brief Compute bary centric coordinates (u,v,w,x) of point p, given 4 points in 3d space (a,b,c,d)
+///
+static Vec4d
+baryCentric(const Vec3d& p, const Vec3d& a, const Vec3d& b, const Vec3d& c, const Vec3d& d)
+{
+    Mat4d A;
+    A << a[0], a[1], a[2], 1.0,
+        b[0], b[1], b[2], 1.0,
+        c[0], c[1], c[2], 1.0,
+        d[0], d[1], d[2], 1.0;
+
+    Vec4d  weights;
+    double det = A.determinant(); // Signed volume
+    for (int i = 0; i < 4; ++i)
+    {
+        Mat4d B = A;
+        B(i, 0)    = p[0];
+        B(i, 1)    = p[1];
+        B(i, 2)    = p[2];
+        weights[i] = B.determinant() / det;
+    }
+    return weights;
+}
+
 ///
 /// \brief Cantors pairing function, take two ints and produce a unique one
 /// The resulting numbers are close for two close A's and B's

Source/Constraint/PbdConstraints/imstkPbdConstantDensityConstraint.h

@@ -30,7 +30,8 @@ namespace imstk
 ///
 /// \class PbdConstantDensityConstraint
 ///
-/// \brief Implements the constant density constraint to simulate fluids
+/// \brief Implements the constant density constraint to simulate fluids.
+/// This constraint is global and applied to all vertices passed in during projection
 ///
 class PbdConstantDensityConstraint : public PbdConstraint
 {
@@ -80,12 +81,7 @@ private:
     double wPoly6(const Vec3d& pi, const Vec3d& pj);
 
     ///
-    /// \brief Smoothing kernel Spiky for gradient calculation
-    ///
-    double wSpiky(const Vec3d& pi, const Vec3d& pj);
-
-    ///
-    /// \brief
+    /// \brief Gradient of density kernel
     ///
     Vec3d gradSpiky(const Vec3d& pi, const Vec3d& pj);
 

Source/DynamicalModels/ObjectModels/imstkPbdConstraintFunctor.h

@@ -41,7 +41,8 @@
 namespace imstk
 {
 ///
-/// \brief PbdConstraintFunctor take input geometry and produce constraints
+/// \brief PbdConstraintFunctor take input geometry and produce constraints.
+/// It exists to allow extensible constraint generation
 ///
 struct PbdConstraintFunctor
 {

Source/Geometry/Analytic/imstkAnalyticalGeometry.h

@@ -55,7 +55,7 @@ public:
     void setPosition(const double x, const double y, const double z);
 
     ///
-    /// \brief Get the local or globa orientation (post transformed)
+    /// \brief Get the local or global orientation (post transformed)
     ///
     Quatd getOrientation(DataType type = DataType::PostTransform);
 

Source/Geometry/Mesh/imstkTetrahedralMesh.cpp

@@ -236,18 +236,16 @@ TetrahedralMesh::extractSurfaceMesh()
     return surfMesh;
 }
 
-void
-TetrahedralMesh::computeBarycentricWeights(const size_t& tetId, const Vec3d& pos,
-                                           WeightsArray& weights) const
+Vec4d
+TetrahedralMesh::computeBarycentricWeights(const size_t& tetId, const Vec3d& pos) const
 {
-    const Vec4i&         tetIndices = (*m_tetrahedraIndices)[tetId];
-    std::array<Vec3d, 4> v;
-    for (size_t i = 0; i < 4; ++i)
-    {
-        v[i] = getVertexPosition(tetIndices[i]);
-    }
-
-    GeometryUtils::computePointBarycentricCoordinates(v, pos, weights);
+    const VecDataArray<double, 3>& vertices     = *m_vertexPositions;
+    const VecDataArray<int, 4>&    tetraIndices = *m_tetrahedraIndices;
+    return baryCentric(pos,
+        vertices[tetraIndices[tetId][0]],
+        vertices[tetraIndices[tetId][1]],
+        vertices[tetraIndices[tetId][2]],
+        vertices[tetraIndices[tetId][3]]);
 }
 
 void

Source/Geometry/Mesh/imstkTetrahedralMesh.h

@@ -36,9 +36,6 @@ class SurfaceMesh;
 ///
 class TetrahedralMesh : public VolumetricMesh
 {
-public:
-    using WeightsArray = std::array<double, 4>;
-
 public:
     ///
     /// \brief Constructor
@@ -81,7 +78,7 @@ public:
     ///
     /// \brief compute the barycentric weights of a given point in 3D space for a given the tetrahedra
     ///
-    void computeBarycentricWeights(const size_t& tetId, const Vec3d& pos, WeightsArray& weights) const;
+    Vec4d computeBarycentricWeights(const size_t& tetId, const Vec3d& pos) const;
 
     ///
     /// \brief Compute the bounding box of a given tetrahedron

Source/Geometry/imstkGeometryUtilities.cpp

@@ -1597,7 +1597,7 @@ GeometryUtils::createTetrahedralMeshCover(std::shared_ptr<SurfaceMesh> surfMesh,
                                  const int tetId0 = numDiv * hexId;
                                  const int tetId1 = tetId0 + numDiv;
 
-                                 static TetrahedralMesh::WeightsArray weights = { 0.0, 0.0, 0.0, 0.0 };
+                                 static Vec4d weights = Vec4d::Zero();
 
                                  // loop over the tets to find the enclosing tets
                                  for (int id = tetId0; id < tetId1; ++id)
@@ -1606,7 +1606,7 @@ GeometryUtils::createTetrahedralMeshCover(std::shared_ptr<SurfaceMesh> surfMesh,
                                      {
                                          continue;
                                      }
-                                     uniformMesh->computeBarycentricWeights(id, xyz, weights);
+                                     weights = uniformMesh->computeBarycentricWeights(id, xyz);
 
                                      if ((weights[0] >= 0) && (weights[1] >= 0) && (weights[2] >= 0) && (weights[3] >= 0))
                                      {

Source/Geometry/imstkGeometryUtilities.h

@@ -242,23 +242,6 @@ std::shared_ptr<TetrahedralMesh> createUniformMesh(const Vec3d& aabbMin, const V
 ///
 std::shared_ptr<TetrahedralMesh> createTetrahedralMeshCover(std::shared_ptr<SurfaceMesh> surfMesh, const int nx, const int ny, int nz);
 
-inline void
-computePointBarycentricCoordinates(const std::array<Vec3d, 4>& v, const Vec3d& p, std::array<double, 4>& weights)
-{
-    Mat4d A;
-    A << v[0][0], v[0][1], v[0][2], 1, v[1][0], v[1][1], v[1][2], 1, v[2][0], v[2][1], v[2][2], 1, v[3][0], v[3][1], v[3][2], 1;
-
-    double det = A.determinant();
-    for (int i = 0; i < 4; ++i)
-    {
-        Mat4d B = A;
-        B(i, 0)    = p[0];
-        B(i, 1)    = p[1];
-        B(i, 2)    = p[2];
-        weights[i] = B.determinant() / det;
-    }
-}
-
 ///
 /// \brief Enumeration for reordering method
 ///

Source/GeometryMappers/imstkTetraTriangleMap.cpp

@@ -72,8 +72,7 @@ TetraTriangleMap::compute()
             }
 
             // Compute the weights
-            TetrahedralMesh::WeightsArray weights = { 0.0, 0.0, 0.0, 0.0 };
-            tetMesh->computeBarycentricWeights(closestTetId, surfVertPos, weights);
+            const Vec4d weights = tetMesh->computeBarycentricWeights(closestTetId, surfVertPos);
 
             m_verticesEnclosingTetraId[vertexIdx] = closestTetId; // store nearest tetrahedron
             m_verticesWeights[vertexIdx] = weights;               // store weights
@@ -215,9 +214,8 @@ TetraTriangleMap::findClosestTetrahedron(const Vec3d& pos) const
 size_t
 TetraTriangleMap::findEnclosingTetrahedron(const Vec3d& pos) const
 {
-    const auto                    tetMesh = std::dynamic_pointer_cast<TetrahedralMesh>(m_master);
-    TetrahedralMesh::WeightsArray weights = { 0.0, 0.0, 0.0, 0.0 };
-    size_t                        enclosingTetrahedron = std::numeric_limits<size_t>::max();
+    const auto tetMesh = std::dynamic_pointer_cast<TetrahedralMesh>(m_master);
+    size_t     enclosingTetrahedron = std::numeric_limits<size_t>::max();
 
     for (int idx = 0; idx < tetMesh->getNumTetrahedra(); ++idx)
     {
@@ -232,7 +230,7 @@ TetraTriangleMap::findEnclosingTetrahedron(const Vec3d& pos) const
             continue;
         }
 
-        tetMesh->computeBarycentricWeights(idx, pos, weights);
+        const Vec4d weights = tetMesh->computeBarycentricWeights(idx, pos);
 
         if (weights[0] >= 0 && weights[1] >= 0 && weights[2] >= 0 && weights[3] >= 0)
         {

Source/GeometryMappers/imstkTetraTriangleMap.h

@@ -23,8 +23,6 @@
 
 #include "imstkGeometryMap.h"
 
-#include <array>
-
 namespace imstk
 {
 template<typename T, int N> class VecDataArray;
@@ -106,9 +104,9 @@ protected:
     ///
     size_t findClosestTetrahedron(const Vec3d& pos) const;
 
-    std::vector<std::array<double, 4>> m_verticesWeights; ///> weights
+    std::vector<Vec4d> m_verticesWeights;           ///> weights
 
-    std::vector<size_t> m_verticesEnclosingTetraId;       ///> Enclosing tetrahedra to interpolate the weights upon
+    std::vector<size_t> m_verticesEnclosingTetraId; ///> Enclosing tetrahedra to interpolate the weights upon
 
     std::vector<Vec3d> m_bBoxMin;
     std::vector<Vec3d> m_bBoxMax;