Commit e4089bb3 authored by Nghia Truong's avatar Nghia Truong

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 {}
};
}
......@@ -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())
{
......@@ -388,4 +391,4 @@ Scene::advance()
this->setFPS(1. / wwt.getTimeElapsed(StopWatch::TimeUnitType::seconds));
}
} // imstk
\ No newline at end of file
} // imstk
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