Commit 2e92b804 authored by Alexis Girault's avatar Alexis Girault
Browse files

ENH: CollisionData per interaction + mesh CCD

1) Change the interactionPair structure to save
only one collisiondata per structure, and inform
the collisionhandling of a "side" defining which
object it is handling, therefore which way it needs
to read the collisionData

2) Update MeshToMeshCD to properly populate data
based on 1.
parent a1fc7e1e
......@@ -22,36 +22,60 @@
#ifndef imstkCollisionData_h
#define imstkCollisionData_h
//#include <pair>
#include "imstkMath.h"
namespace imstk {
struct PositionDirectionCollisionData
{
Vec3d position;
Vec3d direction;
Vec3d posA;
Vec3d posB;
Vec3d dirAtoB;
double penetrationDepth;
};
struct VertexDirectionCollisionData
struct VertexTriangleCollisionData
{
size_t vertexId;
Vec3d direction;
double penetrationDepth;
size_t vertexIdA;
size_t triIdB;
float time;
VertexTriangleCollisionData(size_t vIdA, size_t fIdB, float t = -1)
{
vertexIdA = vIdA;
triIdB = fIdB;
time = t;
}
};
struct VertexTriangleCollisionData
struct TriangleVertexCollisionData
{
size_t vertexId;
size_t triangleId;
double penetrationDepth;
size_t triIdA;
size_t vertexIdB;
float time;
TriangleVertexCollisionData(size_t fIdA, size_t vIdB, float t = -1)
{
triIdA = fIdA;
vertexIdB = vIdB;
time = t;
}
};
struct EdgeEdgeCollisionData
{
size_t edgeId1;
size_t edgeId2;
double shortestDistance;
std::pair<size_t, size_t> edgeIdA;
std::pair<size_t, size_t> edgeIdB;
float time;
EdgeEdgeCollisionData(size_t eA_v1, size_t eA_v2, size_t eB_v1, size_t eB_v2, 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 CollisionData
......@@ -61,14 +85,14 @@ public:
void clearAll()
{
PDColData.clear();
VDColData.clear();
VTColData.clear();
TVColData.clear();
EEColData.clear();
}
std::vector<PositionDirectionCollisionData> PDColData; //!< Position Direction collision data
std::vector<VertexDirectionCollisionData> VDColData; //!< Vertex 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
};
}
......
......@@ -38,8 +38,7 @@ std::shared_ptr<CollisionDetection>
CollisionDetection::make_collision_detection(const Type& type,
std::shared_ptr<CollidingObject> objA,
std::shared_ptr<CollidingObject> objB,
CollisionData &CDA,
CollisionData &CDB)
CollisionData &colData)
{
switch (type)
{
......@@ -55,7 +54,7 @@ CollisionDetection::make_collision_detection(const Type& type,
<< "invalid object geometries for PlaneToSphere collision detection.";
return nullptr;
}
return std::make_shared<PlaneToSphereCD>(plane, sphere, CDA, CDB);
return std::make_shared<PlaneToSphereCD>(plane, sphere, colData);
}break;
case Type::SphereToSphere:
{
......@@ -69,7 +68,7 @@ CollisionDetection::make_collision_detection(const Type& type,
<< "invalid object geometries for SphereToSphere collision detection.";
return nullptr;
}
return std::make_shared<SphereToSphereCD>(sphereA, sphereB, CDA, CDB);
return std::make_shared<SphereToSphereCD>(sphereA, sphereB, colData);
}break;
case Type::MeshToMesh:
{
......@@ -83,7 +82,7 @@ CollisionDetection::make_collision_detection(const Type& type,
<< "invalid object geometries for MeshToMesh collision detection.";
return nullptr;
}
return std::make_shared<MeshToMeshCD>(meshA, meshB, CDA, CDB);
return std::make_shared<MeshToMeshCD>(meshA, meshB, colData);
}break;
default:
{
......@@ -98,4 +97,10 @@ CollisionDetection::getType() const
{
return m_type;
}
const CollisionData&
CollisionDetection::getCollisionData() const
{
return m_colData;
}
}
......@@ -46,16 +46,14 @@ public:
static std::shared_ptr<CollisionDetection> make_collision_detection(const Type& type,
std::shared_ptr<CollidingObject> objA,
std::shared_ptr<CollidingObject> objB,
CollisionData& CDA,
CollisionData& CDB);
CollisionData& colData);
///
/// \brief Constructor
///
CollisionDetection(const Type& type, CollisionData& CDA, CollisionData& CDB) :
CollisionDetection(const Type& type, CollisionData& colData) :
m_type(type),
m_CDA(CDA),
m_CDB(CDB)
m_colData(colData)
{}
///
......@@ -73,11 +71,15 @@ public:
///
const Type& getType() const;
///
/// \brief Returns collision data
///
const CollisionData& getCollisionData() const;
protected:
Type m_type; //!< Collision detection algorithm type
CollisionData& m_CDA;
CollisionData& m_CDB;
Type m_type; //!< Collision detection algorithm type
CollisionData& m_colData; //!< Collision data
};
}
......
......@@ -28,40 +28,10 @@
namespace imstk {
void
EECallback(unsigned int e1_v1, unsigned int e1_v2,
unsigned int e2_v1, unsigned int e2_v2,
float t, void *userdata)
{
auto CD = reinterpret_cast<MeshToMeshCD*>(userdata);
CD->getType();
LOG(INFO) <<"EE: e1("<<e1_v1<<", "<<e1_v2<<"), e2("<<e2_v1<<", "<<e2_v2<<") \t@ t="<<t;
}
void
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: 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),
CollisionData& colData) :
CollisionDetection(CollisionDetection::Type::PlaneToSphere, colData),
m_meshA(meshA),
m_meshB(meshB)
{
......@@ -69,9 +39,9 @@ MeshToMeshCD::MeshToMeshCD(std::shared_ptr<SurfaceMesh> meshA,
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);
m_modelA->SetEECallBack(MeshToMeshCD::EECallback, this);
m_modelA->SetVFCallBack(MeshToMeshCD::VFCallbackA, this);
m_modelB->SetVFCallBack(MeshToMeshCD::VFCallbackB, this);
// Build BVH
m_modelA->BuildBVH(false);
......@@ -99,4 +69,49 @@ MeshToMeshCD::computeCollisionData()
m_modelA->Collide(m_modelB.get());
}
void
MeshToMeshCD::EECallback(unsigned int eA_v1, unsigned int eA_v2,
unsigned int eB_v1, unsigned int eB_v2,
float t, void *userdata)
{
auto CD = reinterpret_cast<MeshToMeshCD*>(userdata);
if(CD == nullptr)
{
return;
}
auto colData = CD->getCollisionData();
colData.EEColData.push_back(EdgeEdgeCollisionData(eA_v1, eA_v2, eB_v1, eB_v2, t));
//LOG(INFO) <<"EE: eA("<<eA_v1<<", "<<eA_v2<<"), eB("<<eB_v1<<", "<<eB_v2<<") \t@ t="<<t;
}
void
MeshToMeshCD::VFCallbackA(unsigned int fidA, unsigned int vidB,
float t, void *userdata)
{
auto CD = reinterpret_cast<MeshToMeshCD*>(userdata);
if(CD == nullptr)
{
return;
}
auto colData = CD->getCollisionData();
colData.TVColData.push_back(TriangleVertexCollisionData(fidA, vidB, t));
//LOG(INFO) <<"VF: fA("<<fidA<<"), vB("<<vidB<<") \t\t@ t="<<t;
}
void
MeshToMeshCD::VFCallbackB(unsigned int fidB, unsigned int vidA,
float t, void *userdata)
{
auto CD = reinterpret_cast<MeshToMeshCD*>(userdata);
if(CD == nullptr)
{
return;
}
auto colData = CD->getCollisionData();
colData.VTColData.push_back(VertexTriangleCollisionData(vidA, fidB, t));
//LOG(INFO) <<"VF: vA("<<vidA<<"), fB("<<fidB<<") \t\t@ t="<<t;
}
}
......@@ -42,8 +42,7 @@ public:
///
MeshToMeshCD(std::shared_ptr<SurfaceMesh> planeA,
std::shared_ptr<SurfaceMesh> sphereB,
CollisionData& CDA,
CollisionData& CDB);
CollisionData& colData);
///
/// \brief Destructor
......@@ -57,7 +56,23 @@ public:
private:
bool m_initialized = false;
///
/// \brief Callback defining edge to edge collision (A - B)
///
static void EECallback(unsigned int eA_v1, unsigned int eA_v2,
unsigned int eB_v1, unsigned int eB_v2,
float t, void *userdata);
///
/// \brief Callback defining vertex to triangle collision (A - B)
///
static void VFCallbackA(unsigned int fidA, unsigned int vidB,
float t, void *userdata);
///
/// \brief Callback defining vertex to triangle collision (B - A)
///
static void VFCallbackB(unsigned int fidB, unsigned int vidA,
float t, void *userdata);
std::shared_ptr<SurfaceMesh> m_meshA;
std::shared_ptr<SurfaceMesh> m_meshB;
std::shared_ptr<DeformModel> m_modelA;
......
......@@ -34,24 +34,23 @@ void
PlaneToSphereCD::computeCollisionData()
{
// Clear collisionData
m_CDA.clearAll();
m_CDB.clearAll();
m_colData.clearAll();
// Get geometry properties
Vec3d sP = m_sphereB->getPosition();
Vec3d sphereBPos = m_sphereB->getPosition();
double r = m_sphereB->getRadius() * m_sphereB->getScaling();
Vec3d pP = m_planeA->getPosition();
Vec3d planeAPos = m_planeA->getPosition();
Vec3d n = m_planeA->getNormal();
// Compute shortest distance
double d = (sP-pP).dot(n);
double d = (sphereBPos-planeAPos).dot(n);
// Define sphere to plane direction
Vec3d dirBToA = -n;
Vec3d dirAToB = n;
if( d < 0 )
{
d = -d;
dirBToA = n;
dirAToB = -n;
}
// Return if no penetration
......@@ -62,12 +61,11 @@ PlaneToSphereCD::computeCollisionData()
}
// Compute collision points
Vec3d pC = sP + dirBToA*d;
Vec3d sC = sP + dirBToA*r;
Vec3d planeAColPt = sphereBPos - dirAToB*d;
Vec3d sphereBColPt = sphereBPos - dirAToB*r;
// Set collisionData
m_CDA.PDColData.push_back({pC, dirBToA, penetrationDepth});
m_CDB.PDColData.push_back({sC, -dirBToA, penetrationDepth});
m_colData.PDColData.push_back({planeAColPt, sphereBColPt, dirAToB, penetrationDepth});
}
}
......@@ -41,9 +41,8 @@ public:
///
PlaneToSphereCD(std::shared_ptr<Plane> planeA,
std::shared_ptr<Sphere> sphereB,
CollisionData& CDA,
CollisionData& CDB) :
CollisionDetection(CollisionDetection::Type::PlaneToSphere, CDA, CDB),
CollisionData& colData) :
CollisionDetection(CollisionDetection::Type::PlaneToSphere, colData),
m_planeA(planeA),
m_sphereB(sphereB)
{}
......
......@@ -33,20 +33,19 @@ void
SphereToSphereCD::computeCollisionData()
{
// Clear collisionData
m_CDA.clearAll();
m_CDB.clearAll();
m_colData.clearAll();
// Get geometry properties
Vec3d sAP = m_sphereA->getPosition();
Vec3d sphereAPos = m_sphereA->getPosition();
double rA = m_sphereA->getRadius() * m_sphereA->getScaling();
Vec3d sBP = m_sphereB->getPosition();
Vec3d sphereBPos = m_sphereB->getPosition();
double rB = m_sphereB->getRadius() * m_sphereB->getScaling();
// Compute direction vector
Vec3d dirBToA = sAP - sBP;
Vec3d dirAToB = sphereBPos - sphereAPos;
// Compute shortest distance
double d = dirBToA.norm();
double d = dirAToB.norm();
// Return if no penetration
double penetrationDepth = rA+rB-d;
......@@ -56,13 +55,12 @@ SphereToSphereCD::computeCollisionData()
}
// Compute collision points
dirBToA.normalize();
Vec3d sAC = sAP - dirBToA*rA;
Vec3d sBC = sBP + dirBToA*rB;
dirAToB.normalize();
Vec3d sphereAColPt = sphereAPos + dirAToB*rA;
Vec3d sphereBColPt = sphereBPos - dirAToB*rB;
// Set collisionData
m_CDA.PDColData.push_back({sAC, dirBToA, penetrationDepth});
m_CDB.PDColData.push_back({sBC, -dirBToA, penetrationDepth});
m_colData.PDColData.push_back({sphereAColPt, sphereBColPt, dirAToB, penetrationDepth});
}
}
......@@ -40,9 +40,8 @@ public:
///
SphereToSphereCD(std::shared_ptr<Sphere> sphereA,
std::shared_ptr<Sphere> sphereB,
CollisionData& CDA,
CollisionData& CDB) :
CollisionDetection(CollisionDetection::Type::SphereToSphere, CDA, CDB),
CollisionData& colData) :
CollisionDetection(CollisionDetection::Type::SphereToSphere, colData),
m_sphereA(sphereA),
m_sphereB(sphereB)
{}
......
......@@ -29,8 +29,9 @@ namespace imstk {
std::shared_ptr<CollisionHandling>
CollisionHandling::make_collision_handling(const Type& type,
const Side& side,
const CollisionData &colData,
std::shared_ptr<CollidingObject> objA,
CollisionData &CDA,
std::shared_ptr<CollidingObject> objB)
{
switch (type)
......@@ -44,7 +45,7 @@ CollisionHandling::make_collision_handling(const Type& type,
<< "penalty collision handling not yet implemented for non-rigid objects.";
return nullptr;
}
return std::make_shared<PenaltyRigidCH>(objA, CDA);
return std::make_shared<PenaltyRigidCH>(side, colData, objA);
}break;
default:
......
......@@ -41,19 +41,33 @@ public:
Penalty
};
enum class Side
{
A,
B,
AB
};
///
/// \brief Static factory for collision handling sub classes
///
static std::shared_ptr<CollisionHandling> make_collision_handling(
const Type& type,
const Side& side,
const CollisionData& colData,
std::shared_ptr<CollidingObject> objA,
CollisionData& CDA,
std::shared_ptr<CollidingObject> objB = nullptr);
///
/// \brief Constructor
///
CollisionHandling(const Type& type) : m_type(type) {}
CollisionHandling(const Type& type,
const Side& side,
const CollisionData& colData) :
m_type(type),
m_side(side),
m_colData(colData)
{}
///
/// \brief Destructor
......@@ -72,7 +86,9 @@ public:
protected:
Type m_type; //!< Collision handling algorithm type
Type m_type; //!< Collision handling algorithm type
Side m_side; //!< Direction of the collisionData
const CollisionData& m_colData; //!< Collision data
};
}
......
......@@ -49,7 +49,14 @@ PenaltyRigidCH::computeContactForces()
{
for(const auto& cd : m_colData.PDColData)
{
force += cd.direction * ((cd.penetrationDepth+1)*(cd.penetrationDepth+1)-1)*10;
if (m_side == CollisionHandling::Side::A)
{
force -= cd.dirAtoB * ((cd.penetrationDepth+1)*(cd.penetrationDepth+1)-1)*10;
}
else if (m_side == CollisionHandling::Side::B)
{
force += cd.dirAtoB * ((cd.penetrationDepth+1)*(cd.penetrationDepth+1)-1)*10;
}
}
}
......
......@@ -38,11 +38,13 @@ public:
///
/// \brief Constructor
///
PenaltyRigidCH(std::shared_ptr<CollidingObject> obj,
CollisionData& colData) :
CollisionHandling(CollisionHandling::Type::Penalty),
m_obj(obj),
m_colData(colData)
PenaltyRigidCH(const Side& side,
const CollisionData& colData,
std::shared_ptr<CollidingObject> obj) :
CollisionHandling(CollisionHandling::Type::Penalty,
side,
colData),
m_obj(obj)
{}
///
......@@ -58,7 +60,6 @@ public:
private:
std::shared_ptr<CollidingObject> m_obj;
CollisionData& m_colData;
};
}
......
......@@ -56,10 +56,7 @@ InteractionPair::InteractionPair(std::shared_ptr<CollidingObject> A,
}
// Collision Detection
m_colDataMap[A] = CollisionData();
m_colDataMap[B] = CollisionData();
std::shared_ptr<CollisionDetection> CD = CollisionDetection::make_collision_detection(CDType, A, B,
m_colDataMap[A], m_colDataMap[B]);
std::shared_ptr<CollisionDetection> CD = CollisionDetection::make_collision_detection(CDType, A, B, m_colData);
if (CD == nullptr)
{
LOG(WARNING) << "InteractionPair error: can not instantiate collision detection algorithm.";
......@@ -70,7 +67,7 @@ InteractionPair::InteractionPair(std::shared_ptr<CollidingObject> A,
std::shared_ptr<CollisionHandling> CHA;
if (A->getType() != SceneObject::Type::Static)
{
CHA = CollisionHandling::make_collision_handling(CHAType, A, m_colDataMap[A], B);
CHA = CollisionHandling::make_collision_handling(CHAType, CollisionHandling::Side::A, m_colData, A, B);
if (CHA == nullptr)
{
LOG(WARNING) << "InteractionPair error: can not instantiate collision handling for '"
......@@ -83,7 +80,7 @@ InteractionPair::InteractionPair(std::shared_ptr<CollidingObject> A,
std::shared_ptr<CollisionHandling> CHB;
if (B->getType() != SceneObject::Type::Static)
{
CHB = CollisionHandling::make_collision_handling(CHBType, B, m_colDataMap[B], A);
CHB = CollisionHandling::make_collision_handling(CHBType, CollisionHandling::Side::B, m_colData, B, A);
if (CHB == nullptr)
{
LOG(WARNING) << "InteractionPair error: can not instantiate collision handling for '"
......@@ -95,8 +92,8 @@ InteractionPair::InteractionPair(std::shared_ptr<CollidingObject> A,
// Init interactionPair
m_objects = ObjectsPair(A, B);
m_colDetect = CD;
m_colHandlingMap[A] = CHA;
m_colHandlingMap[B] = CHB;
m_colHandlingA = CHA;
m_colHandlingB = CHB;
m_valid = true;
}
......@@ -113,7 +110,7 @@ InteractionPair::computeCollisionData()
}
void