diff --git a/scenes/MeshRepulsion_patient_01.scn b/scenes/MeshRepulsion_patient_01.scn
new file mode 100644
index 0000000..8777c5e
--- /dev/null
+++ b/scenes/MeshRepulsion_patient_01.scn
@@ -0,0 +1,120 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/scenes/MeshRepulsion_patient_08.scn b/scenes/MeshRepulsion_patient_08.scn
new file mode 100644
index 0000000..3eecaf3
--- /dev/null
+++ b/scenes/MeshRepulsion_patient_08.scn
@@ -0,0 +1,91 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/scenes/MeshRepulsion_patient_16.scn b/scenes/MeshRepulsion_patient_16.scn
new file mode 100644
index 0000000..e918e9b
--- /dev/null
+++ b/scenes/MeshRepulsion_patient_16.scn
@@ -0,0 +1,91 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/src/CGALPlugin/MeshRepulsionEngine.cpp b/src/CGALPlugin/MeshRepulsionEngine.cpp
index 00d8f08..91d0700 100644
--- a/src/CGALPlugin/MeshRepulsionEngine.cpp
+++ b/src/CGALPlugin/MeshRepulsionEngine.cpp
@@ -1,4 +1,4 @@
-/*****************************************************************************
+/*******************************************************************************
* - Copyright (C) 2020-Present InfinyTech3D - *
* *
* This file is part of the InfinyToolkit plugin for the SOFA framework. *
@@ -16,112 +16,123 @@
* *
* Contact: contact@infinytech3d.com *
* Further information: https://infinytech3d.com *
-****************************************************************************/
-#define SOFA_COMPONENT_NEARESTTEXCOORDS_CPP
+******************************************************************************/
+#define CGALPLUGIN_MESHREPULSIONENGINE_CPP
#include
#include
-#include
-#include
-#include
+#include
#include
+#include
+#include
+#include
+#include
#include
+#include
#include
-#include
+#include
+#include
+#include
#include
-#include
namespace cgal
{
- using namespace sofa;
- using namespace sofa::defaulttype;
- using namespace sofa::helper;
-
void registerMeshRepulsionEngine(sofa::core::ObjectFactory* factory)
{
factory->registerObjects(sofa::core::ObjectRegistrationData(
- "MeshRepulsionEngine: SOFA/CGAL bridge for progressive slave/master repulsion")
- .add());
+ "MeshRepulsionEngine: CGAL/SOFA repulsion ForceField with edge/face crossing support and local force spreading.")
+ .add>());
}
- MeshRepulsionEngine::MeshRepulsionEngine()
+ template
+ MeshRepulsionEngine::MeshRepulsionEngine()
: l_masterState(initLink("masterState", "link to the master MechanicalState"))
, l_masterTopology(initLink("masterTopology", "link to the master BaseMeshTopology"))
- , l_slaveState(initLink("slaveState", "link to the slave MechanicalState"))
, l_slaveTopology(initLink("slaveTopology", "link to the slave BaseMeshTopology"))
- , d_enableCgalDetection(initData(&d_enableCgalDetection, true, "enableCgalDetection", "Enable CGAL inside/outside detection"))
- , d_rebuildMasterEveryStep(initData(&d_rebuildMasterEveryStep, false, "rebuildMasterEveryStep", "Rebuild the master CGAL mesh at every animation step"))
- , d_logEveryNSteps(initData(&d_logEveryNSteps, static_cast(1), "logEveryNSteps", "Print diagnostics every N animation steps"))
- , d_runDetectionAtInit(initData(&d_runDetectionAtInit, true, "runDetectionAtInit", "Run one CGAL detection during init()"))
- , d_logAsWarning(initData(&d_logAsWarning, true, "logAsWarning", "Print diagnostics with msg_warning instead of msg_info"))
+ , d_stiffness(initData(&d_stiffness, Real(350), "stiffness", "Penalty stiffness of the repulsion force"))
+ , d_safetyDistance(initData(&d_safetyDistance, Real(1.0), "safetyDistance", "Target gap kept beyond the master surface"))
+ , d_crossingDepth(initData(&d_crossingDepth, Real(1.0), "crossingDepth", "Penalty depth used for edge/face crossing contacts even when vertices are outside the safety band"))
+ , d_maxForce(initData(&d_maxForce, Real(800), "maxForce", "Clamp on per-contact force magnitude. 0 means no clamp"))
+ , d_contactDamping(initData(&d_contactDamping, Real(20), "contactDamping", "Damping applied only when a contact vertex moves inward"))
+ , d_neighborhoodRings(initData(&d_neighborhoodRings, static_cast(1), "neighborhoodRings", "Number of topological rings receiving a fraction of the contact force"))
+ , d_neighborWeight(initData(&d_neighborWeight, Real(0.35), "neighborWeight", "Weight multiplier for each propagated topological ring"))
+ , d_pushInsideVertices(initData(&d_pushInsideVertices, true, "pushInsideVertices", "Push slave vertices strictly inside the master"))
+ , d_pushEdgeCrossings(initData(&d_pushEdgeCrossings, true, "pushEdgeCrossings", "Push vertices of slave edges crossing the master surface"))
+ , d_pushFaceCrossings(initData(&d_pushFaceCrossings, true, "pushFaceCrossings", "Push vertices of slave faces crossing the master surface"))
+ , d_pushProximityVertices(initData(&d_pushProximityVertices, true, "pushProximityVertices", "Keep outside vertices away from the master inside the safety band"))
+ , d_rebuildMasterEveryStep(initData(&d_rebuildMasterEveryStep, false, "rebuildMasterEveryStep", "Rebuild the master CGAL structures every step"))
+ , d_logEveryNSteps(initData(&d_logEveryNSteps, static_cast(10), "logEveryNSteps", "Print diagnostics every N steps"))
, d_writeCsv(initData(&d_writeCsv, true, "writeCsv", "Write diagnostics to a CSV file"))
- , d_csvFilePath(initData(&d_csvFilePath, std::string("MeshRepulsion_diagnostics.csv"), "csvFilePath", "CSV file path for diagnostics"))
- , d_lastStep(initData(&d_lastStep, static_cast(0), "lastStep", "Last diagnostic step"))
- , d_lastMasterVertexCount(initData(&d_lastMasterVertexCount, static_cast(0), "lastMasterVertexCount", "Last master vertex count"))
- , d_lastMasterTriangleCount(initData(&d_lastMasterTriangleCount, static_cast(0), "lastMasterTriangleCount", "Last master triangle count"))
- , d_lastSlaveVertexCount(initData(&d_lastSlaveVertexCount, static_cast(0), "lastSlaveVertexCount", "Last slave vertex count"))
- , d_lastSlaveTriangleCount(initData(&d_lastSlaveTriangleCount, static_cast(0), "lastSlaveTriangleCount", "Last slave triangle count"))
- , d_lastInsideCount(initData(&d_lastInsideCount, static_cast(0), "lastInsideCount", "Last slave vertices inside master"))
- , d_lastBoundaryCount(initData(&d_lastBoundaryCount, static_cast(0), "lastBoundaryCount", "Last slave vertices on master boundary"))
- , d_lastOutsideCount(initData(&d_lastOutsideCount, static_cast(0), "lastOutsideCount", "Last slave vertices outside master"))
- , d_lastSlaveCgalFaceCount(initData(&d_lastSlaveCgalFaceCount, static_cast(0), "lastSlaveCgalFaceCount", "Last CGAL slave face count"))
-
+ , d_csvFilePath(initData(&d_csvFilePath, std::string("MeshRepulsion_PR03B_diagnostics.csv"), "csvFilePath", "CSV file path"))
+ , d_lastStep(initData(&d_lastStep, static_cast(0), "lastStep", "Last step index"))
+ , d_lastInsideContacts(initData(&d_lastInsideContacts, static_cast(0), "lastInsideContacts", "Inside contacts last step"))
+ , d_lastEdgeContacts(initData(&d_lastEdgeContacts, static_cast(0), "lastEdgeContacts", "Edge crossing contacts last step"))
+ , d_lastFaceContacts(initData(&d_lastFaceContacts, static_cast(0), "lastFaceContacts", "Face crossing contacts last step"))
+ , d_lastProximityContacts(initData(&d_lastProximityContacts, static_cast(0), "lastProximityContacts", "Outside safety-band contacts last step"))
+ , d_lastMaxForce(initData(&d_lastMaxForce, Real(0), "lastMaxForce", "Max per-contact force magnitude last step"))
+ , d_lastMeanForce(initData(&d_lastMeanForce, Real(0), "lastMeanForce", "Mean applied force magnitude last step"))
{
- f_listening.setValue(true);
- std::cout << "LOADED MeshRepulsionEngine loader-position-fix" << std::endl;
}
- MeshRepulsionEngine::~MeshRepulsionEngine() = default;
+ template
+ MeshRepulsionEngine::~MeshRepulsionEngine() = default;
- void MeshRepulsionEngine::init()
+ template
+ typename MeshRepulsionEngine::CgalPoint MeshRepulsionEngine::toCgalPoint(const Coord& p)
{
- // need to add check here if link to the mechanicalObject are correct
- // if not put : sofa::core::objectmodel::BaseObject::d_componentState.setValue(sofa::core::objectmodel::ComponentState::Invalid);
- if (!l_masterState.get() || !l_slaveState.get() || !l_masterTopology.get() || !l_slaveTopology.get())
- {
- msg_error() << "MeshRepulsionEngine invalid links. Expected masterState, slaveState, masterTopology and slaveTopology.";
- sofa::core::objectmodel::BaseObject::d_componentState.setValue(sofa::core::objectmodel::ComponentState::Invalid);
- return;
- }
-
- sofa::core::objectmodel::BaseObject::d_componentState.setValue(sofa::core::objectmodel::ComponentState::Valid);
+ return CgalPoint(static_cast(p[0]), static_cast(p[1]), static_cast(p[2]));
+ }
- std::cout << "[CGAL detection] init(): links are valid. Detection will run in bwdInit(), after loaders/mechanical objects are initialized. runDetectionAtInit="
- << (d_runDetectionAtInit.getValue() ? "true" : "false") << std::endl;
+ template
+ double MeshRepulsionEngine::norm(const CgalVector& v)
+ {
+ return std::sqrt(std::max(0.0, CGAL::to_double(v.squared_length())));
}
- void MeshRepulsionEngine::bwdInit()
+ template
+ typename MeshRepulsionEngine::Deriv MeshRepulsionEngine::makeDeriv(Real x, Real y, Real z)
{
- if (d_componentState.getValue() != sofa::core::objectmodel::ComponentState::Valid)
- {
- return;
- }
+ return Deriv(x, y, z);
+ }
- std::cout << "[CGAL detection] bwdInit(): post-init diagnostic point reached." << std::endl;
+ template
+ typename MeshRepulsionEngine::Real MeshRepulsionEngine::derivNorm(const Deriv& v)
+ {
+ return static_cast(std::sqrt(static_cast(v[0] * v[0] + v[1] * v[1] + v[2] * v[2])));
+ }
- if (d_runDetectionAtInit.getValue())
- {
- updateIntersectedPositions();
- }
+ template
+ typename MeshRepulsionEngine::Real MeshRepulsionEngine::derivDot(const Deriv& a, const Deriv& b)
+ {
+ return a[0] * b[0] + a[1] * b[1] + a[2] * b[2];
}
- void MeshRepulsionEngine::doUpdate()
+ template
+ typename MeshRepulsionEngine::Deriv MeshRepulsionEngine::derivScale(const Deriv& v, Real s)
{
- // The engine is event-driven for now. The real work is performed in handleEvent().
- std::cout << "call MeshRepulsionEngine::doUpdate()" << std::endl;
+ return makeDeriv(v[0] * s, v[1] * s, v[2] * s);
+ }
+ template
+ typename MeshRepulsionEngine::Deriv MeshRepulsionEngine::derivAdd(const Deriv& a, const Deriv& b)
+ {
+ return makeDeriv(a[0] + b[0], a[1] + b[1], a[2] + b[2]);
}
- // HELPER METHOD TO GET A CGAL POINT FROM A SOFA COORD
- MeshRepulsionEngine::CgalPoint MeshRepulsionEngine::toCgalPoint(const Coord& p)
+ template
+ typename MeshRepulsionEngine::Deriv MeshRepulsionEngine::normalizeDeriv(const Deriv& v)
{
- return CgalPoint(static_cast(p[0]), static_cast(p[1]), static_cast(p[2]));
+ const Real n = derivNorm(v);
+ if (n < static_cast(1.0e-12))
+ return makeDeriv(Real(0), Real(0), Real(0));
+ return derivScale(v, Real(1) / n);
}
- bool MeshRepulsionEngine::buildSurfaceMeshFromSofa(
+ template
+ bool MeshRepulsionEngine::buildSurfaceMeshFromSofa(
const VecCoord& positions,
sofa::core::topology::BaseMeshTopology* topology,
CgalSurfaceMesh& outputMesh,
@@ -130,31 +141,16 @@ namespace cgal
outputMesh.clear();
errorMessage.clear();
- if (!topology)
- {
- errorMessage = "null topology";
- return false;
- }
-
- if (positions.empty())
- {
- errorMessage = "empty position vector";
- return false;
- }
+ if (!topology) { errorMessage = "null topology"; return false; }
+ if (positions.empty()) { errorMessage = "empty position vector"; return false; }
const auto nbTriangles = topology->getNbTriangles();
- if (nbTriangles == 0)
- {
- errorMessage = "topology has no triangles; needs a surface topology for inside/outside tests";
- return false;
- }
+ if (nbTriangles == 0) { errorMessage = "topology has no triangles"; return false; }
- std::vector vertexMap;
+ std::vector vertexMap;
vertexMap.reserve(positions.size());
for (const Coord& p : positions)
- {
vertexMap.emplace_back(outputMesh.add_vertex(toCgalPoint(p)));
- }
std::size_t invalidTriangles = 0;
std::size_t rejectedTriangles = 0;
@@ -166,7 +162,8 @@ namespace cgal
const auto b = static_cast(tri[1]);
const auto c = static_cast(tri[2]);
- if (a >= positions.size() || b >= positions.size() || c >= positions.size() || a == b || b == c || a == c)
+ if (a >= positions.size() || b >= positions.size() || c >= positions.size()
+ || a == b || b == c || a == c)
{
++invalidTriangles;
continue;
@@ -174,14 +171,9 @@ namespace cgal
auto f = outputMesh.add_face(vertexMap[a], vertexMap[b], vertexMap[c]);
if (f == CgalSurfaceMesh::null_face())
- {
f = outputMesh.add_face(vertexMap[a], vertexMap[c], vertexMap[b]);
- }
-
if (f == CgalSurfaceMesh::null_face())
- {
++rejectedTriangles;
- }
}
if (outputMesh.number_of_faces() == 0)
@@ -205,9 +197,13 @@ namespace cgal
return true;
}
- bool MeshRepulsionEngine::rebuildMasterCgalMesh(const VecCoord& masterVertices)
+ template
+ bool MeshRepulsionEngine::rebuildMasterCgalStructures(const VecCoord& masterVertices)
{
m_masterCgalMesh = std::make_unique();
+ m_masterAabbTree.reset();
+ m_insideTester.reset();
+ m_hasMasterCentroid = false;
std::string errorMessage;
if (!buildSurfaceMeshFromSofa(masterVertices, l_masterTopology.get(), *m_masterCgalMesh, errorMessage))
@@ -216,120 +212,503 @@ namespace cgal
m_masterCgalMesh.reset();
return false;
}
-
if (!errorMessage.empty())
- {
msg_warning() << "Master CGAL mesh warning: " << errorMessage;
+
+ double sx = 0.0, sy = 0.0, sz = 0.0;
+ for (const Coord& p : masterVertices)
+ {
+ sx += static_cast(p[0]);
+ sy += static_cast(p[1]);
+ sz += static_cast(p[2]);
+ }
+
+ if (!masterVertices.empty())
+ {
+ const double inv = 1.0 / static_cast(masterVertices.size());
+ m_masterCentroid = CgalPoint(sx * inv, sy * inv, sz * inv);
+ m_hasMasterCentroid = true;
}
- msg_info() << "Master CGAL mesh built: vertices=" << m_masterCgalMesh->number_of_vertices()
- << " faces=" << m_masterCgalMesh->number_of_faces();
+ m_masterAabbTree = std::make_unique(
+ faces(*m_masterCgalMesh).first,
+ faces(*m_masterCgalMesh).second,
+ *m_masterCgalMesh);
+ m_masterAabbTree->accelerate_distance_queries();
+
+ m_insideTester = std::make_unique(*m_masterCgalMesh);
+
return true;
}
- void MeshRepulsionEngine::updateIntersectedPositions()
+ template
+ void MeshRepulsionEngine::rebuildSlaveNeighborhood(std::size_t vertexCount)
{
- ++m_stepCounter;
- std::cout << "[CGAL detection] updateIntersectedPositions() called, step=" << m_stepCounter << std::endl;
+ auto* topo = l_slaveTopology.get();
+ m_vertexNeighbors.assign(vertexCount, std::vector());
+ m_neighborhoodVertexCount = vertexCount;
- if (!d_enableCgalDetection.getValue())
- {
+ if (!topo || vertexCount == 0)
return;
+
+ auto addEdge = [&](std::size_t a, std::size_t b)
+ {
+ if (a >= vertexCount || b >= vertexCount || a == b)
+ return;
+ m_vertexNeighbors[a].push_back(b);
+ m_vertexNeighbors[b].push_back(a);
+ };
+
+ const auto nbEdges = topo->getNbEdges();
+ if (nbEdges > 0)
+ {
+ for (std::size_t e = 0; e < static_cast(nbEdges); ++e)
+ {
+ const auto edge = topo->getEdge(e);
+ addEdge(static_cast(edge[0]), static_cast(edge[1]));
+ }
+ }
+ else
+ {
+ const auto nbTris = topo->getNbTriangles();
+ for (std::size_t t = 0; t < static_cast(nbTris); ++t)
+ {
+ const auto tri = topo->getTriangle(t);
+ const auto a = static_cast(tri[0]);
+ const auto b = static_cast(tri[1]);
+ const auto c = static_cast(tri[2]);
+ addEdge(a, b);
+ addEdge(b, c);
+ addEdge(c, a);
+ }
}
- if (!l_masterState.get() || !l_slaveState.get() || !l_masterTopology.get() || !l_slaveTopology.get())
+ for (auto& neigh : m_vertexNeighbors)
{
- msg_error() << "MeshRepulsionEngine links became invalid during simulation.";
- return;
+ std::sort(neigh.begin(), neigh.end());
+ neigh.erase(std::unique(neigh.begin(), neigh.end()), neigh.end());
}
+ }
- sofa::helper::ReadAccessor masterVertices = l_masterState.get()->read(sofa::core::vec_id::read_access::position)->getValue();
- sofa::helper::ReadAccessor slaveVertices = l_slaveState.get()->read(sofa::core::vec_id::read_access::position)->getValue();
+ template
+ typename MeshRepulsionEngine::Deriv MeshRepulsionEngine::orientedNormalFromClosestFace(
+ const CgalPoint& queryPoint,
+ const CgalPoint& closestPoint,
+ typename boost::graph_traits::face_descriptor faceDescriptor) const
+ {
+ namespace PMP = CGAL::Polygon_mesh_processing;
- auto* masterTopology = l_masterTopology.get();
- auto* slaveTopology = l_slaveTopology.get();
+ CgalVector n = PMP::compute_face_normal(faceDescriptor, *m_masterCgalMesh);
+ double nNorm = norm(n);
- if (!m_masterCgalMesh || d_rebuildMasterEveryStep.getValue())
+ if (nNorm < 1.0e-12 && m_hasMasterCentroid)
{
- if (!rebuildMasterCgalMesh(masterVertices))
- {
- return;
- }
+ n = closestPoint - m_masterCentroid;
+ nNorm = norm(n);
}
+ if (nNorm < 1.0e-12)
+ {
+ n = queryPoint - closestPoint;
+ nNorm = norm(n);
+ }
+ if (nNorm < 1.0e-12)
+ return makeDeriv(Real(0), Real(0), Real(1));
+
+ CgalVector u = n / nNorm;
+
+ const double probe = std::max(1.0e-6, static_cast(d_safetyDistance.getValue()) * 0.05);
+ const CgalInsideTester& insideTester = *m_insideTester;
+ const auto positiveSide = insideTester(closestPoint + u * probe);
+ const auto negativeSide = insideTester(closestPoint - u * probe);
+
+ if (positiveSide == CGAL::ON_BOUNDED_SIDE && negativeSide != CGAL::ON_BOUNDED_SIDE)
+ u = -u;
+ else if (positiveSide == negativeSide && m_hasMasterCentroid)
+ {
+ const CgalVector centroidToClosest = closestPoint - m_masterCentroid;
+ if ((u * centroidToClosest) < 0.0)
+ u = -u;
+ }
+
+ return makeDeriv(
+ static_cast(CGAL::to_double(u.x())),
+ static_cast(CGAL::to_double(u.y())),
+ static_cast(CGAL::to_double(u.z())));
+ }
+
+ template
+ void MeshRepulsionEngine::mergeContact(
+ std::vector& perVertexContacts,
+ std::vector& hasContact,
+ std::size_t vertexId,
+ const Deriv& normal,
+ Real depth,
+ Real stiffness,
+ ContactKind kind) const
+ {
+ if (vertexId >= perVertexContacts.size())
+ return;
- CgalSurfaceMesh slaveCgalMesh;
- std::string slaveBuildWarning;
- const bool slaveMeshOk = buildSurfaceMeshFromSofa(slaveVertices, slaveTopology, slaveCgalMesh, slaveBuildWarning);
- if (!slaveMeshOk)
+ const Real nNorm = derivNorm(normal);
+ if (nNorm < static_cast(1.0e-12) || depth <= Real(0))
+ return;
+
+ const Deriv n = derivScale(normal, Real(1) / nNorm);
+
+ if (!hasContact[vertexId])
{
- msg_warning() << "Could not build slave CGAL surface mesh: " << slaveBuildWarning;
+ Contact c;
+ c.vertexId = vertexId;
+ c.normal = n;
+ c.depth = depth;
+ c.stiffnessAlongN = stiffness;
+ c.kind = kind;
+ perVertexContacts[vertexId] = c;
+ hasContact[vertexId] = 1;
+ return;
}
- CGAL::Side_of_triangle_mesh insideTester(*m_masterCgalMesh);
+ Contact& c = perVertexContacts[vertexId];
+ const Real oldDepth = c.depth;
+ const Real newDepth = std::max(oldDepth, depth);
+ const Deriv accumulated = derivAdd(derivScale(c.normal, oldDepth), derivScale(n, depth));
+ const Deriv normalized = normalizeDeriv(accumulated);
+
+ if (derivNorm(normalized) > static_cast(1.0e-12))
+ c.normal = normalized;
+ c.depth = newDepth;
+ c.stiffnessAlongN = std::max(c.stiffnessAlongN, stiffness);
+
+ if (static_cast(kind) > static_cast(c.kind))
+ c.kind = kind;
+ }
+
+ template
+ void MeshRepulsionEngine::detectContacts(const VecCoord& slaveX)
+ {
+ m_contacts.clear();
+
+ if (!m_masterCgalMesh || !m_masterAabbTree)
+ return;
+
+ if (m_neighborhoodVertexCount != slaveX.size())
+ rebuildSlaveNeighborhood(slaveX.size());
+
+ const Real stiffness = d_stiffness.getValue();
+ const Real safety = std::max(Real(0), d_safetyDistance.getValue());
+ const Real crossingDepth = std::max(Real(0), d_crossingDepth.getValue());
- std::size_t insideCount = 0;
- std::size_t boundaryCount = 0;
- std::size_t outsideCount = 0;
+ std::vector perVertexContacts(slaveX.size());
+ std::vector hasContact(slaveX.size(), 0);
- for (const Coord& p : slaveVertices)
+ if (!m_insideTester)
+ return;
+ const CgalInsideTester& insideTester = *m_insideTester;
+
+ if (m_isSurfaceVertex.size() != slaveX.size())
{
- const auto side = insideTester(toCgalPoint(p));
- if (side == CGAL::ON_BOUNDED_SIDE)
+ m_isSurfaceVertex.assign(slaveX.size(), 0);
+ bool anySurfaceVertex = false;
+ if (auto* topo = l_slaveTopology.get())
{
- ++insideCount;
+ const auto nbTri = topo->getNbTriangles();
+ for (std::size_t t = 0; t < static_cast(nbTri); ++t)
+ {
+ const auto tri = topo->getTriangle(t);
+ for (int k = 0; k < 3; ++k)
+ {
+ const auto vid = static_cast(tri[k]);
+ if (vid < m_isSurfaceVertex.size())
+ {
+ m_isSurfaceVertex[vid] = 1;
+ anySurfaceVertex = true;
+ }
+ }
+ }
}
- else if (side == CGAL::ON_BOUNDARY)
+ // Fallback: without a surface topology, keep every vertex active.
+ if (!anySurfaceVertex)
+ m_isSurfaceVertex.assign(slaveX.size(), 1);
+ }
+
+ std::size_t insideContacts = 0;
+ std::size_t edgeContacts = 0;
+ std::size_t faceContacts = 0;
+ std::size_t proximityContacts = 0;
+
+ auto addContactFromPoint = [&](std::size_t vid, const CgalPoint& p, Real depth, ContactKind kind) -> bool
{
- ++boundaryCount;
- }
- else
+ const auto pair = m_masterAabbTree->closest_point_and_primitive(p);
+ const CgalPoint& closest = pair.first;
+ const auto faceDescriptor = pair.second;
+ const Deriv n = orientedNormalFromClosestFace(p, closest, faceDescriptor);
+ if (derivNorm(n) < static_cast(1.0e-12))
+ return false;
+ mergeContact(perVertexContacts, hasContact, vid, n, depth, stiffness, kind);
+ return true;
+ };
+
+ if (d_pushInsideVertices.getValue())
+ {
+ for (std::size_t i = 0; i < slaveX.size(); ++i)
{
- ++outsideCount;
+ if (!m_isSurfaceVertex[i])
+ continue;
+ const CgalPoint p = toCgalPoint(slaveX[i]);
+ if (insideTester(p) == CGAL::ON_BOUNDED_SIDE)
+ {
+ const CgalPoint closest = m_masterAabbTree->closest_point(p);
+ const double dist = std::sqrt(std::max(0.0, CGAL::to_double(CGAL::squared_distance(p, closest))));
+ if (addContactFromPoint(i, p, static_cast(dist) + safety, ContactKind::Inside))
+ ++insideContacts;
+ }
}
}
- const auto masterTriangleCount = static_cast(masterTopology->getNbTriangles());
- const auto slaveTriangleCount = static_cast(slaveTopology->getNbTriangles());
- const auto slaveCgalFaceCount = static_cast(slaveMeshOk ? slaveCgalMesh.number_of_faces() : 0);
-
- d_lastStep.setValue(static_cast(m_stepCounter));
- d_lastMasterVertexCount.setValue(static_cast(masterVertices.size()));
- d_lastMasterTriangleCount.setValue(masterTriangleCount);
- d_lastSlaveVertexCount.setValue(static_cast(slaveVertices.size()));
- d_lastSlaveTriangleCount.setValue(slaveTriangleCount);
- d_lastInsideCount.setValue(static_cast(insideCount));
- d_lastBoundaryCount.setValue(static_cast(boundaryCount));
- d_lastOutsideCount.setValue(static_cast(outsideCount));
- d_lastSlaveCgalFaceCount.setValue(slaveCgalFaceCount);
+ if (d_pushProximityVertices.getValue() && safety > Real(0))
+ {
+ for (std::size_t i = 0; i < slaveX.size(); ++i)
+ {
+ if (!m_isSurfaceVertex[i])
+ continue;
+ const CgalPoint p = toCgalPoint(slaveX[i]);
+ if (insideTester(p) == CGAL::ON_BOUNDED_SIDE)
+ continue;
+ const CgalPoint closest = m_masterAabbTree->closest_point(p);
+ const double dist = std::sqrt(std::max(0.0, CGAL::to_double(CGAL::squared_distance(p, closest))));
+ const Real band = safety - static_cast(dist);
+ if (band > Real(0))
+ {
+ if (addContactFromPoint(i, p, band, ContactKind::Proximity))
+ ++proximityContacts;
+ }
+ }
+ }
- const unsigned int logEvery = std::max(1u, d_logEveryNSteps.getValue());
- if ((m_stepCounter % logEvery) == 0)
+ auto* slaveTopo = l_slaveTopology.get();
+ if (slaveTopo && d_pushEdgeCrossings.getValue() && slaveTopo->getNbEdges() > 0)
{
- std::ostringstream oss;
- oss << "[CGAL detection] step=" << m_stepCounter
- << " masterVertices=" << masterVertices.size()
- << " masterTriangles=" << masterTriangleCount
- << " slaveVertices=" << slaveVertices.size()
- << " slaveTriangles=" << slaveTriangleCount
- << " inside=" << insideCount
- << " boundary=" << boundaryCount
- << " outside=" << outsideCount
- << " slaveCgalFaces=" << slaveCgalFaceCount;
-
- std::cout << oss.str() << std::endl;
- if (d_logAsWarning.getValue())
+ const auto nbEdges = slaveTopo->getNbEdges();
+ for (std::size_t e = 0; e < static_cast(nbEdges); ++e)
{
- msg_warning() << oss.str();
+ const auto edge = slaveTopo->getEdge(e);
+ const std::size_t ia = static_cast(edge[0]);
+ const std::size_t ib = static_cast(edge[1]);
+ if (ia >= slaveX.size() || ib >= slaveX.size())
+ continue;
+
+ const CgalPoint pa = toCgalPoint(slaveX[ia]);
+ const CgalPoint pb = toCgalPoint(slaveX[ib]);
+ const CGAL::Segment_3 seg(pa, pb);
+ if (!m_masterAabbTree->do_intersect(seg))
+ continue;
+
+ // Fix: crossing contacts get a non-zero depth even if their endpoints are outside and far from the closest master face.
+ if (addContactFromPoint(ia, pa, crossingDepth + safety, ContactKind::EdgeCrossing))
+ ++edgeContacts;
+ if (addContactFromPoint(ib, pb, crossingDepth + safety, ContactKind::EdgeCrossing))
+ ++edgeContacts;
}
- else
+ }
+
+ if (slaveTopo && d_pushFaceCrossings.getValue() && slaveTopo->getNbTriangles() > 0)
+ {
+ const auto nbTris = slaveTopo->getNbTriangles();
+ for (std::size_t t = 0; t < static_cast(nbTris); ++t)
{
- msg_info() << oss.str();
+ const auto tri = slaveTopo->getTriangle(t);
+ const std::size_t i0 = static_cast(tri[0]);
+ const std::size_t i1 = static_cast(tri[1]);
+ const std::size_t i2 = static_cast(tri[2]);
+ if (i0 >= slaveX.size() || i1 >= slaveX.size() || i2 >= slaveX.size())
+ continue;
+
+ const CgalPoint p0 = toCgalPoint(slaveX[i0]);
+ const CgalPoint p1 = toCgalPoint(slaveX[i1]);
+ const CgalPoint p2 = toCgalPoint(slaveX[i2]);
+ const CGAL::Triangle_3 triangle(p0, p1, p2);
+ if (!m_masterAabbTree->do_intersect(triangle))
+ continue;
+
+ if (addContactFromPoint(i0, p0, crossingDepth + safety, ContactKind::FaceCrossing))
+ ++faceContacts;
+ if (addContactFromPoint(i1, p1, crossingDepth + safety, ContactKind::FaceCrossing))
+ ++faceContacts;
+ if (addContactFromPoint(i2, p2, crossingDepth + safety, ContactKind::FaceCrossing))
+ ++faceContacts;
}
+ }
+
+ for (std::size_t i = 0; i < perVertexContacts.size(); ++i)
+ {
+ if (hasContact[i])
+ m_contacts.push_back(perVertexContacts[i]);
+ }
+
+ d_lastInsideContacts.setValue(static_cast(insideContacts));
+ d_lastEdgeContacts.setValue(static_cast(edgeContacts));
+ d_lastFaceContacts.setValue(static_cast(faceContacts));
+ d_lastProximityContacts.setValue(static_cast(proximityContacts));
+ }
+
+ template
+ void MeshRepulsionEngine::init()
+ {
+ Inherit::init();
+
+ if (!l_masterState.get() || !l_masterTopology.get())
+ {
+ msg_error() << "MeshRepulsionEngine needs masterState and masterTopology links.";
+ this->d_componentState.setValue(sofa::core::objectmodel::ComponentState::Invalid);
+ return;
+ }
- if (!slaveBuildWarning.empty())
+ if ((d_pushEdgeCrossings.getValue() || d_pushFaceCrossings.getValue()) && !l_slaveTopology.get())
+ {
+ msg_warning() << "Edge/face crossing detection requires slaveTopology. Falling back to vertex/proximity contacts only.";
+ }
+
+ this->d_componentState.setValue(sofa::core::objectmodel::ComponentState::Valid);
+ }
+
+ template
+ void MeshRepulsionEngine::bwdInit()
+ {
+ Inherit::bwdInit();
+ }
+
+ template
+ void MeshRepulsionEngine::applyDistributedForce(DataVecDeriv& f,
+ const VecCoord& x,
+ const VecDeriv& v,
+ const Contact& contact,
+ Real magnitude,
+ Real& maxF,
+ Real& sumF,
+ std::size_t& applied) const
+ {
+ if (contact.vertexId >= x.size())
+ return;
+
+ sofa::helper::WriteAccessor force = f;
+
+ const unsigned int rings = d_neighborhoodRings.getValue();
+ const Real neighborWeight = std::max(Real(0), std::min(Real(1), d_neighborWeight.getValue()));
+ const Deriv baseForce = derivScale(contact.normal, magnitude);
+
+ std::vector visited(x.size(), 0);
+ std::vector current;
+ current.push_back(contact.vertexId);
+ visited[contact.vertexId] = 1;
+
+ Real ringWeight = Real(1);
+ for (unsigned int ring = 0; ring <= rings && !current.empty(); ++ring)
+ {
+ std::vector next;
+ for (const std::size_t vid : current)
{
- msg_warning() << "Slave CGAL mesh warning: " << slaveBuildWarning;
+ if (vid >= x.size())
+ continue;
+
+ Deriv fi = derivScale(baseForce, ringWeight);
+
+ // Damping only if this vertex moves opposite to the outward normal.
+ if (vid < v.size())
+ {
+ const Real vn = derivDot(v[vid], contact.normal);
+ if (vn < Real(0))
+ {
+ const Real dampingMag = -d_contactDamping.getValue() * vn * ringWeight;
+ fi = derivAdd(fi, derivScale(contact.normal, dampingMag));
+ }
+ }
+
+ force[vid] += fi;
+
+ const Real fmag = derivNorm(fi);
+ maxF = std::max(maxF, fmag);
+ sumF += fmag;
+ ++applied;
+
+ if (ring < rings && vid < m_vertexNeighbors.size())
+ {
+ for (std::size_t nb : m_vertexNeighbors[vid])
+ {
+ if (nb < x.size() && !visited[nb])
+ {
+ visited[nb] = 1;
+ next.push_back(nb);
+ }
+ }
+ }
}
+ current.swap(next);
+ ringWeight *= neighborWeight;
+ }
+ }
+
+ template
+ void MeshRepulsionEngine::addForce(
+ const sofa::core::MechanicalParams*,
+ DataVecDeriv& f,
+ const DataVecCoord& x,
+ const DataVecDeriv& v)
+ {
+ if (this->d_componentState.getValue() != sofa::core::objectmodel::ComponentState::Valid)
+ return;
+
+ ++m_stepCounter;
+
+ if (!l_masterState.get() || !l_masterTopology.get())
+ return;
+
+ sofa::helper::ReadAccessor> masterX =
+ *l_masterState.get()->read(sofa::core::vec_id::read_access::position);
+
+ if (!m_masterCgalMesh || !m_masterAabbTree || d_rebuildMasterEveryStep.getValue())
+ {
+ VecCoord masterCopy(masterX.begin(), masterX.end());
+ if (!rebuildMasterCgalStructures(masterCopy))
+ return;
+ }
+
+ const VecCoord& slaveX = x.getValue();
+ const VecDeriv& slaveV = v.getValue();
+
+ detectContacts(slaveX);
+
+ const Real stiffness = d_stiffness.getValue();
+ const Real maxForce = d_maxForce.getValue();
+
+ Real maxF = 0;
+ Real sumF = 0;
+ std::size_t applied = 0;
+
+ for (const Contact& c : m_contacts)
+ {
+ Real magnitude = stiffness * c.depth;
+ if (maxForce > Real(0) && magnitude > maxForce)
+ magnitude = maxForce;
+
+ applyDistributedForce(f, slaveX, slaveV, c, magnitude, maxF, sumF, applied);
+ }
+
+ d_lastStep.setValue(static_cast(m_stepCounter));
+ d_lastMaxForce.setValue(maxF);
+ d_lastMeanForce.setValue(applied > 0 ? sumF / static_cast(applied) : Real(0));
+
+ const unsigned int logEvery = std::max(1u, d_logEveryNSteps.getValue());
+ if ((m_stepCounter % logEvery) == 0)
+ {
+ msg_info() << "[PR03B] step=" << m_stepCounter
+ << " contacts=" << m_contacts.size()
+ << " inside=" << d_lastInsideContacts.getValue()
+ << " edge=" << d_lastEdgeContacts.getValue()
+ << " face=" << d_lastFaceContacts.getValue()
+ << " proximity=" << d_lastProximityContacts.getValue()
+ << " applied=" << applied
+ << " maxF=" << maxF
+ << " meanF=" << (applied > 0 ? sumF / static_cast(applied) : Real(0));
}
if (d_writeCsv.getValue())
@@ -340,85 +719,80 @@ namespace cgal
if (csv)
{
if (writeHeader)
- {
- csv << "step,masterVertices,masterTriangles,slaveVertices,slaveTriangles,inside,boundary,outside,slaveCgalFaces\n";
- }
+ csv << "step,contacts,insideContacts,edgeContacts,faceContacts,proximityContacts,applied,maxForce,meanForce\n";
csv << m_stepCounter << ','
- << masterVertices.size() << ','
- << masterTriangleCount << ','
- << slaveVertices.size() << ','
- << slaveTriangleCount << ','
- << insideCount << ','
- << boundaryCount << ','
- << outsideCount << ','
- << slaveCgalFaceCount << "\n";
- }
- else
- {
- msg_warning() << "Could not open diagnostic CSV: " << path;
+ << m_contacts.size() << ','
+ << d_lastInsideContacts.getValue() << ','
+ << d_lastEdgeContacts.getValue() << ','
+ << d_lastFaceContacts.getValue() << ','
+ << d_lastProximityContacts.getValue() << ','
+ << applied << ','
+ << maxF << ','
+ << (applied > 0 ? sumF / static_cast(applied) : Real(0)) << "\n";
}
}
-
}
- void MeshRepulsionEngine::handleEvent(sofa::core::objectmodel::Event* event)
+ template
+ void MeshRepulsionEngine::addDForce(
+ const sofa::core::MechanicalParams* mparams,
+ DataVecDeriv& df,
+ const DataVecDeriv& dx)
{
- if (d_componentState.getValue() != sofa::core::objectmodel::ComponentState::Valid)
- {
+ if (this->d_componentState.getValue() != sofa::core::objectmodel::ComponentState::Valid)
return;
+
+ const Real kFactor = static_cast(
+ sofa::core::mechanicalparams::kFactorIncludingRayleighDamping(
+ mparams, this->rayleighStiffness.getValue()));
+
+ sofa::helper::WriteAccessor ddf = df;
+ const VecDeriv& ddx = dx.getValue();
+
+ for (const Contact& c : m_contacts)
+ {
+ const std::size_t vid = c.vertexId;
+ if (vid >= ddx.size())
+ continue;
+
+ const Deriv& n = c.normal;
+ const Real dxn = derivDot(ddx[vid], n);
+ const Deriv contrib = derivScale(n, c.stiffnessAlongN * dxn * kFactor);
+ ddf[vid] -= contrib;
}
+ }
+
+ template
+ SReal MeshRepulsionEngine::getPotentialEnergy(const sofa::core::MechanicalParams*, const DataVecCoord&) const {
+ return 0.0;
+ }
- if (sofa::core::objectmodel::KeypressedEvent::checkEventType(event))
+ template
+ void MeshRepulsionEngine::addKToMatrix(
+ sofa::linearalgebra::BaseMatrix* matrix,
+ SReal kFact,
+ unsigned int& offset)
+ {
+ for (const Contact& c : m_contacts)
{
- auto* ev = dynamic_cast(event);
- if (ev)
+ const std::size_t vid = c.vertexId;
+ const Deriv& n = c.normal;
+ const Real k = c.stiffnessAlongN;
+
+ for (int r = 0; r < 3; ++r)
{
- std::cout << " KeypressedEvent received: " << ev->getKey() << std::endl;
+ for (int col = 0; col < 3; ++col)
+ {
+ const double val = -static_cast(kFact)
+ * static_cast(k)
+ * static_cast(n[r])
+ * static_cast(n[col]);
+ matrix->add(offset + 3 * vid + r, offset + 3 * vid + col, val);
+ }
}
}
- else if (sofa::simulation::AnimateEndEvent::checkEventType(event))
- {
- std::cout << " AnimateEndEvent received" << std::endl;
- updateIntersectedPositions();
- }
}
- void MeshRepulsionEngine::draw(const core::visual::VisualParams*)
- {
- //if (m_mapPositionIds.empty() || d_drawInterpolation.getValue() == false)
- // return;
-
- //const auto stateLifeCycle = vparams->drawTool()->makeStateLifeCycle();
- //vparams->drawTool()->setLightingEnabled(false);
- //sofa::helper::ReadAccessor< Data< type::vector< Vec3 > > > fullPositions = d_inputPositions;
- //sofa::helper::ReadAccessor< Data< type::vector< Vec3 > > > mapPositions = d_mapPositions;
-
- //std::vector vertices;
- //std::vector colors;
-
- //if (m_mapPositionIds.size() == fullPositions.size())
- //{
- // for (unsigned int i = 0; i < fullPositions.size(); ++i)
- // {
- // vertices.emplace_back(fullPositions[i]);
- // vertices.emplace_back(mapPositions[m_mapPositionIds[i]]);
- // colors.emplace_back(m_mapColors[i]);
- // }
- //}
- //else
- //{
- // for (unsigned int i = 0; i < fullPositions.size(); ++i)
- // {
- // for (unsigned int j = 0; j < 3; ++j)
- // {
- // vertices.emplace_back(fullPositions[i]);
- // vertices.emplace_back(mapPositions[m_mapPositionIds[i * 3 + j]]);
- // colors.emplace_back(m_mapColors[i]);
- // }
- // }
- //}
-
- //vparams->drawTool()->drawLines(vertices, 1, colors);
- }
+ template class SOFA_CGALPLUGIN_API MeshRepulsionEngine;
-} // namespace cgal
+} // namespace cgal
\ No newline at end of file
diff --git a/src/CGALPlugin/MeshRepulsionEngine.h b/src/CGALPlugin/MeshRepulsionEngine.h
index a90ea42..b3a333d 100644
--- a/src/CGALPlugin/MeshRepulsionEngine.h
+++ b/src/CGALPlugin/MeshRepulsionEngine.h
@@ -1,4 +1,4 @@
-/*****************************************************************************
+/*******************************************************************************
* - Copyright (C) 2020-Present InfinyTech3D - *
* *
* This file is part of the InfinyToolkit plugin for the SOFA framework. *
@@ -16,73 +16,122 @@
* *
* Contact: contact@infinytech3d.com *
* Further information: https://infinytech3d.com *
-****************************************************************************/
+******************************************************************************/
#pragma once
#include
-#include
-#include
+
+#include
+#include
#include
-#include
#include
-#include
+#include
+#include
+#include
+#include
+#include
#include
+#include
#include
#include
#include
#include
+#include
namespace cgal
{
- using sofa::type::Vec3;
-
- /*
- TODO: Attach given pair of particles, projecting the positions of the second particles to the first ones.
- */
-
/**
- * @brief SOFA/CGAL bridge used to detect.
- *
- * reads master/slave MechanicalState and BaseMeshTopology from the scene;
- * converts the master surface topology to a CGAL::Surface_mesh;
- * converts the slave surface topology to a CGAL::Surface_mesh for validation/debug;
- * counts slave vertices inside/on/outside the master using CGAL::Side_of_triangle_mesh;
- * does not move any point yet.
- *
- */
- class SOFA_CGALPLUGIN_API MeshRepulsionEngine : public sofa::core::DataEngine
+ * @brief CGAL/SOFA repulsion ForceField for pushing a slave mesh outside a master mesh.
+ *
+ * This version keeps the ForceField approach:
+ * - edge/face crossing contacts now generate a real force even when their vertices
+ * are outside the safety band;
+ * - contact forces can be propagated to a local topological neighborhood to better
+ * preserve the initial shape of the slave surface.
+ *
+ * CGAL is used only for geometric detection and contact direction. SOFA is in
+ * charge of integrating the motion through mass, FEM, springs and damping.
+ */
+ template
+ class SOFA_CGALPLUGIN_API MeshRepulsionEngine : public sofa::core::behavior::ForceField
{
public:
- SOFA_CLASS(MeshRepulsionEngine, sofa::core::DataEngine);
-
- using DataTypes = sofa::defaulttype::Vec3Types;
- using Coord = DataTypes::Coord;
- using VecCoord = DataTypes::VecCoord;
+ SOFA_CLASS(SOFA_TEMPLATE(MeshRepulsionEngine, DataTypes),
+ SOFA_TEMPLATE(sofa::core::behavior::ForceField, DataTypes));
+
+ using Inherit = sofa::core::behavior::ForceField;
+ using Real = typename DataTypes::Real;
+ using Coord = typename DataTypes::Coord;
+ using Deriv = typename DataTypes::Deriv;
+ using VecCoord = typename DataTypes::VecCoord;
+ using VecDeriv = typename DataTypes::VecDeriv;
+ using DataVecCoord = sofa::core::objectmodel::Data;
+ using DataVecDeriv = sofa::core::objectmodel::Data;
using BaseLink = sofa::core::objectmodel::BaseLink;
using Kernel = CGAL::Exact_predicates_inexact_constructions_kernel;
using CgalPoint = Kernel::Point_3;
+ using CgalVector = Kernel::Vector_3;
using CgalSurfaceMesh = CGAL::Surface_mesh;
+ using CgalAabbPrimitive = CGAL::AABB_face_graph_triangle_primitive;
+ using CgalAabbTraits = CGAL::AABB_traits;
+ using CgalAabbTree = CGAL::AABB_tree;
+ using CgalInsideTester = CGAL::Side_of_triangle_mesh;
+ protected:
MeshRepulsionEngine();
~MeshRepulsionEngine() override;
+ public:
void init() override;
void bwdInit() override;
- void doUpdate() override;
- void handleEvent(sofa::core::objectmodel::Event* event) override;
+ void addForce(const sofa::core::MechanicalParams* mparams,
+ DataVecDeriv& f,
+ const DataVecCoord& x,
+ const DataVecDeriv& v) override;
- void draw(const sofa::core::visual::VisualParams* vparams) override;
+ void addDForce(const sofa::core::MechanicalParams* mparams,
+ DataVecDeriv& df,
+ const DataVecDeriv& dx) override;
+
+ SReal getPotentialEnergy(const sofa::core::MechanicalParams* mparams,
+ const DataVecCoord& x) const override;
+
+ void addKToMatrix(sofa::linearalgebra::BaseMatrix* matrix,
+ SReal kFact,
+ unsigned int& offset) override;
protected:
- void updateIntersectedPositions();
+ enum class ContactKind
+ {
+ Inside,
+ EdgeCrossing,
+ FaceCrossing,
+ Proximity
+ };
+
+ struct Contact
+ {
+ std::size_t vertexId = 0;
+ Deriv normal;
+ Real stiffnessAlongN = 0;
+ Real depth = 0;
+ ContactKind kind = ContactKind::Inside;
+ };
private:
static CgalPoint toCgalPoint(const Coord& p);
+ static double norm(const CgalVector& v);
+ static Deriv makeDeriv(Real x, Real y, Real z);
+ static Real derivNorm(const Deriv& v);
+ static Real derivDot(const Deriv& a, const Deriv& b);
+ static Deriv derivScale(const Deriv& v, Real s);
+ static Deriv derivAdd(const Deriv& a, const Deriv& b);
+ static Deriv normalizeDeriv(const Deriv& v);
bool buildSurfaceMeshFromSofa(
const VecCoord& positions,
@@ -90,50 +139,97 @@ namespace cgal
CgalSurfaceMesh& outputMesh,
std::string& errorMessage) const;
- bool rebuildMasterCgalMesh(const VecCoord& masterVertices);
+ bool rebuildMasterCgalStructures(const VecCoord& masterVertices);
+ void rebuildSlaveNeighborhood(std::size_t vertexCount);
+
+ Deriv orientedNormalFromClosestFace(
+ const CgalPoint& queryPoint,
+ const CgalPoint& closestPoint,
+ typename boost::graph_traits::face_descriptor faceDescriptor) const;
+
+ void detectContacts(const VecCoord& slaveX);
+
+ void mergeContact(std::vector& perVertexContacts,
+ std::vector& hasContact,
+ std::size_t vertexId,
+ const Deriv& normal,
+ Real depth,
+ Real stiffness,
+ ContactKind kind) const;
+
+ void applyDistributedForce(DataVecDeriv& f,
+ const VecCoord& x,
+ const VecDeriv& v,
+ const Contact& contact,
+ Real magnitude,
+ Real& maxF,
+ Real& sumF,
+ std::size_t& applied) const;
public:
- // Inputs Data
-
- sofa::core::objectmodel::SingleLink, BaseLink::FLAG_STOREPATH | BaseLink::FLAG_STRONGLINK> l_masterState;
- sofa::core::objectmodel::SingleLink l_masterTopology;
-
- sofa::core::objectmodel::SingleLink, BaseLink::FLAG_STOREPATH | BaseLink::FLAG_STRONGLINK> l_slaveState;
- sofa::core::objectmodel::SingleLink l_slaveTopology;
-
- // Enable/disable the CGAL detection bridge without removing the component from the scene.
- sofa::core::objectmodel::Data d_enableCgalDetection;
-
- // Rebuild the master CGAL mesh at every step. Keep false for a fixed master.
+ sofa::core::objectmodel::SingleLink<
+ MeshRepulsionEngine,
+ sofa::core::behavior::MechanicalState,
+ BaseLink::FLAG_STOREPATH | BaseLink::FLAG_STRONGLINK> l_masterState;
+
+ sofa::core::objectmodel::SingleLink<
+ MeshRepulsionEngine,
+ sofa::core::topology::BaseMeshTopology,
+ BaseLink::FLAG_STOREPATH | BaseLink::FLAG_STRONGLINK> l_masterTopology;
+
+ sofa::core::objectmodel::SingleLink<
+ MeshRepulsionEngine,
+ sofa::core::topology::BaseMeshTopology,
+ BaseLink::FLAG_STOREPATH | BaseLink::FLAG_STRONGLINK> l_slaveTopology;
+
+ // Repulsion controls.
+ sofa::core::objectmodel::Data d_stiffness;
+ sofa::core::objectmodel::Data d_safetyDistance;
+ sofa::core::objectmodel::Data d_crossingDepth;
+ sofa::core::objectmodel::Data d_maxForce;
+ sofa::core::objectmodel::Data d_contactDamping;
+
+ // Shape-preservation controls.
+ sofa::core::objectmodel::Data d_neighborhoodRings;
+ sofa::core::objectmodel::Data d_neighborWeight;
+
+ // Detection switches.
+ sofa::core::objectmodel::Data d_pushInsideVertices;
+ sofa::core::objectmodel::Data d_pushEdgeCrossings;
+ sofa::core::objectmodel::Data d_pushFaceCrossings;
+ sofa::core::objectmodel::Data d_pushProximityVertices;
sofa::core::objectmodel::Data d_rebuildMasterEveryStep;
- // Print one CGAL diagnostic line every N animation steps (1 means every step).
+ // Diagnostics.
sofa::core::objectmodel::Data d_logEveryNSteps;
-
- // Run one detection automatically after initialization, during bwdInit().
- sofa::core::objectmodel::Data d_runDetectionAtInit;
-
- // Print diagnostics as warnings. Warnings are visible even when info logs are filtered.
- sofa::core::objectmodel::Data d_logAsWarning;
-
- // Write diagnostics to a CSV file in the current working directory.
sofa::core::objectmodel::Data d_writeCsv;
sofa::core::objectmodel::Data d_csvFilePath;
- // Last computed values. These are visible on theEngine in the SOFA GUI data panel.
sofa::core::objectmodel::Data d_lastStep;
- sofa::core::objectmodel::Data d_lastMasterVertexCount;
- sofa::core::objectmodel::Data d_lastMasterTriangleCount;
- sofa::core::objectmodel::Data d_lastSlaveVertexCount;
- sofa::core::objectmodel::Data d_lastSlaveTriangleCount;
- sofa::core::objectmodel::Data d_lastInsideCount;
- sofa::core::objectmodel::Data d_lastBoundaryCount;
- sofa::core::objectmodel::Data d_lastOutsideCount;
- sofa::core::objectmodel::Data d_lastSlaveCgalFaceCount;
+ sofa::core::objectmodel::Data d_lastInsideContacts;
+ sofa::core::objectmodel::Data