From 3937560ac782cd876db02871557dfbce260de1ce Mon Sep 17 00:00:00 2001 From: Hugo Talbot Date: Thu, 16 Dec 2021 18:43:45 +0100 Subject: [PATCH] Fix compilation with v21.12 --- .../constraint/AdaptiveBeamLengthConstraint.h | 324 +++---- .../AdaptiveBeamLengthConstraint.inl | 836 +++++++++--------- .../AdaptiveBeamSlidingConstraint.h | 288 +++--- .../ImplicitSurfaceAdaptiveConstraint.h | 1 + .../component/mapping/BeamLengthMapping.h | 2 +- .../component/mapping/BeamLengthMapping.inl | 2 +- 6 files changed, 727 insertions(+), 726 deletions(-) diff --git a/src/BeamAdapter/component/constraint/AdaptiveBeamLengthConstraint.h b/src/BeamAdapter/component/constraint/AdaptiveBeamLengthConstraint.h index 122762116..6d3925a36 100644 --- a/src/BeamAdapter/component/constraint/AdaptiveBeamLengthConstraint.h +++ b/src/BeamAdapter/component/constraint/AdaptiveBeamLengthConstraint.h @@ -18,165 +18,165 @@ * Authors: see Authors.md * * * * Contact information: contact@sofa-framework.org * -******************************************************************************/ -#ifndef SOFA_COMPONENT_CONSTRAINTSET_ADAPTIVEBEAMLENGTHCONSTRAINT_H -#define SOFA_COMPONENT_CONSTRAINTSET_ADAPTIVEBEAMLENGTHCONSTRAINT_H - -//////////////////////// Inclusion of headers...from wider to narrower/closer ////////////////////// -#include - -#include -#include -#include -#include - -#include - -//////////////////////////////////////////////////////////////////////////////////////////////////// -/// Forward declarations, see https://en.wikipedia.org/wiki/Forward_declaration -//////////////////////////////////////////////////////////////////////////////////////////////////// - - - -//////////////////////////////////////////////////////////////////////////////////////////////////// -/// Declarations -//////////////////////////////////////////////////////////////////////////////////////////////////// -namespace sofa -{ - -namespace component -{ - -namespace constraintset -{ - -/////////////////////////////////// private namespace pattern ////////////////////////////////////// -/// To avoid the lacking of names imported with with 'using' in the other's component namespace -/// you should use a private namespace and "export" only this one in the public namespace. -/// This is done at the end of this file, have a look if you are not used to this pattern. -//////////////////////////////////////////////////////////////////////////////////////////////////// -namespace _adaptivebeamlengthconstraint_ -{ -using sofa::core::behavior::ConstraintResolution ; -using sofa::core::behavior::Constraint ; -using sofa::core::behavior::MechanicalState ; -using sofa::core::ConstraintParams ; -using sofa::core::objectmodel::Data ; -using sofa::type::Vec ; -using sofa::type::vector; -using sofa::defaulttype::SolidTypes; -using sofa::defaulttype::BaseVector ; - - -template -class IntervalDefinition -{ -public: - typedef typename SolidTypes::Transform Transform; - typedef Vec<3, Real> Vec3; - - /// definition of an interval which length is "constrained" - /// positions begin / end of the interval - Vec3 posBegin, posEnd; - - /// positions free : begin / end of the interval - Vec3 posFreeBegin, posFreeEnd; - - /// index of the dofs: begin / end of the interval - unsigned int IdxBegin, IdxEnd; - - /// transform from dof to begin/end of the interval - Transform dof_H_begin, dof_H_end; - - /// rest length of the interval (if no stretching) - Real rest_length; - - /// is it in elongation - bool active; -} ; - -/*! - * \class AdaptiveBeamLengthConstraint - * - * More informations about SOFA components: - * https://www.sofa-framework.org/community/doc/programming-with-sofa/create-your-component/ - * https://www.sofa-framework.org/community/doc/programming-with-sofa/components-api/components-and-datas/ - */ -template -class AdaptiveBeamLengthConstraint : public Constraint -{ -public: - SOFA_CLASS(SOFA_TEMPLATE(AdaptiveBeamLengthConstraint,DataTypes), - SOFA_TEMPLATE(Constraint,DataTypes)); - - typedef typename DataTypes::VecCoord VecCoord; - typedef typename DataTypes::VecDeriv VecDeriv; - typedef typename DataTypes::MatrixDeriv MatrixDeriv; - typedef typename DataTypes::MatrixDeriv::RowIterator MatrixDerivRowIterator; - typedef typename DataTypes::Coord Coord; - typedef typename DataTypes::Deriv Deriv; - typedef typename Coord::value_type Real; - typedef typename SolidTypes::Transform Transform; - typedef typename SolidTypes::SpatialVector SpatialVector; - typedef typename DataTypes::Coord::Pos Pos; - typedef typename DataTypes::Coord::Rot Rot; - typedef typename std::map::iterator MapIterator; - - typedef MechanicalState TypedMechanicalState; - typedef Constraint Inherit; - typedef Data DataVecCoord; - typedef Data DataVecDeriv; - typedef Data DataMatrixDeriv; - typedef Vec<3, Real> Vec3; - typedef Vec<6, Real> Vec6; - -public: - virtual void init() override ; - virtual void reset() override ; - virtual void draw(const core::visual::VisualParams* vparams) override ; - - - virtual void buildConstraintMatrix(const ConstraintParams* cParams, - DataMatrixDeriv &c, unsigned int &cIndex, const DataVecCoord &x) override ; - - virtual void getConstraintViolation(const ConstraintParams* cParams, BaseVector *viol, - const DataVecCoord &x,const DataVecDeriv &v) override ; - - virtual void getConstraintResolution(const ConstraintParams* cParams, std::vector& resTab, unsigned int& offset) override ; - - -protected: - AdaptiveBeamLengthConstraint(TypedMechanicalState* object = nullptr) ; - virtual ~AdaptiveBeamLengthConstraint() ; - - void internalInit(); - - unsigned int m_cid {0} ; - int m_nbConstraints {0}; - vector m_violations; - std::map m_prevForces; /// Map abscissa <-> previous constraint force - - Data m_alarmLength ; - Data m_constrainedLength ; - Data m_maxBendingAngle ; - SingleLink, - fem::WireBeamInterpolation, - BaseLink::FLAG_STOREPATH|BaseLink::FLAG_STRONGLINK> m_interpolation; - -private: - void detectElongation(const VecCoord &x, const VecCoord& xfree); - vector> m_constraintIntervals; -}; - - -} /// namespace _adaptivebeamlengthconstraint_ - -using _adaptivebeamlengthconstraint_::AdaptiveBeamLengthConstraint ; - -} /// namespace constraintset - -} /// namespace component - -} /// namespace sofa - -#endif // SOFA_COMPONENT_CONSTRAINTSET_ADAPTIVEBEAMLENGTHCONSTRAINT_H +******************************************************************************/ +#ifndef SOFA_COMPONENT_CONSTRAINTSET_ADAPTIVEBEAMLENGTHCONSTRAINT_H +#define SOFA_COMPONENT_CONSTRAINTSET_ADAPTIVEBEAMLENGTHCONSTRAINT_H + +//////////////////////// Inclusion of headers...from wider to narrower/closer ////////////////////// +#include + +#include +#include +#include +#include + +#include + +//////////////////////////////////////////////////////////////////////////////////////////////////// +/// Forward declarations, see https://en.wikipedia.org/wiki/Forward_declaration +//////////////////////////////////////////////////////////////////////////////////////////////////// + + + +//////////////////////////////////////////////////////////////////////////////////////////////////// +/// Declarations +//////////////////////////////////////////////////////////////////////////////////////////////////// +namespace sofa +{ + +namespace component +{ + +namespace constraintset +{ + +/////////////////////////////////// private namespace pattern ////////////////////////////////////// +/// To avoid the lacking of names imported with with 'using' in the other's component namespace +/// you should use a private namespace and "export" only this one in the public namespace. +/// This is done at the end of this file, have a look if you are not used to this pattern. +//////////////////////////////////////////////////////////////////////////////////////////////////// +namespace _adaptivebeamlengthconstraint_ +{ +using sofa::core::behavior::ConstraintResolution ; +using sofa::core::behavior::Constraint ; +using sofa::core::behavior::MechanicalState ; +using sofa::core::ConstraintParams ; +using sofa::core::objectmodel::Data ; +using sofa::type::Vec ; +using sofa::type::vector; +using sofa::defaulttype::SolidTypes; +using sofa::linearalgebra::BaseVector ; + + +template +class IntervalDefinition +{ +public: + typedef typename SolidTypes::Transform Transform; + typedef Vec<3, Real> Vec3; + + /// definition of an interval which length is "constrained" + /// positions begin / end of the interval + Vec3 posBegin, posEnd; + + /// positions free : begin / end of the interval + Vec3 posFreeBegin, posFreeEnd; + + /// index of the dofs: begin / end of the interval + unsigned int IdxBegin, IdxEnd; + + /// transform from dof to begin/end of the interval + Transform dof_H_begin, dof_H_end; + + /// rest length of the interval (if no stretching) + Real rest_length; + + /// is it in elongation + bool active; +} ; + +/*! + * \class AdaptiveBeamLengthConstraint + * + * More informations about SOFA components: + * https://www.sofa-framework.org/community/doc/programming-with-sofa/create-your-component/ + * https://www.sofa-framework.org/community/doc/programming-with-sofa/components-api/components-and-datas/ + */ +template +class AdaptiveBeamLengthConstraint : public Constraint +{ +public: + SOFA_CLASS(SOFA_TEMPLATE(AdaptiveBeamLengthConstraint,DataTypes), + SOFA_TEMPLATE(Constraint,DataTypes)); + + typedef typename DataTypes::VecCoord VecCoord; + typedef typename DataTypes::VecDeriv VecDeriv; + typedef typename DataTypes::MatrixDeriv MatrixDeriv; + typedef typename DataTypes::MatrixDeriv::RowIterator MatrixDerivRowIterator; + typedef typename DataTypes::Coord Coord; + typedef typename DataTypes::Deriv Deriv; + typedef typename Coord::value_type Real; + typedef typename SolidTypes::Transform Transform; + typedef typename SolidTypes::SpatialVector SpatialVector; + typedef typename DataTypes::Coord::Pos Pos; + typedef typename DataTypes::Coord::Rot Rot; + typedef typename std::map::iterator MapIterator; + + typedef MechanicalState TypedMechanicalState; + typedef Constraint Inherit; + typedef Data DataVecCoord; + typedef Data DataVecDeriv; + typedef Data DataMatrixDeriv; + typedef Vec<3, Real> Vec3; + typedef Vec<6, Real> Vec6; + +public: + virtual void init() override ; + virtual void reset() override ; + virtual void draw(const core::visual::VisualParams* vparams) override ; + + + virtual void buildConstraintMatrix(const ConstraintParams* cParams, + DataMatrixDeriv &c, unsigned int &cIndex, const DataVecCoord &x) override ; + + virtual void getConstraintViolation(const ConstraintParams* cParams, BaseVector *viol, + const DataVecCoord &x,const DataVecDeriv &v) override ; + + virtual void getConstraintResolution(const ConstraintParams* cParams, std::vector& resTab, unsigned int& offset) override ; + + +protected: + AdaptiveBeamLengthConstraint(TypedMechanicalState* object = nullptr) ; + virtual ~AdaptiveBeamLengthConstraint() ; + + void internalInit(); + + unsigned int m_cid {0} ; + int m_nbConstraints {0}; + vector m_violations; + std::map m_prevForces; /// Map abscissa <-> previous constraint force + + Data m_alarmLength ; + Data m_constrainedLength ; + Data m_maxBendingAngle ; + SingleLink, + fem::WireBeamInterpolation, + BaseLink::FLAG_STOREPATH|BaseLink::FLAG_STRONGLINK> m_interpolation; + +private: + void detectElongation(const VecCoord &x, const VecCoord& xfree); + vector> m_constraintIntervals; +}; + + +} /// namespace _adaptivebeamlengthconstraint_ + +using _adaptivebeamlengthconstraint_::AdaptiveBeamLengthConstraint ; + +} /// namespace constraintset + +} /// namespace component + +} /// namespace sofa + +#endif // SOFA_COMPONENT_CONSTRAINTSET_ADAPTIVEBEAMLENGTHCONSTRAINT_H diff --git a/src/BeamAdapter/component/constraint/AdaptiveBeamLengthConstraint.inl b/src/BeamAdapter/component/constraint/AdaptiveBeamLengthConstraint.inl index 68e6cd685..421a024eb 100644 --- a/src/BeamAdapter/component/constraint/AdaptiveBeamLengthConstraint.inl +++ b/src/BeamAdapter/component/constraint/AdaptiveBeamLengthConstraint.inl @@ -18,421 +18,421 @@ * Authors: see Authors.md * * * * Contact information: contact@sofa-framework.org * -******************************************************************************/ -#ifndef SOFA_COMPONENT_CONSTRAINTSET_ADAPTIVEBEAMLENGTHCONSTRAINT_INL -#define SOFA_COMPONENT_CONSTRAINTSET_ADAPTIVEBEAMLENGTHCONSTRAINT_INL - -//////////////////////// Inclusion of headers...from wider to narrower/closer ////////////////////// -#include -#include -#include -#include -#include -#include - -namespace sofa -{ - -namespace component -{ - -namespace constraintset -{ - -namespace _adaptivebeamlengthconstraint_ -{ - -using helper::ReadAccessor; -using sofa::core::ConstVecCoordId; -using std::stringstream; -using core::ConstraintParams; -using defaulttype::BaseVector; -using core::visual::VisualParams; - -class AdaptiveBeamLengthConstraintResolution : public ConstraintResolution -{ -public: - AdaptiveBeamLengthConstraintResolution(double* initF=NULL, bool* active=NULL) : ConstraintResolution(1) ,m_initF(initF), m_active(active) - { - } - virtual void init(int line, double** w, double* force); - virtual void resolution(int line, double** w, double* d, double* force); - virtual void store(int line, double* force, bool convergence); - -protected: - double* m_initF; - bool* m_active; -}; - - -template -AdaptiveBeamLengthConstraint::AdaptiveBeamLengthConstraint(TypedMechanicalState* object) - : Inherit(object) - , m_alarmLength(initData(&m_alarmLength, (Real)1.02, "alarmLength", "Elongation before creating a constraint (default=1.02)")) - , m_constrainedLength(initData(&m_constrainedLength, (Real)1.05, "constrainedLength", "Allowed elongation of a beam (default=1.05")) - , m_maxBendingAngle(initData(&m_maxBendingAngle, (Real)0.1, "maxBendingAngle", "max bending criterion (in rad) for one constraint interval (default=0.1)")) - , m_interpolation(initLink("interpolation", "link to the interpolation component in the scene")) -{ -} - -template -AdaptiveBeamLengthConstraint::~AdaptiveBeamLengthConstraint() -{ -} - -template -void AdaptiveBeamLengthConstraint::init() -{ - this->mstate= dynamic_cast< MechanicalState *> (this->getContext()->getMechanicalState()); - assert(this->mstate); -} - -template -void AdaptiveBeamLengthConstraint::reset() -{ - internalInit(); -} - -template -void AdaptiveBeamLengthConstraint::internalInit() -{ - /// We search for the closest segment, on which to project each point - /// Convention : object1 is the beam model, object2 is the list of point constraints - if(!m_interpolation.get()) - return; -} - -template -void AdaptiveBeamLengthConstraint::detectElongation(const VecCoord& x, const VecCoord& xfree) -{ - Vec3 P0,P1,P2,P3; - Real length, rest_length, rest_length_interval; - Real alarmLength = m_alarmLength.getValue(); - bool prev_stretch = false; - - fem::WireBeamInterpolation* interpolation = m_interpolation.get(); - - /// storage of the length (and the rest_length) of the interval being stretched - /// length_interval=0.0; //commented to remove compilation warning - rest_length_interval=0.0; - - /// storage of the interval information - IntervalDefinition intervalDef; - Real angleInterval=0.0; - - for (unsigned int b=0; bgetNumBeams(); b++) - { - /// 1. compute the actual length and the rest length of the beams - interpolation->getSplinePoints(b,x,P0,P1,P2,P3); - - //TODO(dmarchal 2017) Please specify who/when this will be done - /// TODO : optimization: finally it is not necessary to compute all the spline points - length=(P0-P3).norm(); - rest_length = interpolation->getLength(b); - - /// 2. compute the bending angle - Transform Tnode0, Tnode1; - interpolation->computeTransform2(b,Tnode0,Tnode1,x); - Real angleBeam = interpolation->ComputeTotalBendingRotationAngle(rest_length/10.0, Tnode0, Tnode1,rest_length , 0.0, 1.0); - - /// 3. treatment of the different case.. - unsigned n0, n1; - interpolation->getNodeIndices(b, n0, n1); - bool case1a = (n0==n1); - - if(prev_stretch) - { - ////CASE 1: previous beam was stretched: - /// find the case that necessitates to stop the interval: - /// (a) rigidification - /// (b) current beam not stretched - /// (c) too large bending angle... - /// (d) last beam ! [ see after the loop "for" ] - //// => store the information : index [begin end], Pos [begin end] - - bool case1b = (rest_length*alarmLength > length); - bool case1c = (angleBeam >= 0.99*m_maxBendingAngle.getValue()); - case1c=false; - case1b=false; - - /// CASE 1 (a) + (b) + (c) - if (case1a || case1b || case1c) - { - - dmsg_info() <<" beam "< ?"< ?" <getDOFtoLocalTransform(b, DOF0_H_local0, DOF1_H_local1); - - intervalDef.dof_H_end = DOF0_H_local0; /// store the transform from dof to pos - - interpolation->getSplinePoints(b,xfree,P0,P1,P2,P3); - - Transform global_H_local0_free, global_H_local1_free; - interpolation->computeTransform2(b, global_H_local0_free, global_H_local1_free, xfree); - intervalDef.posFreeEnd = global_H_local0_free.getOrigin(); /// store the free position - - intervalDef.rest_length=rest_length_interval; /// store the rest_length - - dmsg_info() << " rest_length_interval ="< 1 beam has a bendingAngle > m_maxBendingAngle" ; - else - m_constraintIntervals.push_back(intervalDef); - - /// isolate the case 1 (c) - if (!case1a && !case1b && case1c) // - { - dmsg_info() <<" isolate case 1(c)" ; - - /// create a new interval: - prev_stretch=true; - angleInterval = angleBeam; - rest_length_interval = rest_length; - - /// store the information of the beginning of the new interval - /// -> it corresponds to the end of the previous interval - intervalDef.dof_H_begin = DOF0_H_local0; - intervalDef.IdxBegin = n0; - intervalDef.posBegin = intervalDef.posEnd; - intervalDef.posFreeBegin = intervalDef.posFreeEnd; - } - } - else - { - /// Continue the rigidification - angleInterval+=angleBeam; - rest_length_interval += rest_length; - } - } - else - { - //// CASE 2: previous beam was not stretched: - /// (a) the current beam is stretched: start the interval => store index_begin Pos_begin - /// veriy the bending angle (in case the interval= the beam) - /// (b) the current beam is not stretched=> nothing to do ! - bool case2a = (rest_length*alarmLength < length); - case2a=true; - - if ( case2a && !case1a ) /// CASE 2 (a) - { - dmsg_info() << " beam "< it corresponds to the position of node 0 of the beam - Transform DOF0_H_local0, DOF1_H_local1; - interpolation->getDOFtoLocalTransform(b, DOF0_H_local0, DOF1_H_local1); - - Transform global_H_local0, global_H_local1; - interpolation->computeTransform2(b, global_H_local0, global_H_local1, x); - - Transform global_H_local0_free, global_H_local1_free; - interpolation->computeTransform2(b, global_H_local0_free, global_H_local1_free, xfree); - - intervalDef.dof_H_begin = DOF0_H_local0; - intervalDef.IdxBegin = n0; - intervalDef.posBegin = global_H_local0.getOrigin(); - intervalDef.posFreeBegin = global_H_local0_free.getOrigin(); - } - else - { - dmsg_info() <<" beam "< ?"< it corresponds to the position of node 0 of the beam - Transform DOF0_H_local0, DOF1_H_local1; - interpolation->getDOFtoLocalTransform(b, DOF0_H_local0, DOF1_H_local1); - - Transform global_H_local0, global_H_local1; - - interpolation->computeTransform2(b, global_H_local0, global_H_local1, x); - - - Transform global_H_local0_free, global_H_local1_free; - interpolation->computeTransform2(b, global_H_local0_free, global_H_local1_free, xfree); - - - intervalDef.dof_H_end = DOF1_H_local1; - intervalDef.IdxEnd = n1; - intervalDef.posEnd = global_H_local1.getOrigin(); - intervalDef.posFreeEnd = global_H_local1_free.getOrigin(); - intervalDef.rest_length=rest_length_interval; /// store the rest_length - - dmsg_info() <<" rest_length_interval ="< -void AdaptiveBeamLengthConstraint::buildConstraintMatrix(const ConstraintParams* cParams, DataMatrixDeriv &c_d, - unsigned int &constraintId, const DataVecCoord & x_d) -{ - SOFA_UNUSED(cParams) ; - SOFA_UNUSED(x_d) ; - - m_violations.clear(); - Real constrainedLength = m_constrainedLength.getValue(); - - m_nbConstraints = 0; - m_cid = constraintId; - - ReadAccessor > x = this->mstate->read(ConstVecCoordId::position()) ; - ReadAccessor > xfree = this->mstate->read(ConstVecCoordId::freePosition()) ; - - MatrixDeriv& c = *c_d.beginEdit(); - - m_constraintIntervals.clear(); - detectElongation( x.ref(), xfree.ref()); - - if( this->f_printLog.getValue()) - { - stringstream tmp; - for (unsigned int i=0; i 0.0001) << " lever0 ="< -void AdaptiveBeamLengthConstraint::getConstraintViolation(const ConstraintParams*, BaseVector *v, - const DataVecCoord &, const DataVecDeriv &) -{ - unsigned int nb = m_violations.size(); - for(unsigned int i=0; iset(m_cid+i, m_violations[i]); - } -} - -template -void AdaptiveBeamLengthConstraint::getConstraintResolution(const ConstraintParams*,std::vector& resTab, - unsigned int& offset) -{ - unsigned int nb = m_violations.size(); - for(unsigned int i=0; i -void AdaptiveBeamLengthConstraint::draw(const VisualParams* vparams) -{ -#ifndef SOFA_NO_OPENGL - if (!vparams->displayFlags().getShowInteractionForceFields()) - return; - - if(m_constraintIntervals.size()==0) - return; - - ///trace a point at the beginning and at the end of each constraint interval - glDisable(GL_LIGHTING); - glPointSize(10); - glBegin(GL_POINTS); - glColor4f(0.0f,1.0f,0.0f,1.0f); - for(unsigned int i=0; i +#include +#include +#include +#include +#include + +namespace sofa +{ + +namespace component +{ + +namespace constraintset +{ + +namespace _adaptivebeamlengthconstraint_ +{ + +using helper::ReadAccessor; +using sofa::core::ConstVecCoordId; +using std::stringstream; +using core::ConstraintParams; +using linearalgebra::BaseVector; +using core::visual::VisualParams; + +class AdaptiveBeamLengthConstraintResolution : public ConstraintResolution +{ +public: + AdaptiveBeamLengthConstraintResolution(double* initF=NULL, bool* active=NULL) : ConstraintResolution(1) ,m_initF(initF), m_active(active) + { + } + virtual void init(int line, double** w, double* force); + virtual void resolution(int line, double** w, double* d, double* force); + virtual void store(int line, double* force, bool convergence); + +protected: + double* m_initF; + bool* m_active; +}; + + +template +AdaptiveBeamLengthConstraint::AdaptiveBeamLengthConstraint(TypedMechanicalState* object) + : Inherit(object) + , m_alarmLength(initData(&m_alarmLength, (Real)1.02, "alarmLength", "Elongation before creating a constraint (default=1.02)")) + , m_constrainedLength(initData(&m_constrainedLength, (Real)1.05, "constrainedLength", "Allowed elongation of a beam (default=1.05")) + , m_maxBendingAngle(initData(&m_maxBendingAngle, (Real)0.1, "maxBendingAngle", "max bending criterion (in rad) for one constraint interval (default=0.1)")) + , m_interpolation(initLink("interpolation", "link to the interpolation component in the scene")) +{ +} + +template +AdaptiveBeamLengthConstraint::~AdaptiveBeamLengthConstraint() +{ +} + +template +void AdaptiveBeamLengthConstraint::init() +{ + this->mstate= dynamic_cast< MechanicalState *> (this->getContext()->getMechanicalState()); + assert(this->mstate); +} + +template +void AdaptiveBeamLengthConstraint::reset() +{ + internalInit(); +} + +template +void AdaptiveBeamLengthConstraint::internalInit() +{ + /// We search for the closest segment, on which to project each point + /// Convention : object1 is the beam model, object2 is the list of point constraints + if(!m_interpolation.get()) + return; +} + +template +void AdaptiveBeamLengthConstraint::detectElongation(const VecCoord& x, const VecCoord& xfree) +{ + Vec3 P0,P1,P2,P3; + Real length, rest_length, rest_length_interval; + Real alarmLength = m_alarmLength.getValue(); + bool prev_stretch = false; + + fem::WireBeamInterpolation* interpolation = m_interpolation.get(); + + /// storage of the length (and the rest_length) of the interval being stretched + /// length_interval=0.0; //commented to remove compilation warning + rest_length_interval=0.0; + + /// storage of the interval information + IntervalDefinition intervalDef; + Real angleInterval=0.0; + + for (unsigned int b=0; bgetNumBeams(); b++) + { + /// 1. compute the actual length and the rest length of the beams + interpolation->getSplinePoints(b,x,P0,P1,P2,P3); + + //TODO(dmarchal 2017) Please specify who/when this will be done + /// TODO : optimization: finally it is not necessary to compute all the spline points + length=(P0-P3).norm(); + rest_length = interpolation->getLength(b); + + /// 2. compute the bending angle + Transform Tnode0, Tnode1; + interpolation->computeTransform2(b,Tnode0,Tnode1,x); + Real angleBeam = interpolation->ComputeTotalBendingRotationAngle(rest_length/10.0, Tnode0, Tnode1,rest_length , 0.0, 1.0); + + /// 3. treatment of the different case.. + unsigned n0, n1; + interpolation->getNodeIndices(b, n0, n1); + bool case1a = (n0==n1); + + if(prev_stretch) + { + ////CASE 1: previous beam was stretched: + /// find the case that necessitates to stop the interval: + /// (a) rigidification + /// (b) current beam not stretched + /// (c) too large bending angle... + /// (d) last beam ! [ see after the loop "for" ] + //// => store the information : index [begin end], Pos [begin end] + + bool case1b = (rest_length*alarmLength > length); + bool case1c = (angleBeam >= 0.99*m_maxBendingAngle.getValue()); + case1c=false; + case1b=false; + + /// CASE 1 (a) + (b) + (c) + if (case1a || case1b || case1c) + { + + dmsg_info() <<" beam "< ?"< ?" <getDOFtoLocalTransform(b, DOF0_H_local0, DOF1_H_local1); + + intervalDef.dof_H_end = DOF0_H_local0; /// store the transform from dof to pos + + interpolation->getSplinePoints(b,xfree,P0,P1,P2,P3); + + Transform global_H_local0_free, global_H_local1_free; + interpolation->computeTransform2(b, global_H_local0_free, global_H_local1_free, xfree); + intervalDef.posFreeEnd = global_H_local0_free.getOrigin(); /// store the free position + + intervalDef.rest_length=rest_length_interval; /// store the rest_length + + dmsg_info() << " rest_length_interval ="< 1 beam has a bendingAngle > m_maxBendingAngle" ; + else + m_constraintIntervals.push_back(intervalDef); + + /// isolate the case 1 (c) + if (!case1a && !case1b && case1c) // + { + dmsg_info() <<" isolate case 1(c)" ; + + /// create a new interval: + prev_stretch=true; + angleInterval = angleBeam; + rest_length_interval = rest_length; + + /// store the information of the beginning of the new interval + /// -> it corresponds to the end of the previous interval + intervalDef.dof_H_begin = DOF0_H_local0; + intervalDef.IdxBegin = n0; + intervalDef.posBegin = intervalDef.posEnd; + intervalDef.posFreeBegin = intervalDef.posFreeEnd; + } + } + else + { + /// Continue the rigidification + angleInterval+=angleBeam; + rest_length_interval += rest_length; + } + } + else + { + //// CASE 2: previous beam was not stretched: + /// (a) the current beam is stretched: start the interval => store index_begin Pos_begin + /// veriy the bending angle (in case the interval= the beam) + /// (b) the current beam is not stretched=> nothing to do ! + bool case2a = (rest_length*alarmLength < length); + case2a=true; + + if ( case2a && !case1a ) /// CASE 2 (a) + { + dmsg_info() << " beam "< it corresponds to the position of node 0 of the beam + Transform DOF0_H_local0, DOF1_H_local1; + interpolation->getDOFtoLocalTransform(b, DOF0_H_local0, DOF1_H_local1); + + Transform global_H_local0, global_H_local1; + interpolation->computeTransform2(b, global_H_local0, global_H_local1, x); + + Transform global_H_local0_free, global_H_local1_free; + interpolation->computeTransform2(b, global_H_local0_free, global_H_local1_free, xfree); + + intervalDef.dof_H_begin = DOF0_H_local0; + intervalDef.IdxBegin = n0; + intervalDef.posBegin = global_H_local0.getOrigin(); + intervalDef.posFreeBegin = global_H_local0_free.getOrigin(); + } + else + { + dmsg_info() <<" beam "< ?"< it corresponds to the position of node 0 of the beam + Transform DOF0_H_local0, DOF1_H_local1; + interpolation->getDOFtoLocalTransform(b, DOF0_H_local0, DOF1_H_local1); + + Transform global_H_local0, global_H_local1; + + interpolation->computeTransform2(b, global_H_local0, global_H_local1, x); + + + Transform global_H_local0_free, global_H_local1_free; + interpolation->computeTransform2(b, global_H_local0_free, global_H_local1_free, xfree); + + + intervalDef.dof_H_end = DOF1_H_local1; + intervalDef.IdxEnd = n1; + intervalDef.posEnd = global_H_local1.getOrigin(); + intervalDef.posFreeEnd = global_H_local1_free.getOrigin(); + intervalDef.rest_length=rest_length_interval; /// store the rest_length + + dmsg_info() <<" rest_length_interval ="< +void AdaptiveBeamLengthConstraint::buildConstraintMatrix(const ConstraintParams* cParams, DataMatrixDeriv &c_d, + unsigned int &constraintId, const DataVecCoord & x_d) +{ + SOFA_UNUSED(cParams) ; + SOFA_UNUSED(x_d) ; + + m_violations.clear(); + Real constrainedLength = m_constrainedLength.getValue(); + + m_nbConstraints = 0; + m_cid = constraintId; + + ReadAccessor > x = this->mstate->read(ConstVecCoordId::position()) ; + ReadAccessor > xfree = this->mstate->read(ConstVecCoordId::freePosition()) ; + + MatrixDeriv& c = *c_d.beginEdit(); + + m_constraintIntervals.clear(); + detectElongation( x.ref(), xfree.ref()); + + if( this->f_printLog.getValue()) + { + stringstream tmp; + for (unsigned int i=0; i 0.0001) << " lever0 ="< +void AdaptiveBeamLengthConstraint::getConstraintViolation(const ConstraintParams*, BaseVector *v, + const DataVecCoord &, const DataVecDeriv &) +{ + unsigned int nb = m_violations.size(); + for(unsigned int i=0; iset(m_cid+i, m_violations[i]); + } +} + +template +void AdaptiveBeamLengthConstraint::getConstraintResolution(const ConstraintParams*,std::vector& resTab, + unsigned int& offset) +{ + unsigned int nb = m_violations.size(); + for(unsigned int i=0; i +void AdaptiveBeamLengthConstraint::draw(const VisualParams* vparams) +{ +#ifndef SOFA_NO_OPENGL + if (!vparams->displayFlags().getShowInteractionForceFields()) + return; + + if(m_constraintIntervals.size()==0) + return; + + ///trace a point at the beginning and at the end of each constraint interval + glDisable(GL_LIGHTING); + glPointSize(10); + glBegin(GL_POINTS); + glColor4f(0.0f,1.0f,0.0f,1.0f); + for(unsigned int i=0; i -#include - - -namespace sofa -{ - -namespace component -{ - -namespace constraintset -{ - -/////////////////////////////////// private namespace pattern ////////////////////////////////////// -/// To avoid the lacking of names imported with with 'using' in the other's component namespace -/// you should use a private namespace and "export" only this one in the public namespace. -/// This is done at the end of this file, have a look if you are not used to this pattern. -//////////////////////////////////////////////////////////////////////////////////////////////////// -namespace _adaptiveBeamSlidingConstraint_ -{ - -using sofa::core::behavior::PairInteractionConstraint ; -using sofa::core::behavior::ConstraintResolution ; -using sofa::core::visual::VisualParams ; -using sofa::core::ConstraintParams ; -using sofa::defaulttype::SolidTypes ; -using sofa::defaulttype::BaseVector ; -using sofa::core::objectmodel::Data ; -using sofa::type::Vec ; -using sofa::component::fem::WireBeamInterpolation ; -using std::vector; - -//////////////////////////////////////////////////////////////////////////////////////////////////// -/// Declarations -//////////////////////////////////////////////////////////////////////////////////////////////////// -/*! - * \class AdaptiveBeamSlidingConstraint - * @brief AdaptiveBeamSlidingConstraint Class constrain a rigid to be attached to a beam (only in position, not the orientation) - * - * More informations about SOFA components: - * https://www.sofa-framework.org/community/doc/programming-with-sofa/create-your-component/ - * https://www.sofa-framework.org/community/doc/programming-with-sofa/components-api/components-and-datas/ - */ -template -class SOFA_BEAMADAPTER_API AdaptiveBeamSlidingConstraint : public PairInteractionConstraint -{ -public: - SOFA_CLASS(SOFA_TEMPLATE(AdaptiveBeamSlidingConstraint, DataTypes), - SOFA_TEMPLATE(PairInteractionConstraint, DataTypes)); - - typedef typename DataTypes::VecCoord VecCoord; - typedef typename DataTypes::VecDeriv VecDeriv; - typedef typename DataTypes::MatrixDeriv MatrixDeriv; - typedef typename DataTypes::MatrixDeriv::RowIterator MatrixDerivRowIterator; - typedef typename DataTypes::Coord Coord; - typedef typename DataTypes::Deriv Deriv; - typedef typename Coord::value_type Real; - typedef typename core::behavior::MechanicalState TypedMechanicalState; - typedef typename core::behavior::PairInteractionConstraint Inherit; - typedef Data DataVecCoord; - typedef Data DataVecDeriv; - typedef Data DataMatrixDeriv; - typedef typename SolidTypes::Transform Transform; - typedef typename SolidTypes::SpatialVector SpatialVector; - typedef typename DataTypes::Coord::Pos Pos; - typedef typename DataTypes::Coord::Rot Rot; - typedef Vec<3, Real> Vec3; - typedef Vec<6, Real> Vec6; - - /////////////////////////// Inherited from BaseObject ////////////////////////////////////////// - virtual void init() override ; - virtual void reset() override ; - virtual void draw(const VisualParams* vparams) override ; - //////////////////////////////////////////////////////////////////////////////////////////////// - - - /////////////////////////// Inherited from BaseConstraint ////////////////////////////////////// - virtual void buildConstraintMatrix(const ConstraintParams* cParams, - DataMatrixDeriv &c1, DataMatrixDeriv &c2, - unsigned int &cIndex, - const DataVecCoord &x1, const DataVecCoord &x2) override ; - - virtual void getConstraintViolation(const ConstraintParams* cParams , - BaseVector *v, - const DataVecCoord &x1, const DataVecCoord &x2, - const DataVecDeriv &v1, const DataVecDeriv &v2) override; - - virtual void getConstraintResolution(const ConstraintParams* cParams, vector& resTab, unsigned int& offset) override ; - //////////////////////////////////////////////////////////////////////////////////////////////// - -protected: - AdaptiveBeamSlidingConstraint(TypedMechanicalState* object1, TypedMechanicalState* object2) ; - AdaptiveBeamSlidingConstraint(TypedMechanicalState* object) ; - AdaptiveBeamSlidingConstraint() ; - virtual ~AdaptiveBeamSlidingConstraint(){} - -private: - void internalInit(); - - unsigned int m_cid; - int m_nbConstraints; /*!< number of constraints created */ - vector m_violations; - vector m_previousPositions; /*!< the position on which each point was projected */ - vector m_displacements; /*!< displacement=double for compatibility with constraint resolution */ - vector m_projected; - - /*! link to the interpolation component in the scene */ - SingleLink, - WireBeamInterpolation, - BaseLink::FLAG_STOREPATH|BaseLink::FLAG_STRONGLINK> m_interpolation; - - - ////////////////////////// Inherited attributes //////////////////////////// - /// https://gcc.gnu.org/onlinedocs/gcc/Name-lookup.html - /// Bring inherited attributes and function in the current lookup context. - /// otherwise any access to the base::attribute would require - /// the "this->" approach. - using PairInteractionConstraint::mstate1; - using PairInteractionConstraint::mstate2; - //////////////////////////////////////////////////////////////////////////// -}; - -#if !defined(SOFA_COMPONENT_CONSTRAINTSET_ADAPTIVEBEAMSLIDINGCONSTRAINT_CPP) -extern template class SOFA_BEAMADAPTER_API AdaptiveBeamSlidingConstraint; -#endif - -} // namespace _adaptiveBeamSlidingConstraint_ - -/// 'Export' the objects defined in the private namespace into the 'public' one so that -/// we can use them instead as sofa::component::constraintset::AdaptiveBeamSlidingConstraint. -using _adaptiveBeamSlidingConstraint_::AdaptiveBeamSlidingConstraint ; - -} // namespace constraintset - -} // namespace component - -} // namespace sofa - -#endif // SOFA_COMPONENT_CONSTRAINTSET_AdaptiveBeamSlidingConstraint_H +******************************************************************************/ +#ifndef SOFA_COMPONENT_CONSTRAINTSET_ADAPTIVEBEAMSLIDINGCONSTRAINT_H +#define SOFA_COMPONENT_CONSTRAINTSET_ADAPTIVEBEAMSLIDINGCONSTRAINT_H + +//////////////////////// Inclusion of headers...from wider to narrower/closer ////////////////////// +#include +#include + + +namespace sofa +{ + +namespace component +{ + +namespace constraintset +{ + +/////////////////////////////////// private namespace pattern ////////////////////////////////////// +/// To avoid the lacking of names imported with with 'using' in the other's component namespace +/// you should use a private namespace and "export" only this one in the public namespace. +/// This is done at the end of this file, have a look if you are not used to this pattern. +//////////////////////////////////////////////////////////////////////////////////////////////////// +namespace _adaptiveBeamSlidingConstraint_ +{ + +using sofa::core::behavior::PairInteractionConstraint ; +using sofa::core::behavior::ConstraintResolution ; +using sofa::core::visual::VisualParams ; +using sofa::core::ConstraintParams ; +using sofa::defaulttype::SolidTypes ; +using sofa::linearalgebra::BaseVector ; +using sofa::core::objectmodel::Data ; +using sofa::type::Vec ; +using sofa::component::fem::WireBeamInterpolation ; +using std::vector; + +//////////////////////////////////////////////////////////////////////////////////////////////////// +/// Declarations +//////////////////////////////////////////////////////////////////////////////////////////////////// +/*! + * \class AdaptiveBeamSlidingConstraint + * @brief AdaptiveBeamSlidingConstraint Class constrain a rigid to be attached to a beam (only in position, not the orientation) + * + * More informations about SOFA components: + * https://www.sofa-framework.org/community/doc/programming-with-sofa/create-your-component/ + * https://www.sofa-framework.org/community/doc/programming-with-sofa/components-api/components-and-datas/ + */ +template +class SOFA_BEAMADAPTER_API AdaptiveBeamSlidingConstraint : public PairInteractionConstraint +{ +public: + SOFA_CLASS(SOFA_TEMPLATE(AdaptiveBeamSlidingConstraint, DataTypes), + SOFA_TEMPLATE(PairInteractionConstraint, DataTypes)); + + typedef typename DataTypes::VecCoord VecCoord; + typedef typename DataTypes::VecDeriv VecDeriv; + typedef typename DataTypes::MatrixDeriv MatrixDeriv; + typedef typename DataTypes::MatrixDeriv::RowIterator MatrixDerivRowIterator; + typedef typename DataTypes::Coord Coord; + typedef typename DataTypes::Deriv Deriv; + typedef typename Coord::value_type Real; + typedef typename core::behavior::MechanicalState TypedMechanicalState; + typedef typename core::behavior::PairInteractionConstraint Inherit; + typedef Data DataVecCoord; + typedef Data DataVecDeriv; + typedef Data DataMatrixDeriv; + typedef typename SolidTypes::Transform Transform; + typedef typename SolidTypes::SpatialVector SpatialVector; + typedef typename DataTypes::Coord::Pos Pos; + typedef typename DataTypes::Coord::Rot Rot; + typedef Vec<3, Real> Vec3; + typedef Vec<6, Real> Vec6; + + /////////////////////////// Inherited from BaseObject ////////////////////////////////////////// + virtual void init() override ; + virtual void reset() override ; + virtual void draw(const VisualParams* vparams) override ; + //////////////////////////////////////////////////////////////////////////////////////////////// + + + /////////////////////////// Inherited from BaseConstraint ////////////////////////////////////// + virtual void buildConstraintMatrix(const ConstraintParams* cParams, + DataMatrixDeriv &c1, DataMatrixDeriv &c2, + unsigned int &cIndex, + const DataVecCoord &x1, const DataVecCoord &x2) override ; + + virtual void getConstraintViolation(const ConstraintParams* cParams , + BaseVector *v, + const DataVecCoord &x1, const DataVecCoord &x2, + const DataVecDeriv &v1, const DataVecDeriv &v2) override; + + virtual void getConstraintResolution(const ConstraintParams* cParams, vector& resTab, unsigned int& offset) override ; + //////////////////////////////////////////////////////////////////////////////////////////////// + +protected: + AdaptiveBeamSlidingConstraint(TypedMechanicalState* object1, TypedMechanicalState* object2) ; + AdaptiveBeamSlidingConstraint(TypedMechanicalState* object) ; + AdaptiveBeamSlidingConstraint() ; + virtual ~AdaptiveBeamSlidingConstraint(){} + +private: + void internalInit(); + + unsigned int m_cid; + int m_nbConstraints; /*!< number of constraints created */ + vector m_violations; + vector m_previousPositions; /*!< the position on which each point was projected */ + vector m_displacements; /*!< displacement=double for compatibility with constraint resolution */ + vector m_projected; + + /*! link to the interpolation component in the scene */ + SingleLink, + WireBeamInterpolation, + BaseLink::FLAG_STOREPATH|BaseLink::FLAG_STRONGLINK> m_interpolation; + + + ////////////////////////// Inherited attributes //////////////////////////// + /// https://gcc.gnu.org/onlinedocs/gcc/Name-lookup.html + /// Bring inherited attributes and function in the current lookup context. + /// otherwise any access to the base::attribute would require + /// the "this->" approach. + using PairInteractionConstraint::mstate1; + using PairInteractionConstraint::mstate2; + //////////////////////////////////////////////////////////////////////////// +}; + +#if !defined(SOFA_COMPONENT_CONSTRAINTSET_ADAPTIVEBEAMSLIDINGCONSTRAINT_CPP) +extern template class SOFA_BEAMADAPTER_API AdaptiveBeamSlidingConstraint; +#endif + +} // namespace _adaptiveBeamSlidingConstraint_ + +/// 'Export' the objects defined in the private namespace into the 'public' one so that +/// we can use them instead as sofa::component::constraintset::AdaptiveBeamSlidingConstraint. +using _adaptiveBeamSlidingConstraint_::AdaptiveBeamSlidingConstraint ; + +} // namespace constraintset + +} // namespace component + +} // namespace sofa + +#endif // SOFA_COMPONENT_CONSTRAINTSET_AdaptiveBeamSlidingConstraint_H diff --git a/src/BeamAdapter/component/constraint/ImplicitSurfaceAdaptiveConstraint.h b/src/BeamAdapter/component/constraint/ImplicitSurfaceAdaptiveConstraint.h index c9e515d72..62ee09255 100644 --- a/src/BeamAdapter/component/constraint/ImplicitSurfaceAdaptiveConstraint.h +++ b/src/BeamAdapter/component/constraint/ImplicitSurfaceAdaptiveConstraint.h @@ -72,6 +72,7 @@ using sofa::core::behavior::PairInteractionConstraint; using core::behavior::BaseMechanicalState; using core::visual::VisualParams; using sofa::core::ConstraintParams; +using sofa::linearalgebra::BaseVector ; /*! diff --git a/src/BeamAdapter/component/mapping/BeamLengthMapping.h b/src/BeamAdapter/component/mapping/BeamLengthMapping.h index b8c14a606..0dea3e326 100644 --- a/src/BeamAdapter/component/mapping/BeamLengthMapping.h +++ b/src/BeamAdapter/component/mapping/BeamLengthMapping.h @@ -191,7 +191,7 @@ class BeamLengthMapping : public Mapping // interface of baseMapping.h virtual void updateK( const MechanicalParams* /*mparams*/, core::ConstMultiVecDerivId /*outForce*/ ) override; - const defaulttype::BaseMatrix* getK() override; + const linearalgebra::BaseMatrix* getK() override; diff --git a/src/BeamAdapter/component/mapping/BeamLengthMapping.inl b/src/BeamAdapter/component/mapping/BeamLengthMapping.inl index cdf4ee542..841bcbd32 100644 --- a/src/BeamAdapter/component/mapping/BeamLengthMapping.inl +++ b/src/BeamAdapter/component/mapping/BeamLengthMapping.inl @@ -797,7 +797,7 @@ void BeamLengthMapping::updateK(const core::MechanicalParams* mparams template -const defaulttype::BaseMatrix* BeamLengthMapping::getK() +const sofa::linearalgebra::BaseMatrix* BeamLengthMapping::getK() { return &K_geom; }