Commit c2f5edb6 authored by Sreekanth Arikatla's avatar Sreekanth Arikatla

Merge branch 'RefactorCollisionDetection' into 'master'

Refactor Collision Detection

See merge request iMSTK/iMSTK!346
parents 7bdfdbb5 a2b90a8a
......@@ -129,7 +129,7 @@ main()
// Compute the scale factor to scale meshes such that meshes with different sizes are still visualized consistently
Vec3d lowerCorner, upperCorner;
const auto pointset = std::dynamic_pointer_cast<PointSet>(triMeshes.front());
ParallelUtils::ParallelReduce::findAABB(pointset->getVertexPositions(), lowerCorner, upperCorner);
ParallelUtils::findAABB(pointset->getVertexPositions(), lowerCorner, upperCorner);
const auto scaleFactor = 20.0 / (upperCorner - lowerCorner).norm();
for (const auto& mesh: triMeshes)
{
......@@ -197,7 +197,7 @@ main()
}
Vec3d lowerCorners, upperCorner;
ParallelUtils::ParallelReduce::findAABB(centers, lowerCorners, upperCorner);
ParallelUtils::findAABB(centers, lowerCorners, upperCorner);
if ((lowerCorners - upperCorner).norm() > 70.0)
{
for (size_t i = 0; i < dirs.size(); ++i)
......
......@@ -23,52 +23,121 @@
#include <array>
// imstk
#include "imstkMath.h"
#include "imstkLogUtility.h"
#include "imstkParallelUtils.h"
namespace imstk
{
template<class DataElement>
class CollisionDataBase
{
public:
///
/// \brief operator [] (const accessor)
///
const DataElement& operator[](const size_t idx) const
{
#if defined(DEBUG) || defined(_DEBUG) || !defined(NDEBUG)
LOG_IF(FATAL, (idx >= m_Data.size())) << "Invalid index";
#endif
return m_Data[idx];
}
///
/// \brief Thread-safe append a data element
///
void safeAppend(const DataElement& data)
{
m_Lock.lock();
m_Data.push_back(data);
m_Lock.unlock();
}
///
/// \brief Append a data element, this is a non thread-safe operation
///
void unsafeAppend(const DataElement& data) { m_Data.push_back(data); }
///
/// \brief Overwrite a data element, this is a non thread-safe operation
///
void setElement(size_t idx, const DataElement& data)
{
#if defined(DEBUG) || defined(_DEBUG) || !defined(NDEBUG)
LOG_IF(FATAL, (idx >= m_Data.size())) << "Invalid index";
#endif
m_Data[idx] = data;
}
///
/// \brief Check if the data array is emtpy
///
bool isEmpty() const { return m_Data.empty(); }
///
/// \brief Get the size of the data
///
size_t getSize() const { return m_Data.size(); }
///
/// \brief Resize the data array
///
void resize(size_t newSize) { m_Data.resize(newSize); }
///
/// \brief Clear all data
///
void clear() { m_Data.resize(0); }
private:
std::vector<DataElement> m_Data;
ParallelUtils::SpinLock m_Lock;
};
///
/// \struct PositionDirectionCollisionData
///
/// \brief Point-penetration depth collision data
///
struct PositionDirectionCollisionData
struct PositionDirectionCollisionDataElement
{
Vec3d posA;
Vec3d posB;
Vec3d dirAtoB;
double penetrationDepth;
};
class PositionDirectionCollisionData : public CollisionDataBase<PositionDirectionCollisionDataElement>
{
};
///
/// \struct MeshToAnalyticalCollisionData
///
/// \brief Mesh to analytical point-penetration depth collision data
///
struct MeshToAnalyticalCollisionData
struct MeshToAnalyticalCollisionDataElement
{
size_t nodeId;
uint32_t nodeIdx;
Vec3d penetrationVector;
};
class MeshToAnalyticalCollisionData : public CollisionDataBase<MeshToAnalyticalCollisionDataElement>
{
};
///
/// \struct VertexTriangleCollisionData
///
/// \brief Vertex-triangle collision data
///
struct VertexTriangleCollisionData
struct VertexTriangleCollisionDataElement
{
uint32_t vertexIdx;
uint32_t triIdx;
double closestDistance;
};
class VertexTriangleCollisionData : public CollisionDataBase<VertexTriangleCollisionDataElement>
{
size_t vertexIdA;
size_t triIdB;
float time;
VertexTriangleCollisionData(size_t vIdA, size_t fIdB, float t = -1)
{
vertexIdA = vIdA;
triIdB = fIdB;
time = t;
}
};
///
......@@ -76,18 +145,14 @@ struct VertexTriangleCollisionData
///
/// \brief Triangle-vertex collision data
///
struct TriangleVertexCollisionData
struct TriangleVertexCollisionDataElement
{
uint32_t triIdx;
uint32_t vertexIdx;
double closestDistance;
};
class TriangleVertexCollisionData : public CollisionDataBase<TriangleVertexCollisionDataElement>
{
size_t triIdA;
size_t vertexIdB;
float time;
TriangleVertexCollisionData(const size_t fIdA, const size_t vIdB, const float t = -1)
{
triIdA = fIdA;
vertexIdB = vIdB;
time = t;
}
};
///
......@@ -95,18 +160,14 @@ struct TriangleVertexCollisionData
///
/// \brief Edge-Edge collision data
///
struct EdgeEdgeCollisionData
struct EdgeEdgeCollisionDataElement
{
std::pair<size_t, size_t> edgeIdA;
std::pair<size_t, size_t> edgeIdB;
std::pair<uint32_t, uint32_t> edgeIdA;
std::pair<uint32_t, uint32_t> edgeIdB;
float time;
EdgeEdgeCollisionData(const size_t eA_v1, const size_t eA_v2, const size_t eB_v1, const size_t eB_v2, const float t = -1)
{
edgeIdA = std::pair<size_t, size_t>(eA_v1, eA_v2);
edgeIdB = std::pair<size_t, size_t>(eB_v1, eB_v2);
time = t;
}
};
class EdgeEdgeCollisionData : public CollisionDataBase<EdgeEdgeCollisionDataElement>
{
};
///
......@@ -114,7 +175,7 @@ struct EdgeEdgeCollisionData
///
/// \brief Point-tetrahedron collision data
///
struct PointTetrahedronCollisionData
struct PointTetrahedronCollisionDataElement
{
enum CollisionType
{
......@@ -123,29 +184,37 @@ struct PointTetrahedronCollisionData
bPenetratingA = 2, // vertex is from mesh B, tetrahedron is from mesh A
bPenetratingB = 3 // B self-penetration
} collisionType;
size_t vertexId;
size_t tetreahedronId;
uint32_t vertexIdx;
uint32_t tetreahedronIdx;
using WeightsArray = std::array<double, 4>;
WeightsArray BarycentricCoordinates;
};
class PointTetrahedronCollisionData : public CollisionDataBase<PointTetrahedronCollisionDataElement>
{
};
struct PickingCollisionData
///
/// \brief The PickingCollisionData struct
///
struct PickingCollisionDataElement
{
// map of node and point position
Vec3d ptPos;
size_t nodeId;
uint32_t nodeIdx;
bool touchStatus;
};
class PickingCollisionData : public CollisionDataBase<PickingCollisionDataElement>
{
};
///
/// \struct CollisionData
///
/// \brief Class that is the holder of all types of collision data
///
class CollisionData
struct CollisionData
{
public:
void clearAll()
{
PDColData.clear();
......@@ -157,14 +226,12 @@ public:
NodePickData.clear();
}
CollisionData() {}
std::vector<PositionDirectionCollisionData> PDColData; ///< Position Direction collision data
std::vector<VertexTriangleCollisionData> VTColData; ///< Vertex Triangle collision data
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
std::vector<PickingCollisionData> NodePickData; ///< List of points that are picked
PositionDirectionCollisionData PDColData; ///< Position Direction collision data
VertexTriangleCollisionData VTColData; ///< Vertex Triangle collision data
TriangleVertexCollisionData TVColData; ///< Triangle Vertex collision data
EdgeEdgeCollisionData EEColData; ///< Edge Edge collision data
MeshToAnalyticalCollisionData MAColData; ///< Mesh to analytical collision data
PointTetrahedronCollisionData PTColData; ///< Point Tetrahedron collision data
PickingCollisionData NodePickData; ///< List of points that are picked
};
}
......@@ -20,51 +20,24 @@
=========================================================================*/
#include "imstkBidirectionalPlaneToSphereCD.h"
#include "imstkCollidingObject.h"
#include "imstkNarrowPhaseCD.h"
#include "imstkCollisionData.h"
#include "imstkPlane.h"
#include "imstkSphere.h"
#include <g3log/g3log.hpp>
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
BidirectionalPlaneToSphere::computeCollisionData()
BidirectionalPlaneToSphereCD::computeCollisionData()
{
// Clear collisionData
m_colData->clearAll();
// 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 });
}
NarrowPhaseCD::bidirectionalPlaneToSphere(m_planeA.get(), m_sphereB.get(), m_colData);
}
} // namespace imstk
......@@ -22,39 +22,29 @@
#pragma once
#include <memory>
#include "imstkCollisionDetection.h"
namespace imstk
{
class Plane;
class Sphere;
class CollisionData;
class Geometry;
struct CollisionData;
///
/// \class PlaneToSphereCD
///
/// \brief Plane to sphere collision detection
///
class BidirectionalPlaneToSphere : public CollisionDetection
class BidirectionalPlaneToSphereCD : public CollisionDetection
{
public:
///
/// \brief Constructor
///
BidirectionalPlaneToSphere(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)
{}
///
/// \brief Destructor
///
~BidirectionalPlaneToSphere() = default;
BidirectionalPlaneToSphereCD(std::shared_ptr<Plane> planeA,
std::shared_ptr<Sphere> sphereB,
std::shared_ptr<CollisionData> colData);
///
/// \brief Detect collision and compute collision data
......@@ -62,8 +52,7 @@ public:
void computeCollisionData() override;
private:
std::shared_ptr<Plane> m_planeA; ///>
std::shared_ptr<Sphere> m_sphereB; ///>
std::shared_ptr<Plane> m_planeA;
std::shared_ptr<Sphere> m_sphereB;
};
}
} // namespace imstk
......@@ -20,186 +20,152 @@
=========================================================================*/
#include "imstkCollisionDetection.h"
#include "imstkCollisionData.h"
#include "imstkUnidirectionalPlaneToSphereCD.h"
#include "imstkBidirectionalPlaneToSphereCD.h"
#include "imstkSphereToSphereCD.h"
// Points to objects
#include "imstkPointSetToCapsuleCD.h"
#include "imstkPointSetToSphereCD.h"
#include "imstkPointSetToPlaneCD.h"
#include "imstkMeshToMeshCD.h"
#include "imstkSphereCylinderCD.h"
#include "imstkPointSetToVolumeMeshCD.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 "imstkPlane.h"
#include "imstkSphere.h"
#include "imstkCylinder.h"
#include "imstkCapsule.h"
#include "imstkSurfaceMesh.h"
#include "imstkTetrahedralMesh.h"
#include <g3log/g3log.hpp>
#define IMSTK_CHECK_FOR_VALID_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
{
std::shared_ptr<CollisionDetection>
CollisionDetection::makeCollisionDetectionObject(const Type& type,
CollisionDetection::makeCollisionDetectionObject(const Type type,
std::shared_ptr<CollidingObject> objA,
std::shared_ptr<CollidingObject> objB,
std::shared_ptr<CollisionData> colData)
{
switch (type)
{
case Type::UnidirectionalPlaneToSphere:
// Points to objects
case Type::PointSetToSphere:
{
auto plane = std::dynamic_pointer_cast<Plane>(objA->getCollidingGeometry());
auto sphere = std::dynamic_pointer_cast<Sphere>(objB->getCollidingGeometry());
// Geometries check
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);
auto pointset = std::dynamic_pointer_cast<PointSet>(objA->getCollidingGeometry());
auto sphere = std::dynamic_pointer_cast<Sphere>(objB->getCollidingGeometry());
IMSTK_CHECK_FOR_VALID_GEOMETRIES(pointset, sphere)
return std::make_shared<PointSetToSphereCD>(pointset, sphere, colData);
}
break;
case Type::BidirectionalPlaneToSphere:
case Type::PointSetToPlane:
{
auto plane = std::dynamic_pointer_cast<Plane>(objA->getCollidingGeometry());
auto sphere = std::dynamic_pointer_cast<Sphere>(objB->getCollidingGeometry());
// Geometries check
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);
auto pointset = std::dynamic_pointer_cast<PointSet>(objA->getCollidingGeometry());
auto plane = std::dynamic_pointer_cast<Plane>(objB->getCollidingGeometry());
IMSTK_CHECK_FOR_VALID_GEOMETRIES(pointset, plane)
return std::make_shared<PointSetToPlaneCD>(pointset, plane, colData);
}
break;
case Type::SphereToSphere:
case Type::PointSetToCapsule:
{
auto sphereA = std::dynamic_pointer_cast<Sphere>(objA->getCollidingGeometry());
auto sphereB = std::dynamic_pointer_cast<Sphere>(objB->getCollidingGeometry());
// Geometries check
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);
auto pointset = std::dynamic_pointer_cast<PointSet>(objA->getCollidingGeometry());
auto capsule = std::dynamic_pointer_cast<Capsule>(objB->getCollidingGeometry());
IMSTK_CHECK_FOR_VALID_GEOMETRIES(pointset, capsule)
return std::make_shared<PointSetToCapsuleCD>(pointset, capsule, colData);
}
break;
case Type::SphereToCylinder:
case Type::PointSetToSpherePicking:
{
auto pointset = std::dynamic_pointer_cast<PointSet>(objA->getCollidingGeometry());
auto sphere = std::dynamic_pointer_cast<Sphere>(objB->getCollidingGeometry());
auto cylinder = std::dynamic_pointer_cast<Cylinder>(objA->getCollidingGeometry());
// 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);
IMSTK_CHECK_FOR_VALID_GEOMETRIES(pointset, sphere)
return std::make_shared<PointSetToSpherePickingCD>(pointset, sphere, colData);
}
break;
case Type::PointSetToSphere:
case Type::PointSetToVolumeMesh:
{
auto mesh = std::dynamic_pointer_cast<PointSet>(objA->getCollidingGeometry());
auto sphere = std::dynamic_pointer_cast<Sphere>(objB->getCollidingGeometry());
// Geometries check
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);
auto pointset = std::dynamic_pointer_cast<PointSet>(objA->getCollidingGeometry());
auto triMesh = std::dynamic_pointer_cast<SurfaceMesh>(objB->getCollidingGeometry());
IMSTK_CHECK_FOR_VALID_GEOMETRIES(pointset, triMesh)
return std::make_shared<PointSetToVolumeMeshCD>(pointset, triMesh, colData);
}
break;
case Type::PointSetToPlane:
// Mesh to mesh
case Type::SurfaceMeshToSurfaceMesh:
{
auto mesh = std::dynamic_pointer_cast<PointSet>(objA->getCollidingGeometry());
auto plane = std::dynamic_pointer_cast<Plane>(objB->getCollidingGeometry());
// Geometries check
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);
auto meshA = std::dynamic_pointer_cast<SurfaceMesh>(objA->getCollidingGeometry());
auto meshB = std::dynamic_pointer_cast<SurfaceMesh>(objB->getCollidingGeometry());
IMSTK_CHECK_FOR_VALID_GEOMETRIES(meshA, meshB)
return std::make_shared<SurfaceMeshToSurfaceMeshCD>(meshA, meshB, colData);
}
break;
case Type::MeshToMesh:
case Type::SurfaceMeshToSurfaceMeshCCD:
{
auto meshA = std::dynamic_pointer_cast<SurfaceMesh>(objA->getCollidingGeometry());
auto meshB = std::dynamic_pointer_cast<SurfaceMesh>(objB->getCollidingGeometry());
IMSTK_CHECK_FOR_VALID_GEOMETRIES(meshA, meshB)
return std::make_shared<SurfaceMeshToSurfaceMeshCCD>(meshA, meshB, colData);
}
// Geometries check
if (meshA == nullptr || meshB == nullptr)
{
LOG(WARNING) << "CollisionDetection::make_collision_detection error: "
<< "invalid object geometries for MeshToMesh collision detection.";
return nullptr;
}
return std::make_shared<MeshToMeshCD>(meshA, meshB, colData);
case Type::VolumeMeshToVolumeMesh: