diff --git a/src/BeamAdapter/component/mapping/BeamProjectionDifferenceMultiMapping.h b/src/BeamAdapter/component/mapping/BeamProjectionDifferenceMultiMapping.h index 070b13580..9f74ab3b3 100644 --- a/src/BeamAdapter/component/mapping/BeamProjectionDifferenceMultiMapping.h +++ b/src/BeamAdapter/component/mapping/BeamProjectionDifferenceMultiMapping.h @@ -164,6 +164,8 @@ class SOFA_BEAMADAPTER_API BeamProjectionDifferenceMultiMapping : public sofa::c sofa::Data d_updateProjectionOrientation; sofa::Data d_draw; sofa::Data d_drawSize; + sofa::Data> d_projections; + sofa::Data d_filter; sofa::SingleLink, sofa::core::topology::BaseMeshTopology, sofa::BaseLink::FLAG_STOREPATH | sofa::BaseLink::FLAG_STRONGLINK> l_in2Topology; sofa::SingleLink, BeamInterpolation, sofa::BaseLink::FLAG_STOREPATH | sofa::BaseLink::FLAG_STRONGLINK> l_interpolation; diff --git a/src/BeamAdapter/component/mapping/BeamProjectionDifferenceMultiMapping.inl b/src/BeamAdapter/component/mapping/BeamProjectionDifferenceMultiMapping.inl index 4b46b032b..3392ed0fa 100644 --- a/src/BeamAdapter/component/mapping/BeamProjectionDifferenceMultiMapping.inl +++ b/src/BeamAdapter/component/mapping/BeamProjectionDifferenceMultiMapping.inl @@ -55,6 +55,9 @@ BeamProjectionDifferenceMultiMapping::BeamProjectionDifference , d_updateProjectionOrientation(initData(&d_updateProjectionOrientation, false, "updateProjectionOrientation", "Update the projection on the beam at each time step even when direction[0]=1.")) , d_draw(initData(&d_draw, "draw", "Draw projection points and directions")) , d_drawSize(initData(&d_drawSize, Real(3), "drawSize", "")) + , d_projections(initData(&d_projections, "projections", "")) + , d_filter(initData(&d_filter, Real(1e-2), "filter", "When updating the projection at each time step, use this parameter to filter small changes: " + "If the projection is on the same edge as the previous step, and the weight (in [0, 1]) is smaller than the value of 'filter', skip.")) , l_in2Topology(initLink("topologyInput2", "link to input2's topology container (beams to project on)")) , l_interpolation(initLink("interpolationInput2", "link to input2's interpolation component (BeamInterpolation)")) , m_fromModel1(NULL) @@ -124,6 +127,10 @@ void BeamProjectionDifferenceMultiMapping::init() msg_warning() << "Wrong size for directions, [" << directions << "]. The size should be " << OutCoord::total_size << "."; directions.resize(OutCoord::total_size); } + + auto projections = sofa::helper::getWriteAccessor(d_projections); + const auto& indices = sofa::helper::getReadAccessor(d_indices); + projections.resize(indices.size()); } @@ -134,8 +141,9 @@ void BeamProjectionDifferenceMultiMapping::computeProjection(c auto interpolation = l_interpolation.get(); auto edges = l_in2Topology.get()->getEdges(); - // for each point of P of xFrom + // for each point P of xFrom const auto& indices = sofa::helper::getReadAccessor(d_indices); + const auto& filter = sofa::helper::getReadAccessor(d_filter); m_mappedPoints.resize(indices.size()); for (unsigned int i=0; i::computeProjection(c OutDeriv dirAxe = Out::coordDifference(Q2, Q1); Real normAxe = In1::getDPos(dirAxe).norm(); - if (std::abs(normAxe)::epsilon()) // edge is degenerated, continue with next edge + SReal epsilon = std::numeric_limits::epsilon(); + if (std::abs(normAxe) 1) // not on the edge, continue with next edge + if (alpha < 0 - epsilon || alpha > 1 + epsilon) // not on the edge, continue with next edge continue; In1Coord proj; @@ -198,16 +207,19 @@ void BeamProjectionDifferenceMultiMapping::computeProjection(c { mp.interpolatedTransform = m_mappedPoints[i].interpolatedTransform; } - m_mappedPoints[i] = mp; + + // When updating the projection at each time step, filter small changes + if (m_mappedPoints[i].edgeIndex != mp.edgeIndex || std::abs(m_mappedPoints[i].alpha - mp.alpha) > filter) + m_mappedPoints[i] = mp; } } template -void BeamProjectionDifferenceMultiMapping::apply( const sofa::core::MechanicalParams* mparams, - const sofa::type::vector& dataVecOutPos, - const sofa::type::vector& dataVecIn1Pos , - const sofa::type::vector& dataVecIn2Pos) +void BeamProjectionDifferenceMultiMapping::apply(const sofa::core::MechanicalParams* mparams, + const sofa::type::vector& dataVecOutPos, + const sofa::type::vector& dataVecIn1Pos, + const sofa::type::vector& dataVecIn2Pos) { SOFA_UNUSED(mparams); @@ -222,6 +234,7 @@ void BeamProjectionDifferenceMultiMapping::apply( const sofa:: OutVecCoord& out = *dataVecOutPos[0]->beginEdit(); const auto& directions = sofa::helper::getReadAccessor(d_directions); auto interpolation = l_interpolation.get(); + auto projections = sofa::helper::getWriteAccessor(d_projections); if (m_mappedPoints.empty() || !directions[0]) { @@ -243,26 +256,22 @@ void BeamProjectionDifferenceMultiMapping::apply( const sofa:: MappedPoint& mp = m_mappedPoints[i]; if (mp.onBeam) { - Transform interpolatedTransform; - interpolation->InterpolateTransformUsingSpline(interpolatedTransform, + Transform R0_H_projection; // projection of the constraint on the beam + interpolation->InterpolateTransformUsingSpline(R0_H_projection, mp.alpha, Transform(in2[mp.pi1].getCenter(), in2[mp.pi1].getOrientation()), Transform(in2[mp.pi2].getCenter(), in2[mp.pi2].getOrientation()), interpolation->getLength(mp.edgeIndex)); - In1Coord projection; - projection.getCenter() = interpolatedTransform.getOrigin(); - projection.getOrientation() = interpolatedTransform.getOrientation(); - - In1Coord p; - for (size_t j=0; j