Commit b2581bd4 authored by Nghia Truong's avatar Nghia Truong

ENH: Fix PBD solver to allow process PBD collision between meshes with mutual collisions

parent 521aa1cd
......@@ -26,13 +26,26 @@
#include "imstkPbdObject.h"
#include "imstkPbdEdgeEdgeCollisionConstraint.h"
#include "imstkPbdPointTriCollisionConstraint.h"
#include "imstkPbdSolver.h"
#include "imstkParallelUtils.h"
#include <memory>
#include <g3log/g3log.hpp>
namespace imstk
{
PBDCollisionHandling::~PBDCollisionHandling()
{
for (const auto ptr: m_EEConstraintPool)
{
delete ptr;
}
for (const auto ptr: m_VTConstraintPool)
{
delete ptr;
}
}
void
PBDCollisionHandling::processCollisionData()
{
......@@ -64,13 +77,9 @@ PBDCollisionHandling::processCollisionData()
void
PBDCollisionHandling::generatePBDConstraints()
{
// clear the constraints before populating wit new ones
m_PBDConstraints.clear();
const auto dynaModel1 = std::static_pointer_cast<PbdModel>(m_pbdObject1->getDynamicalModel());
const auto dynaModel2 = std::static_pointer_cast<PbdModel>(m_pbdObject2->getDynamicalModel());
const auto colGeo2 = std::static_pointer_cast<SurfaceMesh>(m_pbdObject2->getCollidingGeometry());
const auto colGeo2 = std::static_pointer_cast<SurfaceMesh>(m_pbdObject2->getCollidingGeometry());
const auto map1 = m_pbdObject1->getPhysicsToCollidingMap();
const auto map2 = m_pbdObject2->getPhysicsToCollidingMap();
......@@ -78,73 +87,92 @@ PBDCollisionHandling::generatePBDConstraints()
// std::cout << "EE: " << m_colData->EEColData.getSize() << "TV: " << m_colData->VTColData.getSize() << std::endl;
// Generate edge-edge pbd constraints
for (size_t i = 0; i < m_colData->EEColData.getSize(); ++i)
if (m_EEConstraintPool.size() < m_colData->EEColData.getSize())
{
const auto& colData = m_colData->EEColData[i];
auto c = std::make_shared<PbdEdgeEdgeConstraint>();
size_t edgeA1, edgeA2;
if (map1)
for (size_t i = m_EEConstraintPool.size(); i < m_colData->EEColData.getSize(); ++i)
{
edgeA1 = map1->getMapIdx(colData.edgeIdA.first);
edgeA2 = map1->getMapIdx(colData.edgeIdA.second);
}
else
{
edgeA1 = colData.edgeIdA.first;
edgeA2 = colData.edgeIdA.second;
m_EEConstraintPool.push_back(new PbdEdgeEdgeConstraint);
}
}
size_t edgeB1, edgeB2;
if (map2)
{
edgeB1 = map2->getMapIdx(colData.edgeIdB.first);
edgeB2 = map2->getMapIdx(colData.edgeIdB.second);
}
else
ParallelUtils::parallelFor(m_colData->EEColData.getSize(),
[&] (const size_t idx)
{
edgeB1 = colData.edgeIdB.first;
edgeB2 = colData.edgeIdB.second;
}
c->initConstraint(dynaModel1, edgeA1, edgeA2,
dynaModel2, edgeB1, edgeB2);
m_PBDConstraints.push_back(c);
}
const auto& colData = m_colData->EEColData[idx];
size_t edgeA1, edgeA2;
if (map1)
{
edgeA1 = map1->getMapIdx(colData.edgeIdA.first);
edgeA2 = map1->getMapIdx(colData.edgeIdA.second);
}
else
{
edgeA1 = colData.edgeIdA.first;
edgeA2 = colData.edgeIdA.second;
}
size_t edgeB1, edgeB2;
if (map2)
{
edgeB1 = map2->getMapIdx(colData.edgeIdB.first);
edgeB2 = map2->getMapIdx(colData.edgeIdB.second);
}
else
{
edgeB1 = colData.edgeIdB.first;
edgeB2 = colData.edgeIdB.second;
}
const auto constraint = m_EEConstraintPool[idx];
constraint->initConstraint(dynaModel1, edgeA1, edgeA2, dynaModel2, edgeB1, edgeB2);
});
// Generate vertex-triangle pbd constraints
for (size_t i = 0; i < m_colData->VTColData.getSize(); ++i)
if (m_VTConstraintPool.size() < m_colData->VTColData.getSize())
{
const auto& colData = m_colData->VTColData[i];
const auto& triVerts = colGeo2->getTrianglesVertices()[colData.triIdx];
const auto c = std::make_shared<PbdPointTriangleConstraint>();
size_t v1, v2, v3;
if (map2)
for (size_t i = m_VTConstraintPool.size(); i < m_colData->VTColData.getSize(); ++i)
{
v1 = map2->getMapIdx(triVerts[0]);
v2 = map2->getMapIdx(triVerts[1]);
v3 = map2->getMapIdx(triVerts[2]);
}
else
{
v1 = triVerts[0];
v2 = triVerts[1];
v3 = triVerts[2];
m_VTConstraintPool.push_back(new PbdPointTriangleConstraint);
}
}
c->initConstraint(dynaModel1,
colData.vertexIdx,
dynaModel2,
v1,
v2,
v3);
ParallelUtils::parallelFor(m_colData->VTColData.getSize(),
[&] (const size_t idx)
{
const auto& colData = m_colData->VTColData[idx];
const auto& triVerts = colGeo2->getTrianglesVertices()[colData.triIdx];
size_t v1, v2, v3;
if (map2)
{
v1 = map2->getMapIdx(triVerts[0]);
v2 = map2->getMapIdx(triVerts[1]);
v3 = map2->getMapIdx(triVerts[2]);
}
else
{
v1 = triVerts[0];
v2 = triVerts[1];
v3 = triVerts[2];
}
const auto constraint = m_VTConstraintPool[idx];
constraint->initConstraint(dynaModel1, colData.vertexIdx, dynaModel2, v1, v2, v3);
});
// Copy constraints
m_PBDConstraints.resize(0);
m_PBDConstraints.reserve(m_colData->EEColData.getSize() + m_colData->VTColData.getSize());
m_PBDConstraints.push_back(c);
for (size_t i = 0; i < m_colData->EEColData.getSize(); ++i)
{
m_PBDConstraints.push_back(static_cast<PbdCollisionConstraint*>(m_EEConstraintPool[i]));
}
//TODO: generating PbdPointTriangleConstraint from the VTColData should be added
for (size_t i = 0; i < m_colData->VTColData.getSize(); ++i)
{
m_PBDConstraints.push_back(static_cast<PbdCollisionConstraint*>(m_VTConstraintPool[i]));
}
}
}
......@@ -32,6 +32,8 @@ namespace imstk
class CollidingObject;
class PbdObject;
class PbdCollisionConstraint;
class PbdEdgeEdgeConstraint;
class PbdPointTriangleConstraint;
class PbdSolver;
struct CollisionData;
......@@ -42,7 +44,7 @@ struct CollisionData;
///
class PBDCollisionHandling : public CollisionHandling
{
typedef std::vector<std::shared_ptr<PbdCollisionConstraint>> PBDConstraintVector;
typedef std::vector<PbdCollisionConstraint*> PBDConstraintVector;
public:
///
......@@ -61,9 +63,9 @@ public:
PBDCollisionHandling() = delete;
///
/// \brief Destructor
/// \brief Destructor, clear memory pool
///
~PBDCollisionHandling() = default;
virtual ~PBDCollisionHandling() override;
///
/// \brief Compute forces based on collision data
......@@ -81,5 +83,8 @@ private:
std::shared_ptr<PbdObject> m_pbdObject2; ///> PBD object
PBDConstraintVector m_PBDConstraints; ///> List of PBD constraints
std::shared_ptr<PbdSolver> m_PBDSolver; /// The Solver for the collision constraints
std::vector<PbdEdgeEdgeConstraint*> m_EEConstraintPool;
std::vector<PbdPointTriangleConstraint*> m_VTConstraintPool;
};
}
......@@ -166,7 +166,8 @@ CollisionGraph::getInteractionPair(CollidingObjectPtr A, CollidingObjectPtr B)
for (const auto& intPair : m_interactionPairList)
{
if (intPair->getObjectsPair() == std::pair<CollidingObjectPtr, CollidingObjectPtr>(A, B)
|| intPair->getObjectsPair() == std::pair<CollidingObjectPtr, CollidingObjectPtr>(B, A))
/*|| intPair->getObjectsPair() == std::pair<CollidingObjectPtr, CollidingObjectPtr>(B, A)*/
)
{
return intPair;
}
......
......@@ -38,17 +38,17 @@ PbdSolver::solve()
void
PbdSolver::resolveCollisionConstraints()
{
if (m_PBDConstraints != nullptr)
if (m_PBDConstraints.size() > 0)
{
unsigned int maxIter = 2;
if (!m_PBDConstraints->empty())
uint32_t maxIter = 3u;
uint32_t i = 0;
while (++i < maxIter)
{
unsigned int i = 0;
while (++i < maxIter)
for (const auto constraintList : m_PBDConstraints)
{
for (size_t k = 0; k < m_PBDConstraints->size(); ++k)
for (size_t k = 0; k < constraintList->size(); ++k)
{
(*m_PBDConstraints)[k]->solvePositionConstraint();
(*constraintList)[k]->solvePositionConstraint();
}
}
}
......
......@@ -25,6 +25,8 @@
#include "imstkPbdObject.h"
#include "imstkGraph.h"
#include <unordered_set>
namespace imstk
{
class PbdObject;
......@@ -36,7 +38,7 @@ class PbdCollisionConstraint;
///
class PbdSolver : public SolverBase
{
typedef std::vector<std::shared_ptr<PbdCollisionConstraint>> PBDConstraintVector;
using PBDConstraintVector = std::vector<PbdCollisionConstraint*>;
public:
///
......@@ -71,7 +73,7 @@ public:
///
/// \brief Add the global collision contraints to this solver
///
void addCollisionConstraints(PBDConstraintVector* constraints) { m_PBDConstraints = constraints; }
void addCollisionConstraints(PBDConstraintVector* constraints) { m_PBDConstraints.insert(constraints); }
///
/// \brief Solve the global collision contraints charged to this solver
......@@ -79,8 +81,8 @@ public:
void resolveCollisionConstraints();
private:
size_t m_maxIterations = 20; ///< Maximum number of NL Gauss-Seidel iterations
PBDConstraintVector* m_PBDConstraints = nullptr; /// collision contraints charged to this solver
size_t m_maxIterations = 20; ///< Maximum number of NL Gauss-Seidel iterations
std::unordered_set<PBDConstraintVector*> m_PBDConstraints; ///< Collision contraints charged to this solver
std::shared_ptr<PbdObject> m_pbdObject;
};
} // imstk
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