Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Follow SOFA Lifecycle v24.06 #102

Merged
merged 1 commit into from
Mar 19, 2024
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
3 changes: 2 additions & 1 deletion src/Cosserat/constraint/CosseratActuatorConstraint.inl
Original file line number Diff line number Diff line change
Expand Up @@ -83,7 +83,8 @@ void CosseratActuatorConstraint<DataTypes>::internalInit()
if(d_value.getValue().size()==0)
{
WriteAccessor<Data<vector<Real>>> value = d_value;
value.resize(1,0.);
value.resize(1);
value[0] = 0.;
}

// check for errors in the initialization
Expand Down
10 changes: 4 additions & 6 deletions src/Cosserat/constraint/CosseratNeedleSlidingConstraint.h
Original file line number Diff line number Diff line change
Expand Up @@ -109,13 +109,11 @@ namespace sofa::component::constraintset
j.multTransposeBaseVector(res, lambda); // lambda is a vector of scalar value so block size is one.
}

void storeLambda(const ConstraintParams */*cParams*/, MultiVecDerivId res, const sofa::linearalgebra::BaseVector *lambda) override
void storeLambda(const ConstraintParams *cParams, MultiVecDerivId res, const sofa::linearalgebra::BaseVector *lambda) override
{
// if (cParams)
// {
// storeLambda(cParams, *res[m_state1->getMstate()].write(), *cParams->readJ(m_state1->getMstate()), lambda);
// storeLambda(cParams, *res[m_state2->getMstate()].write(), *cParams->readJ(m_state2->getMstate()), lambda);
// }
SOFA_UNUSED(cParams);
SOFA_UNUSED(res);
SOFA_UNUSED(lambda);
}

protected:
Expand Down
5 changes: 3 additions & 2 deletions src/Cosserat/constraint/CosseratNeedleSlidingConstraint.inl
Original file line number Diff line number Diff line change
Expand Up @@ -32,7 +32,7 @@

#include <sofa/core/visual/VisualParams.h>
#include <sofa/type/Vec.h>
#include <sofa/component/constraint/lagrangian/model/BilateralInteractionConstraint.h>
#include <sofa/component/constraint/lagrangian/model/BilateralLagrangianConstraint.h>
#include "CosseratNeedleSlidingConstraint.h"

namespace sofa::component::constraintset
Expand Down Expand Up @@ -91,7 +91,8 @@ namespace sofa::component::constraintset
if (d_value.getValue().size() == 0)
{
WriteAccessor<Data<vector<Real>>> value = d_value;
value.resize(1, 0.);
value.resize(1);
value[0] = 0.;
}

// check for errors in the initialization
Expand Down
2 changes: 1 addition & 1 deletion src/Cosserat/constraint/QPSlidingConstraint.h
Original file line number Diff line number Diff line change
Expand Up @@ -35,7 +35,7 @@

#include <SoftRobots/component/constraint/model/CableModel.h>
#include <SoftRobots/component/behavior/SoftRobotsConstraint.h>
#include <sofa/component/constraint/lagrangian/model/BilateralInteractionConstraint.h>
#include <sofa/component/constraint/lagrangian/model/BilateralLagrangianConstraint.h>

namespace sofa::component::constraintset
{
Expand Down
10 changes: 5 additions & 5 deletions src/Cosserat/constraint/QPSlidingConstraint.inl
Original file line number Diff line number Diff line change
Expand Up @@ -32,7 +32,7 @@

#include <sofa/core/visual/VisualParams.h>
#include <sofa/type/Vec.h>
#include <sofa/component/constraint/lagrangian/model/BilateralInteractionConstraint.h>
#include <sofa/component/constraint/lagrangian/model/BilateralLagrangianConstraint.h>

#include "QPSlidingConstraint.h"

Expand Down Expand Up @@ -95,7 +95,8 @@ void QPSlidingConstraint<DataTypes>::internalInit()
if(d_value.getValue().size()==0)
{
WriteAccessor<Data<vector<Real>>> value = d_value;
value.resize(1,0.);
value.resize(1);
value[0] = 0.;
}

// check for errors in the initialization
Expand Down Expand Up @@ -205,8 +206,7 @@ void QPSlidingConstraint<DataTypes>::getConstraintResolution(const ConstraintPar
unsigned int& offset)
{
ReadAccessor<Data<VecCoord>> positions = m_state->readPositions();
// std::cout << "The position size is : " << positions.size()<< std::endl;
double imposedValue= 1.0;

for (size_t i = 0; i < positions.size(); i++){

resTab[offset++] = new BilateralConstraintResolution();
Expand All @@ -215,13 +215,13 @@ void QPSlidingConstraint<DataTypes>::getConstraintResolution(const ConstraintPar
resTab[offset++] = new BilateralConstraintResolution();
}
}
// std::cout << "The position size is END " << std::endl;
}


template<class DataTypes>
void QPSlidingConstraint<DataTypes>::draw(const VisualParams* vparams)
{
SOFA_UNUSED(vparams);
if(d_componentState.getValue() != ComponentState::Valid)
return ;

Expand Down
2 changes: 1 addition & 1 deletion src/Cosserat/engine/CosseratState.h
Original file line number Diff line number Diff line change
Expand Up @@ -35,7 +35,7 @@ namespace sofa::component::container
using namespace core::objectmodel;
using namespace sofa::defaulttype;
using namespace sofa::core::topology;
using sofa::defaulttype::Vector3;
using sofa::type::Vec3;

template <class DataTypes>
class TemperatureState : public sofa::component::container::MechanicalObject<DataTypes>
Expand Down
4 changes: 2 additions & 2 deletions src/Cosserat/engine/PointsManager.h
Original file line number Diff line number Diff line change
Expand Up @@ -43,8 +43,8 @@ namespace sofa::core::behavior
PointSetTopologyModifier *m_modifier;
core::behavior::MechanicalState<DataTypes> *m_beam;

void init();
void handleEvent(sofa::core::objectmodel::Event *event);
void init() override;
void handleEvent(sofa::core::objectmodel::Event *event) override;
// void draw(const core::visual::VisualParams *vparams);

void addNewPointToState();
Expand Down
2 changes: 0 additions & 2 deletions src/Cosserat/engine/PointsManager.inl
Original file line number Diff line number Diff line change
Expand Up @@ -76,10 +76,8 @@ namespace sofa::core::behavior
{
helper::WriteAccessor<Data<VecCoord>> x = *this->getMstate()->write(core::VecCoordId::position());
helper::WriteAccessor<Data<VecCoord>> xfree = *this->getMstate()->write(core::VecCoordId::freePosition());
const helper::ReadAccessor<Data<VecCoord>> &beam = m_beam->readPositions();
unsigned nbPoints = this->getTopology()->getNbPoints(); // do not take the last point because there is a bug

size_t beamSz = beam.size();
sofa::type::vector<unsigned int> Indices;
if (nbPoints > 0)
{
Expand Down
2 changes: 1 addition & 1 deletion src/Cosserat/mapping/BaseCosserat.h
Original file line number Diff line number Diff line change
Expand Up @@ -38,7 +38,7 @@ using sofa::defaulttype::SolidTypes ;
using sofa::core::objectmodel::BaseContext ;
using sofa::type::Matrix3;
using sofa::type::Matrix4;
using type::Vector3;
using type::Vec3;
using type::Vec6;
using std::get;
using sofa::core::objectmodel::BaseObject;
Expand Down
2 changes: 1 addition & 1 deletion src/Cosserat/mapping/DifferenceMultiMapping.inl
Original file line number Diff line number Diff line change
Expand Up @@ -701,7 +701,7 @@ namespace sofa::component::mapping

if (d_lastPointIsFixed.getValue())
{
if ((rowIt.index() / 2) < (x1from.size() - 1))
if ((rowIt.index() / 2) < (int)(x1from.size() - 1))
{
while (colIt != colItEnd)
{
Expand Down
2 changes: 1 addition & 1 deletion src/Cosserat/mapping/DiscreteCosseratMapping.h
Original file line number Diff line number Diff line change
Expand Up @@ -108,7 +108,7 @@ class DiscreteCosseratMapping : public core::Multi2Mapping<TIn1, TIn2, TOut>, pu
Data<Real2> d_min;
Data<Real1> d_radius ;
Data<bool> d_drawMapBeam ;
Data<type::Vec4f> d_color;
Data<sofa::type::RGBAColor> d_color;
Data<type::vector<int> > d_index;
Data<unsigned int> d_baseIndex;

Expand Down
4 changes: 2 additions & 2 deletions src/Cosserat/mapping/DiscreteCosseratMapping.inl
Original file line number Diff line number Diff line change
Expand Up @@ -60,7 +60,7 @@ DiscreteCosseratMapping<TIn1, TIn2, TOut>::DiscreteCosseratMapping()
"the axis in which we want to show the deformation.\n"))
, d_drawMapBeam(initData(&d_drawMapBeam, true,"nonColored", "if this parameter is false, you draw the beam with "
"color according to the force apply to each beam"))
, d_color(initData(&d_color, type::Vec4f (40/255.0, 104/255.0, 137/255.0, 0.8) ,"color", "The default beam color"))
, d_color(initData(&d_color, sofa::type::RGBAColor(40/255.0, 104/255.0, 137/255.0, 0.8) ,"color", "The default beam color"))
, d_index(initData(&d_index, "index", "if this parameter is false, you draw the beam with color "
"according to the force apply to each beam"))
, d_baseIndex(initData(&d_baseIndex, (unsigned int) 0, "baseIndex", "This parameter defines the index of the rigid "
Expand Down Expand Up @@ -588,7 +588,7 @@ void DiscreteCosseratMapping<TIn1, TIn2, TOut>::draw(const core::visual::VisualP
glLineWidth(d_radius.getValue());
glBegin(GL_LINES);
if(d_drawMapBeam.getValue()){
type::Vec4f _color = d_color.getValue();
sofa::type::RGBAColor _color = d_color.getValue();
RGBAColor colorL = RGBAColor(_color[0],_color[1],_color[2],_color[3]);
glColor4f(colorL[0], colorL[1], colorL[2],colorL[3]);
for (unsigned int i=0; i<sz-1; i++) {
Expand Down
2 changes: 1 addition & 1 deletion src/Cosserat/mapping/DiscreteDynamicCosseratMapping.h
Original file line number Diff line number Diff line change
Expand Up @@ -35,7 +35,7 @@ using sofa::defaulttype::SolidTypes ;
using sofa::core::objectmodel::BaseContext ;
using sofa::type::Matrix3;
using sofa::type::Matrix4;
using type::Vector3;
using type::Vec3;
using type::Vec6;
using std::get;

Expand Down
1 change: 0 additions & 1 deletion src/Cosserat/mapping/DiscreteDynamicCosseratMapping.inl
Original file line number Diff line number Diff line change
Expand Up @@ -265,7 +265,6 @@ void DiscreteDynamicCosseratMapping<TIn1, TIn2, TOut>::computeJ_Jdot_i(const Mat
Mat6x6 M;

Mat6x3 Si_dot;
Mat6x6 M_dot;
Mat6x6 adj_eta; //to be computed
//std::cout << "indice vector : "<< this->m_indicesVectors <<" vecId :"<< this->m_indicesVectors[frameId] << std::endl;
//std::cout << "this->m_framesTangExpVectors[frameId] :"<< this->m_framesTangExpVectors[frameId] << std::endl;
Expand Down
2 changes: 1 addition & 1 deletion src/Cosserat/mapping/LegendrePolynomialsMapping.h
Original file line number Diff line number Diff line change
Expand Up @@ -20,7 +20,7 @@ namespace sofa::component::mapping {
using sofa::core::objectmodel::BaseContext;
using sofa::type::Matrix3;
using sofa::type::Matrix4;
using type::Vector3;
using type::Vec3;
using type::Vec6;
using std::get;

Expand Down
1 change: 0 additions & 1 deletion src/Cosserat/mapping/LegendrePolynomialsMapping.inl
Original file line number Diff line number Diff line change
Expand Up @@ -92,7 +92,6 @@ namespace sofa::component::mapping {
helper::ReadAccessor< Data<InVecDeriv> > velIn = dIn;

helper::WriteOnlyAccessor< Data<VecDeriv> > out = dOut;
helper::ReadAccessor< Data<InVecDeriv> > in = dIn;

const auto sz = d_vectorOfCurvilinearAbscissa.getValue().size();
out.resize(sz-1);
Expand Down
2 changes: 1 addition & 1 deletion src/Cosserat/mapping/RigidDistanceMapping.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -19,7 +19,7 @@
* *
* Contact information: [email protected] *
******************************************************************************/
#pragma once
#define SOFA_COSSERAT_CPP_RigidDistanceMapping
#include "RigidDistanceMapping.inl"
#include <sofa/defaulttype/VecTypes.h>
#include <sofa/defaulttype/RigidTypes.h>
Expand Down
6 changes: 2 additions & 4 deletions src/Cosserat/mapping/RigidDistanceMapping.inl
Original file line number Diff line number Diff line change
Expand Up @@ -45,8 +45,7 @@ namespace sofa::component::mapping

template <class TIn1, class TIn2, class TOut>
RigidDistanceMapping<TIn1, TIn2, TOut>::RigidDistanceMapping()
: m_toModel(NULL)
, d_index1(initData(&d_index1, "first_point", "index of the first model \n"))
: d_index1(initData(&d_index1, "first_point", "index of the first model \n"))
, d_index2(initData(&d_index2, "second_point", "index of the second model \n"))
, d_max(initData(&d_max, (Real)1.0e-2, "max", "the maximum of the deformation.\n"))
, d_min(initData(&d_min, (Real)0.0, "min", "the minimum of the deformation.\n"))
Expand All @@ -55,6 +54,7 @@ RigidDistanceMapping<TIn1, TIn2, TOut>::RigidDistanceMapping()
, d_index(initData(&d_index, "index", "if this parameter is false, you draw the beam with color "
"according to the force apply to each beam"))
, d_debug(initData(&d_debug, false, "debug", "show debug output.\n"))
, m_toModel(NULL)
{
d_debug.setValue(false);
}
Expand Down Expand Up @@ -139,8 +139,6 @@ void RigidDistanceMapping<TIn1, TIn2, TOut>:: applyJ(
const auto &m1Indices = d_index1.getValue();
const auto &m2Indices = d_index2.getValue();

SpatialVector vDOF1, vDOF2;

for (sofa::Index index = 0; index < m_minInd; index++) {
getVCenter(outVel[index]) = getVCenter(in2Vel[m2Indices[index]]) - getVCenter(in1Vel[m1Indices[index]]);
getVOrientation(outVel[index]) = getVOrientation(in2Vel[m2Indices[index]]) - getVOrientation(in1Vel[m1Indices[index]]) ;
Expand Down
Loading