Commit c85ade66 authored by Nghia Truong's avatar Nghia Truong

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

parent 7bdfdbb5
......@@ -23,52 +23,121 @@
#include <array>
// imstk
#include "imstkMath.h"
#include "imstkLogUtility.h"
#include "imstkParallelUtils.h"
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
///
/// \brief Point-penetration depth collision data
///
struct PositionDirectionCollisionData
struct PositionDirectionCollisionDataElement
{
Vec3d posA;
Vec3d posB;
Vec3d dirAtoB;
double penetrationDepth;
};
class PositionDirectionCollisionData : public CollisionDataBase<PositionDirectionCollisionDataElement>
{
};
///
/// \struct MeshToAnalyticalCollisionData
///
/// \brief Mesh to analytical point-penetration depth collision data
///
struct MeshToAnalyticalCollisionData
struct MeshToAnalyticalCollisionDataElement
{
size_t nodeId;
uint32_t nodeIdx;
Vec3d penetrationVector;
};
class MeshToAnalyticalCollisionData : public CollisionDataBase<MeshToAnalyticalCollisionDataElement>
{
};
///
/// \struct VertexTriangleCollisionData
///
/// \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
///
/// \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
///
/// \brief Edge-Edge collision data
///
struct EdgeEdgeCollisionData
struct EdgeEdgeCollisionDataElement
{
std::pair<size_t, size_t> edgeIdA;
std::pair<size_t, size_t> edgeIdB;
std::pair<uint32_t, uint32_t> edgeIdA;
std::pair<uint32_t, uint32_t> edgeIdB;
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)
{
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 EdgeEdgeCollisionData : public CollisionDataBase<EdgeEdgeCollisionDataElement>
{
};
///
......@@ -114,7 +175,7 @@ struct EdgeEdgeCollisionData
///
/// \brief Point-tetrahedron collision data
///
struct PointTetrahedronCollisionData
struct PointTetrahedronCollisionDataElement
{
enum CollisionType
{
......@@ -123,29 +184,37 @@ struct PointTetrahedronCollisionData
bPenetratingA = 2, // vertex is from mesh B, tetrahedron is from mesh A
bPenetratingB = 3 // B self-penetration
} collisionType;
size_t vertexId;
size_t tetreahedronId;
uint32_t vertexIdx;
uint32_t tetreahedronIdx;
using WeightsArray = std::array<double, 4>;
WeightsArray BarycentricCoordinates;
};
class PointTetrahedronCollisionData : public CollisionDataBase<PointTetrahedronCollisionDataElement>
{
};
struct PickingCollisionData
///
/// \brief The PickingCollisionData struct
///
struct PickingCollisionDataElement
{
// map of node and point position
Vec3d ptPos;
size_t nodeId;
uint32_t nodeIdx;
bool touchStatus;
};
class PickingCollisionData : public CollisionDataBase<PickingCollisionDataElement>
{
};
///
/// \struct CollisionData
///
/// \brief Class that is the holder of all types of collision data
///
class CollisionData
struct CollisionData
{
public:
void clearAll()
{
PDColData.clear();
......@@ -157,14 +226,12 @@ public:
NodePickData.clear();
}
CollisionData() {}
std::vector<PositionDirectionCollisionData> PDColData; ///< Position 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
std::vector<MeshToAnalyticalCollisionData> MAColData; ///< Mesh to analytical collision data
std::vector<PointTetrahedronCollisionData> PTColData; ///< Point Tetrahedron collision data
std::vector<PickingCollisionData> NodePickData; ///< List of points that are picked
PositionDirectionCollisionData PDColData; ///< Position Direction collision data
VertexTriangleCollisionData VTColData; ///< Vertex Triangle collision data
TriangleVertexCollisionData TVColData; ///< Triangle Vertex collision data
EdgeEdgeCollisionData EEColData; ///< Edge Edge collision data
MeshToAnalyticalCollisionData MAColData; ///< Mesh to analytical collision data
PointTetrahedronCollisionData PTColData; ///< Point Tetrahedron collision data
PickingCollisionData NodePickData; ///< List of points that are picked
};
}
......@@ -82,7 +82,7 @@ BoneDrillingCH::erodeBone()
{
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)
{
auto& cd = m_colData->MAColData[idx];
......@@ -116,7 +116,7 @@ BoneDrillingCH::processCollisionData()
{
// Check if any collisions
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
m_drill->getVisualGeometry()->setTranslation(devicePosition);
......@@ -128,9 +128,10 @@ BoneDrillingCH::processCollisionData()
// Aggregate collision data
Vec3d t = Vec3d::Zero();
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;
}
......
......@@ -75,12 +75,13 @@ PBDCollisionHandling::generatePBDConstraints()
const auto map1 = m_pbdObject1->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
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;
if (map1)
......@@ -113,9 +114,10 @@ PBDCollisionHandling::generatePBDConstraints()
}
// 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>();
......@@ -134,7 +136,7 @@ PBDCollisionHandling::generatePBDConstraints()
}
c->initConstraint(dynaModel1,
colData.vertexIdB,
colData.vertexIdx,
dynaModel2,
v1,
v2,
......
......@@ -61,7 +61,7 @@ PenaltyCH::processCollisionData()
void
PenaltyCH::computeContactForcesAnalyticRigid(const std::shared_ptr<CollidingObject>& analyticObj)
{
if (m_colData->PDColData.empty())
if (m_colData->PDColData.isEmpty())
{
return;
}
......@@ -76,8 +76,9 @@ PenaltyCH::computeContactForcesAnalyticRigid(const std::shared_ptr<CollidingObje
// If collision data is valid, append forces
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)
{
force -= cd.dirAtoB * ((cd.penetrationDepth + 1) * (cd.penetrationDepth + 1) - 1) * 10;
......@@ -95,7 +96,7 @@ PenaltyCH::computeContactForcesAnalyticRigid(const std::shared_ptr<CollidingObje
void
PenaltyCH::computeContactForcesDiscreteDeformable(const std::shared_ptr<DeformableObject>& deformableObj)
{
if (m_colData->MAColData.empty())
if (m_colData->MAColData.isEmpty())
{
return;
}
......@@ -113,7 +114,7 @@ PenaltyCH::computeContactForcesDiscreteDeformable(const std::shared_ptr<Deformab
// If collision data, append forces
ParallelUtils::SpinLock lock;
ParallelUtils::parallelFor(m_colData->MAColData.size(),
ParallelUtils::parallelFor(m_colData->MAColData.getSize(),
[&](const size_t idx) {
const auto& cd = m_colData->MAColData[idx];
const auto nodeDofID = static_cast<Eigen::Index>(3 * cd.nodeId);
......
......@@ -52,7 +52,7 @@ PickingCH::addPickConstraints(std::shared_ptr<DeformableObject> deformableObj)
{
m_DynamicLinearProjConstraints->clear();
if (m_colData->NodePickData.empty())
if (m_colData->NodePickData.isEmpty())
{
return;
}
......@@ -72,7 +72,7 @@ PickingCH::addPickConstraints(std::shared_ptr<DeformableObject> deformableObj)
// If collision data, append LPC constraints
ParallelUtils::SpinLock lock;
ParallelUtils::parallelFor(m_colData->NodePickData.size(),
ParallelUtils::parallelFor(m_colData->NodePickData.getSize(),
[&](const size_t idx) {
const auto& cd = m_colData->NodePickData[idx];
const auto nodeDof = static_cast<Eigen::Index>(3 * cd.nodeId);
......
......@@ -49,7 +49,7 @@ SPHCollisionHandling::processCollisionData()
#endif
auto& state = SPHModel->getState();
ParallelUtils::parallelFor(m_colData->MAColData.size(),
ParallelUtils::parallelFor(m_colData->MAColData.getSize(),
[&](const size_t idx)
{
const auto& cd = m_colData->MAColData[idx];
......
......@@ -39,7 +39,7 @@ VirtualCouplingCH::processCollisionData()
// Check if any collisions
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
visualGeometry->setPosition(collidingObjPos);
......@@ -48,8 +48,9 @@ VirtualCouplingCH::processCollisionData()
// Aggregate collision data
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;
}
......
......@@ -28,6 +28,7 @@
#include "imstkIsometricMap.h"
#include "imstkMeshIO.h"
#include "imstkTetraToTetraCD.h"
#include "imstkTetrahedralMesh.h"
using namespace imstk;
......@@ -65,11 +66,11 @@ TEST_F(imstkTetraToTetraCDTest, NoSelfIntersection)
m_CD = new TetraToTetraCD(a, b, cd);
m_CD->computeCollisionData();
EXPECT_EQ(cd->PTColData.size(), 0);
EXPECT_EQ(cd->PTColData.getSize(), 0);
m_CD = new TetraToTetraCD(b, a, cd);
m_CD->computeCollisionData();
EXPECT_EQ(cd->PTColData.size(), 0);
EXPECT_EQ(cd->PTColData.getSize(), 0);
}
TEST_F(imstkTetraToTetraCDTest, IntersectionThenNoIntersection1T)
......@@ -82,28 +83,28 @@ TEST_F(imstkTetraToTetraCDTest, IntersectionThenNoIntersection1T)
auto cd = std::make_shared<CollisionData>();
m_CD = new TetraToTetraCD(a, b, cd);
m_CD->computeCollisionData();
EXPECT_EQ(cd->PTColData.size(), 1);
EXPECT_EQ(cd->PTColData[0].collisionType, PointTetrahedronCollisionData::bPenetratingA);
EXPECT_EQ(cd->PTColData[0].vertexId, 0);
EXPECT_EQ(cd->PTColData[0].tetreahedronId, 0);
EXPECT_EQ(cd->PTColData.getSize(), 1);
EXPECT_EQ(cd->PTColData[0].collisionType, PointTetrahedronCollisionDataElement::bPenetratingA);
EXPECT_EQ(cd->PTColData[0].vertexIdx, 0);
EXPECT_EQ(cd->PTColData[0].tetreahedronIdx, 0);
m_CD = new TetraToTetraCD(b, a, cd);
m_CD->computeCollisionData();
EXPECT_EQ(cd->PTColData.size(), 1);
EXPECT_EQ(cd->PTColData[0].collisionType, PointTetrahedronCollisionData::aPenetratingB);
EXPECT_EQ(cd->PTColData[0].vertexId, 0);
EXPECT_EQ(cd->PTColData[0].tetreahedronId, 0);
EXPECT_EQ(cd->PTColData.getSize(), 1);
EXPECT_EQ(cd->PTColData[0].collisionType, PointTetrahedronCollisionDataElement::aPenetratingB);
EXPECT_EQ(cd->PTColData[0].vertexIdx, 0);
EXPECT_EQ(cd->PTColData[0].tetreahedronIdx, 0);
//now translate b more so there is no intersection
b->translateVertices(imstk::Vec3d(0.0, 2.0, 0.0));
m_CD = new TetraToTetraCD(b, a, cd);
m_CD->computeCollisionData();
EXPECT_EQ(cd->PTColData.size(), 0);
EXPECT_EQ(cd->PTColData.getSize(), 0);
m_CD = new TetraToTetraCD(a, b, cd);
m_CD->computeCollisionData();
EXPECT_EQ(cd->PTColData.size(), 0);
EXPECT_EQ(cd->PTColData.getSize(), 0);
}
TEST_F(imstkTetraToTetraCDTest, IntersectionThenNoIntersectionHuman)
......@@ -117,34 +118,34 @@ TEST_F(imstkTetraToTetraCDTest, IntersectionThenNoIntersectionHuman)
m_CD = new TetraToTetraCD(a, b, cd);
m_CD->computeCollisionData();
EXPECT_EQ(cd->PTColData.size(), 4);
EXPECT_EQ(cd->PTColData.getSize(), 4);
m_CD = new TetraToTetraCD(b, a, cd);
m_CD->computeCollisionData();
EXPECT_EQ(cd->PTColData.size(), 4);
EXPECT_EQ(cd->PTColData.getSize(), 4);
//this additional translation produces a different intersection
b->translateVertices(imstk::Vec3d(0.0, 0.0, 0.5));
m_CD = new TetraToTetraCD(a, b, cd);
m_CD->computeCollisionData();
EXPECT_EQ(cd->PTColData.size(), 1);
EXPECT_EQ(cd->PTColData[0].collisionType, PointTetrahedronCollisionData::aPenetratingB);
EXPECT_EQ(cd->PTColData[0].vertexId, 81);
EXPECT_EQ(cd->PTColData[0].tetreahedronId, 367);
EXPECT_EQ(cd->PTColData.getSize(), 1);
EXPECT_EQ(cd->PTColData[0].collisionType, PointTetrahedronCollisionDataElement::aPenetratingB);
EXPECT_EQ(cd->PTColData[0].vertexIdx, 81);
EXPECT_EQ(cd->PTColData[0].tetreahedronIdx, 367);
m_CD = new TetraToTetraCD(b, a, cd);
m_CD->computeCollisionData();
EXPECT_EQ(cd->PTColData.size(), 1);
EXPECT_EQ(cd->PTColData[0].collisionType, PointTetrahedronCollisionData::bPenetratingA);
EXPECT_EQ(cd->PTColData[0].vertexId, 81);
EXPECT_EQ(cd->PTColData[0].tetreahedronId, 367);
EXPECT_EQ(cd->PTColData.getSize(), 1);
EXPECT_EQ(cd->PTColData[0].collisionType, PointTetrahedronCollisionDataElement::bPenetratingA);
EXPECT_EQ(cd->PTColData[0].vertexIdx, 81);
EXPECT_EQ(cd->PTColData[0].tetreahedronIdx, 367);
//now translate b more so there is no intersection
b->translateVertices(imstk::Vec3d(0.0, 0.0, 1.0));
m_CD = new TetraToTetraCD(a, b, cd);
m_CD->computeCollisionData();
EXPECT_EQ(cd->PTColData.size(), 0);
EXPECT_EQ(cd->PTColData.getSize(), 0);
}
TEST_F(imstkTetraToTetraCDTest, IntersectionOfDifferentMeshes)
......@@ -155,7 +156,7 @@ TEST_F(imstkTetraToTetraCDTest, IntersectionOfDifferentMeshes)
auto cd = std::make_shared<CollisionData>();
m_CD = new TetraToTetraCD(a, b, cd);
m_CD->computeCollisionData();
EXPECT_EQ(cd->PTColData.size(), 595);
EXPECT_EQ(cd->PTColData.getSize(), 595);
}
int
......
Markdown is supported
0%
or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment