From da367dfc918797fc397bd0792201f97ace1c5e6e Mon Sep 17 00:00:00 2001 From: bakpaul Date: Wed, 5 Jun 2024 22:28:56 +0200 Subject: [PATCH 1/2] FIX constraint index modification --- .../constraint/CosseratActuatorConstraint.h | 2 +- .../constraint/CosseratActuatorConstraint.inl | 9 ++++--- .../CosseratNeedleSlidingConstraint.h | 4 +-- .../CosseratNeedleSlidingConstraint.inl | 11 ++++---- src/Cosserat/constraint/QPSlidingConstraint.h | 2 +- .../constraint/QPSlidingConstraint.inl | 26 ++++++++++--------- 6 files changed, 29 insertions(+), 25 deletions(-) diff --git a/src/Cosserat/constraint/CosseratActuatorConstraint.h b/src/Cosserat/constraint/CosseratActuatorConstraint.h index 41e127d..aceb33a 100644 --- a/src/Cosserat/constraint/CosseratActuatorConstraint.h +++ b/src/Cosserat/constraint/CosseratActuatorConstraint.h @@ -174,7 +174,7 @@ class CosseratActuatorConstraint : public CableModel using CableModel::d_displacement ; using CableModel::d_componentState ; using CableModel::m_nbLines ; - using CableModel::m_constraintId ; + using CableModel::m_constraintIndex ; using CableModel::m_state ; using CableModel::d_indices ; using CableModel::d_pullPoint; diff --git a/src/Cosserat/constraint/CosseratActuatorConstraint.inl b/src/Cosserat/constraint/CosseratActuatorConstraint.inl index 8300c90..218e996 100644 --- a/src/Cosserat/constraint/CosseratActuatorConstraint.inl +++ b/src/Cosserat/constraint/CosseratActuatorConstraint.inl @@ -104,11 +104,12 @@ void CosseratActuatorConstraint::buildConstraintMatrix(const Constrai SOFA_UNUSED(cParams); - m_constraintId = cIndex; + m_constraintIndex.setValue(cIndex); + const auto& constraintIndex = sofa::helper::getReadAccessor(m_constraintIndex); MatrixDeriv& matrix = *cMatrix.beginEdit(); - MatrixDerivRowIterator rowIterator = matrix.writeLine(m_constraintId); + MatrixDerivRowIterator rowIterator = matrix.writeLine(constraintIndex); VecCoord positions = x.getValue(); @@ -144,7 +145,7 @@ void CosseratActuatorConstraint::buildConstraintMatrix(const Constrai } cIndex++; cMatrix.endEdit(); - m_nbLines = cIndex - m_constraintId; + m_nbLines = cIndex - constraintIndex; } @@ -168,7 +169,7 @@ void CosseratActuatorConstraint::getConstraintViolation(const Constra for (unsigned i=0;iset(m_constraintId, dfree); + resV->set(m_constraintIndex.getValue(), dfree); } } diff --git a/src/Cosserat/constraint/CosseratNeedleSlidingConstraint.h b/src/Cosserat/constraint/CosseratNeedleSlidingConstraint.h index 0f24090..5aee9c7 100644 --- a/src/Cosserat/constraint/CosseratNeedleSlidingConstraint.h +++ b/src/Cosserat/constraint/CosseratNeedleSlidingConstraint.h @@ -134,8 +134,8 @@ namespace sofa::component::constraintset using Constraint::d_componentState; //////////////////////////////////////////////////////////////////////////// /// \brief internalInit - unsigned int m_nbLines; - unsigned int m_constraintId; + using Constraint::m_nbLines ; + using Constraint::m_constraintIndex ; void internalInit(); }; diff --git a/src/Cosserat/constraint/CosseratNeedleSlidingConstraint.inl b/src/Cosserat/constraint/CosseratNeedleSlidingConstraint.inl index 022e845..d579bbb 100644 --- a/src/Cosserat/constraint/CosseratNeedleSlidingConstraint.inl +++ b/src/Cosserat/constraint/CosseratNeedleSlidingConstraint.inl @@ -113,7 +113,7 @@ namespace sofa::component::constraintset SOFA_UNUSED(cParams); MatrixDeriv &matrix = *cMatrix.beginEdit(); VecCoord positions = x.getValue(); - m_constraintId = cIndex; + m_constraintIndex.setValue(cIndex); type::Vec<3, bool> use = d_useDirections.getValue(); @@ -131,7 +131,7 @@ namespace sofa::component::constraintset } } cMatrix.endEdit(); - m_nbLines = cIndex - m_constraintId; + m_nbLines = cIndex - m_constraintIndex.getValue(); } @@ -151,20 +151,21 @@ namespace sofa::component::constraintset //ReadAccessor> positions = this->mstate->readPositions(); const VecCoord positions = x.getValue(); type::Vec<3, bool> use = d_useDirections.getValue(); + const auto& constraintIndex = sofa::helper::getReadAccessor(m_constraintIndex); for (unsigned int i = 0; i < positions.size(); i++) { if (use[0]){ Real dfree0 = positions[i][0]; - resV->set(m_constraintId + 2 * i, dfree0); + resV->set(constraintIndex + 2 * i, dfree0); } if (use[1]){ Real dfree1 = positions[i][1]; - resV->set(m_constraintId + 2 * i, dfree1); + resV->set(constraintIndex + 2 * i, dfree1); } if (use[2]){ Real dfree2 = positions[i][2]; - resV->set(m_constraintId + 2 * i + 1, dfree2); + resV->set(constraintIndex + 2 * i + 1, dfree2); } } } diff --git a/src/Cosserat/constraint/QPSlidingConstraint.h b/src/Cosserat/constraint/QPSlidingConstraint.h index 31fa3bc..80590ee 100644 --- a/src/Cosserat/constraint/QPSlidingConstraint.h +++ b/src/Cosserat/constraint/QPSlidingConstraint.h @@ -162,7 +162,7 @@ class QPSlidingConstraint : public CableModel //////////////////////////////////////////////////////////////////////////// /// \brief internalInit using SoftRobotsConstraint::m_nbLines ; - using SoftRobotsConstraint::m_constraintId ; + using SoftRobotsConstraint::m_constraintIndex ; void internalInit(); }; diff --git a/src/Cosserat/constraint/QPSlidingConstraint.inl b/src/Cosserat/constraint/QPSlidingConstraint.inl index 11798e2..7a188a0 100644 --- a/src/Cosserat/constraint/QPSlidingConstraint.inl +++ b/src/Cosserat/constraint/QPSlidingConstraint.inl @@ -118,7 +118,8 @@ void QPSlidingConstraint::buildConstraintMatrix(const ConstraintParam SOFA_UNUSED(cParams); MatrixDeriv& matrix = *cMatrix.beginEdit(); VecCoord positions = x.getValue(); - m_constraintId= cIndex; + m_constraintIndex.setValue(cIndex); + const auto& constraintIndex = sofa::helper::getReadAccessor(m_constraintIndex); for (unsigned int i=0; i::buildConstraintMatrix(const ConstraintParam } cMatrix.endEdit(); - m_nbLines = cIndex - m_constraintId; + m_nbLines = cIndex - constraintIndex; } @@ -155,6 +156,7 @@ void QPSlidingConstraint::getConstraintViolation(const ConstraintPara SOFA_UNUSED(cParams); ReadAccessor> positions = m_state->readPositions(); + const auto& constraintIndex = sofa::helper::getReadAccessor(m_constraintIndex); if(Jdx->size()==0){ //std::cout << "Size JDX = "<< Jdx->size() << std::endl; @@ -163,16 +165,16 @@ void QPSlidingConstraint::getConstraintViolation(const ConstraintPara Real dfree1 = positions[i][1]; Real dfree2 = positions[i][2]; - resV->set(m_constraintId + 2*i , dfree1); - resV->set(m_constraintId + 2*i +1, dfree2); + resV->set(constraintIndex + 2*i , dfree1); + resV->set(constraintIndex + 2*i +1, dfree2); }else{ Real dfree0 = positions[i][0]; Real dfree1 = positions[i][1]; Real dfree2 = positions[i][2]; - resV->set(m_constraintId + 2*i , dfree0); - resV->set(m_constraintId + 2*i +1, dfree1); - resV->set(m_constraintId + 2*i +2, dfree2); + resV->set(constraintIndex + 2*i , dfree0); + resV->set(constraintIndex + 2*i +1, dfree1); + resV->set(constraintIndex + 2*i +2, dfree2); } } @@ -183,8 +185,8 @@ void QPSlidingConstraint::getConstraintViolation(const ConstraintPara Real dfree1 = Jdx->element(2*i) + positions[i][1]; Real dfree2 = Jdx->element(2*i+1) + positions[i][2]; - resV->set(m_constraintId + 2*i , dfree1); - resV->set(m_constraintId + 2*i +1, dfree2); + resV->set(constraintIndex + 2*i , dfree1); + resV->set(constraintIndex + 2*i +1, dfree2); }else{ //std::cout << " The laste position : "<< positions[i] << "; Jdx->element() "<< Jdx->element(2*i) <<" "<< Jdx->element(2*i+1) <<" "<< Jdx->element(2*i+2) << std::endl; Real dfree0 = Jdx->element(2*i) + positions[i][0]; @@ -192,9 +194,9 @@ void QPSlidingConstraint::getConstraintViolation(const ConstraintPara Real dfree2 = Jdx->element(2*i+2) + positions[i][2]; //std::cout << " m_constraintId + 2*i : "<< m_constraintId + 2*i << std::endl; - resV->set(m_constraintId + 2*i , dfree0); - resV->set(m_constraintId + 2*i +1, dfree1); - resV->set(m_constraintId + 2*i +2, dfree2); + resV->set(constraintIndex + 2*i , dfree0); + resV->set(constraintIndex + 2*i +1, dfree1); + resV->set(constraintIndex + 2*i +2, dfree2); } } } From bef1e626a0bbaf87736e2ffbc98086cbaf88bccd Mon Sep 17 00:00:00 2001 From: bakpaul Date: Wed, 5 Jun 2024 22:52:48 +0200 Subject: [PATCH 2/2] FIX compilation --- src/Cosserat/constraint/CosseratNeedleSlidingConstraint.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/Cosserat/constraint/CosseratNeedleSlidingConstraint.h b/src/Cosserat/constraint/CosseratNeedleSlidingConstraint.h index 5aee9c7..fd78495 100644 --- a/src/Cosserat/constraint/CosseratNeedleSlidingConstraint.h +++ b/src/Cosserat/constraint/CosseratNeedleSlidingConstraint.h @@ -134,7 +134,7 @@ namespace sofa::component::constraintset using Constraint::d_componentState; //////////////////////////////////////////////////////////////////////////// /// \brief internalInit - using Constraint::m_nbLines ; + unsigned m_nbLines ; using Constraint::m_constraintIndex ; void internalInit();