From 87b9a42b2bbc999e2e7f9434475e7b25d2b52504 Mon Sep 17 00:00:00 2001 From: hugtalbot Date: Tue, 27 May 2025 22:34:43 +0200 Subject: [PATCH] Apply template method on BaseForceField: ClosestPointRFF IntensityProfileRFF --- .../ClosestPointRegistrationForceField.h | 2 +- .../ClosestPointRegistrationForceField.inl | 11 ++++------- .../IntensityProfileRegistrationForceField.h | 2 +- .../IntensityProfileRegistrationForceField.inl | 14 ++++++-------- 4 files changed, 12 insertions(+), 17 deletions(-) diff --git a/src/Registration/ClosestPointRegistrationForceField.h b/src/Registration/ClosestPointRegistrationForceField.h index 1f1c4c5..28033dc 100644 --- a/src/Registration/ClosestPointRegistrationForceField.h +++ b/src/Registration/ClosestPointRegistrationForceField.h @@ -96,7 +96,7 @@ class SOFA_REGISTRATION_API ClosestPointRegistrationForceField : public core::be void addForce(const core::MechanicalParams* /*mparams*/,DataVecDeriv& f , const DataVecCoord& x , const DataVecDeriv& v) override; void addDForce(const core::MechanicalParams* mparams ,DataVecDeriv& df , const DataVecDeriv& dx) override; SReal getPotentialEnergy(const core::MechanicalParams* ,const DataVecCoord&) const override { return m_potentialEnergy; } - void addKToMatrix( const core::MechanicalParams* mparams,const sofa::core::behavior::MultiMatrixAccessor* matrix) override; + void addKToMatrix(sofa::linearalgebra::BaseMatrix * matrix, SReal kFact, unsigned int & offset) override; Real getStiffness() const{ return ks.getValue(); } Real getDamping() const{ return kd.getValue(); } diff --git a/src/Registration/ClosestPointRegistrationForceField.inl b/src/Registration/ClosestPointRegistrationForceField.inl index 4f3dfdc..1db1523 100644 --- a/src/Registration/ClosestPointRegistrationForceField.inl +++ b/src/Registration/ClosestPointRegistrationForceField.inl @@ -387,18 +387,15 @@ void ClosestPointRegistrationForceField::addDForce(const core::Mechan } template -void ClosestPointRegistrationForceField::addKToMatrix(const core::MechanicalParams* mparams,const sofa::core::behavior::MultiMatrixAccessor* matrix) +void ClosestPointRegistrationForceField::addKToMatrix(sofa::linearalgebra::BaseMatrix * matrix, SReal kFact, unsigned int & offset) { - Real k = (Real)sofa::core::mechanicalparams::kFactorIncludingRayleighDamping(mparams, this->rayleighStiffness.getValue()) * this->ks.getValue(); - if(!k) return; - sofa::core::behavior::MultiMatrixAccessor::MatrixRef mref = matrix->getMatrix(this->mstate); - sofa::linearalgebra::BaseMatrix *mat = mref.matrix; - const int offset = (int)mref.offset; + if(!kFact) + return; const int N = Coord::total_size; const int nb = this->closestPos.size(); for (int index = 0; index < nb; index++) for(int i = 0; i < N; i++) - mat->add(offset + N * index + i, offset + N * index + i, -k); + matrix->add(offset + N * index + i, offset + N * index + i, -kFact); } template diff --git a/src/Registration/IntensityProfileRegistrationForceField.h b/src/Registration/IntensityProfileRegistrationForceField.h index 215dbf5..1fdb903 100644 --- a/src/Registration/IntensityProfileRegistrationForceField.h +++ b/src/Registration/IntensityProfileRegistrationForceField.h @@ -139,7 +139,7 @@ class IntensityProfileRegistrationForceField : public core::behavior::ForceField void addForce(const core::MechanicalParams* /*mparams*/,DataVecDeriv& f , const DataVecCoord& x , const DataVecDeriv& v) override; void addDForce(const core::MechanicalParams* mparams ,DataVecDeriv& df , const DataVecDeriv& dx) override; SReal getPotentialEnergy(const core::MechanicalParams* ,const DataVecCoord&) const override { return m_potentialEnergy; } - void addKToMatrix( const core::MechanicalParams* mparams,const sofa::core::behavior::MultiMatrixAccessor* matrix) override; + void addKToMatrix(sofa::linearalgebra::BaseMatrix * matrix, SReal kFact, unsigned int &offset) override; Real getStiffness() const{ return ks.getValue(); } Real getDamping() const{ return kd.getValue(); } diff --git a/src/Registration/IntensityProfileRegistrationForceField.inl b/src/Registration/IntensityProfileRegistrationForceField.inl index 7c5d4ce..4614f1e 100644 --- a/src/Registration/IntensityProfileRegistrationForceField.inl +++ b/src/Registration/IntensityProfileRegistrationForceField.inl @@ -438,13 +438,11 @@ void IntensityProfileRegistrationForceField::addDForce(con template -void IntensityProfileRegistrationForceField::addKToMatrix(const core::MechanicalParams* mparams,const sofa::core::behavior::MultiMatrixAccessor* matrix) +void IntensityProfileRegistrationForceField::addKToMatrix(sofa::linearalgebra::BaseMatrix * matrix, SReal kFactor, unsigned int &offset) { - Real k = (Real)sofa::core::mechanicalparams::kFactorIncludingRayleighDamping(mparams, this->rayleighStiffness.getValue()) * this->ks.getValue(); - if(!k) return; - sofa::core::behavior::MultiMatrixAccessor::MatrixRef mref = matrix->getMatrix(this->mstate); - sofa::linearalgebra::BaseMatrix *mat = mref.matrix; - const int offset = (int)mref.offset; + if(!kFactor) + return; + const int N = Coord::total_size; const int nb = this->targetPos.size(); @@ -453,13 +451,13 @@ void IntensityProfileRegistrationForceField::addKToMatrix( for (int index = 0; index add(offset + N * index + i, offset + N * index + j, -k*this->dfdx[index][i][j]); + matrix->add(offset + N * index + i, offset + N * index + j, -kFactor*this->dfdx[index][i][j]); } else { for (int index = 0; index add(offset + N * index + i, offset + N * index + i, -k); + matrix->add(offset + N * index + i, offset + N * index + i, -kFactor); } }