Merge branch 'spatialHashingCD' into 'master'

ENH: Spatial hashing CD

Closes #109

See merge request !155

Base/Collision/CMakeLists.txt

@@ -12,6 +12,7 @@ imstk_add_library( Collision
 #-----------------------------------------------------------------------------
 # Testing
 #-----------------------------------------------------------------------------
-if( iMSTK_BUILD_TESTING )
-  add_subdirectory( Testing )
+if( BUILD_TESTING )
+  include(imstkAddTest)
+  imstk_add_test( Collision )
 endif()

Base/Collision/CollisionData/imstkCollisionData.h

@@ -22,6 +22,8 @@
 #ifndef imstkCollisionData_h
 #define imstkCollisionData_h
 
+#include <array>
+
 // imstk
 #include "imstkMath.h"
 
@@ -109,6 +111,25 @@ struct EdgeEdgeCollisionData
     }
 };
 
+///
+/// \struct PointTetrahedronCollisionData
+///
+/// \brief Point-tetrahedron collision data
+///
+struct PointTetrahedronCollisionData
+{
+    enum CollisionType {
+        aPenetratingA = 0, // A self-penetration
+        aPenetratingB = 1, // vertex is from mesh A, tetrahedron is from mesh B
+        bPenetratingA = 2, // vertex is from mesh B, tetrahedron is from mesh A
+        bPenetratingB = 3  // B self-penetration
+    } collisionType;
+    size_t vertexId;
+    size_t tetreahedronId;
+    using WeightsArray = std::array<double, 4>;
+    WeightsArray BarycentricCoordinates;
+};
+
 ///
 /// \struct CollisionData
 ///
@@ -124,6 +145,8 @@ public:
         VTColData.clear();
         TVColData.clear();
         EEColData.clear();
+        MAColData.clear();
+        PTColData.clear();
     }
 
     std::vector<PositionDirectionCollisionData> PDColData; ///< Position Direction collision data
@@ -131,6 +154,7 @@ public:
     std::vector<TriangleVertexCollisionData> TVColData;    ///< Triangle Vertex collision data
     std::vector<EdgeEdgeCollisionData> EEColData;          ///< Edge Edge collision data
     std::vector<MeshToAnalyticalCollisionData> MAColData;  ///< Mesh to analytical collision data
+    std::vector<PointTetrahedronCollisionData> PTColData;  ///< Point Tetrahedron collision data
 };
 
 }

Base/Collision/CollisionDetection/DataStructures/imstkSpatialHashTable.h

@@ -39,13 +39,13 @@ public:
     /// \brief Protected constructor
     /// \param x,y,z Dimensions for each cell
     ///
-    virtual void setCellSize(double x, double y, double z);
+    virtual void setCellSize(double x, double y, double z) = 0;
 
 protected:
     ///
     /// \brief Rehash the hash table
     ///
-    virtual void rehash();
+    virtual void rehash() = 0;
 
     ///
     /// \brief Protected constructor

Base/Collision/CollisionDetection/DataStructures/imstkSpatialHashTableSeparateChaining.cpp

@@ -61,7 +61,7 @@ SpatialHashTableSeparateChaining::clear()
     m_currentID = 0;
 }
 
-std::vector<Vec3d>
+std::vector<size_t>
 SpatialHashTableSeparateChaining::getPointsInAABB(const Vec3d& corner1, const Vec3d& corner2)
 {
     auto min_x = std::fmin(corner1.x(), corner2.x());
@@ -94,9 +94,8 @@ SpatialHashTableSeparateChaining::getPointsInAABB(const Vec3d& corner1, const Ve
     }
 
     // Allocate beforehand
-    std::vector<Vec3d> points(tempPoints.size());
-
-    unsigned int numPoints = 0;
+    std::vector<size_t> points(0);
+    points.reserve(tempPoints.size());
 
     // Fine iteration
     for (auto p = tempPoints.begin(); p != tempPoints.end(); ++p)
@@ -106,13 +105,10 @@ SpatialHashTableSeparateChaining::getPointsInAABB(const Vec3d& corner1, const Ve
             point.y() >= min_y && point.y() <= max_y &&
             point.z() >= min_z && point.z() <= max_z)
         {
-            points[numPoints] = p->point;
-            numPoints++;
+            points.push_back(p->ID);
         }
     }
 
-    points.resize(numPoints);
-
     return points;
 }
 

Base/Collision/CollisionDetection/DataStructures/imstkSpatialHashTableSeparateChaining.h

@@ -59,12 +59,12 @@ template<> struct equal_to<imstk::PointEntry>
 {
     size_t operator()(const imstk::PointEntry& point1, const imstk::PointEntry& point2) const
     {
-        if (point1.point != point2.point)
+        if (point1.ID != point2.ID)
         {
             return false;
         }
 
-        if (point1.ID != point2.ID)
+        if (point1.point != point2.point)
         {
             return false;
         }
@@ -110,11 +110,11 @@ public:
     void setLoadFactorMax(float loadFactorMax);
 
     ///
-    /// \brief Finds all points in an AABB
+    /// \brief Finds IDs of all points in an AABB
     /// \param corner1 One corner to the box
     /// \param corner2 The other corner to the box
     ///
-    std::vector<Vec3d> getPointsInAABB(const Vec3d& corner1, const Vec3d& corner2);
+    std::vector<size_t> getPointsInAABB(const Vec3d& corner1, const Vec3d& corner2);
 
     ///
     /// \brief Clears the table
@@ -125,19 +125,13 @@ public:
     /// \brief Protected constructor
     /// \param x,y,z Dimensions for each cell
     ///
-    virtual void setCellSize(double x, double y, double z);
+    virtual void setCellSize(double x, double y, double z) override;
 
 protected:
     ///
     /// \brief Rehash the hash table
     ///
-    virtual void rehash();
-
-    ///
-    /// \brief Hash function
-    /// \param point A point
-    ///
-    inline unsigned int generateHash(Vec3d point);
+    virtual void rehash() override;
 
     float m_loadFactorMax = 10.0f;
 

Base/Collision/CollisionDetection/imstkCollisionDetectionUtils.h → Base/Collision/CollisionDetection/imstkIntersectionTestUtils.h

@@ -19,8 +19,8 @@
 
 =========================================================================*/
 
-#ifndef imstkCollisionDetectionUtils_h
-#define imstkCollisionDetectionUtils_h
+#ifndef imstkIntersectionTestUtils_h
+#define imstkIntersectionTestUtils_h
 
 #include <algorithm>
 
@@ -28,7 +28,7 @@ namespace imstk
 {
 
 ///
-/// \brief
+/// \brief Do ranges [a,b] and [c,d] intersect?
 ///
 inline bool
 isIntersect(const double& a, const double& b, const double& c, const double& d)
@@ -161,4 +161,4 @@ testPointToTriAABB(const double& x1, const double& y1, const double& z1,
 
 }
 
-#endif // ifndef imstkCollisionDetectionUtils_h
+#endif // ifndef imstkIntersectionTestUtils_h

Base/Collision/CollisionDetection/imstkMeshToMeshCD.h

@@ -47,8 +47,8 @@ public:
     ///
     /// \brief Constructor
     ///
-    MeshToMeshCD(std::shared_ptr<SurfaceMesh> planeA,
-                 std::shared_ptr<SurfaceMesh> sphereB,
+    MeshToMeshCD(std::shared_ptr<SurfaceMesh> meshA,
+                 std::shared_ptr<SurfaceMesh> meshB,
                  CollisionData& colData);
 
     ///

Base/Collision/CollisionDetection/imstkTetraToTetraCD.cpp

+/*=========================================================================
+
+   Library: iMSTK
+
+   Copyright (c) Kitware, Inc. & Center for Modeling, Simulation,
+   & Imaging in Medicine, Rensselaer Polytechnic Institute.
+
+   Licensed under the Apache License, Version B.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-B.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 "imstkTetraToTetraCD.h"
+
+#include "imstkCollisionData.h"
+#include "imstkMath.h"
+
+#include <g3log/g3log.hpp>
+
+namespace imstk {
+
+TetraToTetraCD::TetraToTetraCD(std::shared_ptr<TetrahedralMesh> meshA,
+                               std::shared_ptr<TetrahedralMesh> meshB,
+                               CollisionData& colData) :
+    CollisionDetection(CollisionDetection::Type::MeshToMesh, colData), //is TetrahedralMeshToTetrahedralMesh type needed?
+    m_meshA(meshA),
+    m_meshB(meshB)
+{
+}
+
+void
+TetraToTetraCD::findCollisionsForMeshWithinHashTable(const std::shared_ptr<TetrahedralMesh> mesh, size_t idOffset)
+{
+    Vec3d min, max; //bounding box of a tetrahedron
+    Vec3d vPos;
+    const auto eps = MACHINE_PRECISION;
+    const double eps2 = 1e-10;
+    
+    //tetrahedron belonging part of penetration type does not change
+    PointTetrahedronCollisionData::CollisionType cType
+        = static_cast<PointTetrahedronCollisionData::CollisionType>(idOffset > 0);
+
+    for (size_t tId = 0; tId < mesh->getNumTetrahedra(); ++tId) //TODO: parallelize!
+    {
+        TetrahedralMesh::TetraArray vInd = mesh->getTetrahedronVertices(tId);
+        for (int i = 0; i < 4; i++) //if idOffset!=0 ?
+        {
+            vInd[i] += idOffset;
+        }
+        mesh->computeTetrahedronBoundingBox(tId, min, max);
+        std::vector<size_t> collP = m_hashTable.getPointsInAABB(min, max);
+        assert(collP.size() >= 4);
+        if (collP.size() > 4)
+        {
+            for (size_t vId : collP)
+            {
+                //vertex does not belong to this tetrahedron
+                if (vId != vInd[0] &&
+                    vId != vInd[1] &&
+                    vId != vInd[2] &&
+                    vId != vInd[3])
+                {                    
+                    //this determines vertex belonging part of the penetration type
+                    //and gets vertex position
+                    if (vId < m_meshA->getNumVertices())
+                    {
+                        vPos = m_meshA->getVertexPosition(vId);
+                        cType = static_cast<PointTetrahedronCollisionData::CollisionType>((cType & 1) + 0);
+                    }
+                    else
+                    {
+                        vId -= m_meshA->getNumVertices();
+                        vPos = m_meshB->getVertexPosition(vId);
+                        cType = static_cast<PointTetrahedronCollisionData::CollisionType>((cType & 1) + 2);
+                    }
+
+                    TetrahedralMesh::WeightsArray bCoord; //barycentric coordinates of the vertex in tetrahedron
+                    mesh->computeBarycentricWeights(tId, vPos, bCoord);
+                    if (bCoord[0] >= -eps &&
+                        bCoord[1] >= -eps &&
+                        bCoord[2] >= -eps &&
+                        bCoord[3] >= -eps)
+                    {
+                        auto coordSum = bCoord[0] + bCoord[1] + bCoord[2] + bCoord[3];
+                        assert(coordSum <= 1 + eps2 && coordSum >= 1 - eps2);
+
+                        PointTetrahedronCollisionData ptColl = { cType, vId, tId, bCoord };
+                        m_colData.PTColData.push_back(ptColl);
+                    }
+                } //if not this tetrahedron
+            } //for vertices
+        }
+    } //for tetrahedra
+}
+
+void
+TetraToTetraCD::computeCollisionData()
+{
+    m_hashTable.clear();
+    m_hashTable.insertPoints(m_meshA->getVertexPositions());
+    m_hashTable.insertPoints(m_meshB->getVertexPositions());
+
+    m_colData.clearAll();
+    this->findCollisionsForMeshWithinHashTable(m_meshA, 0);
+    this->findCollisionsForMeshWithinHashTable(m_meshB, m_meshA->getNumVertices());
+}
+
+}

Base/Collision/CollisionDetection/imstkTetraToTetraCD.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 imstkTetrahedralMeshCD_h
+#define imstkTetrahedralMeshCD_h
+
+// std library
+#include <memory>
+
+// imstk
+#include "imstkCollisionDetection.h"
+#include "imstkTetrahedralMesh.h"
+#include "DataStructures/imstkSpatialHashTableSeparateChaining.h"
+#include "DeformModel.h"
+
+namespace imstk
+{
+
+class CollisionData;
+
+///
+/// \class TetraToTetraCD
+///
+/// \brief Base class for mesh-to-mesh collision detection
+///
+class TetraToTetraCD : public CollisionDetection
+{
+public:
+
+    ///
+    /// \brief Constructor
+    ///
+    TetraToTetraCD(std::shared_ptr<TetrahedralMesh> meshA,
+                   std::shared_ptr<TetrahedralMesh> meshB,
+                   CollisionData& colData);
+
+    ///
+    /// \brief Destructor
+    ///
+    ~TetraToTetraCD() = default;
+
+    ///
+    /// \brief Detect collision and compute collision data
+    ///
+    void computeCollisionData() override;
+
+private:
+
+    ///
+    /// \brief Processes tetrahedrons of either mesh A or B.
+    ///
+    /// This method goes through tetrahedrons of the supplied mesh (A or B),
+    /// and examines vertices currently in the hash table.
+    /// It first does rough intersection check using AABB,
+    /// and then finer check using barycentric coordinates.
+    /// Collisions are added to m_colData
+    /// Self collisions and mutual collisions between A and B are supported.
+    ///
+    /// \param mesh must be either m_meshA or m_meshB
+    /// \param idOffset must be 0 for A, and A.getNumVertices() for B
+    ///
+    void findCollisionsForMeshWithinHashTable(const std::shared_ptr<TetrahedralMesh> mesh, size_t idOffset);
+
+    std::shared_ptr<TetrahedralMesh> m_meshA;     ///> Mesh A
+    std::shared_ptr<TetrahedralMesh> m_meshB;     ///> Mesh B
+    SpatialHashTableSeparateChaining m_hashTable; ///> Spatial hash table
+};
+
+}
+
+#endif // ifndef imstkTetrahedralMeshCD_h

Base/Collision/Testing/imstkTetraToTetraCDTest.cpp

+#include "gtest/gtest.h"
+#include "gmock/gmock.h"
+
+#include  <memory>
+
+#include "imstkCollisionData.h"
+#include "imstkIsometricMap.h"
+#include "imstkMeshIO.h"
+#include "imstkTetraToTetraCD.h"
+
+using namespace imstk;
+
+class imstkTetraToTetraCDTest : public ::testing::Test
+{
+protected:
+    TetraToTetraCD *m_CD;
+};
+
+std::shared_ptr<TetrahedralMesh> loadMesh(std::string externalDataSuffix)
+{
+    std::string file = iMSTK_DATA_ROOT + externalDataSuffix;
+    std::shared_ptr<TetrahedralMesh> volMesh
+        = std::static_pointer_cast<TetrahedralMesh>(imstk::MeshIO::read(file));
+    if (!volMesh)
+    {
+        LOG(FATAL) << "Failed to read a volumetric mesh file : " << file;
+    }
+    return volMesh;
+}
+
+std::shared_ptr<TetrahedralMesh> duplicate(std::shared_ptr<TetrahedralMesh> mesh)
+{
+    return std::make_shared<TetrahedralMesh>(*mesh.get());
+}
+
+TEST_F(imstkTetraToTetraCDTest, NoSelfIntersection)
+{
+    std::shared_ptr<TetrahedralMesh> a = loadMesh("/asianDragon/asianDragon.veg");
+    auto b = std::make_shared<TetrahedralMesh>(TetrahedralMesh()); //empty mesh
+    CollisionData cd;
+
+    m_CD = new TetraToTetraCD(a, b, cd);
+    m_CD->computeCollisionData();
+    EXPECT_EQ(cd.PTColData.size(), 0);
+
+    m_CD = new TetraToTetraCD(b, a, cd);
+    m_CD->computeCollisionData();
+    EXPECT_EQ(cd.PTColData.size(), 0);
+}
+
+TEST_F(imstkTetraToTetraCDTest, IntersectionThenNoIntersection1T)
+{
+    std::shared_ptr<TetrahedralMesh> a = loadMesh("/oneTet/oneTet.veg");
+    auto b = duplicate(a);
+
+    b->translateVertices(imstk::Vec3d(0.0, 1.0, 2.5));
+
+    CollisionData cd;
+    m_CD = new TetraToTetraCD(a, b, cd);
+    m_CD->computeCollisionData();    
+    EXPECT_EQ(cd.PTColData.size(), 1);
+    EXPECT_EQ(cd.PTColData[0].collisionType, PointTetrahedronCollisionData::bPenetratingA);
+    EXPECT_EQ(cd.PTColData[0].vertexId, 0);
+    EXPECT_EQ(cd.PTColData[0].tetreahedronId, 0);
+
+    m_CD = new TetraToTetraCD(b, a, cd);
+    m_CD->computeCollisionData();
+    EXPECT_EQ(cd.PTColData.size(), 1);
+    EXPECT_EQ(cd.PTColData[0].collisionType, PointTetrahedronCollisionData::aPenetratingB);
+    EXPECT_EQ(cd.PTColData[0].vertexId, 0);
+    EXPECT_EQ(cd.PTColData[0].tetreahedronId, 0);
+
+    //now translate b more so there is no intersection
+    b->translateVertices(imstk::Vec3d(0.0, 2.0, 0.0));
+
+    m_CD = new TetraToTetraCD(b, a, cd);
+    m_CD->computeCollisionData();
+    EXPECT_EQ(cd.PTColData.size(), 0);
+
+    m_CD = new TetraToTetraCD(a, b, cd);
+    m_CD->computeCollisionData();
+    EXPECT_EQ(cd.PTColData.size(), 0);
+}
+
+TEST_F(imstkTetraToTetraCDTest, IntersectionThenNoIntersectionHuman)
+{
+    std::shared_ptr<TetrahedralMesh> a = loadMesh("/human/human.veg");
+    auto b = duplicate(a);
+
+    b->translateVertices(imstk::Vec3d(16.0, 0.0, 1.0));
+    CollisionData cd;
+
+    m_CD = new TetraToTetraCD(a, b, cd);
+    m_CD->computeCollisionData();
+    EXPECT_EQ(cd.PTColData.size(), 4);
+
+    m_CD = new TetraToTetraCD(b, a, cd);
+    m_CD->computeCollisionData();
+    EXPECT_EQ(cd.PTColData.size(), 4);
+
+    //this additional translation produces a different intersection
+    b->translateVertices(imstk::Vec3d(0.0, 0.0, 0.5));
+
+    m_CD = new TetraToTetraCD(a, b, cd);
+    m_CD->computeCollisionData();
+    EXPECT_EQ(cd.PTColData.size(), 1);
+    EXPECT_EQ(cd.PTColData[0].collisionType, PointTetrahedronCollisionData::aPenetratingB);
+    EXPECT_EQ(cd.PTColData[0].vertexId, 81);
+    EXPECT_EQ(cd.PTColData[0].tetreahedronId, 367);
+
+    m_CD = new TetraToTetraCD(b, a, cd);
+    m_CD->computeCollisionData();
+    EXPECT_EQ(cd.PTColData.size(), 1);
+    EXPECT_EQ(cd.PTColData[0].collisionType, PointTetrahedronCollisionData::bPenetratingA);
+    EXPECT_EQ(cd.PTColData[0].vertexId, 81);
+    EXPECT_EQ(cd.PTColData[0].tetreahedronId, 367);
+
+    //now translate b more so there is no intersection
+    b->translateVertices(imstk::Vec3d(0.0, 0.0, 1.0));
+    m_CD = new TetraToTetraCD(a, b, cd);
+    m_CD->computeCollisionData();
+    EXPECT_EQ(cd.PTColData.size(), 0);
+}
+
+TEST_F(imstkTetraToTetraCDTest, IntersectionOfDifferentMeshes)
+{
+    std::shared_ptr<TetrahedralMesh> a = loadMesh("/asianDragon/asianDragon.veg");
+    std::shared_ptr<TetrahedralMesh> b = loadMesh("/human/human.veg");
+
+    CollisionData cd;
+    m_CD = new TetraToTetraCD(a, b, cd);
+    m_CD->computeCollisionData();
+    EXPECT_EQ(cd.PTColData.size(), 595);
+}
+
+int imstkTetraToTetraCDTest(int argc, char* argv[])
+{
+    // Init Google Test & Mock
+    ::testing::InitGoogleTest(&argc, argv);
+
+    // Run tests with gtest
+    return RUN_ALL_TESTS();
+}

Base/Collision/imstkPbdInteractionPair.cpp

@@ -23,7 +23,7 @@
 #include "imstkSurfaceMesh.h"
 #include "imstkLineMesh.h"
 #include "imstkGeometryMap.h"
-#include "imstkCollisionDetectionUtils.h"
+#include "imstkIntersectionTestUtils.h"
 
 namespace imstk
 {

Base/Geometry/Map/imstkOneToOneMap.cpp

@@ -34,7 +34,7 @@ OneToOneMap::compute()
     }
 
     // returns the first matching vertex
-    auto findMatchingVertex = [](std::shared_ptr<Mesh> masterMesh, const Vec3d& p) -> int
+    auto findMatchingVertex = [](std::shared_ptr<Mesh> masterMesh, const Vec3d& p) -> size_t
     {
         for (size_t nodeId = 0; nodeId < masterMesh->getNumVertices(); ++nodeId)
         {
@@ -53,7 +53,7 @@ OneToOneMap::compute()
     for (size_t nodeId = 0; nodeId < meshSlave->getNumVertices(); ++nodeId)
     {
         // Find the enclosing or closest tetrahedron
-        int matchingNodeId = findMatchingVertex(meshMaster, meshSlave->getVertexPosition(nodeId));
+        size_t matchingNodeId = findMatchingVertex(meshMaster, meshSlave->getVertexPosition(nodeId));
 
         if (matchingNodeId < 0)
         {

Base/Geometry/Map/imstkTetraTriangleMap.cpp

@@ -160,7 +160,7 @@ TetraTriangleMap::findClosestTetrahedron(std::shared_ptr<TetrahedralMesh> tetraM
                                          const Vec3d& pos)
 {
     double closestDistance = MAX_D;
-    int closestTetrahedron = -1;
+    size_t closestTetrahedron = -1;
     for (size_t tetId = 0; tetId < tetraMesh->getNumTetrahedra(); ++tetId)
     {
         Vec3d center(0, 0, 0);

Base/Geometry/Mesh/imstkMesh.cpp

@@ -29,6 +29,8 @@ Mesh::initialize(const StdVectorOfVec3d& vertices)