Commit 764f5696 authored by Dženan Zukić's avatar Dženan Zukić

ENH: implementing collision detection of two tetrahedral meshes using spatial hashing

parent 3c10408a
......@@ -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()
......@@ -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
};
}
......
......@@ -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
......
......@@ -109,10 +109,6 @@ SpatialHashTableSeparateChaining::getPointsInAABB(const Vec3d& corner1, const Ve
}
}
//there is little need to waste time on this reallocation,
//as this is a temporary return array
//points.shrink_to_fit();
return points;
}
......
......@@ -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;
}
......@@ -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;
......
/*=========================================================================
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());
}
}
/*=========================================================================
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
#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();
}
......@@ -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)
{
......
......@@ -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);
......
......@@ -29,6 +29,8 @@ Mesh::initialize(const StdVectorOfVec3d& vertices)
{
this->setInitialVertexPositions(vertices);
this->setVertexPositions(vertices);
StdVectorOfVec3d disp = StdVectorOfVec3d(vertices.size(), imstk::Vec3d(0.0, 0.0, 0.0));
this->setVertexDisplacements(disp);
}
void
......@@ -130,6 +132,7 @@ Mesh::setVertexDisplacements(const StdVectorOfVec3d& diff)
void
Mesh::setVertexDisplacements(const Vectord& u)
{
assert(u.size() == 3 * m_vertexDisplacements.size());
size_t dofId = 0;
for (auto &vDisp : m_vertexDisplacements)
{
......@@ -143,6 +146,19 @@ Mesh::setVertexDisplacements(const Vectord& u)
}
}
void Mesh::translateVertices(const Vec3d& t)
{
for (auto &vDisp : m_vertexDisplacements)
{
vDisp += t;
}
for (size_t i = 0; i < m_vertexPositions.size(); ++i)
{
m_vertexPositions[i] = m_initialVertexPositions[i] + m_vertexDisplacements[i];
}
}
const StdVectorOfVec3d&
Mesh::getVertexDisplacements() const
{
......
......@@ -108,6 +108,11 @@ public:
///
void setVertexDisplacements(const Vectord& u);
///
/// \brief Deep translation of vertices using the given 3D vector
///
void translateVertices(const Vec3d& t);
///
/// \brief Returns the vector displacements of mesh vertices
///
......
......@@ -180,6 +180,11 @@ if(BUILD_TESTING)
include_directories(${GoogleMock_INCLUDE_DIRS})
endif()
# External data
if(BUILD_TESTING OR BUILD_EXAMPLES )
include(imstkExternalData)
endif()
#--------------------------------------------------------------------------
# Add Source code subdirectories
#--------------------------------------------------------------------------
......
#-----------------------------------------------------------------------------
# ExternalData module
#-----------------------------------------------------------------------------
include(imstkExternalData)
#-----------------------------------------------------------------------------
# Add ExternalData
#-----------------------------------------------------------------------------
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment