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
......@@ -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_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_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_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_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_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_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_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_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:
{
auto tet1 = std::dynamic_pointer_cast<TetrahedralMesh>(objA->getCollidingGeometry());
auto tet2 = std::dynamic_pointer_cast<TetrahedralMesh>(objB->getCollidingGeometry());
IMSTK_CHECK_GEOMETRIES(tet1, tet2)
return std::make_shared<TetraToTetraCD>(tet1, tet2, 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());
// 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<PointSetToSpherePickingCD>(mesh, sphere, colData);
IMSTK_CHECK_GEOMETRIES(plane, sphere)
return std::make_shared<UnidirectionalPlaneToSphereCD>(plane, sphere, colData);
}
break;
case Type::MeshToMeshBruteForce:
case Type::BidirectionalPlaneToSphere:
{
auto geometry1 = std::dynamic_pointer_cast<Geometry>(objA->getCollidingGeometry());
auto surfaceGeo = std::dynamic_pointer_cast<SurfaceMesh>(objB->getCollidingGeometry());
// Geometries check
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);
auto plane = std::dynamic_pointer_cast<Plane>(objA->getCollidingGeometry());
auto sphere = std::dynamic_pointer_cast<Sphere>(objB->getCollidingGeometry());
IMSTK_CHECK_GEOMETRIES(plane, sphere)
return std::make_shared<BidirectionalPlaneToSphereCD>(plane, sphere, 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:
{
LOG(WARNING) << "CollisionDetection::make_collision_detection error: type not implemented.";
LOG(FATAL) << "CollisionDetection::makeCollisionDetectionObject error: type not implemented.";
return nullptr;
}
}
}
const CollisionDetection::Type&
CollisionDetection::getType() const
{
return m_type;
}
const std::shared_ptr<CollisionData>
CollisionDetection::getCollisionData() const
CollisionDetection::CollisionDetection(const CollisionDetection::Type& type, std::shared_ptr<CollisionData> colData) : m_type(type)
{
return m_colData;
m_colData = (colData == nullptr) ? std::make_shared<CollisionData>() : colData;
}
}
......@@ -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
......
/*=========================================================================
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.
=========================================================================*/
#include "imstkNarrowPhaseCD.h"
#include "imstkCollisionUtils.h"
#include "imstkLogUtility.h"
#include "imstkCollisionData.h"
#include "imstkPlane.h"
#include "imstkSphere.h"
#include "imstkCapsule.h"
#include "imstkCylinder.h"
#include "imstkSurfaceMesh.h"
namespace imstk
{
namespace NarrowPhaseCD
{
void
bidirectionalPlaneToSphere(Geometry* const planeGeo, Geometry* const sphereGeo,
const std::shared_ptr<CollisionData>& colData)
{
const auto plane = static_cast<Plane*>(planeGeo);
const auto sphere = static_cast<Sphere*>(sphereGeo);
#if defined(DEBUG) || defined(_DEBUG) || !defined(NDEBUG)
LOG_IF(FATAL, (dynamic_cast<Plane*>(planeGeo) == nullptr
|| dynamic_cast<Sphere*>(sphereGeo) == nullptr)) << "Invalid geometries";
#endif
bidirectionalPlaneToSphere(plane, sphere, colData);
}
void
bidirectionalPlaneToSphere(Plane* const planeA, Sphere* const sphereB,
const std::shared_ptr<CollisionData>& colData)
{
// Get geometry properties
Vec3d sphereBPos = sphereB->getPosition();
double r = sphereB->getRadius() * sphereB->getScaling();
Vec3d planeAPos = planeA->getPosition();