From 89860d596efc6e9797e80eeea41fe3f5adc2da41 Mon Sep 17 00:00:00 2001 From: fadi_wahbeh Date: Thu, 9 Jul 2026 13:48:53 +0200 Subject: [PATCH 1/3] [Intersection Push] Adding Repulsive Force Field using tetrahedron meshes --- scenes/MeshRepulsion_patient_01.scn | 110 ++++ scenes/MeshRepulsion_patient_08.scn | 99 +++ scenes/MeshRepulsion_patient_16.scn | 99 +++ src/CGALPlugin/MeshRepulsionEngine.cpp | 832 ++++++++++++++++++------- src/CGALPlugin/MeshRepulsionEngine.h | 222 +++++-- 5 files changed, 1070 insertions(+), 292 deletions(-) create mode 100644 scenes/MeshRepulsion_patient_01.scn create mode 100644 scenes/MeshRepulsion_patient_08.scn create mode 100644 scenes/MeshRepulsion_patient_16.scn diff --git a/scenes/MeshRepulsion_patient_01.scn b/scenes/MeshRepulsion_patient_01.scn new file mode 100644 index 0000000..b22482d --- /dev/null +++ b/scenes/MeshRepulsion_patient_01.scn @@ -0,0 +1,110 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/scenes/MeshRepulsion_patient_08.scn b/scenes/MeshRepulsion_patient_08.scn new file mode 100644 index 0000000..b89b8af --- /dev/null +++ b/scenes/MeshRepulsion_patient_08.scn @@ -0,0 +1,99 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/scenes/MeshRepulsion_patient_16.scn b/scenes/MeshRepulsion_patient_16.scn new file mode 100644 index 0000000..cafe4db --- /dev/null +++ b/scenes/MeshRepulsion_patient_16.scn @@ -0,0 +1,99 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 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 d_lastEdgeContacts; + sofa::core::objectmodel::Data d_lastFaceContacts; + sofa::core::objectmodel::Data d_lastProximityContacts; + sofa::core::objectmodel::Data d_lastMaxForce; + sofa::core::objectmodel::Data d_lastMeanForce; private: std::unique_ptr m_masterCgalMesh; + std::unique_ptr m_masterAabbTree; + std::unique_ptr m_insideTester; + CgalPoint m_masterCentroid = CgalPoint(0.0, 0.0, 0.0); + bool m_hasMasterCentroid = false; + + std::vector> m_vertexNeighbors; + std::size_t m_neighborhoodVertexCount = 0; + std::vector m_isSurfaceVertex; + + std::vector m_contacts; std::size_t m_stepCounter = 0; }; -} // namespace cgal +#if !defined(CGALPLUGIN_MESHREPULSIONENGINE_CPP) + extern template class SOFA_CGALPLUGIN_API MeshRepulsionEngine; +#endif + +} // namespace cgal \ No newline at end of file From 803071bd6cf90e7e237fd263c3f96c3814e7fc5b Mon Sep 17 00:00:00 2001 From: epernod Date: Thu, 9 Jul 2026 17:13:41 +0200 Subject: [PATCH 2/3] Update scene to use a RestShapeSpringFF as boundary condition --- scenes/MeshRepulsion_patient_01.scn | 31 ++++++++++++++--------------- scenes/MeshRepulsion_patient_08.scn | 22 +++++++------------- scenes/MeshRepulsion_patient_16.scn | 22 +++++++------------- 3 files changed, 29 insertions(+), 46 deletions(-) diff --git a/scenes/MeshRepulsion_patient_01.scn b/scenes/MeshRepulsion_patient_01.scn index b22482d..a7f4342 100644 --- a/scenes/MeshRepulsion_patient_01.scn +++ b/scenes/MeshRepulsion_patient_01.scn @@ -35,11 +35,11 @@ - + - + @@ -55,15 +55,23 @@ - + + + + + + @@ -71,18 +79,6 @@ - - + logEveryNSteps="20" + stiffness="10000" + maxForce="10000" + /> diff --git a/scenes/MeshRepulsion_patient_08.scn b/scenes/MeshRepulsion_patient_08.scn index b89b8af..3eecaf3 100644 --- a/scenes/MeshRepulsion_patient_08.scn +++ b/scenes/MeshRepulsion_patient_08.scn @@ -28,7 +28,7 @@ - + @@ -44,7 +44,7 @@ - + @@ -52,6 +52,7 @@ + @@ -60,18 +61,6 @@ - - + logEveryNSteps="20" + stiffness="10000" + maxForce="10000" + /> diff --git a/scenes/MeshRepulsion_patient_16.scn b/scenes/MeshRepulsion_patient_16.scn index cafe4db..e918e9b 100644 --- a/scenes/MeshRepulsion_patient_16.scn +++ b/scenes/MeshRepulsion_patient_16.scn @@ -28,7 +28,7 @@ - + @@ -44,7 +44,7 @@ - + @@ -52,6 +52,7 @@ + @@ -60,18 +61,6 @@ - - + logEveryNSteps="20" + stiffness="10000" + maxForce="10000" + /> From 7ea8d2ac950cdc496ba877292ddcc289c6d795e3 Mon Sep 17 00:00:00 2001 From: epernod Date: Fri, 10 Jul 2026 11:52:25 +0200 Subject: [PATCH 3/3] [scenes] Add node to map collision obj mesh. Also add MeshExporter but not tested --- scenes/MeshRepulsion_patient_01.scn | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/scenes/MeshRepulsion_patient_01.scn b/scenes/MeshRepulsion_patient_01.scn index a7f4342..8777c5e 100644 --- a/scenes/MeshRepulsion_patient_01.scn +++ b/scenes/MeshRepulsion_patient_01.scn @@ -104,6 +104,17 @@ + + + + + + + + + + +