Commit 700011a0 authored by Nghia Truong's avatar Nghia Truong
Browse files

ENH: Rewrite collision detection classes, moving their code to a static...

ENH: Rewrite collision detection classes, moving their code to a static namespace called "NarrowPhaseCD". Also add new collision detection classes, such as "PointSetToVolumeMesh" and "SurfaceMeshToSurfaceMesh"
parent 9fb1f933
...@@ -20,51 +20,24 @@ ...@@ -20,51 +20,24 @@
=========================================================================*/ =========================================================================*/
#include "imstkBidirectionalPlaneToSphereCD.h" #include "imstkBidirectionalPlaneToSphereCD.h"
#include "imstkNarrowPhaseCD.h"
#include "imstkCollidingObject.h"
#include "imstkCollisionData.h" #include "imstkCollisionData.h"
#include "imstkPlane.h"
#include "imstkSphere.h"
#include <g3log/g3log.hpp>
namespace imstk namespace imstk
{ {
BidirectionalPlaneToSphereCD::BidirectionalPlaneToSphereCD(std::shared_ptr<Plane> planeA,
std::shared_ptr<Sphere> sphereB,
std::shared_ptr<CollisionData> colData) :
CollisionDetection(CollisionDetection::Type::BidirectionalPlaneToSphere, colData),
m_planeA(planeA),
m_sphereB(sphereB)
{
}
void void
BidirectionalPlaneToSphere::computeCollisionData() BidirectionalPlaneToSphereCD::computeCollisionData()
{ {
// Clear collisionData
m_colData->clearAll(); m_colData->clearAll();
NarrowPhaseCD::bidirectionalPlaneToSphere(m_planeA.get(), m_sphereB.get(), m_colData);
// Get geometry properties
Vec3d sphereBPos = m_sphereB->getPosition();
double r = m_sphereB->getRadius() * m_sphereB->getScaling();
Vec3d planeAPos = m_planeA->getPosition();
Vec3d n = m_planeA->getNormal();
// Compute shortest distance
double d = (sphereBPos - planeAPos).dot(n);
// Define sphere to plane direction
Vec3d dirAToB = n;
if (d < 0)
{
d = -d;
dirAToB = -n;
}
// Return if no penetration
double penetrationDepth = r - d;
if (penetrationDepth <= 0)
{
return;
}
// Compute collision points
Vec3d planeAColPt = sphereBPos - dirAToB * d;
Vec3d sphereBColPt = sphereBPos - dirAToB * r;
// Set collisionData
m_colData->PDColData.push_back({ planeAColPt, sphereBColPt, dirAToB, penetrationDepth });
}
} }
} // namespace imstk
...@@ -22,39 +22,29 @@ ...@@ -22,39 +22,29 @@
#pragma once #pragma once
#include <memory> #include <memory>
#include "imstkCollisionDetection.h" #include "imstkCollisionDetection.h"
namespace imstk namespace imstk
{ {
class Plane; class Plane;
class Sphere; class Sphere;
class CollisionData; class Geometry;
struct CollisionData;
/// ///
/// \class PlaneToSphereCD /// \class PlaneToSphereCD
/// ///
/// \brief Plane to sphere collision detection /// \brief Plane to sphere collision detection
/// ///
class BidirectionalPlaneToSphere : public CollisionDetection class BidirectionalPlaneToSphereCD : public CollisionDetection
{ {
public: public:
/// ///
/// \brief Constructor /// \brief Constructor
/// ///
BidirectionalPlaneToSphere(std::shared_ptr<Plane> planeA, BidirectionalPlaneToSphereCD(std::shared_ptr<Plane> planeA,
std::shared_ptr<Sphere> sphereB, std::shared_ptr<Sphere> sphereB,
std::shared_ptr<CollisionData> colData) : std::shared_ptr<CollisionData> colData);
CollisionDetection(CollisionDetection::Type::BidirectionalPlaneToSphere, colData),
m_planeA(planeA),
m_sphereB(sphereB)
{}
///
/// \brief Destructor
///
~BidirectionalPlaneToSphere() = default;
/// ///
/// \brief Detect collision and compute collision data /// \brief Detect collision and compute collision data
...@@ -62,8 +52,7 @@ public: ...@@ -62,8 +52,7 @@ public:
void computeCollisionData() override; void computeCollisionData() override;
private: private:
std::shared_ptr<Plane> m_planeA;
std::shared_ptr<Plane> m_planeA; ///> std::shared_ptr<Sphere> m_sphereB;
std::shared_ptr<Sphere> m_sphereB; ///>
}; };
} } // namespace imstk
...@@ -20,186 +20,152 @@ ...@@ -20,186 +20,152 @@
=========================================================================*/ =========================================================================*/
#include "imstkCollisionDetection.h" #include "imstkCollisionDetection.h"
#include "imstkCollisionData.h"
#include "imstkUnidirectionalPlaneToSphereCD.h" // Points to objects
#include "imstkBidirectionalPlaneToSphereCD.h" #include "imstkPointSetToCapsuleCD.h"
#include "imstkSphereToSphereCD.h"
#include "imstkPointSetToSphereCD.h" #include "imstkPointSetToSphereCD.h"
#include "imstkPointSetToPlaneCD.h" #include "imstkPointSetToPlaneCD.h"
#include "imstkMeshToMeshCD.h" #include "imstkPointSetToVolumeMeshCD.h"
#include "imstkSphereCylinderCD.h"
#include "imstkPointSetToSpherePickingCD.h" #include "imstkPointSetToSpherePickingCD.h"
#include "imstkMeshToMeshBruteforceCD.h"
// Mesh to mesh
#include "imstkSurfaceMeshToSurfaceMeshCD.h"
#include "imstkSurfaceMeshToSurfaceMeshCCD.h"
#include "imstkTetraToTetraCD.h"
// Analytical object to analytical object
#include "imstkUnidirectionalPlaneToSphereCD.h"
#include "imstkBidirectionalPlaneToSphereCD.h"
#include "imstkSphereToCylinderCD.h"
#include "imstkSphereToSphereCD.h"
#include "imstkCollidingObject.h" #include "imstkCollidingObject.h"
#include "imstkPlane.h" #include "imstkPlane.h"
#include "imstkSphere.h" #include "imstkSphere.h"
#include "imstkCylinder.h"
#include "imstkCapsule.h"
#include "imstkSurfaceMesh.h" #include "imstkSurfaceMesh.h"
#include "imstkTetrahedralMesh.h"
#include <g3log/g3log.hpp> #include <g3log/g3log.hpp>
#define IMSTK_CHECK_GEOMETRIES(obj1, obj2) \
LOG_IF(FATAL, (obj1 == nullptr || obj2 == nullptr)) << \
"CollisionDetection::makeCollisionDetectionObject() error: " << \
"Invalid object geometries were provided for the given collision detection type.\n" << \
" Line: " << __LINE__ << ", File: " << __FILE__;
namespace imstk namespace imstk
{ {
std::shared_ptr<CollisionDetection> std::shared_ptr<CollisionDetection>
CollisionDetection::makeCollisionDetectionObject(const Type& type, CollisionDetection::makeCollisionDetectionObject(const Type type,
std::shared_ptr<CollidingObject> objA, std::shared_ptr<CollidingObject> objA,
std::shared_ptr<CollidingObject> objB, std::shared_ptr<CollidingObject> objB,
std::shared_ptr<CollisionData> colData) std::shared_ptr<CollisionData> colData)
{ {
switch (type) switch (type)
{ {
case Type::UnidirectionalPlaneToSphere: // Points to objects
case Type::PointSetToSphere:
{ {
auto plane = std::dynamic_pointer_cast<Plane>(objA->getCollidingGeometry()); auto pointset = std::dynamic_pointer_cast<PointSet>(objA->getCollidingGeometry());
auto sphere = std::dynamic_pointer_cast<Sphere>(objB->getCollidingGeometry()); auto sphere = std::dynamic_pointer_cast<Sphere>(objB->getCollidingGeometry());
IMSTK_CHECK_GEOMETRIES(pointset, sphere)
// Geometries check return std::make_shared<PointSetToSphereCD>(pointset, sphere, colData);
if (plane == nullptr || sphere == nullptr)
{
LOG(WARNING) << "CollisionDetection::make_collision_detection error: "
<< "invalid object geometries for UnidirectionalPlaneToSphere collision detection.";
return nullptr;
}
return std::make_shared<UnidirectionalPlaneToSphereCD>(plane, sphere, colData);
} }
break; case Type::PointSetToPlane:
case Type::BidirectionalPlaneToSphere:
{ {
auto plane = std::dynamic_pointer_cast<Plane>(objA->getCollidingGeometry()); auto pointset = std::dynamic_pointer_cast<PointSet>(objA->getCollidingGeometry());
auto sphere = std::dynamic_pointer_cast<Sphere>(objB->getCollidingGeometry()); auto plane = std::dynamic_pointer_cast<Plane>(objB->getCollidingGeometry());
IMSTK_CHECK_GEOMETRIES(pointset, plane)
// Geometries check return std::make_shared<PointSetToPlaneCD>(pointset, plane, colData);
if (plane == nullptr || sphere == nullptr)
{
LOG(WARNING) << "CollisionDetection::make_collision_detection error: "
<< "invalid object geometries for BidirectionalPlaneToSphere collision detection.";
return nullptr;
}
return std::make_shared<BidirectionalPlaneToSphere>(plane, sphere, colData);
} }
break; case Type::PointSetToCapsule:
case Type::SphereToSphere:
{ {
auto sphereA = std::dynamic_pointer_cast<Sphere>(objA->getCollidingGeometry()); auto pointset = std::dynamic_pointer_cast<PointSet>(objA->getCollidingGeometry());
auto sphereB = std::dynamic_pointer_cast<Sphere>(objB->getCollidingGeometry()); auto capsule = std::dynamic_pointer_cast<Capsule>(objB->getCollidingGeometry());
IMSTK_CHECK_GEOMETRIES(pointset, capsule)
// Geometries check return std::make_shared<PointSetToCapsuleCD>(pointset, capsule, colData);
if (sphereA == nullptr || sphereB == nullptr)
{
LOG(WARNING) << "CollisionDetection::make_collision_detection error: "
<< "invalid object geometries for SphereToSphere collision detection.";
return nullptr;
}
return std::make_shared<SphereToSphereCD>(sphereA, sphereB, colData);
} }
break; case Type::PointSetToSpherePicking:
case Type::SphereToCylinder:
{ {
auto pointset = std::dynamic_pointer_cast<PointSet>(objA->getCollidingGeometry());
auto sphere = std::dynamic_pointer_cast<Sphere>(objB->getCollidingGeometry()); auto sphere = std::dynamic_pointer_cast<Sphere>(objB->getCollidingGeometry());
auto cylinder = std::dynamic_pointer_cast<Cylinder>(objA->getCollidingGeometry()); IMSTK_CHECK_GEOMETRIES(pointset, sphere)
return std::make_shared<PointSetToSpherePickingCD>(pointset, sphere, colData);
// Geometries check
if (sphere == nullptr || cylinder == nullptr)
{
LOG(WARNING) << "CollisionDetection::make_collision_detection error: "
<< "invalid object geometries for SphereToCylinder collision detection.";
return nullptr;
}
return std::make_shared<SphereCylinderCD>(sphere, cylinder, colData);
} }
break; case Type::PointSetToVolumeMesh:
case Type::PointSetToSphere:
{ {
auto mesh = std::dynamic_pointer_cast<PointSet>(objA->getCollidingGeometry()); auto pointset = std::dynamic_pointer_cast<PointSet>(objA->getCollidingGeometry());
auto sphere = std::dynamic_pointer_cast<Sphere>(objB->getCollidingGeometry()); auto triMesh = std::dynamic_pointer_cast<SurfaceMesh>(objB->getCollidingGeometry());
IMSTK_CHECK_GEOMETRIES(pointset, triMesh)
// Geometries check return std::make_shared<PointSetToVolumeMeshCD>(pointset, triMesh, colData);
if (mesh == nullptr || sphere == nullptr)
{
LOG(WARNING) << "CollisionDetection::make_collision_detection error: "
<< "invalid object geometries for SphereToSphere collision detection.";
return nullptr;
}
return std::make_shared<PointSetToSphereCD>(mesh, sphere, colData);
} }
break; // Mesh to mesh
case Type::PointSetToPlane: case Type::SurfaceMeshToSurfaceMesh:
{ {
auto mesh = std::dynamic_pointer_cast<PointSet>(objA->getCollidingGeometry()); auto meshA = std::dynamic_pointer_cast<SurfaceMesh>(objA->getCollidingGeometry());
auto plane = std::dynamic_pointer_cast<Plane>(objB->getCollidingGeometry()); auto meshB = std::dynamic_pointer_cast<SurfaceMesh>(objB->getCollidingGeometry());
IMSTK_CHECK_GEOMETRIES(meshA, meshB)
// Geometries check return std::make_shared<SurfaceMeshToSurfaceMeshCD>(meshA, meshB, colData);
if (mesh == nullptr || plane == nullptr)
{
LOG(WARNING) << "CollisionDetection::make_collision_detection error: "
<< "invalid object geometries for SphereToSphere collision detection.";
return nullptr;
}
return std::make_shared<PointSetToPlaneCD>(mesh, plane, colData);
} }
break; case Type::SurfaceMeshToSurfaceMeshCCD:
case Type::MeshToMesh:
{ {
auto meshA = std::dynamic_pointer_cast<SurfaceMesh>(objA->getCollidingGeometry()); auto meshA = std::dynamic_pointer_cast<SurfaceMesh>(objA->getCollidingGeometry());
auto meshB = std::dynamic_pointer_cast<SurfaceMesh>(objB->getCollidingGeometry()); auto meshB = std::dynamic_pointer_cast<SurfaceMesh>(objB->getCollidingGeometry());
IMSTK_CHECK_GEOMETRIES(meshA, meshB)
return std::make_shared<SurfaceMeshToSurfaceMeshCCD>(meshA, meshB, colData);
}
// Geometries check case Type::VolumeMeshToVolumeMesh:
if (meshA == nullptr || meshB == nullptr) {
{ auto tet1 = std::dynamic_pointer_cast<TetrahedralMesh>(objA->getCollidingGeometry());
LOG(WARNING) << "CollisionDetection::make_collision_detection error: " auto tet2 = std::dynamic_pointer_cast<TetrahedralMesh>(objB->getCollidingGeometry());
<< "invalid object geometries for MeshToMesh collision detection."; IMSTK_CHECK_GEOMETRIES(tet1, tet2)
return nullptr; return std::make_shared<TetraToTetraCD>(tet1, tet2, colData);
}
return std::make_shared<MeshToMeshCD>(meshA, meshB, colData);
} }
break;
case Type::PointSetToSpherePicking: // Analytical object to analytical object
case Type::UnidirectionalPlaneToSphere:
{ {
auto mesh = std::dynamic_pointer_cast<PointSet>(objA->getCollidingGeometry()); auto plane = std::dynamic_pointer_cast<Plane>(objA->getCollidingGeometry());
auto sphere = std::dynamic_pointer_cast<Sphere>(objB->getCollidingGeometry()); auto sphere = std::dynamic_pointer_cast<Sphere>(objB->getCollidingGeometry());
IMSTK_CHECK_GEOMETRIES(plane, sphere)
// Geometries check return std::make_shared<UnidirectionalPlaneToSphereCD>(plane, sphere, colData);
if (mesh == nullptr || sphere == nullptr)
{
LOG(WARNING) << "CollisionDetection::make_collision_detection error: "
<< "invalid object geometries for SphereToSphere collision detection.";
return nullptr;
}
return std::make_shared<PointSetToSpherePickingCD>(mesh, sphere, colData);
} }
break; case Type::BidirectionalPlaneToSphere:
case Type::MeshToMeshBruteForce:
{ {
auto geometry1 = std::dynamic_pointer_cast<Geometry>(objA->getCollidingGeometry()); auto plane = std::dynamic_pointer_cast<Plane>(objA->getCollidingGeometry());
auto surfaceGeo = std::dynamic_pointer_cast<SurfaceMesh>(objB->getCollidingGeometry()); auto sphere = std::dynamic_pointer_cast<Sphere>(objB->getCollidingGeometry());
IMSTK_CHECK_GEOMETRIES(plane, sphere)
// Geometries check return std::make_shared<BidirectionalPlaneToSphereCD>(plane, sphere, colData);
if (geometry1 == nullptr || surfaceGeo == nullptr)
{
LOG(WARNING) << "CollisionDetection::make_collision_detection error: "
<< "invalid object geometries for MeshToMeshBruteForce collision detection.";
return nullptr;
}
return std::make_shared<MeshToMeshBruteForceCD>(geometry1, surfaceGeo, colData);
} }
break; case Type::SphereToSphere:
{
auto sphereA = std::dynamic_pointer_cast<Sphere>(objA->getCollidingGeometry());
auto sphereB = std::dynamic_pointer_cast<Sphere>(objB->getCollidingGeometry());
IMSTK_CHECK_GEOMETRIES(sphereA, sphereB)
return std::make_shared<SphereToSphereCD>(sphereA, sphereB, colData);
}
case Type::SphereToCylinder:
{
auto sphere = std::dynamic_pointer_cast<Sphere>(objB->getCollidingGeometry());
auto cylinder = std::dynamic_pointer_cast<Cylinder>(objA->getCollidingGeometry());
IMSTK_CHECK_GEOMETRIES(sphere, cylinder)
return std::make_shared<SphereToCylinderCD>(sphere, cylinder, colData);
}
default: default:
{ {
LOG(WARNING) << "CollisionDetection::make_collision_detection error: type not implemented."; LOG(FATAL) << "CollisionDetection::makeCollisionDetectionObject error: type not implemented.";
return nullptr; return nullptr;
} }
} }
} }
const CollisionDetection::Type& CollisionDetection::CollisionDetection(const CollisionDetection::Type& type, std::shared_ptr<CollisionData> colData) : m_type(type)
CollisionDetection::getType() const
{
return m_type;
}
const std::shared_ptr<CollisionData>
CollisionDetection::getCollisionData() const
{ {
return m_colData; m_colData = (colData == nullptr) ? std::make_shared<CollisionData>() : colData;
} }
} }
...@@ -21,15 +21,12 @@ ...@@ -21,15 +21,12 @@
#pragma once #pragma once
#include "imstkCollisionData.h"
#include <memory> #include <memory>
namespace imstk namespace imstk
{ {
class CollidingObject; class CollidingObject;
//class CollisionData; struct CollisionData;
//class CollisionData::CollisionData;
/// ///
/// \class CollisionDetection /// \class CollisionDetection
...@@ -45,48 +42,45 @@ public: ...@@ -45,48 +42,45 @@ public:
/// ///
enum class Type enum class Type
{ {
UnidirectionalPlaneToSphere, // Points to objects
BidirectionalPlaneToSphere,
SphereToCylinder,
SphereToSphere,
PointSetToSphere, PointSetToSphere,
PointSetToPlane, PointSetToPlane,
MeshToMesh,
PointSetToCapsule, PointSetToCapsule,
PointSetToSpherePicking, PointSetToSpherePicking,
PointSetToVolumeMesh,
// Mesh to mesh (mesh to analytical object = mesh vertices to analytical object)
SurfaceMeshToSurfaceMesh,
SurfaceMeshToSurfaceMeshCCD,
VolumeMeshToVolumeMesh,
MeshToMeshBruteForce, MeshToMeshBruteForce,
// Analytical object to analytical object
UnidirectionalPlaneToSphere,
BidirectionalPlaneToSphere,
SphereToCylinder,
SphereToSphere,
Custom Custom
}; };
/// ///
/// \brief Static factory for collision detection sub classes /// \brief Static factory for collision detection sub classes
/// ///
static std::shared_ptr<CollisionDetection> makeCollisionDetectionObject(const Type& type, static std::shared_ptr<CollisionDetection> makeCollisionDetectionObject(
std::shared_ptr<CollidingObject> objA, const Type type,
std::shared_ptr<CollidingObject> objB, std::shared_ptr<CollidingObject> objA,
std::shared_ptr<CollisionData> colData); std::shared_ptr<CollidingObject> objB,
std::shared_ptr<CollisionData> colData);
/// ///
/// \brief Constructor /// \brief Constructor
/// ///
CollisionDetection(const Type& type, std::shared_ptr<CollisionData> colData) : CollisionDetection(const Type& type, std::shared_ptr<CollisionData> colData);
m_type(type)//,
//m_colData(colData)
{
if (colData == nullptr)
{
m_colData = std::make_shared<CollisionData>();
}
else
{
m_colData = colData;
}
}
CollisionDetection() = delete; CollisionDetection() = delete;
/// ///
/// \brief Destructor /// \brief Destructor for base class
/// ///
virtual ~CollisionDetection() = default; virtual ~CollisionDetection() = default;
...@@ -98,15 +92,14 @@ public: ...@@ -98,15 +92,14 @@ public: