Commit a1fc7e1e authored by Alexis Girault's avatar Alexis Girault
Browse files

ENH: Store pointers in Collision Detect & Handling

To geometries, objects, collision data
parent 160786ce
......@@ -26,7 +26,9 @@
#include "imstkMeshToMeshCD.h"
#include "imstkCollidingObject.h"
#include "imstkGeometry.h"
#include "imstkPlane.h"
#include "imstkSphere.h"
#include "imstkSurfaceMesh.h"
#include <g3log/g3log.hpp>
......@@ -35,42 +37,53 @@ namespace imstk {
std::shared_ptr<CollisionDetection>
CollisionDetection::make_collision_detection(const Type& type,
std::shared_ptr<CollidingObject> objA,
std::shared_ptr<CollidingObject> objB)
std::shared_ptr<CollidingObject> objB,
CollisionData &CDA,
CollisionData &CDB)
{
switch (type)
{
case Type::PlaneToSphere:
{
if (objA->getCollidingGeometry()->getType() != Geometry::Type::Plane ||
objB->getCollidingGeometry()->getType() != Geometry::Type::Sphere)
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 PlaneToSphere collision detection.";
return nullptr;
}
return std::make_shared<PlaneToSphereCD>();
return std::make_shared<PlaneToSphereCD>(plane, sphere, CDA, CDB);
}break;
case Type::SphereToSphere:
{
if (objA->getCollidingGeometry()->getType() != Geometry::Type::Sphere ||
objB->getCollidingGeometry()->getType() != Geometry::Type::Sphere)
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>();
return std::make_shared<SphereToSphereCD>(sphereA, sphereB, CDA, CDB);
}break;
case Type::MeshToMesh:
{
if (objA->getCollidingGeometry()->getType() != Geometry::Type::SurfaceMesh ||
objB->getCollidingGeometry()->getType() != Geometry::Type::SurfaceMesh)
auto meshA = std::dynamic_pointer_cast<SurfaceMesh>(objA->getCollidingGeometry());
auto meshB = std::dynamic_pointer_cast<SurfaceMesh>(objB->getCollidingGeometry());
// 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>();
return std::make_shared<MeshToMeshCD>(meshA, meshB, CDA, CDB);
}break;
default:
{
......@@ -78,7 +91,6 @@ CollisionDetection::make_collision_detection(const Type& type,
return nullptr;
}
}
}
const CollisionDetection::Type&
......
......@@ -43,15 +43,20 @@ public:
///
/// \brief Static factory for collision detection sub classes
///
static std::shared_ptr<CollisionDetection> make_collision_detection(
const Type& type,
static std::shared_ptr<CollisionDetection> make_collision_detection(const Type& type,
std::shared_ptr<CollidingObject> objA,
std::shared_ptr<CollidingObject> objB);
std::shared_ptr<CollidingObject> objB,
CollisionData& CDA,
CollisionData& CDB);
///
/// \brief Constructor
///
CollisionDetection(const Type& type) : m_type(type) {}
CollisionDetection(const Type& type, CollisionData& CDA, CollisionData& CDB) :
m_type(type),
m_CDA(CDA),
m_CDB(CDB)
{}
///
/// \brief Destructor
......@@ -61,10 +66,7 @@ public:
///
/// \brief Detect collision and compute collision data (pure virtual)
///
virtual void computeCollisionData(std::shared_ptr<CollidingObject> objA,
std::shared_ptr<CollidingObject> objB,
CollisionData& colDataA,
CollisionData& colDataB) = 0;
virtual void computeCollisionData() = 0;
///
/// \brief Returns collision detection type
......@@ -74,6 +76,8 @@ public:
protected:
Type m_type; //!< Collision detection algorithm type
CollisionData& m_CDA;
CollisionData& m_CDB;
};
}
......
......@@ -21,7 +21,6 @@
#include "imstkMeshToMeshCD.h"
#include "imstkCollidingObject.h"
#include "imstkCollisionData.h"
#include "imstkSurfaceMesh.h"
......@@ -41,66 +40,63 @@ EECallback(unsigned int e1_v1, unsigned int e1_v2,
}
void
VFCallback(unsigned int vid, unsigned int fid,
float t, void *userdata)
VFCallback1(unsigned int fid1, unsigned int vid2,
float t, void *userdata)
{
auto CD = reinterpret_cast<MeshToMeshCD*>(userdata);
CD->getType();
LOG(INFO) <<"VF: v2("<<vid2<<"), f1("<<fid1<<") \t\t@ t="<<t;
}
void
VFCallback2(unsigned int fid2, unsigned int vid1,
float t, void *userdata)
{
auto CD = reinterpret_cast<MeshToMeshCD*>(userdata);
CD->getType();
LOG(INFO) <<"VF: v("<<vid<<"), f("<<fid<<") \t\t@ t="<<t;
LOG(INFO) <<"VF: v1("<<vid1<<"), f2("<<fid2<<") \t\t@ t="<<t;
}
MeshToMeshCD::MeshToMeshCD(std::shared_ptr<SurfaceMesh> meshA,
std::shared_ptr<SurfaceMesh> meshB,
CollisionData& CDA,
CollisionData& CDB) :
CollisionDetection(CollisionDetection::Type::PlaneToSphere, CDA, CDB),
m_meshA(meshA),
m_meshB(meshB)
{
m_modelA = std::make_shared<DeformModel>(meshA->getVerticesPositions(), meshA->getTrianglesVertices());
m_modelB = std::make_shared<DeformModel>(meshB->getVerticesPositions(), meshB->getTrianglesVertices());
// Setup Callbacks
m_modelA->SetEECallBack(EECallback, this);
m_modelA->SetVFCallBack(VFCallback1, this);
m_modelB->SetVFCallBack(VFCallback2, this);
// Build BVH
m_modelA->BuildBVH(false);
m_modelB->BuildBVH(false);
}
void
MeshToMeshCD::computeCollisionData(std::shared_ptr<CollidingObject> objA,
std::shared_ptr<CollidingObject> objB,
CollisionData& colDataA,
CollisionData& colDataB)
MeshToMeshCD::computeCollisionData()
{
auto geomA = std::dynamic_pointer_cast<SurfaceMesh>(objA->getCollidingGeometry());
auto geomB = std::dynamic_pointer_cast<SurfaceMesh>(objB->getCollidingGeometry());
// Geometries check
if (geomA == nullptr || geomB == nullptr)
{
LOG(WARNING) << "MeshToMeshCD::computeCollisionData error: invalid geometries.";
return;
}
// Init models for continuous CD
if(!m_initialized)
{
// Init
modelA = std::make_shared<DeformModel>(geomA->getVerticesPositions(), geomA->getTrianglesVertices());
modelB = std::make_shared<DeformModel>(geomB->getVerticesPositions(), geomB->getTrianglesVertices());
// Setup Callbacks
modelA->SetEECallBack(EECallback, this);
modelA->SetVFCallBack(VFCallback, this);
// Build BVH
modelA->BuildBVH(false);
modelB->BuildBVH(false);
m_initialized = true;
}
else
{
// Update model
modelA->UpdateVert(geomA->getVerticesPositions());
modelB->UpdateVert(geomB->getVerticesPositions());
modelB->UpdateBoxes();
modelB->UpdateBoxes();
// Update BVH
modelA->RefitBVH();
modelB->RefitBVH();
// Reset Results
modelA->ResetCounter();
modelB->ResetCounter();
// Collide
modelA->Collide(modelB.get());
}
// Update model
m_modelA->UpdateVert(m_meshA->getVerticesPositions());
m_modelB->UpdateVert(m_meshB->getVerticesPositions());
m_modelB->UpdateBoxes();
m_modelB->UpdateBoxes();
// Update BVH
m_modelA->RefitBVH();
m_modelB->RefitBVH();
// Reset Results
m_modelA->ResetCounter();
m_modelB->ResetCounter();
// Collide
m_modelA->Collide(m_modelB.get());
}
}
......@@ -30,7 +30,7 @@
namespace imstk {
class CollidingObject;
class SurfaceMesh;
class CollisionData;
class MeshToMeshCD : public CollisionDetection
......@@ -40,7 +40,10 @@ public:
///
/// \brief Constructor
///
MeshToMeshCD() : CollisionDetection(CollisionDetection::Type::MeshToMesh) {}
MeshToMeshCD(std::shared_ptr<SurfaceMesh> planeA,
std::shared_ptr<SurfaceMesh> sphereB,
CollisionData& CDA,
CollisionData& CDB);
///
/// \brief Destructor
......@@ -50,16 +53,15 @@ public:
///
/// \brief Detect collision and compute collision data
///
void computeCollisionData(std::shared_ptr<CollidingObject> objA,
std::shared_ptr<CollidingObject> objB,
CollisionData& colDataA,
CollisionData& colDataB) override;
void computeCollisionData() override;
private:
bool m_initialized = false;
std::shared_ptr<DeformModel> modelA;
std::shared_ptr<DeformModel> modelB;
std::shared_ptr<SurfaceMesh> m_meshA;
std::shared_ptr<SurfaceMesh> m_meshB;
std::shared_ptr<DeformModel> m_modelA;
std::shared_ptr<DeformModel> m_modelB;
};
}
......
......@@ -31,30 +31,17 @@
namespace imstk {
void
PlaneToSphereCD::computeCollisionData(std::shared_ptr<CollidingObject> objA,
std::shared_ptr<CollidingObject> objB,
CollisionData& colDataA,
CollisionData& colDataB)
PlaneToSphereCD::computeCollisionData()
{
auto planeGeom = std::dynamic_pointer_cast<Plane>(objA->getCollidingGeometry());
auto sphereGeom = std::dynamic_pointer_cast<Sphere>(objB->getCollidingGeometry());
// Geometries check
if (planeGeom == nullptr || sphereGeom == nullptr)
{
LOG(WARNING) << "PlaneToSphereCD::computeCollisionData error: invalid geometries.";
return;
}
// Clear collisionData
colDataA.clearAll();
colDataB.clearAll();
m_CDA.clearAll();
m_CDB.clearAll();
// Get geometry properties
Vec3d sP = sphereGeom->getPosition();
double r = sphereGeom->getRadius() * sphereGeom->getScaling();
Vec3d pP = planeGeom->getPosition();
Vec3d n = planeGeom->getNormal();
Vec3d sP = m_sphereB->getPosition();
double r = m_sphereB->getRadius() * m_sphereB->getScaling();
Vec3d pP = m_planeA->getPosition();
Vec3d n = m_planeA->getNormal();
// Compute shortest distance
double d = (sP-pP).dot(n);
......@@ -79,8 +66,8 @@ PlaneToSphereCD::computeCollisionData(std::shared_ptr<CollidingObject> objA,
Vec3d sC = sP + dirBToA*r;
// Set collisionData
colDataA.PDColData.push_back({pC, dirBToA, penetrationDepth});
colDataB.PDColData.push_back({sC, -dirBToA, penetrationDepth});
m_CDA.PDColData.push_back({pC, dirBToA, penetrationDepth});
m_CDB.PDColData.push_back({sC, -dirBToA, penetrationDepth});
}
}
......@@ -28,7 +28,8 @@
namespace imstk {
class CollidingObject;
class Plane;
class Sphere;
class CollisionData;
class PlaneToSphereCD : public CollisionDetection
......@@ -38,7 +39,14 @@ public:
///
/// \brief Constructor
///
PlaneToSphereCD() : CollisionDetection(CollisionDetection::Type::PlaneToSphere) {}
PlaneToSphereCD(std::shared_ptr<Plane> planeA,
std::shared_ptr<Sphere> sphereB,
CollisionData& CDA,
CollisionData& CDB) :
CollisionDetection(CollisionDetection::Type::PlaneToSphere, CDA, CDB),
m_planeA(planeA),
m_sphereB(sphereB)
{}
///
/// \brief Destructor
......@@ -48,10 +56,12 @@ public:
///
/// \brief Detect collision and compute collision data
///
void computeCollisionData(std::shared_ptr<CollidingObject> objA,
std::shared_ptr<CollidingObject> objB,
CollisionData& colDataA,
CollisionData& colDataB) override;
void computeCollisionData() override;
private:
std::shared_ptr<Plane> m_planeA;
std::shared_ptr<Sphere> m_sphereB;
};
}
......
......@@ -30,30 +30,17 @@
namespace imstk {
void
SphereToSphereCD::computeCollisionData(std::shared_ptr<CollidingObject> objA,
std::shared_ptr<CollidingObject> objB,
CollisionData& colDataA,
CollisionData& colDataB)
SphereToSphereCD::computeCollisionData()
{
auto sphereGeomA = std::dynamic_pointer_cast<Sphere>(objA->getCollidingGeometry());
auto sphereGeomB = std::dynamic_pointer_cast<Sphere>(objB->getCollidingGeometry());
// Geometries check
if (sphereGeomA == nullptr || sphereGeomB == nullptr)
{
LOG(WARNING) << "SphereToSphereCD::computeCollisionData error: invalid geometries.";
return;
}
// Clear collisionData
colDataA.clearAll();
colDataB.clearAll();
m_CDA.clearAll();
m_CDB.clearAll();
// Get geometry properties
Vec3d sAP = sphereGeomA->getPosition();
double rA = sphereGeomA->getRadius() * sphereGeomA->getScaling();
Vec3d sBP = sphereGeomB->getPosition();
double rB = sphereGeomB->getRadius() * sphereGeomB->getScaling();
Vec3d sAP = m_sphereA->getPosition();
double rA = m_sphereA->getRadius() * m_sphereA->getScaling();
Vec3d sBP = m_sphereB->getPosition();
double rB = m_sphereB->getRadius() * m_sphereB->getScaling();
// Compute direction vector
Vec3d dirBToA = sAP - sBP;
......@@ -74,8 +61,8 @@ SphereToSphereCD::computeCollisionData(std::shared_ptr<CollidingObject> objA,
Vec3d sBC = sBP + dirBToA*rB;
// Set collisionData
colDataA.PDColData.push_back({sAC, dirBToA, penetrationDepth});
colDataB.PDColData.push_back({sBC, -dirBToA, penetrationDepth});
m_CDA.PDColData.push_back({sAC, dirBToA, penetrationDepth});
m_CDB.PDColData.push_back({sBC, -dirBToA, penetrationDepth});
}
}
......@@ -28,7 +28,7 @@
namespace imstk {
class CollidingObject;
class Sphere;
class CollisionData;
class SphereToSphereCD : public CollisionDetection
......@@ -38,7 +38,14 @@ public:
///
/// \brief Constructor
///
SphereToSphereCD() : CollisionDetection(CollisionDetection::Type::SphereToSphere) {}
SphereToSphereCD(std::shared_ptr<Sphere> sphereA,
std::shared_ptr<Sphere> sphereB,
CollisionData& CDA,
CollisionData& CDB) :
CollisionDetection(CollisionDetection::Type::SphereToSphere, CDA, CDB),
m_sphereA(sphereA),
m_sphereB(sphereB)
{}
///
/// \brief Destructor
......@@ -48,10 +55,12 @@ public:
///
/// \brief Detect collision and compute collision data
///
void computeCollisionData(std::shared_ptr<CollidingObject> objA,
std::shared_ptr<CollidingObject> objB,
CollisionData& colDataA,
CollisionData& colDataB) override;
void computeCollisionData() override;
private:
std::shared_ptr<Sphere> m_sphereA;
std::shared_ptr<Sphere> m_sphereB;
};
}
......
......@@ -29,24 +29,22 @@ namespace imstk {
std::shared_ptr<CollisionHandling>
CollisionHandling::make_collision_handling(const Type& type,
std::shared_ptr<CollidingObject> obj)
std::shared_ptr<CollidingObject> objA,
CollisionData &CDA,
std::shared_ptr<CollidingObject> objB)
{
switch (type)
{
case Type::Penalty:
{
if (obj->getType() == SceneObject::Type::VirtualCoupling ||
obj->getType() == SceneObject::Type::Rigid)
{
return std::make_shared<PenaltyRigidCH>();
}
else
if (objA->getType() != SceneObject::Type::VirtualCoupling &&
objA->getType() == SceneObject::Type::Rigid)
{
LOG(WARNING) << "CollisionHandling::make_collision_handling error: "
<< "penalty collision handling not yet implemented for non-rigid objects.";
return nullptr;
}
return std::make_shared<PenaltyRigidCH>(objA, CDA);
}break;
default:
......
......@@ -46,7 +46,9 @@ public:
///
static std::shared_ptr<CollisionHandling> make_collision_handling(
const Type& type,
std::shared_ptr<CollidingObject> obj);
std::shared_ptr<CollidingObject> objA,
CollisionData& CDA,
std::shared_ptr<CollidingObject> objB = nullptr);
///
/// \brief Constructor
......@@ -61,8 +63,7 @@ public:
///
/// \brief Compute forces based on collision data (pure virtual)
///
virtual void computeContactForces(std::shared_ptr<CollidingObject> obj,
CollisionData& colData) = 0;
virtual void computeContactForces() = 0;
///
/// \brief Returns collision handling type
......
......@@ -21,21 +21,22 @@
#include "imstkPenaltyRigidCH.h"
#include "imstkCollidingObject.h"
#include "imstkVirtualCouplingObject.h"
#include "imstkCollisionData.h"
#include <g3log/g3log.hpp>
namespace imstk {
void
PenaltyRigidCH::computeContactForces(std::shared_ptr<CollidingObject> obj,
CollisionData& colData)
PenaltyRigidCH::computeContactForces()
{
auto movableObj = std::dynamic_pointer_cast<VirtualCouplingObject>(obj);
auto movableObj = std::dynamic_pointer_cast<VirtualCouplingObject>(m_obj);
if(movableObj == nullptr)
{
LOG(WARNING) << "PenaltyRigidCH::computeContactForces error: "
<< obj->getName() << " is not a virtualcoupling object. "
<< m_obj->getName() << " is not a virtualcoupling object. "
<< "(Rigid not yet supported, coming soon)";
return;
}
......@@ -44,11 +45,11 @@ PenaltyRigidCH::computeContactForces(std::shared_ptr<CollidingObject> obj,
Vec3d force = movableObj->getForce();
// If collision data, append forces
if(!colData.PDColData.empty())
if(!m_colData.PDColData.empty())
{
for(const auto& cd : colData.PDColData)
for(const auto& cd : m_colData.PDColData)
{
force += cd.direction * ((cd.penetrationDepth+1)*(cd.penetrationDepth+1)-1)*5;
force += cd.direction * ((cd.penetrationDepth+1)*(cd.penetrationDepth+1)-1)*10;
}
}
......
......@@ -22,14 +22,15 @@
#ifndef imstkPenaltyRigidCH_h
#define imstkPenaltyRigidCH_h
#include <memory>
#include "imstkCollisionHandling.h"
#include "imstkCollidingObject.h"
#include "imstkCollisionData.h"
#include <memory>
namespace imstk {
class CollidingObject;
class CollisionData;
class PenaltyRigidCH : public CollisionHandling