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

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 @@
=========================================================================*/
#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
};
......
......@@ -21,11 +21,10 @@
#pragma once
#include "imstkCollisionDetection.h"
#include <memory>
#include <iostream>
#include "imstkCollisionDetection.h"
namespace imstk
{
class Geometry;
......@@ -46,17 +45,7 @@ public:
///
MeshToMeshBruteForceCD(std::shared_ptr<Geometry> obj1,
std::shared_ptr<SurfaceMesh> obj2,
std::shared_ptr<CollisionData> colData) :
CollisionDetection(CollisionDetection::Type::MeshToMeshBruteForce,
colData),
m_object1(obj1),
m_object2(obj2)
{}
///
/// \brief Destructor
///
~MeshToMeshBruteForceCD() = default;
std::shared_ptr<CollisionData> colData);
///
/// \brief Detect collision and compute collision data
......
This diff is collapsed.
/*=========================================================================
Library: iMSTK
Copyright (c) Kitware, Inc. & Center for Modeling, Simulation,
& Imaging in Medicine, Rensselaer Polytechnic Institute.
Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at
http://www.apache.org/licenses/LICENSE-2.0.txt
Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
=========================================================================*/
#pragma once
#include "imstkMath.h"
#include <memory>
namespace imstk
{
struct CollisionData;
class Geometry;
class Plane;
class Sphere;
class Capsule;
class Cylinder;
class SurfaceMesh;
///
/// \brief Implement narrow-phase collision detection for various primitives
///
namespace NarrowPhaseCD
{
///
/// \brief BidirectionalPlaneToSphere
///
void bidirectionalPlaneToSphere(Geometry* const planeGeo, Geometry* const sphereGeo,
const std::shared_ptr<CollisionData>& colData);
void bidirectionalPlaneToSphere(Plane* const planeA, Sphere* const sphereB,
const std::shared_ptr<CollisionData>& colData);
inline void
bidirectionalPlaneToSphere(const std::shared_ptr<Plane>& plane, const std::shared_ptr<Sphere>& sphere,
const std::shared_ptr<CollisionData>& colData)
{
bidirectionalPlaneToSphere(plane.get(), sphere.get(), colData);
}
///
/// \brief UnidirectionalPlaneToSphere
///
void unidirectionalPlaneToSphere(Geometry* const planeGeo, Geometry* const sphereGeo,
const std::shared_ptr<CollisionData>& colData);
void unidirectionalPlaneToSphere(Plane* const plane, Sphere* const sphere,
const std::shared_ptr<CollisionData>& colData);
inline void
unidirectionalPlaneToSphere(const std::shared_ptr<Plane>& plane, const std::shared_ptr<Sphere>& sphere,
const std::shared_ptr<CollisionData>& colData)
{
unidirectionalPlaneToSphere(plane.get(), sphere.get(), colData);
}
///
/// \brief SphereToCylinder
///
void sphereToCylinder(Geometry* const sphereGeo, Geometry* const cylinderGeo,
const std::shared_ptr<CollisionData>& colData);
void sphereToCylinder(Sphere* const sphere, Cylinder* const cylinder,
const std::shared_ptr<CollisionData>& colData);
inline void
sphereToCylinder(const std::shared_ptr<Sphere>& sphere, const std::shared_ptr<Cylinder>& cylinder,
const std::shared_ptr<CollisionData>& colData)
{
sphereToCylinder(sphere.get(), cylinder.get(), colData);
}
///
/// \brief SphereToSphere
///
void sphereToSphere(Geometry* const sphereGeoA, Geometry* const sphereGeoB,
const std::shared_ptr<CollisionData>& colData);
void sphereToSphere(Sphere* const sphereA, Sphere* const sphereB,
const std::shared_ptr<CollisionData>& colData);
inline void
sphereToSphere(const std::shared_ptr<Sphere>& sphereA, const std::shared_ptr<Sphere>& sphereB,
const std::shared_ptr<CollisionData>& colData)
{
sphereToSphere(sphereA.get(), sphereB.get(), colData);
}
///
/// \brief PointToCapsule
///
void pointToCapsule(const Vec3r& point, uint32_t pointIdx, Geometry* const capsuleGeo,
const std::shared_ptr<CollisionData>& colData);
void pointToCapsule(const Vec3r& point, uint32_t pointIdx, Capsule* const capsule,
const std::shared_ptr<CollisionData>& colData);
inline void
pointToCapsule(const Vec3r& point, uint32_t pointIdx, const std::shared_ptr<Capsule>& capsule,
const std::shared_ptr<CollisionData>& colData)
{
pointToCapsule(point, pointIdx, capsule.get(), colData);
}
///
/// \brief PointToPlane
///
void pointToPlane(const Vec3r& point, uint32_t pointIdx, Geometry* const planeGeo,
const std::shared_ptr<CollisionData>& colData);
void pointToPlane(const Vec3r& point, uint32_t pointIdx, Plane* const plane,
const std::shared_ptr<CollisionData>& colData);
inline void
pointToPlane(const Vec3r& point, uint32_t pointIdx, const std::shared_ptr<Plane>& plane,
const std::shared_ptr<CollisionData>& colData)
{
pointToPlane(point, pointIdx, plane.get(), colData);
}
///
/// \brief PointToSphere
///
void pointToSphere(const Vec3r& point, uint32_t pointIdx, Geometry* const sphereGeo,
const std::shared_ptr<CollisionData>& colData);
void pointToSphere(const Vec3r& point, uint32_t pointIdx, Sphere* const sphere,
const std::shared_ptr<CollisionData>& colData);
inline void
pointToSphere(const Vec3r& point, uint32_t pointIdx, const std::shared_ptr<Sphere>& sphere,
const std::shared_ptr<CollisionData>& colData)
{
pointToSphere(point, pointIdx, sphere.get(), colData);
}
///
/// \brief PointToSpherePicking
///
void pointToSpherePicking(const Vec3r& point, uint32_t pointIdx, Geometry* const sphereGeo,
const std::shared_ptr<CollisionData>& colData);
void pointToSpherePicking(const Vec3r& point, uint32_t pointIdx, Sphere* const sphere,
const std::shared_ptr<CollisionData>& colData);
inline void
pointToSpherePicking(const Vec3r& point, uint32_t pointIdx, const std::shared_ptr<Sphere>& sphere,
const std::shared_ptr<CollisionData>& colData)
{
pointToSpherePicking(point, pointIdx, sphere.get(), colData);
}
///
/// \brief TriangleToTriangle
///
void triangleToTriangle(uint32_t triIdx1, Geometry* const triMeshGeo1,
uint32_t triIdx2, Geometry* const triMeshGeo2,
const std::shared_ptr<CollisionData>& colData);
void triangleToTriangle(uint32_t triIdx1, SurfaceMesh* const triMesh1,
uint32_t triIdx2, SurfaceMesh* const triMesh2,
const std::shared_ptr<CollisionData>& colData);
inline void
triangleToTriangle(uint32_t triIdx1, const std::shared_ptr<SurfaceMesh>& triMesh1,
uint32_t triIdx2, const std::shared_ptr<SurfaceMesh>& triMesh2,
const std::shared_ptr<CollisionData>& colData)
{
triangleToTriangle(triIdx1, triMesh1.get(), triIdx2, triMesh2.get(), colData);
}
///
/// \brief PointToTriangle
/// \return true if the point 'penetrated' triangle (a point P penetrates a triangle ABC if dot(P - A, n) < 0, where n is triangle normal)
///
bool pointToTriangle(const Vec3r& point, uint32_t pointIdx,
uint32_t triIdx, Geometry* const triMeshGeo,
const std::shared_ptr<CollisionData>& colData);
bool pointToTriangle(const Vec3r& point, uint32_t pointIdx,
uint32_t triIdx, SurfaceMesh* const triMesh,
const std::shared_ptr<CollisionData>& colData);
inline void
pointToTriangle(const Vec3r& point, uint32_t pointIdx, uint32_t triIdx, const std::shared_ptr<SurfaceMesh>& triMesh,
const std::shared_ptr<CollisionData>& colData)
{
pointToTriangle(point, pointIdx, triIdx, triMesh.get(), colData);
}
} // namespace NarrowPhaseCD
} // namespace imstk
......@@ -20,12 +20,10 @@
=========================================================================*/
#include "imstkPointSetToCapsuleCD.h"
#include "imstkNarrowPhaseCD.h"
#include "imstkCollisionData.h"
#include "imstkCapsule.h"
#include "imstkPointSet.h"
#include "imstkMath.h"
#include "imstkParallelUtils.h"
#include "imstkPointSet.h"
namespace imstk
{
......@@ -41,49 +39,12 @@ PointSetToCapsuleCD::PointSetToCapsuleCD(std::shared_ptr<PointSet> pointSet
void
PointSetToCapsuleCD::computeCollisionData()
{
// Clear collisionData
m_colData->clearAll();
auto capsulePos = m_capsule->getPosition();
auto length = m_capsule->getLength();
auto radius = m_capsule->getRadius();
// Get position of end points of the capsule
// TODO: Fix this issue of extra computation in future
auto p0 = capsulePos;
auto p1 = p0 + m_capsule->getOrientationAxis() * length;
auto mid = 0.5 * (p0 + p1);
auto p = p1 - p0;
auto pDotp = p.dot(p);
auto pDotp0 = p.dot(p0);
ParallelUtils::SpinLock lock;
ParallelUtils::parallelFor(m_pointSet->getVertexPositions().size(),
[&](const size_t idx)
ParallelUtils::parallelFor(static_cast<unsigned int>(m_pointSet->getVertexPositions().size()),
[&](const unsigned int idx)
{
const auto q = m_pointSet->getVertexPosition(idx);
// First, check collision with bounding sphere
if ((mid - q).norm() > (radius + length * 0.5))
{
return;
}
// Do the actual check
auto alpha = (q.dot(p) - pDotp0) / pDotp;
auto closestPoint = p0 + p * alpha;
// If the point is inside the bounding sphere then the closest point
// should be inside the capsule
auto dist = (closestPoint - q).norm();
if (dist <= radius)
{
auto direction = (closestPoint - q) / dist;
auto pointOnCapsule = closestPoint - radius * direction;
lock.lock();
m_colData->MAColData.push_back({ idx, p - pointOnCapsule });
lock.unlock();
}
const auto& point = m_pointSet->getVertexPosition(idx);
NarrowPhaseCD::pointToCapsule(point, idx, m_capsule.get(), m_colData);
});
}
} // imstk
......@@ -21,15 +21,15 @@
#pragma once
#include <memory>
#include "imstkCollisionDetection.h"
#include <memory>
namespace imstk
{
class PointSet;
class Capsule;
class CollisionData;
class Geometry;
struct CollisionData;
///
/// \class PointSetToCapsuleCD
......@@ -47,19 +47,13 @@ public:
std::shared_ptr<Capsule> capsule,
std::shared_ptr<CollisionData> colData);
///
/// \brief Destructor
///
virtual ~PointSetToCapsuleCD() override = default;
///
/// \brief Detect collision and compute collision data
///
void computeCollisionData() override;
private:
std::shared_ptr<PointSet> m_pointSet; ///> PointSet
std::shared_ptr<Capsule> m_capsule; ///> Capsule
std::shared_ptr<PointSet> m_pointSet;
std::shared_ptr<Capsule> m_capsule;
};
}
......@@ -20,12 +20,10 @@
=========================================================================*/
#include "imstkPointSetToPlaneCD.h"
#include "imstkCollidingObject.h"
#include "imstkNarrowPhaseCD.h"
#include "imstkCollisionData.h"
#include "imstkPlane.h"
#include "imstkPointSet.h"
#include "imstkParallelUtils.h"
#include "imstkPointSet.h"
namespace imstk
{
......@@ -41,27 +39,12 @@ PointSetToPlaneCD::PointSetToPlaneCD(std::shared_ptr<PointSet> pointSet,
void
PointSetToPlaneCD::computeCollisionData()
{
// Clear collisionData
m_colData->clearAll();
// Get plane properties
auto planePos = m_plane->getPosition();
auto planeNormal = m_plane->getNormal();
ParallelUtils::SpinLock lock;
ParallelUtils::parallelFor(m_pointSet->getVertexPositions().size(),
[&](const size_t idx)
ParallelUtils::parallelFor(static_cast<unsigned int>(m_pointSet->getVertexPositions().size()),
[&](const unsigned int idx)
{
const auto p = m_pointSet->getVertexPosition(idx);
const auto penetrationDist = (p - planePos).dot(planeNormal);
if (penetrationDist <= 0.0)
{
const auto penetrationDir = planeNormal * penetrationDist;
lock.lock();
m_colData->MAColData.push_back({ idx, penetrationDir });
lock.unlock();
}
const auto& point = m_pointSet->getVertexPosition(idx);
NarrowPhaseCD::pointToPlane(point, idx, m_plane.get(), m_colData);
});
}
} // imstk
......@@ -21,15 +21,15 @@
#pragma once
#include <memory>
#include "imstkCollisionDetection.h"
#include <memory>
namespace imstk
{
class PointSet;
class Plane;
class CollisionData;
class Geometry;
struct CollisionData;
///
/// \class PointSetToPlaneCD
......@@ -47,19 +47,13 @@ public:
std::shared_ptr<Plane> plane,
std::shared_ptr<CollisionData> colData);
///
/// \brief Destructor
///
virtual ~PointSetToPlaneCD() override = default;
///
/// \brief Detect collision and compute collision data
///
void computeCollisionData() override;
private:
std::shared_ptr<PointSet> m_pointSet; ///> PointSet
std::shared_ptr<Plane> m_plane; ///> Plane
std::shared_ptr<PointSet> m_pointSet;
std::shared_ptr<Plane> m_plane;
};
}
......@@ -20,12 +20,10 @@
=========================================================================*/
#include "imstkPointSetToSphereCD.h"
#include "imstkCollidingObject.h"
#include "imstkNarrowPhaseCD.h"
#include "imstkCollisionData.h"
#include "imstkSphere.h"
#include "imstkPointSet.h"
#include "imstkParallelUtils.h"
#include "imstkPointSet.h"
namespace imstk
{
......@@ -41,31 +39,12 @@ PointSetToSphereCD::PointSetToSphereCD(std::shared_ptr<PointSet> pointSet,
void
PointSetToSphereCD::computeCollisionData()
{
// Clear collisionData
m_colData->clearAll();
// Get sphere properties
const auto sphereCenter = m_sphere->getPosition();
const auto sphereRadius = m_sphere->getRadius();
const auto sphereRadiusSqr = sphereRadius * sphereRadius;
ParallelUtils::SpinLock lock;
ParallelUtils::parallelFor(m_pointSet->getVertexPositions().size(),
[&](const size_t idx)
ParallelUtils::parallelFor<