Skip to content
GitLab
Projects
Groups
Snippets
/
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in / Register
Toggle navigation
Menu
Open sidebar
iMSTK
iMSTK
Commits
91d09f63
Commit
91d09f63
authored
Jul 28, 2021
by
Andrew Wilson
🐘
Committed by
Andrew Wilson
Aug 03, 2021
Browse files
TEST: Unit tests for MeshToMeshBruteForceCD, edge-edge hashing to avoid duplicates.
parent
3d66ae7c
Changes
2
Hide whitespace changes
Inline
Side-by-side
Source/CollisionDetection/CollisionDetection/imstkMeshToMeshBruteForceCD.cpp
View file @
91d09f63
...
...
@@ -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
0 → 100644
View file @
91d09f63
/*=========================================================================
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
();
}
Write
Preview
Supports
Markdown
0%
Try again
or
attach a new file
.
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment