Commit e4089bb3 authored by Nghia Truong's avatar Nghia Truong
Browse files

ENH: Implement collision detection for point-mesh and mesh-mesh using an...

ENH: Implement collision detection for point-mesh and mesh-mesh using an internal octree as a static member of the CollisionDetection class. In addition, unit tests for those collision detections were also added.
parent 6cb1b2ee
......@@ -21,6 +21,7 @@
#include "imstkCollisionDetection.h"
#include "imstkCollisionData.h"
#include "imstkOctreeBasedCD.h"
// Points to objects
#include "imstkPointSetToCapsuleCD.h"
......@@ -97,17 +98,23 @@ CollisionDetection::makeCollisionDetectionObject(const Type
}
case Type::PointSetToSurfaceMesh:
{
auto pointset = std::dynamic_pointer_cast<PointSet>(objA->getCollidingGeometry());
auto triMesh = std::dynamic_pointer_cast<SurfaceMesh>(objB->getCollidingGeometry());
const auto& geomA = objA->getCollidingGeometry();
const auto& geomB = objB->getCollidingGeometry();
auto pointset = std::dynamic_pointer_cast<PointSet>(geomA);
auto triMesh = std::dynamic_pointer_cast<SurfaceMesh>(geomB);
IMSTK_CHECK_FOR_VALID_GEOMETRIES(pointset, triMesh)
addCollisionPairToOctree(geomA, geomB, type, colData);
return std::make_shared<PointSetToSurfaceMeshCD>(pointset, triMesh, colData);
}
// Mesh to mesh
case Type::SurfaceMeshToSurfaceMesh:
{
auto meshA = std::dynamic_pointer_cast<SurfaceMesh>(objA->getCollidingGeometry());
auto meshB = std::dynamic_pointer_cast<SurfaceMesh>(objB->getCollidingGeometry());
const auto& geomA = objA->getCollidingGeometry();
const auto& geomB = objB->getCollidingGeometry();
auto meshA = std::dynamic_pointer_cast<SurfaceMesh>(geomA);
auto meshB = std::dynamic_pointer_cast<SurfaceMesh>(geomB);
IMSTK_CHECK_FOR_VALID_GEOMETRIES(meshA, meshB)
addCollisionPairToOctree(geomA, geomB, type, colData);
return std::make_shared<SurfaceMeshToSurfaceMeshCD>(meshA, meshB, colData);
}
case Type::SurfaceMeshToSurfaceMeshCCD:
......@@ -168,4 +175,54 @@ CollisionDetection::CollisionDetection(const CollisionDetection::Type& type, std
{
m_colData = (colData == nullptr) ? std::make_shared<CollisionData>() : colData;
}
// Static functions ==>
void
CollisionDetection::addCollisionPairToOctree(const std::shared_ptr<Geometry>& geomA,
const std::shared_ptr<Geometry>& geomB,
const Type collisionType,
const std::shared_ptr<CollisionData>& collisionData)
{
auto addToOctree =
[&](const std::shared_ptr<Geometry>& geom) {
if (!s_OctreeCD->hasGeometry(geom->getGlobalIndex()))
{
if (geom->getType() == Geometry::Type::PointSet)
{
s_OctreeCD->addPointSet(std::dynamic_pointer_cast<PointSet>(geom));
}
else if (geom->getType() == Geometry::Type::SurfaceMesh)
{
s_OctreeCD->addTriangleMesh(std::dynamic_pointer_cast<SurfaceMesh>(geom));
}
else
{
s_OctreeCD->addAnalyticalGeometry(geom);
}
}
};
addToOctree(geomA);
addToOctree(geomB);
s_OctreeCD->addCollisionPair(geomA, geomB, collisionType, collisionData);
}
void
CollisionDetection::updateInternalOctreeAndDetectCollision()
{
if (s_OctreeCD->getNumCollisionPairs() > 0)
{
s_OctreeCD->update();
s_OctreeCD->detectCollision();
}
}
void
CollisionDetection::clearInternalOctree()
{
s_OctreeCD->clear();
}
// Static octree
std::shared_ptr<OctreeBasedCD> CollisionDetection::s_OctreeCD = std::make_shared<OctreeBasedCD>(Vec3d(0, 0, 0), 100.0, 0.1, 1);
}
......@@ -26,6 +26,8 @@
namespace imstk
{
class CollidingObject;
class OctreeBasedCD;
class Geometry;
struct CollisionData;
///
......@@ -66,6 +68,9 @@ public:
///
/// \brief Static factory for collision detection sub classes
/// If the collision pair is PointSet to SurfaceMesh, or SurfaceMesh to SurfaceMesh,
/// it will be added to an internal static octree for detecting collision
/// \todo Other collision pair may be considered to use octree too
///
static std::shared_ptr<CollisionDetection> makeCollisionDetectionObject(
const Type type,
......@@ -99,8 +104,32 @@ public:
///
const std::shared_ptr<CollisionData> getCollisionData() const { return m_colData; }
///
/// \brief Update the intrernal octree, preparing for collision detection
///
static void updateInternalOctreeAndDetectCollision();
///
/// \brief Reset the internal octree, clearing all geometry data and collision pairs from it
///
static void clearInternalOctree();
protected:
Type m_type = Type::Custom; ///< Collision detection algorithm type
std::shared_ptr<CollisionData> m_colData; ///< Collision data
///
/// \brief Add the geometry into the background octree for collision detection
/// \todo Add line primitive geometry
///
static void addCollisionPairToOctree(const std::shared_ptr<Geometry>& geomA,
const std::shared_ptr<Geometry>& geomB,
const CollisionDetection::Type collisionType,
const std::shared_ptr<CollisionData>& collisionData);
/// Static octree for collision detection
/// This octree is valid throughout the lifetime of the program
/// and will serve as a background mean to detect collision between geometries
static std::shared_ptr<OctreeBasedCD> s_OctreeCD;
};
}
......@@ -29,7 +29,7 @@ namespace imstk
{
class Geometry;
class SurfaceMesh;
class CollisionData;
struct CollisionData;
///
/// \class MeshToMeshBruteForceCD
......
......@@ -184,10 +184,9 @@ OctreeBasedCD::detectCollision()
// From the second data element, check for valid and duplication
for (size_t readIdx = 1; readIdx < collisionData->VTColData.getSize(); ++readIdx)
{
const auto& vtPrevValid = collisionData->VTColData[writeIdx - 1];
const auto& vt = collisionData->VTColData[readIdx];
const auto& vt = collisionData->VTColData[readIdx];
if (pointStillColliding(vt.vertexIdx, geomIdxPointSet, geomIdxMesh)
&& vt.vertexIdx != vtPrevValid.vertexIdx)
&& (writeIdx == 0 || collisionData->VTColData[writeIdx - 1].vertexIdx != vt.vertexIdx))
{
if (readIdx != writeIdx)
{
......
......@@ -20,37 +20,24 @@
=========================================================================*/
#include "imstkPointSetToSurfaceMeshCD.h"
#include "imstkNarrowPhaseCD.h"
#include "imstkCollisionData.h"
#include "imstkParallelUtils.h"
#include "imstkPointSet.h"
#include "imstkSurfaceMesh.h"
#include "imstkOctreeBasedCD.h"
namespace imstk
{
PointSetToSurfaceMeshCD::PointSetToSurfaceMeshCD(std::shared_ptr<PointSet> pointset,
std::shared_ptr<SurfaceMesh> triMesh,
std::shared_ptr<CollisionData> colData) :
CollisionDetection(CollisionDetection::Type::PointSetToSurfaceMesh, colData),
m_pointset(pointset), m_triMesh(triMesh)
PointSetToSurfaceMeshCD::PointSetToSurfaceMeshCD(const std::shared_ptr<PointSet>& pointset,
const std::shared_ptr<SurfaceMesh>& triMesh,
const std::shared_ptr<CollisionData>& colData) :
CollisionDetection(CollisionDetection::Type::PointSetToSurfaceMesh, colData)
{
}
void
PointSetToSurfaceMeshCD::computeCollisionData()
{
m_colData->clearAll();
// This is brute force collision detection
// \todo replace by octree
ParallelUtils::parallelFor(static_cast<unsigned int>(m_pointset->getVertexPositions().size()),
[&](const unsigned int idx)
{
const auto& point = m_pointset->getVertexPosition(idx);
for (unsigned int idx2 = 0; idx2 < static_cast<unsigned int>(m_triMesh->getNumVertices()); ++idx2)
{
NarrowPhaseCD::pointToTriangle(point, idx, idx2, m_triMesh.get(), m_colData);
}
});
if (!s_OctreeCD->hasCollisionPair(pointset->getGlobalIndex(), triMesh->getGlobalIndex()))
{
addCollisionPairToOctree(std::static_pointer_cast<Geometry>(pointset),
std::static_pointer_cast<Geometry>(triMesh),
getType(),
colData);
}
}
} // imstk
......@@ -42,17 +42,15 @@ public:
///
/// \brief Constructor
///
PointSetToSurfaceMeshCD(std::shared_ptr<PointSet> pointset,
std::shared_ptr<SurfaceMesh> triMesh,
std::shared_ptr<CollisionData> colData);
PointSetToSurfaceMeshCD(const std::shared_ptr<PointSet>& pointset,
const std::shared_ptr<SurfaceMesh>& surfMesh,
const std::shared_ptr<CollisionData>& colData);
///
/// \brief Detect collision and compute collision data
/// Do nothing here, as the collision detection is performed by a static octree,
/// which is a static member of CollisionDetection class
///
void computeCollisionData() override;
private:
std::shared_ptr<PointSet> m_pointset;
std::shared_ptr<SurfaceMesh> m_triMesh;
void computeCollisionData() override {}
};
}
......@@ -23,31 +23,21 @@
#include "imstkNarrowPhaseCD.h"
#include "imstkCollisionData.h"
#include "imstkSurfaceMesh.h"
#include "imstkOctreeBasedCD.h"
namespace imstk
{
SurfaceMeshToSurfaceMeshCD::SurfaceMeshToSurfaceMeshCD(std::shared_ptr<SurfaceMesh> meshA,
std::shared_ptr<SurfaceMesh> meshB,
std::shared_ptr<CollisionData> colData) :
CollisionDetection(CollisionDetection::Type::SurfaceMeshToSurfaceMesh, colData),
m_meshA(meshA), m_meshB(meshB)
SurfaceMeshToSurfaceMeshCD::SurfaceMeshToSurfaceMeshCD(const std::shared_ptr<SurfaceMesh>& meshA,
const std::shared_ptr<SurfaceMesh>& meshB,
const std::shared_ptr<CollisionData>& colData) :
CollisionDetection(CollisionDetection::Type::SurfaceMeshToSurfaceMesh, colData)
{
}
void
SurfaceMeshToSurfaceMeshCD::computeCollisionData()
{
m_colData->clearAll();
// This is brute force collision detection
// \todo use octree
ParallelUtils::parallelFor(static_cast<unsigned int>(m_meshA->getNumTriangles()),
[&](const unsigned int idx1)
{
for (unsigned int idx2 = 0; idx2 < static_cast<unsigned int>(m_meshA->getNumTriangles()); ++idx2)
{
NarrowPhaseCD::triangleToTriangle(idx1, m_meshA.get(), idx2, m_meshB.get(), m_colData);
}
});
if (!s_OctreeCD->hasCollisionPair(meshA->getGlobalIndex(), meshB->getGlobalIndex()))
{
addCollisionPairToOctree(std::static_pointer_cast<Geometry>(meshA),
std::static_pointer_cast<Geometry>(meshB),
getType(),
colData);
}
}
} // imstk
......@@ -41,17 +41,15 @@ public:
///
/// \brief Constructor
///
SurfaceMeshToSurfaceMeshCD(std::shared_ptr<SurfaceMesh> meshA,
std::shared_ptr<SurfaceMesh> meshB,
std::shared_ptr<CollisionData> colData);
SurfaceMeshToSurfaceMeshCD(const std::shared_ptr<SurfaceMesh>& meshA,
const std::shared_ptr<SurfaceMesh>& meshB,
const std::shared_ptr<CollisionData>& colData);
///
/// \brief Detect collision and compute collision data
/// Do nothing here, as the collision detection is performed by a static octree,
/// which is a static member of CollisionDetection class
///
void computeCollisionData() override;
private:
std::shared_ptr<SurfaceMesh> m_meshA;
std::shared_ptr<SurfaceMesh> m_meshB;
void computeCollisionData() override {}
};
}
......@@ -30,6 +30,8 @@
#include "imstkNarrowPhaseCD.h"
#include "imstkOctreeBasedCD.h"
#include "imstkPointSetToSurfaceMeshCD.h"
#include "imstkSurfaceMeshToSurfaceMeshCD.h"
#include <iostream>
#include <unordered_set>
......@@ -190,7 +192,7 @@ public:
m_OctreeCD.reset(new OctreeBasedCD(Vec3d(0, 0, 0), 100.0, 0.1, 2));
}
void testPointMesh()
void testPointMeshManual()
{
reset();
const Real sphereRadius = rand01() * SPHERE_RADIUS + 0.5;
......@@ -201,12 +203,14 @@ public:
m_OctreeCD->build();
// Manually check for penetration
std::vector<int> pointPenetration(pointset->getNumVertices());
size_t numPenetrations = 0;
std::vector<int> pointPenetrations(pointset->getNumVertices());
std::vector<double> pointPenetrationDistances(pointset->getNumVertices());
size_t numPenetrations = 0;
for (uint32_t p = 0; p < pointset->getNumVertices(); ++p)
{
const auto& point = pointset->getVertexPositions()[p];
bool bPenetration = true;
const auto& point = pointset->getVertexPositions()[p];
bool bPenetration = true;
double penetrationDistance = 1e10;
for (uint32_t i = 0; i < 3; ++i)
{
if (point[i] < -0.5 || point[i] > 0.5)
......@@ -214,8 +218,11 @@ public:
bPenetration = false;
break;
}
penetrationDistance = std::min(penetrationDistance, std::abs(point[i] - 0.5));
penetrationDistance = std::min(penetrationDistance, std::abs(point[i] + 0.5));
}
pointPenetration[p] = bPenetration;
pointPenetrations[p] = bPenetration;
pointPenetrationDistances[p] = penetrationDistance;
if (bPenetration)
{
++numPenetrations;
......@@ -231,11 +238,70 @@ public:
#endif
// Compare result
EXPECT_EQ(collisionData->VTColData.getSize(), numPenetrations);
std::unordered_set<uint32_t> penetratedPoints;
for (size_t i = 0; i < collisionData->VTColData.getSize(); ++i)
{
penetratedPoints.insert(collisionData->VTColData[i].vertexIdx);
EXPECT_EQ(pointPenetrations[collisionData->VTColData[i].vertexIdx], true);
EXPECT_EQ(std::abs(pointPenetrationDistances[collisionData->VTColData[i].vertexIdx] -
collisionData->VTColData[i].closestDistance) < 1e-10, true);
}
EXPECT_EQ(penetratedPoints.size(), numPenetrations);
}
void testPointMeshUsingPointSetToSurfaceMeshCD()
{
const Real sphereRadius = rand01() * SPHERE_RADIUS + 0.5;
const auto pointset = generatePointSet(sphereRadius);
const auto mesh = generateBoxMesh();
// Manually check for penetration
std::vector<int> pointPenetrations(pointset->getNumVertices());
std::vector<double> pointPenetrationDistances(pointset->getNumVertices());
size_t numPenetrations = 0;
for (uint32_t p = 0; p < pointset->getNumVertices(); ++p)
{
const auto& point = pointset->getVertexPositions()[p];
bool bPenetration = true;
double penetrationDistance = 1e10;
for (uint32_t i = 0; i < 3; ++i)
{
if (point[i] < -0.5 || point[i] > 0.5)
{
bPenetration = false;
break;
}
penetrationDistance = std::min(penetrationDistance, std::abs(point[i] - 0.5));
penetrationDistance = std::min(penetrationDistance, std::abs(point[i] + 0.5));
}
pointPenetrations[p] = bPenetration;
pointPenetrationDistances[p] = penetrationDistance;
if (bPenetration)
{
++numPenetrations;
}
}
// Detect penetration using PointSetToSurfaceMeshCD
// Firstly we must reset the octree
CollisionDetection::clearInternalOctree();
const auto collisionData = std::make_shared<CollisionData>();
const auto CD = std::make_shared<PointSetToSurfaceMeshCD>(pointset, mesh, collisionData);
CollisionDetection::updateInternalOctreeAndDetectCollision();
#ifdef PRINT_DEBUG
std::cout << "VTColData.getSize() = " << collisionData->VTColData.getSize() << std::endl;
#endif
// Compare result
EXPECT_EQ(collisionData->VTColData.getSize(), numPenetrations);
std::unordered_set<uint32_t> penetratedPoints;
for (size_t i = 0; i < collisionData->VTColData.getSize(); ++i)
{
penetratedPoints.insert(collisionData->VTColData[i].vertexIdx);
EXPECT_EQ(pointPenetration[collisionData->VTColData[i].vertexIdx], true);
EXPECT_EQ(pointPenetrations[collisionData->VTColData[i].vertexIdx], true);
EXPECT_EQ(std::abs(pointPenetrationDistances[collisionData->VTColData[i].vertexIdx] -
collisionData->VTColData[i].closestDistance) < 1e-10, true);
}
EXPECT_EQ(penetratedPoints.size(), numPenetrations);
}
......@@ -284,7 +350,7 @@ public:
}
}
void testMeshMesh()
void testMeshMeshManual()
{
reset();
const auto mesh = generateMesh();
......@@ -353,24 +419,101 @@ public:
}
}
void testMeshMeshUsingSurfaceMeshToSurfaceMeshCD()
{
const auto mesh = generateMesh();
const auto box = generateBoxMesh();
// Brute-force check for collision
auto collisionData = std::make_shared<CollisionData>();
for (uint32_t i = 0; i < mesh->getNumTriangles(); ++i)
{
for (uint32_t j = 0; j < box->getNumTriangles(); ++j)
{
NarrowPhaseCD::triangleToTriangle(i, mesh.get(), j, box.get(), collisionData);
}
}
auto computePairHash = [](uint32_t idx1, uint32_t idx2)
{
const uint64_t first = static_cast<uint64_t>(idx1);
const uint64_t second = static_cast<uint64_t>(idx2);
const auto pair = (first << 32) | second;
return pair;
};
std::unordered_set<uint64_t> VTCollisions;
std::unordered_map<uint64_t, std::unordered_set<uint64_t>> EECollisions;
uint64_t numEECollisions = 0;
for (size_t i = 0; i < collisionData->VTColData.getSize(); ++i)
{
VTCollisions.insert(computePairHash(collisionData->VTColData[i].vertexIdx, collisionData->VTColData[i].triIdx));
}
for (size_t i = 0; i < collisionData->EEColData.getSize(); ++i)
{
const auto edgeA = computePairHash(collisionData->EEColData[i].edgeIdA.first, collisionData->EEColData[i].edgeIdA.second);
const auto edgeB = computePairHash(collisionData->EEColData[i].edgeIdB.first, collisionData->EEColData[i].edgeIdB.second);
EECollisions[edgeA].insert(edgeB);
++numEECollisions;
}
// Detect penetration using SurfaceMeshToSurfaceMeshCD
// Firstly we must reset the octree
CollisionDetection::clearInternalOctree();
const auto CD = std::make_shared<SurfaceMeshToSurfaceMeshCD>(mesh, box, collisionData);
CollisionDetection::updateInternalOctreeAndDetectCollision();
#ifdef PRINT_DEBUG
std::cout << "VTColData.getSize() = " << collisionData->VTColData.getSize() << std::endl;
std::cout << "EEColData.getSize() = " << collisionData->EEColData.getSize() << std::endl;
#endif
// Compare result
EXPECT_EQ(collisionData->VTColData.getSize(), VTCollisions.size());
EXPECT_EQ(collisionData->EEColData.getSize(), numEECollisions);
for (size_t i = 0; i < collisionData->VTColData.getSize(); ++i)
{
const auto vt = computePairHash(collisionData->VTColData[i].vertexIdx, collisionData->VTColData[i].triIdx);
EXPECT_EQ(VTCollisions.find(vt) != VTCollisions.end(), true);
}
for (size_t i = 0; i < collisionData->EEColData.getSize(); ++i)
{
const auto edgeA = computePairHash(collisionData->EEColData[i].edgeIdA.first, collisionData->EEColData[i].edgeIdA.second);
const auto edgeB = computePairHash(collisionData->EEColData[i].edgeIdB.first, collisionData->EEColData[i].edgeIdB.second);
EXPECT_EQ(EECollisions.find(edgeA) != EECollisions.end(), true);
EXPECT_EQ(EECollisions[edgeA].find(edgeB) != EECollisions[edgeA].end(), true);
}
}
protected:
std::shared_ptr<OctreeBasedCD> m_OctreeCD;
};
} // end namespace imstk
///
/// \brief Test octree update while primitives moving around randomly
/// \brief Test collision detection between pointset and surface mesh
///
TEST_F(OctreeBasedCDTest, TestPointMeshManual)
{
for (int iter = 0; iter < ITERATIONS; ++iter)
{
testPointMeshManual();
}
}
///
/// \brief Test collision detection between pointset and surface mesh
///
TEST_F(OctreeBasedCDTest, TestPointMesh)
TEST_F(OctreeBasedCDTest, TestPointMeshUsingPointSetToSurfaceMeshCD)
{
for (int iter = 0; iter < ITERATIONS; ++iter)
{
testPointMesh();
testPointMeshUsingPointSetToSurfaceMeshCD();
}
}
///
/// \brief Test octree update while primitives moving around randomly
/// \brief Test collision detection between pointset and sphere
///
TEST_F(OctreeBasedCDTest, TestPointSphere)
{
......@@ -381,13 +524,25 @@ TEST_F(OctreeBasedCDTest, TestPointSphere)
}
///
/// \brief Test octree update while primitives moving around randomly
/// \brief Test collision detection between surface mesh and surface mesh
///
TEST_F(OctreeBasedCDTest, TestMeshMeshManual)
{
for (int iter = 0; iter < ITERATIONS; ++iter)
{
testMeshMeshManual();
}
}
///
/// \brief Test collision detection between surface mesh and surface mesh
///
TEST_F(OctreeBasedCDTest, TestMeshMesh)
TEST_F(OctreeBasedCDTest, TestMeshMeshUsingSurfaceMeshToSurfaceMeshCD)
{
testMeshMeshUsingSurfaceMeshToSurfaceMeshCD();
for (int iter = 0; iter < ITERATIONS; ++iter)
{
testMeshMesh();
testMeshMeshUsingSurfaceMeshToSurfaceMeshCD();
}
}
......
......@@ -323,6 +323,9 @@ Scene::advance()
controller->updateControlledObjects();
}
// Update the static octree and perform collision detection for some collision pairs
CollisionDetection::updateInternalOctreeAndDetectCollision();
// Compute collision data per interaction pair
for (auto intPair : this->getCollisionGraph()->getInteractionPairList())