Commit c2f5edb6 authored by Sreekanth Arikatla's avatar Sreekanth Arikatla

Merge branch 'RefactorCollisionDetection' into 'master'

Refactor Collision Detection

See merge request !346
parents 7bdfdbb5 a2b90a8a
Pipeline #142040 running with stage
......@@ -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
......@@ -21,15 +21,12 @@
#pragma once
#include "imstkCollisionData.h"
#include <memory>
namespace imstk
{
class CollidingObject;
//class CollisionData;
//class CollisionData::CollisionData;
struct CollisionData;
///
/// \class CollisionDetection
......@@ -45,48 +42,45 @@ public:
///
enum class Type
{
UnidirectionalPlaneToSphere,
BidirectionalPlaneToSphere,
SphereToCylinder,
SphereToSphere,
// Points to objects
PointSetToSphere,
PointSetToPlane,
MeshToMesh,
PointSetToCapsule,
PointSetToSpherePicking,
PointSetToVolumeMesh,
// Mesh to mesh (mesh to analytical object = mesh vertices to analytical object)
SurfaceMeshToSurfaceMesh,
SurfaceMeshToSurfaceMeshCCD,
VolumeMeshToVolumeMesh,
MeshToMeshBruteForce,
// Analytical object to analytical object
UnidirectionalPlaneToSphere,
BidirectionalPlaneToSphere,
SphereToCylinder,
SphereToSphere,
Custom
};
///
/// \brief Static factory for collision detection sub classes
///
static std::shared_ptr<CollisionDetection> makeCollisionDetectionObject(const Type& type,
std::shared_ptr<CollidingObject> objA,
std::shared_ptr<CollidingObject> objB,
std::shared_ptr<CollisionData> colData);
static std::shared_ptr<CollisionDetection> makeCollisionDetectionObject(
const Type type,
std::shared_ptr<CollidingObject> objA,
std::shared_ptr<CollidingObject> objB,
std::shared_ptr<CollisionData> colData);
///
/// \brief Constructor
///
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(const Type& type, std::shared_ptr<CollisionData> colData);
CollisionDetection() = delete;
///
/// \brief Destructor
/// \brief Destructor for base class
///
virtual ~CollisionDetection() = default;
......@@ -98,15 +92,14 @@ public:
///
/// \brief Returns collision detection type
///
const Type& getType() const;
const Type& getType() const { return m_type; }
///
/// \brief Returns collision data
///
const std::shared_ptr<CollisionData> getCollisionData() const;
const std::shared_ptr<CollisionData> getCollisionData() const { return m_colData; }
protected:
Type m_type = Type::Custom; ///< Collision detection algorithm type
std::shared_ptr<CollisionData> m_colData; ///< Collision data
};
......
......@@ -19,46 +19,18 @@
=========================================================================*/
#pragma once
#include <algorithm>
#include "imstkCollisionUtils.h"
namespace imstk
{
///
/// \brief Do ranges [a,b] and [c,d] intersect?
///
inline bool
isIntersect(const double& a, const double& b, const double& c, const double& d)
namespace CollisionUtils
{
return ((a <= d && a >= c) || (c <= b && c >= a)) ? true : false;
}
///
/// \brief Check if two AABBs are intersecting
///
inline bool
testAABBToAABB(const double& min1_x, const double& max1_x,
const double& min1_y, const double& max1_y,
const double& min1_z, const double& max1_z,
const double& min2_x, const double& max2_x,
const double& min2_y, const double& max2_y,
const double& min2_z, const double& max2_z)
{
return (isIntersect(min1_x, max1_x, min2_x, max2_x)
&& isIntersect(min1_y, max1_y, min2_y, max2_y)
&& isIntersect(min1_z, max1_z, min2_z, max2_z));
}
///
/// \brief Check if two lines are intersecting with AABB intersection test
///
inline bool
testLineToLineAABB(const double& x1, const double& y1, const double& z1,
const double& x2, const double& y2, const double& z2,
const double& x3, const double& y3, const double& z3,
const double& x4, const double& y4, const double& z4,
const double& prox1, const double& prox2)
bool
testLineToLineAABB(const double x1, const double y1, const double z1,
const double x2, const double y2, const double z2,
const double x3, const double y3, const double z3,
const double x4, const double y4, const double z4,
const double prox1 /*= VERY_SMALL_EPSILON_D*/, const double prox2 /*= VERY_SMALL_EPSILON_D*/)
{
double min1_x, max1_x, min1_y, max1_y, min1_z, max1_z;
......@@ -135,25 +107,109 @@ testLineToLineAABB(const double& x1, const double& y1, const double& z1,
min2_y - prox2, max2_y + prox2, min2_z - prox2, max2_z + prox2);
}
///
/// \brief Check if triangle and point are intersecting with AABB test
///
inline bool
testPointToTriAABB(const double& x1, const double& y1, const double& z1,
const double& x2, const double& y2, const double& z2,
const double& x3, const double& y3, const double& z3,
const double& x4, const double& y4, const double& z4,
const double& prox1, const double& prox2)
bool
segmentIntersectsTriangle(const Vec3r& pA, const Vec3r& pB,
const Vec3r& v0, const Vec3r& v1, const Vec3r& v2)
{
static const Real EPSILON = 1e-8;
Vec3r dirAB = pB - pA;
const Real lAB = dirAB.norm();
if (lAB < 1e-8)
{
return false;
}
dirAB /= lAB;
Vec3r edge1 = v1 - v0;
Vec3r edge2 = v2 - v0;
Vec3r h = dirAB.cross(edge2);
Real a = edge1.dot(h);
if (a > -EPSILON && a < EPSILON)
{
return false; // This ray is parallel to this triangle.
}
Real f = Real(1.0) / a;
Vec3r s = pA - v0;
Real u = f * s.dot(h);
if (u < Real(0) || u > Real(1.0))
{
return false;
}
Vec3r q = s.cross(edge1);
Real v = f * dirAB.dot(q);
if (v < Real(0) || u + v > Real(1.0))
{
return false;
}
// At this stage we can compute t to find out where the intersection point is on the line.
Real t = f * edge2.dot(q);
// ray - triangle intersection
return (t > EPSILON && t + EPSILON < lAB);
}
Real
pointSegmentClosestDistance(const Vec3r& point, const Vec3r& x1, const Vec3r& x2)
{
auto min_x = std::min(x2, std::min(x3, x4));
auto max_x = std::max(x2, std::max(x3, x4));
auto min_y = std::min(y2, std::min(y3, y4));
auto max_y = std::max(y2, std::max(y3, y4));
auto min_z = std::min(z2, std::min(z3, z4));
auto max_z = std::max(z2, std::max(z3, z4));
return testAABBToAABB(x1 - prox1, x1 + prox1, y1 - prox1, y1 + prox1,
z1 - prox1, z1 + prox1, min_x - prox2, max_x + prox2,
min_y - prox2, max_y + prox2, min_z - prox2, max_z + prox2);
Vec3r dx = x2 - x1;
Real m2 = dx.squaredNorm();
if (m2 < Real(1e-20))
{
return (point - x1).norm();
}
// find parameter value of closest point on segment
Real s12 = dx.dot(x2 - point) / m2;
if (s12 < 0)
{
s12 = 0;
}
else if (s12 > 1.0)
{
s12 = 1.0;
}
return (point - s12 * x1 + (1.0 - s12) * x2).norm();
}
Real
pointTriangleClosestDistance(const Vec3r& point, const Vec3r& x1, const Vec3r& x2, const Vec3r& x3)
{
// first find barycentric coordinates of closest point on infinite plane
Vec3r x13(x1 - x3), x23(x2 - x3), xp3(point - x3);
Real m13 = x13.squaredNorm(), m23 = x23.squaredNorm(), d = x13.dot(x23);
Real invdet = 1.0 / std::max(m13 * m23 - d * d, 1e-30);
Real a = x13.dot(xp3), b = x23.dot(xp3);
// Barycentric coordinates
Real w23 = invdet * (m23 * a - d * b);
Real w31 = invdet * (m13 * b - d * a);
Real w12 = 1.0 - w23 - w31;
if (w23 >= 0 && w31 >= 0 && w12 >= 0) // inside the triangle