From 91d09f6308b994af8dd3731f1ce28ec8244bf8a1 Mon Sep 17 00:00:00 2001
From: Andrew Wilson <andx_roo@live.com>
Date: Wed, 28 Jul 2021 20:25:38 -0400
Subject: [PATCH] TEST: Unit tests for MeshToMeshBruteForceCD, edge-edge
hashing to avoid duplicates.
---
.../imstkMeshToMeshBruteForceCD.cpp | 113 +++++++--
.../imstkMeshToMeshBruteForceCDTest.cpp | 215 ++++++++++++++++++
2 files changed, 314 insertions(+), 14 deletions(-)
create mode 100644 Source/CollisionDetection/Testing/imstkMeshToMeshBruteForceCDTest.cpp
diff --git a/Source/CollisionDetection/CollisionDetection/imstkMeshToMeshBruteForceCD.cpp b/Source/CollisionDetection/CollisionDetection/imstkMeshToMeshBruteForceCD.cpp
index 1a14cdec2..689d5a215 100644
--- a/Source/CollisionDetection/CollisionDetection/imstkMeshToMeshBruteForceCD.cpp
+++ b/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);
+ }
}
}
}
diff --git a/Source/CollisionDetection/Testing/imstkMeshToMeshBruteForceCDTest.cpp b/Source/CollisionDetection/Testing/imstkMeshToMeshBruteForceCDTest.cpp
new file mode 100644
index 000000000..8f653db1d
--- /dev/null
+++ b/Source/CollisionDetection/Testing/imstkMeshToMeshBruteForceCDTest.cpp
@@ -0,0 +1,215 @@
+/*=========================================================================
+
+ 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();
+}
--
GitLab