Commit c85ade66 authored by Nghia Truong's avatar Nghia Truong
Browse files

ENH: Rewrite CollisionData, support thread-safe operation (thread-safe push_back)

parent 7bdfdbb5
...@@ -23,52 +23,121 @@ ...@@ -23,52 +23,121 @@
#include <array> #include <array>
// imstk
#include "imstkMath.h" #include "imstkMath.h"
#include "imstkLogUtility.h"
#include "imstkParallelUtils.h"
namespace imstk 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 /// \struct PositionDirectionCollisionData
/// ///
/// \brief Point-penetration depth collision data /// \brief Point-penetration depth collision data
/// ///
struct PositionDirectionCollisionData struct PositionDirectionCollisionDataElement
{ {
Vec3d posA; Vec3d posA;
Vec3d posB; Vec3d posB;
Vec3d dirAtoB; Vec3d dirAtoB;
double penetrationDepth; double penetrationDepth;
}; };
class PositionDirectionCollisionData : public CollisionDataBase<PositionDirectionCollisionDataElement>
{
};
/// ///
/// \struct MeshToAnalyticalCollisionData /// \struct MeshToAnalyticalCollisionData
/// ///
/// \brief Mesh to analytical point-penetration depth collision data /// \brief Mesh to analytical point-penetration depth collision data
/// ///
struct MeshToAnalyticalCollisionData struct MeshToAnalyticalCollisionDataElement
{ {
size_t nodeId; uint32_t nodeIdx;
Vec3d penetrationVector; Vec3d penetrationVector;
}; };
class MeshToAnalyticalCollisionData : public CollisionDataBase<MeshToAnalyticalCollisionDataElement>
{
};
/// ///
/// \struct VertexTriangleCollisionData /// \struct VertexTriangleCollisionData
/// ///
/// \brief Vertex-triangle collision data /// \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 ...@@ -76,18 +145,14 @@ struct VertexTriangleCollisionData
/// ///
/// \brief Triangle-vertex collision data /// \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 ...@@ -95,18 +160,14 @@ struct TriangleVertexCollisionData
/// ///
/// \brief Edge-Edge collision data /// \brief Edge-Edge collision data
/// ///
struct EdgeEdgeCollisionData struct EdgeEdgeCollisionDataElement
{ {
std::pair<size_t, size_t> edgeIdA; std::pair<uint32_t, uint32_t> edgeIdA;
std::pair<size_t, size_t> edgeIdB; std::pair<uint32_t, uint32_t> edgeIdB;
float time; 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) class EdgeEdgeCollisionData : public CollisionDataBase<EdgeEdgeCollisionDataElement>
{ {
edgeIdA = std::pair<size_t, size_t>(eA_v1, eA_v2);
edgeIdB = std::pair<size_t, size_t>(eB_v1, eB_v2);
time = t;
}
}; };
/// ///
...@@ -114,7 +175,7 @@ struct EdgeEdgeCollisionData ...@@ -114,7 +175,7 @@ struct EdgeEdgeCollisionData
/// ///
/// \brief Point-tetrahedron collision data /// \brief Point-tetrahedron collision data
/// ///
struct PointTetrahedronCollisionData struct PointTetrahedronCollisionDataElement
{ {
enum CollisionType enum CollisionType
{ {
...@@ -123,29 +184,37 @@ struct PointTetrahedronCollisionData ...@@ -123,29 +184,37 @@ struct PointTetrahedronCollisionData
bPenetratingA = 2, // vertex is from mesh B, tetrahedron is from mesh A bPenetratingA = 2, // vertex is from mesh B, tetrahedron is from mesh A
bPenetratingB = 3 // B self-penetration bPenetratingB = 3 // B self-penetration
} collisionType; } collisionType;
size_t vertexId;
size_t tetreahedronId; uint32_t vertexIdx;
uint32_t tetreahedronIdx;
using WeightsArray = std::array<double, 4>; using WeightsArray = std::array<double, 4>;
WeightsArray BarycentricCoordinates; WeightsArray BarycentricCoordinates;
}; };
class PointTetrahedronCollisionData : public CollisionDataBase<PointTetrahedronCollisionDataElement>
{
};
struct PickingCollisionData ///
/// \brief The PickingCollisionData struct
///
struct PickingCollisionDataElement
{ {
// map of node and point position // map of node and point position
Vec3d ptPos; Vec3d ptPos;
size_t nodeId; uint32_t nodeIdx;
bool touchStatus; bool touchStatus;
}; };
class PickingCollisionData : public CollisionDataBase<PickingCollisionDataElement>
{
};
/// ///
/// \struct CollisionData /// \struct CollisionData
/// ///
/// \brief Class that is the holder of all types of collision data /// \brief Class that is the holder of all types of collision data
/// ///
class CollisionData struct CollisionData
{ {
public:
void clearAll() void clearAll()
{ {
PDColData.clear(); PDColData.clear();
...@@ -157,14 +226,12 @@ public: ...@@ -157,14 +226,12 @@ public:
NodePickData.clear(); NodePickData.clear();
} }
CollisionData() {} PositionDirectionCollisionData PDColData; ///< Position Direction collision data
VertexTriangleCollisionData VTColData; ///< Vertex Triangle collision data
std::vector<PositionDirectionCollisionData> PDColData; ///< Position Direction collision data TriangleVertexCollisionData TVColData; ///< Triangle Vertex collision data
std::vector<VertexTriangleCollisionData> VTColData; ///< Vertex Triangle collision data EdgeEdgeCollisionData EEColData; ///< Edge Edge collision data
std::vector<TriangleVertexCollisionData> TVColData; ///< Triangle Vertex collision data MeshToAnalyticalCollisionData MAColData; ///< Mesh to analytical collision data
std::vector<EdgeEdgeCollisionData> EEColData; ///< Edge Edge collision data PointTetrahedronCollisionData PTColData; ///< Point Tetrahedron collision data
std::vector<MeshToAnalyticalCollisionData> MAColData; ///< Mesh to analytical collision data PickingCollisionData NodePickData; ///< List of points that are picked
std::vector<PointTetrahedronCollisionData> PTColData; ///< Point Tetrahedron collision data
std::vector<PickingCollisionData> NodePickData; ///< List of points that are picked
}; };
} }
...@@ -82,7 +82,7 @@ BoneDrillingCH::erodeBone() ...@@ -82,7 +82,7 @@ BoneDrillingCH::erodeBone()
{ {
auto boneTetMesh = std::dynamic_pointer_cast<TetrahedralMesh>(m_bone->getCollidingGeometry()); auto boneTetMesh = std::dynamic_pointer_cast<TetrahedralMesh>(m_bone->getCollidingGeometry());
ParallelUtils::parallelFor(m_colData->MAColData.size(), ParallelUtils::parallelFor(m_colData->MAColData.getSize(),
[&](const size_t idx) [&](const size_t idx)
{ {
auto& cd = m_colData->MAColData[idx]; auto& cd = m_colData->MAColData[idx];
...@@ -116,7 +116,7 @@ BoneDrillingCH::processCollisionData() ...@@ -116,7 +116,7 @@ BoneDrillingCH::processCollisionData()
{ {
// Check if any collisions // Check if any collisions
const auto devicePosition = m_drill->getCollidingGeometry()->getTranslation(); const auto devicePosition = m_drill->getCollidingGeometry()->getTranslation();
if (m_colData->MAColData.empty()) if (m_colData->MAColData.isEmpty())
{ {
// Set the visual object position same as the colliding object position // Set the visual object position same as the colliding object position
m_drill->getVisualGeometry()->setTranslation(devicePosition); m_drill->getVisualGeometry()->setTranslation(devicePosition);
...@@ -128,9 +128,10 @@ BoneDrillingCH::processCollisionData() ...@@ -128,9 +128,10 @@ BoneDrillingCH::processCollisionData()
// Aggregate collision data // Aggregate collision data
Vec3d t = Vec3d::Zero(); Vec3d t = Vec3d::Zero();
double maxDepthSqr = MIN_D; double maxDepthSqr = MIN_D;
for (const auto& cd : m_colData->MAColData) for (size_t i = 0; i < m_colData->MAColData.getSize(); ++i)
{ {
if (m_nodeRemovalStatus[cd.nodeId]) const auto& cd = m_colData->MAColData[i];
if (m_nodeRemovalStatus[cd.nodeIdx])
{ {
continue; continue;
} }
......
...@@ -75,12 +75,13 @@ PBDCollisionHandling::generatePBDConstraints() ...@@ -75,12 +75,13 @@ PBDCollisionHandling::generatePBDConstraints()
const auto map1 = m_pbdObject1->getPhysicsToCollidingMap(); const auto map1 = m_pbdObject1->getPhysicsToCollidingMap();
const auto map2 = m_pbdObject2->getPhysicsToCollidingMap(); const auto map2 = m_pbdObject2->getPhysicsToCollidingMap();
//std::cout << "EE: " << m_colData->EEColData.size() << "TV: " << m_colData->TVColData.size() << std::endl; // std::cout << "EE: " << m_colData->EEColData.getSize() << "TV: " << m_colData->VTColData.getSize() << std::endl;
// Generate edge-edge pbd constraints // Generate edge-edge pbd constraints
for (auto& colData : m_colData->EEColData) for (size_t i = 0; i < m_colData->EEColData.getSize(); ++i)
{ {
auto c = std::make_shared<PbdEdgeEdgeConstraint>(); const auto& colData = m_colData->EEColData[i];
auto c = std::make_shared<PbdEdgeEdgeConstraint>();
size_t edgeA1, edgeA2; size_t edgeA1, edgeA2;
if (map1) if (map1)
...@@ -113,9 +114,10 @@ PBDCollisionHandling::generatePBDConstraints() ...@@ -113,9 +114,10 @@ PBDCollisionHandling::generatePBDConstraints()
} }
// Generate triangle-vertex pbd constraints // Generate triangle-vertex pbd constraints
for (auto& colData : m_colData->TVColData) for (size_t i = 0; i < m_colData->TVColData.getSize(); ++i)
{ {
const auto& triVerts = colGeo2->getTrianglesVertices()[colData.triIdA]; const auto& colData = m_colData->TVColData[i];
const auto& triVerts = colGeo2->getTrianglesVertices()[colData.triIdx];
const auto c = std::make_shared<PbdPointTriangleConstraint>(); const auto c = std::make_shared<PbdPointTriangleConstraint>();
...@@ -134,7 +136,7 @@ PBDCollisionHandling::generatePBDConstraints() ...@@ -134,7 +136,7 @@ PBDCollisionHandling::generatePBDConstraints()
} }
c->initConstraint(dynaModel1, c->initConstraint(dynaModel1,
colData.vertexIdB, colData.vertexIdx,
dynaModel2, dynaModel2,
v1, v1,
v2, v2,
......
...@@ -61,7 +61,7 @@ PenaltyCH::processCollisionData() ...@@ -61,7 +61,7 @@ PenaltyCH::processCollisionData()
void void
PenaltyCH::computeContactForcesAnalyticRigid(const std::shared_ptr<CollidingObject>& analyticObj) PenaltyCH::computeContactForcesAnalyticRigid(const std::shared_ptr<CollidingObject>& analyticObj)
{ {
if (m_colData->PDColData.empty()) if (m_colData->PDColData.isEmpty())
{ {
return; return;
} }
...@@ -76,8 +76,9 @@ PenaltyCH::computeContactForcesAnalyticRigid(const std::shared_ptr<CollidingObje ...@@ -76,8 +76,9 @@ PenaltyCH::computeContactForcesAnalyticRigid(const std::shared_ptr<CollidingObje
// If collision data is valid, append forces // If collision data is valid, append forces
Vec3d force(0., 0., 0.); Vec3d force(0., 0., 0.);
for (const auto& cd : m_colData->PDColData) for (size_t i = 0; i < m_colData->PDColData.getSize(); ++i)
{ {
const auto& cd = m_colData->PDColData[i];
if (m_side == CollisionHandling::Side::A) if (m_side == CollisionHandling::Side::A)
{ {
force -= cd.dirAtoB * ((cd.penetrationDepth + 1) * (cd.penetrationDepth + 1) - 1) * 10; force -= cd.dirAtoB * ((cd.penetrationDepth + 1) * (cd.penetrationDepth + 1) - 1) * 10;
...@@ -95,7 +96,7 @@ PenaltyCH::computeContactForcesAnalyticRigid(const std::shared_ptr<CollidingObje ...@@ -95,7 +96,7 @@ PenaltyCH::computeContactForcesAnalyticRigid(const std::shared_ptr<CollidingObje
void void
PenaltyCH::computeContactForcesDiscreteDeformable(const std::shared_ptr<DeformableObject>& deformableObj) PenaltyCH::computeContactForcesDiscreteDeformable(const std::shared_ptr<DeformableObject>& deformableObj)
{ {
if (m_colData->MAColData.empty()) if (m_colData->MAColData.isEmpty())
{ {
return; return;
} }
...@@ -113,7 +114,7 @@ PenaltyCH::computeContactForcesDiscreteDeformable(const std::shared_ptr<Deformab ...@@ -113,7 +114,7 @@ PenaltyCH::computeContactForcesDiscreteDeformable(const std::shared_ptr<Deformab
// If collision data, append forces // If collision data, append forces
ParallelUtils::SpinLock lock; ParallelUtils::SpinLock lock;
ParallelUtils::parallelFor(m_colData->MAColData.size(), ParallelUtils::parallelFor(m_colData->MAColData.getSize(),
[&](const size_t idx) { [&](const size_t idx) {
const auto& cd = m_colData->MAColData[idx]; const auto& cd = m_colData->MAColData[idx];
const auto nodeDofID = static_cast<Eigen::Index>(3 * cd.nodeId); const auto nodeDofID = static_cast<Eigen::Index>(3 * cd.nodeId);
......
...@@ -52,7 +52,7 @@ PickingCH::addPickConstraints(std::shared_ptr<DeformableObject> deformableObj) ...@@ -52,7 +52,7 @@ PickingCH::addPickConstraints(std::shared_ptr<DeformableObject> deformableObj)
{ {
m_DynamicLinearProjConstraints->clear(); m_DynamicLinearProjConstraints->clear();
if (m_colData->NodePickData.empty()) if (m_colData->NodePickData.isEmpty())
{ {
return; return;
} }
...@@ -72,7 +72,7 @@ PickingCH::addPickConstraints(std::shared_ptr<DeformableObject> deformableObj) ...@@ -72,7 +72,7 @@ PickingCH::addPickConstraints(std::shared_ptr<DeformableObject> deformableObj)
// If collision data, append LPC constraints // If collision data, append LPC constraints
ParallelUtils::SpinLock lock; ParallelUtils::SpinLock lock;
ParallelUtils::parallelFor(m_colData->NodePickData.size(), ParallelUtils::parallelFor(m_colData->NodePickData.getSize(),
[&](const size_t idx) { [&](const size_t idx) {
const auto& cd = m_colData->NodePickData[idx]; const auto& cd = m_colData->NodePickData[idx];
const auto nodeDof = static_cast<Eigen::Index>(3 * cd.nodeId); const auto nodeDof = static_cast<Eigen::Index>(3 * cd.nodeId);
......
...@@ -49,7 +49,7 @@ SPHCollisionHandling::processCollisionData() ...@@ -49,7 +49,7 @@ SPHCollisionHandling::processCollisionData()
#endif #endif
auto& state = SPHModel->getState(); auto& state = SPHModel->getState();
ParallelUtils::parallelFor(m_colData->MAColData.size(), ParallelUtils::parallelFor(m_colData->MAColData.getSize(),
[&](const size_t idx) [&](const size_t idx)
{ {
const auto& cd = m_colData->MAColData[idx]; const auto& cd = m_colData->MAColData[idx];
......
...@@ -39,7 +39,7 @@ VirtualCouplingCH::processCollisionData() ...@@ -39,7 +39,7 @@ VirtualCouplingCH::processCollisionData()
// Check if any collisions // Check if any collisions
const auto collidingObjPos = collidingGeometry->getPosition(); const auto collidingObjPos = collidingGeometry->getPosition();
if (m_colData->PDColData.empty()) if (m_colData->PDColData.isEmpty())
{ {
// Set the visual object position same as the colliding object position // Set the visual object position same as the colliding object position
visualGeometry->setPosition(collidingObjPos); visualGeometry->setPosition(collidingObjPos);
...@@ -48,8 +48,9 @@ VirtualCouplingCH::processCollisionData() ...@@ -48,8 +48,9 @@ VirtualCouplingCH::processCollisionData()
// Aggregate collision data // Aggregate collision data
Vec3d t = Vec3d::Zero(); Vec3d t = Vec3d::Zero();
for (const auto& cd : m_colData->PDColData) for (size_t i = 0; i < m_colData->PDColData.getSize(); ++i)
{ {
const auto& cd = m_colData->PDColData[i];
t += cd.dirAtoB * cd.penetrationDepth; t += cd.dirAtoB * cd.penetrationDepth;
} }
......
...@@ -28,6 +28,7 @@ ...@@ -28,6 +28,7 @@
#include "imstkIsometricMap.h" #include "imstkIsometricMap.h"
#include "imstkMeshIO.h" #include "imstkMeshIO.h"
#include "imstkTetraToTetraCD.h" #include "imstkTetraToTetraCD.h"
#include "imstkTetrahedralMesh.h"
using namespace imstk; using namespace imstk;
...@@ -65,11 +66,11 @@ TEST_F(imstkTetraToTetraCDTest, NoSelfIntersection) ...@@ -65,11 +66,11 @@ TEST_F(imstkTetraToTetraCDTest, NoSelfIntersection)
m_CD = new TetraToTetraCD(a, b, cd); m_CD = new TetraToTetraCD(a, b, cd);
m_CD->computeCollisionData(); m_CD->computeCollisionData();
EXPECT_EQ(cd->PTColData.size(), 0); EXPECT_EQ(cd->PTColData.getSize(), 0);
m_CD = new TetraToTetraCD(b, a, cd); m_CD = new TetraToTetraCD(b, a, cd);
m_CD->computeCollisionData(); m_CD->computeCollisionData();
EXPECT_EQ(cd->PTColData.size(), 0); EXPECT_EQ(cd->PTColData.getSize(), 0);
} }
TEST_F(imstkTetraToTetraCDTest, IntersectionThenNoIntersection1T) TEST_F(imstkTetraToTetraCDTest, IntersectionThenNoIntersection1T)
...@@ -82,28 +83,28 @@ TEST_F(imstkTetraToTetraCDTest, IntersectionThenNoIntersection1T) ...@@ -82,28 +83,28 @@ TEST_F(imstkTetraToTetraCDTest, IntersectionThenNoIntersection1T)
auto cd = std::make_shared<CollisionData>(); auto cd = std::make_shared<CollisionData>();
m_CD = new TetraToTetraCD(a, b, cd); m_CD = new TetraToTetraCD(a, b, cd);
m_CD->computeCollisionData(); m_CD->computeCollisionData();
EXPECT_EQ(cd->PTColData.size(), 1); EXPECT_EQ(cd->PTColData.getSize(), 1);
EXPECT_EQ(cd->PTColData[0].collisionType, PointTetrahedronCollisionData::bPenetratingA); EXPECT_EQ(cd->PTColData[0].collisionType, PointTetrahedronCollisionDataElement::bPenetratingA);