diff --git a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/LinearVelocityProjectiveConstraint.h b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/LinearVelocityProjectiveConstraint.h index 74c67b83997..8227ce39d06 100644 --- a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/LinearVelocityProjectiveConstraint.h +++ b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/LinearVelocityProjectiveConstraint.h @@ -71,6 +71,9 @@ public : /// the coordinates on which to applay velocities SetIndex d_coordinates; + /// If set to true then the last velocity will still be applied after all the key events + Data d_continueAfterEnd; + /// the key times surrounding the current simulation time (for interpolation) Real prevT, nextT; ///the velocities corresponding to the surrouding key times @@ -109,6 +112,13 @@ public : void projectPosition(const core::MechanicalParams* mparams, DataVecCoord& xData) override; void projectJacobianMatrix(const core::MechanicalParams* mparams, DataMatrixDeriv& cData) override; + void applyConstraint(const core::MechanicalParams* mparams, linearalgebra::BaseVector* vector, const sofa::core::behavior::MultiMatrixAccessor* matrix) override; + void applyConstraint(const core::MechanicalParams* mparams, const sofa::core::behavior::MultiMatrixAccessor* matrix) override; + + void projectMatrix( sofa::linearalgebra::BaseMatrix* /*M*/, unsigned /*offset*/ ) override; + + void applyConstraint(sofa::core::behavior::ZeroDirichletCondition* matrix) override; + void draw(const core::visual::VisualParams* vparams) override; private: diff --git a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/LinearVelocityProjectiveConstraint.inl b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/LinearVelocityProjectiveConstraint.inl index 80ff4e51937..85c80e5b1b2 100644 --- a/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/LinearVelocityProjectiveConstraint.inl +++ b/Sofa/Component/Constraint/Projective/src/sofa/component/constraint/projective/LinearVelocityProjectiveConstraint.inl @@ -23,6 +23,8 @@ #include #include +#include + #include #include #include @@ -40,6 +42,7 @@ LinearVelocityProjectiveConstraint::LinearVelocityProjectiveConstrai , d_keyTimes( initData(&d_keyTimes,"keyTimes","key times for the movements") ) , d_keyVelocities( initData(&d_keyVelocities,"velocities","velocities corresponding to the key times") ) , d_coordinates( initData(&d_coordinates, "coordinates", "coordinates on which to apply velocities") ) + , d_continueAfterEnd( initData(&d_continueAfterEnd, false, "continueAfterEnd", "If set to true then the last velocity will still be applied after all the key events") ) , l_topology(initLink("topology", "link to the topology container")) , finished(false) { @@ -156,7 +159,7 @@ void LinearVelocityProjectiveConstraint::projectResponse(const core: findKeyTimes(); } - if (finished && nextT != prevT) + if ((finished || d_continueAfterEnd.getValue()) && nextT != prevT) { const SetIndexArray & indices = d_indices.getValue(); @@ -180,7 +183,7 @@ void LinearVelocityProjectiveConstraint::projectVelocity(const core: findKeyTimes(); } - if (finished && nextT != prevT) + if ((finished || d_continueAfterEnd.getValue()) && nextT != prevT) { //if we found 2 keyTimes, we have to interpolate a velocity (linear interpolation) Deriv v = ((nextV - prevV)*((cT - prevT)/(nextT - prevT))) + prevV; @@ -239,7 +242,7 @@ void LinearVelocityProjectiveConstraint::projectPosition(const core: Real dTsimu = (Real) this->getContext()->getDt(); - if(finished) + if((finished || d_continueAfterEnd.getValue())) { Real dt = (cT - prevT) / (nextT - prevT); Deriv m = (nextV-prevV)*dt + prevV; @@ -311,6 +314,88 @@ void LinearVelocityProjectiveConstraint::projectJacobianMatrix(const } +template +void LinearVelocityProjectiveConstraint::applyConstraint(const core::MechanicalParams* mparams, linearalgebra::BaseVector* vector, const sofa::core::behavior::MultiMatrixAccessor* matrix) +{ + SOFA_UNUSED(mparams); + const int o = matrix->getGlobalOffset(this->mstate.get()); + if (o >= 0) + { + const unsigned int offset = (unsigned int)o; + const unsigned int N = Deriv::size(); + + const SetIndexArray & indices = this->d_indices.getValue(); + for (const unsigned int index : indices) + { + for (unsigned int c = 0; c < N; ++c) + { + vector->clear(offset + N * index + c); + } + } + + } +} + +template +void LinearVelocityProjectiveConstraint::applyConstraint(const core::MechanicalParams* mparams, const sofa::core::behavior::MultiMatrixAccessor* matrix) +{ + SOFA_UNUSED(mparams); + if(const core::behavior::MultiMatrixAccessor::MatrixRef r = matrix->getMatrix(this->mstate.get())) + { + const unsigned int N = Deriv::size(); + const SetIndexArray & indices = this->d_indices.getValue(); + + for (SetIndexArray::const_iterator it = indices.begin(); it != indices.end(); ++it) + { + // Reset Fixed Row and Col + for (unsigned int c=0; cclearRowCol(r.offset + N * (*it) + c); + } + // Set Fixed Vertex + for (unsigned int c=0; cset(r.offset + N * (*it) + c, r.offset + N * (*it) + c, 1.0); + } + } + + } +} + +template +void LinearVelocityProjectiveConstraint::projectMatrix( sofa::linearalgebra::BaseMatrix* M, unsigned offset ) +{ + static const unsigned blockSize = DataTypes::deriv_total_size; + + const SetIndexArray & indices = this->d_indices.getValue(); + for (SetIndexArray::const_iterator it = indices.begin(); it != indices.end(); ++it) + { + for (unsigned int c = 0; c < blockSize; ++c) + { + M->clearRowCol( offset + (*it) * blockSize + c); + } + } + +} + +template +void LinearVelocityProjectiveConstraint::applyConstraint( + sofa::core::behavior::ZeroDirichletCondition* matrix) +{ + static constexpr unsigned int N = Deriv::size(); + + const SetIndexArray & indices = this->d_indices.getValue(); + + for (const auto index : indices) + { + for (unsigned int c = 0; c < N; ++c) + { + matrix->discardRowCol(N * index + c, N * index + c); + } + } + +} + //display the path the constrained dofs will go through template void LinearVelocityProjectiveConstraint::draw(const core::visual::VisualParams* vparams)