TEST: Unit tests for MeshToMeshBruteForceCD, edge-edge hashing to avoid duplicates.

Source/CollisionDetection/CollisionDetection/imstkMeshToMeshBruteForceCD.cpp

@@ -26,6 +26,79 @@
 #include "imstkSurfaceMesh.h"
 
 #include <stdint.h>
+#include <unordered_set>
+
+namespace imstk
+{
+///
+/// \brief Hash together a pair of edges
+///
+struct EdgePair
+{
+    EdgePair(uint32_t a1, uint32_t a2, uint32_t b1, uint32_t b2)
+    {
+        edgeA[0] = a1;
+        edgeA[1] = a2;
+        edgeB[0] = b1;
+        edgeB[1] = b2;
+
+        edgeAId = getIdA();
+        edgeBId = getIdB();
+    }
+
+    ///
+    /// \brief Reversible edges are equivalent, reversible vertices in the edges are equivalent as well
+    /// EdgePair(0,1,5,2)==EdgePair(1,0,5,2)==EdgePair(1,0,2,5)==...
+    ///
+    bool operator==(const EdgePair& other) const
+    {
+        return (edgeAId == other.edgeAId && edgeBId == other.edgeBId)
+               || (edgeAId == other.edgeBId && edgeBId == other.edgeAId);
+    }
+
+    // These functions return a unique int for an edge, order doesn't matter
+    // ie: f(vertexId1, vertexId2)=f(vertexId2, vertexId1)
+    const uint32_t getIdA() const
+    {
+        const uint32_t max = std::max(edgeA[0], edgeA[1]);
+        const uint32_t min = std::min(edgeA[0], edgeA[1]);
+        return max * (max + 1) / 2 + min;
+    }
+
+    const uint32_t getIdB() const
+    {
+        const uint32_t max = std::max(edgeB[0], edgeB[1]);
+        const uint32_t min = std::min(edgeB[0], edgeB[1]);
+        return max * (max + 1) / 2 + min;
+    }
+
+    uint32_t edgeA[2];
+    uint32_t edgeAId;
+    uint32_t edgeB[2];
+    uint32_t edgeBId;
+};
+}
+
+namespace std
+{
+template<>
+struct hash<imstk::EdgePair>
+{
+    // EdgePair has 4 uints to hash, they bound the same range, 0 to max vertices of a mesh
+    // A complete unique hash split into 4, would limit us to 256 max vertices so we will have
+    // collisions but they will be unlikely given small portions of the mesh are in contact at
+    // any one time
+    std::size_t operator()(const imstk::EdgePair& k) const
+    {
+        // Shift by 8 each time, there will be overlap every 256 ints
+        //return ((k.edgeA[0] ^ (k.edgeA[1] << 8)) ^ (k.edgeB[0] << 16)) ^ (k.edgeB[1] << 24);
+
+        // The edge ids are more compact since f(1,0)=f(0,1) there are fewer permutations,
+        // This should allow up to ~360 max vertices..., not that much better
+        return (k.edgeAId ^ (k.edgeBId << 16));
+    }
+};
+}
 
 namespace imstk
 {
@@ -464,6 +537,8 @@ MeshToMeshBruteForceCD::surfMeshEdgeToTriangleTest(
     std::shared_ptr<VecDataArray<int, 3>>    meshACellsPtr    = surfMeshA->getTriangleIndices();
     VecDataArray<int, 3>&                    meshACells       = *meshACellsPtr;
 
+    std::unordered_set<EdgePair> hashedEdges;
+
     const int triEdgePattern[3][2] = { { 0, 1 }, { 1, 2 }, { 2, 0 } };
     if (m_generateEdgeEdgeContacts)
     {
@@ -524,20 +599,30 @@ MeshToMeshBruteForceCD::surfMeshEdgeToTriangleTest(
 
                     if (closestTriId != -1)
                     {
-                        CellIndexElement elemA;
-                        elemA.ids[0]   = edgeA[0];
-                        elemA.ids[1]   = edgeA[1];
-                        elemA.idCount  = 2;
-                        elemA.cellType = IMSTK_EDGE;
-
-                        CellIndexElement elemB;
-                        elemB.ids[0]   = surfMeshBData.cells[closestTriId][triEdgePattern[closestEdgeId][0]];
-                        elemB.ids[1]   = surfMeshBData.cells[closestTriId][triEdgePattern[closestEdgeId][1]];
-                        elemB.idCount  = 2;
-                        elemB.cellType = IMSTK_EDGE;
-
-                        elementsA.safeAppend(elemA);
-                        elementsB.safeAppend(elemB);
+                        // Before inserting check if it already exists
+                        EdgePair edgePair(
+                            edgeA[0], edgeA[1],
+                            surfMeshBData.cells[closestTriId][triEdgePattern[closestEdgeId][0]],
+                            surfMeshBData.cells[closestTriId][triEdgePattern[closestEdgeId][1]]);
+                        if (hashedEdges.count(edgePair) == 0)
+                        {
+                            CellIndexElement elemA;
+                            elemA.ids[0]   = edgeA[0];
+                            elemA.ids[1]   = edgeA[1];
+                            elemA.idCount  = 2;
+                            elemA.cellType = IMSTK_EDGE;
+
+                            CellIndexElement elemB;
+                            elemB.ids[0]   = surfMeshBData.cells[closestTriId][triEdgePattern[closestEdgeId][0]];
+                            elemB.ids[1]   = surfMeshBData.cells[closestTriId][triEdgePattern[closestEdgeId][1]];
+                            elemB.idCount  = 2;
+                            elemB.cellType = IMSTK_EDGE;
+
+                            elementsA.safeAppend(elemA);
+                            elementsB.safeAppend(elemB);
+
+                            hashedEdges.insert(edgePair);
+                        }
                     }
                 }
             }

Source/CollisionDetection/Testing/imstkMeshToMeshBruteForceCDTest.cpp

+/*=========================================================================
+
+   Library: iMSTK
+
+   Copyright (c) Kitware, Inc. & Center for Modeling, Simulation,
+   & Imaging in Medicine, Rensselaer Polytechnic Institute.
+
+   Licensed under the Apache License, Version 2.0 (the "License");
+   you may not use this file except in compliance with the License.
+   You may obtain a copy of the License at
+
+      http://www.apache.org/licenses/LICENSE-2.0.txt
+
+   Unless required by applicable law or agreed to in writing, software
+   distributed under the License is distributed on an "AS IS" BASIS,
+   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+   See the License for the specific language governing permissions and
+   limitations under the License.
+
+=========================================================================*/
+
+#include "gtest/gtest.h"
+
+#include "imstkOrientedBox.h"
+#include "imstkSurfaceMesh.h"
+#include "imstkMeshToMeshBruteForceCD.h"
+#include "imstkGeometryUtilities.h"
+
+using namespace imstk;
+
+///
+/// \brief TODO
+///
+class imstkMeshToMeshBruteForceCDTest : public ::testing::Test
+{
+protected:
+    MeshToMeshBruteForceCD m_meshToMeshBruteForceCD;
+};
+
+TEST_F(imstkMeshToMeshBruteForceCDTest, IntersectionTestAB_EdgeToEdge)
+{
+    // Create two cubes
+    auto box1 = std::make_shared<OrientedBox>(Vec3d::Zero(), Vec3d(0.5, 0.5, 0.5), Quatd::Identity());
+    auto box2 = std::make_shared<OrientedBox>(Vec3d::Zero(), Vec3d(0.4, 0.4, 0.4), Quatd::Identity());
+
+    std::shared_ptr<SurfaceMesh> box1Mesh = GeometryUtils::toSurfaceMesh(box1);
+    std::shared_ptr<SurfaceMesh> box2Mesh = GeometryUtils::toSurfaceMesh(box2);
+    box2Mesh->rotate(Vec3d(0.0, 0.0, 1.0), PI_2 * 0.5);
+    box2Mesh->rotate(Vec3d(1.0, 0.0, 0.0), PI_2 * 0.5);
+    box2Mesh->translate(Vec3d(0.0, 0.8, 0.8));
+    box2Mesh->updatePostTransformData();
+
+    m_meshToMeshBruteForceCD.setInput(box1Mesh, 0);
+    m_meshToMeshBruteForceCD.setInput(box2Mesh, 1);
+    m_meshToMeshBruteForceCD.setGenerateCD(true, true); // Generate both A and B
+    m_meshToMeshBruteForceCD.setGenerateEdgeEdgeContacts(true);
+    m_meshToMeshBruteForceCD.update();
+
+    std::shared_ptr<CollisionData> colData = m_meshToMeshBruteForceCD.getCollisionData();
+
+    // Check for a single edge vs edge
+    ASSERT_EQ(1, colData->elementsA.getSize());
+    ASSERT_EQ(1, colData->elementsB.getSize());
+
+    EXPECT_EQ(CollisionElementType::CellIndex, colData->elementsA[0].m_type);
+    EXPECT_EQ(CollisionElementType::CellIndex, colData->elementsB[0].m_type);
+
+    EXPECT_EQ(colData->elementsA[0].m_element.m_CellIndexElement.cellType, IMSTK_EDGE);
+    EXPECT_EQ(colData->elementsB[0].m_element.m_CellIndexElement.cellType, IMSTK_EDGE);
+
+    EXPECT_EQ(colData->elementsA[0].m_element.m_CellIndexElement.idCount, 2);
+    EXPECT_EQ(colData->elementsB[0].m_element.m_CellIndexElement.idCount, 2);
+}
+
+TEST_F(imstkMeshToMeshBruteForceCDTest, IntersectionTestAB_VertexToTriangle)
+{
+    // Create triangle on z plane
+    auto triMesh = std::make_shared<SurfaceMesh>();
+    {
+        auto verticesPtr = std::make_shared<VecDataArray<double, 3>>(3);
+        (*verticesPtr)[0] = Vec3d(0.5, 0.0, -0.5);
+        (*verticesPtr)[1] = Vec3d(-0.5, 0.0, -0.5);
+        (*verticesPtr)[2] = Vec3d(0.0, 0.0, 0.5);
+        auto indicesPtr = std::make_shared<VecDataArray<int, 3>>(1);
+        (*indicesPtr)[0] = Vec3i(0, 1, 2);
+        triMesh->initialize(verticesPtr, indicesPtr);
+    }
+
+    // Create a test PointSet that causes this vertex to be closest to the face of the triangle
+    auto vertexMesh = std::make_shared<PointSet>();
+    {
+        auto verticesPtr = std::make_shared<VecDataArray<double, 3>>(1);
+        (*verticesPtr)[0] = Vec3d(0.0, -1.0, 0.0);
+        vertexMesh->initialize(verticesPtr);
+    }
+
+    m_meshToMeshBruteForceCD.setInput(triMesh, 0);
+    m_meshToMeshBruteForceCD.setInput(vertexMesh, 1);
+    m_meshToMeshBruteForceCD.setGenerateCD(true, true); // Generate both A and B
+    m_meshToMeshBruteForceCD.setGenerateEdgeEdgeContacts(true);
+    m_meshToMeshBruteForceCD.update();
+
+    std::shared_ptr<CollisionData> colData = m_meshToMeshBruteForceCD.getCollisionData();
+
+    // Check for a single vertex-triangle case
+    ASSERT_EQ(1, colData->elementsA.getSize());
+    ASSERT_EQ(1, colData->elementsB.getSize());
+
+    EXPECT_EQ(CollisionElementType::CellIndex, colData->elementsA[0].m_type);
+    EXPECT_EQ(CollisionElementType::CellIndex, colData->elementsB[0].m_type);
+
+    EXPECT_EQ(colData->elementsA[0].m_element.m_CellIndexElement.cellType, IMSTK_TRIANGLE);
+    EXPECT_EQ(colData->elementsB[0].m_element.m_CellIndexElement.cellType, IMSTK_VERTEX);
+
+    EXPECT_EQ(colData->elementsA[0].m_element.m_CellIndexElement.idCount, 3);
+    EXPECT_EQ(colData->elementsB[0].m_element.m_CellIndexElement.idCount, 1);
+}
+
+TEST_F(imstkMeshToMeshBruteForceCDTest, IntersectionTestAB_VertexToVertex)
+{
+    // Create triangle on z plane
+    auto triMesh = std::make_shared<SurfaceMesh>();
+    {
+        auto verticesPtr = std::make_shared<VecDataArray<double, 3>>(3);
+        (*verticesPtr)[0] = Vec3d(0.5, 0.0, -0.5);
+        (*verticesPtr)[1] = Vec3d(-0.5, 0.0, -0.5);
+        (*verticesPtr)[2] = Vec3d(0.0, 0.0, 0.5);
+        auto indicesPtr = std::make_shared<VecDataArray<int, 3>>(1);
+        (*indicesPtr)[0] = Vec3i(0, 1, 2);
+        triMesh->initialize(verticesPtr, indicesPtr);
+    }
+
+    // Create a test PointSet that causes this vertex to be closest to the first vertex of the triangle
+    auto vertexMesh = std::make_shared<PointSet>();
+    {
+        auto verticesPtr = std::make_shared<VecDataArray<double, 3>>(1);
+        (*verticesPtr)[0] = Vec3d(0.5, -1.0, -0.5);
+        vertexMesh->initialize(verticesPtr);
+    }
+
+    m_meshToMeshBruteForceCD.setInput(triMesh, 0);
+    m_meshToMeshBruteForceCD.setInput(vertexMesh, 1);
+    m_meshToMeshBruteForceCD.setGenerateCD(true, true); // Generate both A and B
+    m_meshToMeshBruteForceCD.setGenerateEdgeEdgeContacts(true);
+    m_meshToMeshBruteForceCD.update();
+
+    std::shared_ptr<CollisionData> colData = m_meshToMeshBruteForceCD.getCollisionData();
+
+    // Check for a single vertex-triangle case
+    ASSERT_EQ(1, colData->elementsA.getSize());
+    ASSERT_EQ(1, colData->elementsB.getSize());
+
+    EXPECT_EQ(CollisionElementType::CellIndex, colData->elementsA[0].m_type);
+    EXPECT_EQ(CollisionElementType::CellIndex, colData->elementsB[0].m_type);
+
+    EXPECT_EQ(colData->elementsA[0].m_element.m_CellIndexElement.cellType, IMSTK_VERTEX);
+    EXPECT_EQ(colData->elementsB[0].m_element.m_CellIndexElement.cellType, IMSTK_VERTEX);
+
+    EXPECT_EQ(colData->elementsA[0].m_element.m_CellIndexElement.idCount, 1);
+    EXPECT_EQ(colData->elementsB[0].m_element.m_CellIndexElement.idCount, 1);
+}
+
+TEST_F(imstkMeshToMeshBruteForceCDTest, IntersectionTestAB_VertexToEdge)
+{
+    // Create triangle on z plane
+    auto triMesh = std::make_shared<SurfaceMesh>();
+    {
+        auto verticesPtr = std::make_shared<VecDataArray<double, 3>>(3);
+        (*verticesPtr)[0] = Vec3d(0.5, 0.0, -0.5);
+        (*verticesPtr)[1] = Vec3d(-0.5, 0.0, -0.5);
+        (*verticesPtr)[2] = Vec3d(0.0, 0.0, 0.5);
+        auto indicesPtr = std::make_shared<VecDataArray<int, 3>>(1);
+        (*indicesPtr)[0] = Vec3i(0, 1, 2);
+        triMesh->initialize(verticesPtr, indicesPtr);
+    }
+
+    // Create a test PointSet that causes this vertex to be closest to the edge of a cube
+    auto vertexMesh = std::make_shared<PointSet>();
+    {
+        auto verticesPtr = std::make_shared<VecDataArray<double, 3>>(1);
+        (*verticesPtr)[0] = Vec3d(0.0, -1.0, -0.5);
+        vertexMesh->initialize(verticesPtr);
+    }
+
+    m_meshToMeshBruteForceCD.setInput(triMesh, 0);
+    m_meshToMeshBruteForceCD.setInput(vertexMesh, 1);
+    m_meshToMeshBruteForceCD.setGenerateCD(true, true); // Generate both A and B
+    m_meshToMeshBruteForceCD.setGenerateEdgeEdgeContacts(true);
+    m_meshToMeshBruteForceCD.update();
+
+    std::shared_ptr<CollisionData> colData = m_meshToMeshBruteForceCD.getCollisionData();
+
+    // Check for a single vertex-triangle case
+    ASSERT_EQ(1, colData->elementsA.getSize());
+    ASSERT_EQ(1, colData->elementsB.getSize());
+
+    EXPECT_EQ(CollisionElementType::CellIndex, colData->elementsA[0].m_type);
+    EXPECT_EQ(CollisionElementType::CellIndex, colData->elementsB[0].m_type);
+
+    EXPECT_EQ(colData->elementsA[0].m_element.m_CellIndexElement.cellType, IMSTK_EDGE);
+    EXPECT_EQ(colData->elementsB[0].m_element.m_CellIndexElement.cellType, IMSTK_VERTEX);
+
+    EXPECT_EQ(colData->elementsA[0].m_element.m_CellIndexElement.idCount, 2);
+    EXPECT_EQ(colData->elementsB[0].m_element.m_CellIndexElement.idCount, 1);
+}
+
+int
+imstkMeshToMeshBruteForceCDTest(int argc, char* argv[])
+{
+    // Init Google Test & Mock
+    ::testing::InitGoogleTest(&argc, argv);
+
+    // Run tests with gtest
+    return RUN_ALL_TESTS();
+}