diff --git a/scenes/MeshRepulsion.scn b/scenes/MeshRepulsion.scn index 32ca6bf..4afeaf3 100644 --- a/scenes/MeshRepulsion.scn +++ b/scenes/MeshRepulsion.scn @@ -1,67 +1,77 @@ - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + - - - + + + - + - + - - - + + + + - + - + - - - - + - - + + + - + - - - - + enableCgalDetection="true" + rebuildMasterEveryStep="false" + logEveryNSteps="1" + runDetectionAtInit="true" + logAsWarning="true" + writeCsv="true" + csvFilePath="MeshRepulsion_PR02B_diagnostics.csv" + enablePositionUpdate="true" + safetyDistance="0.25" + maxStep="0.10" + alpha="0.35" + maxMovedVerticesPerStep="0" + updateRestPosition="true" + rejectIfInsideIncreases="true" + maxCorrectionSteps="300"/> diff --git a/src/CGALPlugin/MeshRepulsionEngine.cpp b/src/CGALPlugin/MeshRepulsionEngine.cpp index 00d8f08..cc011d5 100644 --- a/src/CGALPlugin/MeshRepulsionEngine.cpp +++ b/src/CGALPlugin/MeshRepulsionEngine.cpp @@ -23,15 +23,19 @@ #include #include #include -#include #include +#include +#include #include +#include #include #include +#include #include #include +#include namespace cgal { @@ -43,7 +47,7 @@ namespace cgal void registerMeshRepulsionEngine(sofa::core::ObjectFactory* factory) { factory->registerObjects(sofa::core::ObjectRegistrationData( - "MeshRepulsionEngine: SOFA/CGAL bridge for progressive slave/master repulsion") + "MeshRepulsionEngine: SOFA/CGAL local non-rigid repulsion with SOFA rest-position stabilization") .add()); } @@ -55,31 +59,41 @@ namespace cgal , 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_runDetectionAtInit(initData(&d_runDetectionAtInit, true, "runDetectionAtInit", "Run one detection/update during bwdInit()")) , d_logAsWarning(initData(&d_logAsWarning, true, "logAsWarning", "Print diagnostics with msg_warning instead of msg_info")) , 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_csvFilePath(initData(&d_csvFilePath, std::string("MeshRepulsion_PR02B_diagnostics.csv"), "csvFilePath", "CSV file path for diagnostics")) + , d_enablePositionUpdate(initData(&d_enablePositionUpdate, true, "enablePositionUpdate", "Enable PR02B local non-rigid position update for inside slave vertices")) + , d_safetyDistance(initData(&d_safetyDistance, 0.25, "safetyDistance", "Distance kept outside the master surface after closest-point projection")) + , d_maxStep(initData(&d_maxStep, 0.25, "maxStep", "Maximum displacement applied to one vertex per update step")) + , d_alpha(initData(&d_alpha, 0.5, "alpha", "Smoothness factor applied to the computed correction before clamping")) + , d_maxMovedVerticesPerStep(initData(&d_maxMovedVerticesPerStep, static_cast(0), "maxMovedVerticesPerStep", "Maximum number of inside vertices moved per step. 0 means unlimited")) + , d_updateRestPosition(initData(&d_updateRestPosition, true, "updateRestPosition", "Also update slave restPosition after accepting the CGAL correction so SOFA FEM does not pull vertices back inside the master")) + , d_rejectIfInsideIncreases(initData(&d_rejectIfInsideIncreases, true, "rejectIfInsideIncreases", "Reject the whole correction step if the number of inside vertices increases")) + , d_maxCorrectionSteps(initData(&d_maxCorrectionSteps, static_cast(300), "maxCorrectionSteps", "Stop applying CGAL correction after this number of update steps. 0 means unlimited")) , 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_lastInsideCount(initData(&d_lastInsideCount, static_cast(0), "lastInsideCount", "Last slave vertices inside master after the update")) + , d_lastBoundaryCount(initData(&d_lastBoundaryCount, static_cast(0), "lastBoundaryCount", "Last slave vertices on master boundary after the update")) + , d_lastOutsideCount(initData(&d_lastOutsideCount, static_cast(0), "lastOutsideCount", "Last slave vertices outside master after the update")) , d_lastSlaveCgalFaceCount(initData(&d_lastSlaveCgalFaceCount, static_cast(0), "lastSlaveCgalFaceCount", "Last CGAL slave face count")) - + , d_lastInsideBeforeCount(initData(&d_lastInsideBeforeCount, static_cast(0), "lastInsideBeforeCount", "Inside count before PR02A update")) + , d_lastInsideAfterCount(initData(&d_lastInsideAfterCount, static_cast(0), "lastInsideAfterCount", "Inside count after PR02A update")) + , d_lastMovedCount(initData(&d_lastMovedCount, static_cast(0), "lastMovedCount", "Number of vertices moved by the last PR02A update")) + , d_lastMaxAppliedDisplacement(initData(&d_lastMaxAppliedDisplacement, 0.0, "lastMaxAppliedDisplacement", "Maximum displacement applied during the last PR02A update")) + , d_lastMeanAppliedDisplacement(initData(&d_lastMeanAppliedDisplacement, 0.0, "lastMeanAppliedDisplacement", "Mean displacement applied during the last PR02A update")) { f_listening.setValue(true); - std::cout << "LOADED MeshRepulsionEngine loader-position-fix" << std::endl; + std::cout << "LOADED MeshRepulsionEngine PR02B rest-position-stabilized-local-repulsion" << std::endl; } MeshRepulsionEngine::~MeshRepulsionEngine() = default; void MeshRepulsionEngine::init() { - // 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."; @@ -89,8 +103,10 @@ namespace cgal sofa::core::objectmodel::BaseObject::d_componentState.setValue(sofa::core::objectmodel::ComponentState::Valid); - 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; + std::cout << " init(): links are valid. Detection/update will run in bwdInit() and on AnimateEndEvent. " + << "runDetectionAtInit=" << (d_runDetectionAtInit.getValue() ? "true" : "false") + << " enablePositionUpdate=" << (d_enablePositionUpdate.getValue() ? "true" : "false") + << std::endl; } void MeshRepulsionEngine::bwdInit() @@ -100,7 +116,7 @@ namespace cgal return; } - std::cout << "[CGAL detection] bwdInit(): post-init diagnostic point reached." << std::endl; + std::cout << " bwdInit(): post-init diagnostic/update point reached." << std::endl; if (d_runDetectionAtInit.getValue()) { @@ -110,17 +126,24 @@ namespace cgal void MeshRepulsionEngine::doUpdate() { - // The engine is event-driven for now. The real work is performed in handleEvent(). - std::cout << "call MeshRepulsionEngine::doUpdate()" << std::endl; - + // Work is performed in bwdInit() and handleEvent(). } - // HELPER METHOD TO GET A CGAL POINT FROM A SOFA COORD MeshRepulsionEngine::CgalPoint MeshRepulsionEngine::toCgalPoint(const Coord& p) { return CgalPoint(static_cast(p[0]), static_cast(p[1]), static_cast(p[2])); } + double MeshRepulsionEngine::squaredNorm(const CgalVector& v) + { + return CGAL::to_double(v.squared_length()); + } + + double MeshRepulsionEngine::norm(const CgalVector& v) + { + return std::sqrt(std::max(0.0, squaredNorm(v))); + } + bool MeshRepulsionEngine::buildSurfaceMeshFromSofa( const VecCoord& positions, sofa::core::topology::BaseMeshTopology* topology, @@ -145,7 +168,7 @@ namespace cgal const auto nbTriangles = topology->getNbTriangles(); if (nbTriangles == 0) { - errorMessage = "topology has no triangles; needs a surface topology for inside/outside tests"; + errorMessage = "topology has no triangles; PR02A needs a surface topology for inside/outside tests"; return false; } @@ -205,9 +228,11 @@ namespace cgal return true; } - bool MeshRepulsionEngine::rebuildMasterCgalMesh(const VecCoord& masterVertices) + bool MeshRepulsionEngine::rebuildMasterCgalStructures(const VecCoord& masterVertices) { m_masterCgalMesh = std::make_unique(); + m_masterAabbTree.reset(); + m_hasMasterCentroid = false; std::string errorMessage; if (!buildSurfaceMeshFromSofa(masterVertices, l_masterTopology.get(), *m_masterCgalMesh, errorMessage)) @@ -222,15 +247,108 @@ namespace cgal msg_warning() << "Master CGAL mesh warning: " << errorMessage; } - msg_info() << "Master CGAL mesh built: vertices=" << m_masterCgalMesh->number_of_vertices() + double sx = 0.0; + double sy = 0.0; + double 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; + } + + m_masterAabbTree = std::make_unique( + faces(*m_masterCgalMesh).first, + faces(*m_masterCgalMesh).second, + *m_masterCgalMesh); + m_masterAabbTree->accelerate_distance_queries(); + + msg_info() << "Master CGAL structures built: vertices=" << m_masterCgalMesh->number_of_vertices() << " faces=" << m_masterCgalMesh->number_of_faces(); return true; } + bool MeshRepulsionEngine::computeRepulsionMove(const Coord& currentPosition, Coord& movedPosition, double& appliedDisplacement) const + { + appliedDisplacement = 0.0; + movedPosition = currentPosition; + + if (!m_masterAabbTree) + { + return false; + } + + const double alpha = std::max(0.0, d_alpha.getValue()); + const double maxStep = std::max(0.0, d_maxStep.getValue()); + const double safetyDistance = std::max(0.0, d_safetyDistance.getValue()); + + if (alpha <= 0.0 || maxStep <= 0.0) + { + return false; + } + + const CgalPoint p = toCgalPoint(currentPosition); + const CgalPoint closest = m_masterAabbTree->closest_point(p); + + // Direction from the inside point toward the closest master surface point. + CgalVector outwardDirection = closest - p; + double directionNorm = norm(outwardDirection); + + // Degenerate fallback. This can happen when the point is already exactly on the surface. + if (directionNorm < 1.0e-12 && m_hasMasterCentroid) + { + outwardDirection = p - m_masterCentroid; + directionNorm = norm(outwardDirection); + } + + if (directionNorm < 1.0e-12) + { + outwardDirection = CgalVector(1.0, 0.0, 0.0); + directionNorm = 1.0; + } + + const CgalVector unitDirection = outwardDirection / directionNorm; + const CgalPoint target = closest + unitDirection * safetyDistance; + const CgalVector fullDelta = target - p; + const double fullDeltaNorm = norm(fullDelta); + + if (fullDeltaNorm < 1.0e-12) + { + return false; + } + + double scale = alpha; + const double scaledLength = fullDeltaNorm * scale; + if (scaledLength > maxStep) + { + scale = maxStep / fullDeltaNorm; + } + + const CgalVector appliedDelta = fullDelta * scale; + appliedDisplacement = norm(appliedDelta); + + if (appliedDisplacement < 1.0e-12) + { + return false; + } + + movedPosition[0] = currentPosition[0] + static_cast(appliedDelta.x()); + movedPosition[1] = currentPosition[1] + static_cast(appliedDelta.y()); + movedPosition[2] = currentPosition[2] + static_cast(appliedDelta.z()); + return true; + } + void MeshRepulsionEngine::updateIntersectedPositions() { ++m_stepCounter; - std::cout << "[CGAL detection] updateIntersectedPositions() called, step=" << m_stepCounter << std::endl; + std::cout << "[PR02B] updateIntersectedPositions() called, step=" << m_stepCounter << std::endl; if (!d_enableCgalDetection.getValue()) { @@ -243,77 +361,199 @@ namespace cgal return; } - 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(); + sofa::helper::ReadAccessor masterVertices = + l_masterState.get()->read(sofa::core::vec_id::read_access::position)->getValue(); auto* masterTopology = l_masterTopology.get(); auto* slaveTopology = l_slaveTopology.get(); - if (!m_masterCgalMesh || d_rebuildMasterEveryStep.getValue()) + if (!m_masterCgalMesh || !m_masterAabbTree || d_rebuildMasterEveryStep.getValue()) { - if (!rebuildMasterCgalMesh(masterVertices)) + if (!rebuildMasterCgalStructures(masterVertices)) { return; } } - CgalSurfaceMesh slaveCgalMesh; - std::string slaveBuildWarning; - const bool slaveMeshOk = buildSurfaceMeshFromSofa(slaveVertices, slaveTopology, slaveCgalMesh, slaveBuildWarning); - if (!slaveMeshOk) + CGAL::Side_of_triangle_mesh insideTester(*m_masterCgalMesh); + + sofa::helper::ReadAccessor slaveVerticesRead = + l_slaveState.get()->read(sofa::core::vec_id::read_access::position)->getValue(); + + VecCoord currentPositions; + currentPositions.resize(slaveVerticesRead.size()); + for (std::size_t i = 0; i < slaveVerticesRead.size(); ++i) { - msg_warning() << "Could not build slave CGAL surface mesh: " << slaveBuildWarning; + currentPositions[i] = slaveVerticesRead[i]; } - CGAL::Side_of_triangle_mesh insideTester(*m_masterCgalMesh); + std::size_t insideBeforeCount = 0; + std::size_t boundaryBeforeCount = 0; + std::size_t outsideBeforeCount = 0; + std::vector insideIndices; + insideIndices.reserve(currentPositions.size()); - std::size_t insideCount = 0; - std::size_t boundaryCount = 0; - std::size_t outsideCount = 0; + for (std::size_t i = 0; i < currentPositions.size(); ++i) + { + const auto side = insideTester(toCgalPoint(currentPositions[i])); + if (side == CGAL::ON_BOUNDED_SIDE) + { + ++insideBeforeCount; + insideIndices.emplace_back(i); + } + else if (side == CGAL::ON_BOUNDARY) + { + ++boundaryBeforeCount; + } + else + { + ++outsideBeforeCount; + } + } + + VecCoord proposedPositions; + proposedPositions.resize(currentPositions.size()); + for (std::size_t i = 0; i < currentPositions.size(); ++i) + { + proposedPositions[i] = currentPositions[i]; + } + + std::size_t movedCount = 0; + double maxAppliedDisplacement = 0.0; + double sumAppliedDisplacement = 0.0; + + const unsigned int maxCorrectionSteps = d_maxCorrectionSteps.getValue(); + const bool correctionAllowedByStepLimit = (maxCorrectionSteps == 0) + || (m_stepCounter <= static_cast(maxCorrectionSteps)); + + if (d_enablePositionUpdate.getValue() && correctionAllowedByStepLimit && !insideIndices.empty()) + { + const unsigned int maxMoved = d_maxMovedVerticesPerStep.getValue(); + const std::size_t movementLimit = (maxMoved == 0) + ? insideIndices.size() + : std::min(insideIndices.size(), static_cast(maxMoved)); + + for (std::size_t k = 0; k < movementLimit; ++k) + { + const std::size_t vertexId = insideIndices[k]; + Coord movedPosition; + double displacement = 0.0; + if (computeRepulsionMove(currentPositions[vertexId], movedPosition, displacement)) + { + proposedPositions[vertexId] = movedPosition; + ++movedCount; + sumAppliedDisplacement += displacement; + maxAppliedDisplacement = std::max(maxAppliedDisplacement, displacement); + } + } + } - for (const Coord& p : slaveVertices) + std::size_t insideAfterCount = 0; + std::size_t boundaryAfterCount = 0; + std::size_t outsideAfterCount = 0; + for (std::size_t i = 0; i < proposedPositions.size(); ++i) { - const auto side = insideTester(toCgalPoint(p)); + const auto side = insideTester(toCgalPoint(proposedPositions[i])); if (side == CGAL::ON_BOUNDED_SIDE) { - ++insideCount; + ++insideAfterCount; } else if (side == CGAL::ON_BOUNDARY) { - ++boundaryCount; + ++boundaryAfterCount; } else { - ++outsideCount; + ++outsideAfterCount; } } + bool acceptCorrection = (movedCount > 0); + if (d_rejectIfInsideIncreases.getValue() && insideAfterCount > insideBeforeCount) + { + acceptCorrection = false; + insideAfterCount = insideBeforeCount; + boundaryAfterCount = boundaryBeforeCount; + outsideAfterCount = outsideBeforeCount; + movedCount = 0; + sumAppliedDisplacement = 0.0; + maxAppliedDisplacement = 0.0; + proposedPositions = currentPositions; + msg_warning() << "[PR02B] Rejected correction step because inside vertices increased."; + } + + if (acceptCorrection) + { + sofa::helper::WriteAccessor > slaveVerticesWrite = + *l_slaveState.get()->write(sofa::core::vec_id::write_access::position); + + for (std::size_t i = 0; i < proposedPositions.size(); ++i) + { + slaveVerticesWrite[i] = proposedPositions[i]; + } + + if (d_updateRestPosition.getValue()) + { + sofa::helper::WriteAccessor > slaveRestVerticesWrite = + *l_slaveState.get()->write(sofa::core::vec_id::write_access::restPosition); + + for (std::size_t i = 0; i < proposedPositions.size(); ++i) + { + slaveRestVerticesWrite[i] = proposedPositions[i]; + } + } + } + + CgalSurfaceMesh slaveCgalMesh; + std::string slaveBuildWarning; + const bool slaveMeshOk = buildSurfaceMeshFromSofa(proposedPositions, slaveTopology, slaveCgalMesh, slaveBuildWarning); + if (!slaveMeshOk) + { + msg_warning() << "Could not build slave CGAL surface mesh: " << slaveBuildWarning; + } + 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); + const double meanAppliedDisplacement = (movedCount > 0) + ? sumAppliedDisplacement / static_cast(movedCount) + : 0.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_lastSlaveVertexCount.setValue(static_cast(currentPositions.size())); d_lastSlaveTriangleCount.setValue(slaveTriangleCount); - d_lastInsideCount.setValue(static_cast(insideCount)); - d_lastBoundaryCount.setValue(static_cast(boundaryCount)); - d_lastOutsideCount.setValue(static_cast(outsideCount)); + d_lastInsideCount.setValue(static_cast(insideAfterCount)); + d_lastBoundaryCount.setValue(static_cast(boundaryAfterCount)); + d_lastOutsideCount.setValue(static_cast(outsideAfterCount)); d_lastSlaveCgalFaceCount.setValue(slaveCgalFaceCount); + d_lastInsideBeforeCount.setValue(static_cast(insideBeforeCount)); + d_lastInsideAfterCount.setValue(static_cast(insideAfterCount)); + d_lastMovedCount.setValue(static_cast(movedCount)); + d_lastMaxAppliedDisplacement.setValue(maxAppliedDisplacement); + d_lastMeanAppliedDisplacement.setValue(meanAppliedDisplacement); const unsigned int logEvery = std::max(1u, d_logEveryNSteps.getValue()); if ((m_stepCounter % logEvery) == 0) { std::ostringstream oss; - oss << "[CGAL detection] step=" << m_stepCounter + oss << "[PR02B CGAL local repulsion + rest update] step=" << m_stepCounter << " masterVertices=" << masterVertices.size() << " masterTriangles=" << masterTriangleCount - << " slaveVertices=" << slaveVertices.size() + << " slaveVertices=" << currentPositions.size() << " slaveTriangles=" << slaveTriangleCount - << " inside=" << insideCount - << " boundary=" << boundaryCount - << " outside=" << outsideCount + << " insideBefore=" << insideBeforeCount + << " boundaryBefore=" << boundaryBeforeCount + << " outsideBefore=" << outsideBeforeCount + << " moved=" << movedCount + << " accepted=" << (acceptCorrection ? 1 : 0) + << " updateRestPosition=" << (d_updateRestPosition.getValue() ? 1 : 0) + << " maxAppliedDisplacement=" << maxAppliedDisplacement + << " meanAppliedDisplacement=" << meanAppliedDisplacement + << " insideAfter=" << insideAfterCount + << " boundaryAfter=" << boundaryAfterCount + << " outsideAfter=" << outsideAfterCount << " slaveCgalFaces=" << slaveCgalFaceCount; std::cout << oss.str() << std::endl; @@ -341,16 +581,24 @@ namespace cgal { if (writeHeader) { - csv << "step,masterVertices,masterTriangles,slaveVertices,slaveTriangles,inside,boundary,outside,slaveCgalFaces\n"; + csv << "step,masterVertices,masterTriangles,slaveVertices,slaveTriangles,insideBefore,boundaryBefore,outsideBefore,moved,accepted,updateRestPosition,maxAppliedDisplacement,meanAppliedDisplacement,insideAfter,boundaryAfter,outsideAfter,slaveCgalFaces\n"; } csv << m_stepCounter << ',' << masterVertices.size() << ',' << masterTriangleCount << ',' - << slaveVertices.size() << ',' + << currentPositions.size() << ',' << slaveTriangleCount << ',' - << insideCount << ',' - << boundaryCount << ',' - << outsideCount << ',' + << insideBeforeCount << ',' + << boundaryBeforeCount << ',' + << outsideBeforeCount << ',' + << movedCount << ',' + << (acceptCorrection ? 1 : 0) << ',' + << (d_updateRestPosition.getValue() ? 1 : 0) << ',' + << maxAppliedDisplacement << ',' + << meanAppliedDisplacement << ',' + << insideAfterCount << ',' + << boundaryAfterCount << ',' + << outsideAfterCount << ',' << slaveCgalFaceCount << "\n"; } else @@ -358,7 +606,6 @@ namespace cgal msg_warning() << "Could not open diagnostic CSV: " << path; } } - } void MeshRepulsionEngine::handleEvent(sofa::core::objectmodel::Event* event) @@ -368,57 +615,15 @@ namespace cgal return; } - if (sofa::core::objectmodel::KeypressedEvent::checkEventType(event)) - { - auto* ev = dynamic_cast(event); - if (ev) - { - std::cout << " KeypressedEvent received: " << ev->getKey() << std::endl; - } - } - else if (sofa::simulation::AnimateEndEvent::checkEventType(event)) + 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); + // No custom debug for now. } -} // namespace cgal +} diff --git a/src/CGALPlugin/MeshRepulsionEngine.h b/src/CGALPlugin/MeshRepulsionEngine.h index a90ea42..b311723 100644 --- a/src/CGALPlugin/MeshRepulsionEngine.h +++ b/src/CGALPlugin/MeshRepulsionEngine.h @@ -20,39 +20,36 @@ #pragma once #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. - * - */ + * @brief SOFA/CGAL bridge used to detect and progressively push slave vertices out of a master mesh. + * + * PR01 validated the detection bridge only. + * PR02 Push slave vertices outside of the master object. + * + */ class SOFA_CGALPLUGIN_API MeshRepulsionEngine : public sofa::core::DataEngine { public: @@ -65,7 +62,11 @@ namespace cgal 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; MeshRepulsionEngine(); ~MeshRepulsionEngine() override; @@ -83,6 +84,8 @@ namespace cgal private: static CgalPoint toCgalPoint(const Coord& p); + static double squaredNorm(const CgalVector& v); + static double norm(const CgalVector& v); bool buildSurfaceMeshFromSofa( const VecCoord& positions, @@ -90,37 +93,38 @@ namespace cgal CgalSurfaceMesh& outputMesh, std::string& errorMessage) const; - bool rebuildMasterCgalMesh(const VecCoord& masterVertices); + bool rebuildMasterCgalStructures(const VecCoord& masterVertices); + bool computeRepulsionMove(const Coord& currentPosition, Coord& movedPosition, double& appliedDisplacement) 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. + // Detection controls. sofa::core::objectmodel::Data d_enableCgalDetection; - - // Rebuild the master CGAL mesh at every step. Keep false for a fixed master. sofa::core::objectmodel::Data d_rebuildMasterEveryStep; - - // Print one CGAL diagnostic line every N animation steps (1 means every step). 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. + // CSV diagnostics. 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. + // Local position update controls. + sofa::core::objectmodel::Data d_enablePositionUpdate; + sofa::core::objectmodel::Data d_safetyDistance; + sofa::core::objectmodel::Data d_maxStep; + sofa::core::objectmodel::Data d_alpha; + sofa::core::objectmodel::Data d_maxMovedVerticesPerStep; + sofa::core::objectmodel::Data d_updateRestPosition; + sofa::core::objectmodel::Data d_rejectIfInsideIncreases; + sofa::core::objectmodel::Data d_maxCorrectionSteps; + + // Computed values visible in the SOFA's panel. sofa::core::objectmodel::Data d_lastStep; sofa::core::objectmodel::Data d_lastMasterVertexCount; sofa::core::objectmodel::Data d_lastMasterTriangleCount; @@ -131,9 +135,19 @@ namespace cgal sofa::core::objectmodel::Data d_lastOutsideCount; sofa::core::objectmodel::Data d_lastSlaveCgalFaceCount; + // Correction values. + sofa::core::objectmodel::Data d_lastInsideBeforeCount; + sofa::core::objectmodel::Data d_lastInsideAfterCount; + sofa::core::objectmodel::Data d_lastMovedCount; + sofa::core::objectmodel::Data d_lastMaxAppliedDisplacement; + sofa::core::objectmodel::Data d_lastMeanAppliedDisplacement; + private: std::unique_ptr m_masterCgalMesh; + std::unique_ptr m_masterAabbTree; + CgalPoint m_masterCentroid = CgalPoint(0.0, 0.0, 0.0); + bool m_hasMasterCentroid = false; std::size_t m_stepCounter = 0; }; -} // namespace cgal +}