diff --git a/Sofa/Component/StateContainer/src/sofa/component/statecontainer/MechanicalObject.cpp b/Sofa/Component/StateContainer/src/sofa/component/statecontainer/MechanicalObject.cpp index 9763be97bde..d9e211fb710 100644 --- a/Sofa/Component/StateContainer/src/sofa/component/statecontainer/MechanicalObject.cpp +++ b/Sofa/Component/StateContainer/src/sofa/component/statecontainer/MechanicalObject.cpp @@ -55,9 +55,9 @@ void registerMechanicalObject(sofa::core::ObjectFactory* factory) template<> void MechanicalObject::applyRotation (const type::Quat q) { - helper::WriteAccessor< Data > x = *this->write(core::vec_id::write_access::position); + helper::WriteAccessor< Data > x_wa = *this->write(core::vec_id::write_access::position); - for (RigidCoord<3, SReal>& xi : x) + for (RigidCoord<3, SReal>& xi : x_wa) { xi.getCenter() = q.rotate(xi.getCenter()); xi.getOrientation() = q * xi.getOrientation(); @@ -198,17 +198,17 @@ void MechanicalObject::draw(const core::visual::Visual if (showObject.getValue()) { - const float scale = showObjectScale.getValue(); - const helper::ReadAccessor > x = *this->read(core::vec_id::write_access::position); + const float scaleValue = showObjectScale.getValue(); + const helper::ReadAccessor > x_ra = *this->read(core::vec_id::write_access::position); const size_t vsize = d_size.getValue(); for (size_t i = 0; i < vsize; ++i) { vparams->drawTool()->pushMatrix(); float glTransform[16]; ///TODO: check if the drawtool use OpenGL-shaped matrix - x[i].writeOpenGlMatrix ( glTransform ); + x_ra[i].writeOpenGlMatrix ( glTransform ); vparams->drawTool()->multMatrix( glTransform ); - vparams->drawTool()->scale ( scale ); + vparams->drawTool()->scale ( scaleValue ); constexpr type::Vec3f sizes ( 1.f,1.f,1.f ); diff --git a/Sofa/Component/StateContainer/src/sofa/component/statecontainer/MechanicalObject.h b/Sofa/Component/StateContainer/src/sofa/component/statecontainer/MechanicalObject.h index 03ae874b42e..83833bc092c 100644 --- a/Sofa/Component/StateContainer/src/sofa/component/statecontainer/MechanicalObject.h +++ b/Sofa/Component/StateContainer/src/sofa/component/statecontainer/MechanicalObject.h @@ -119,23 +119,23 @@ class MechanicalObject : public sofa::core::behavior::MechanicalState void reset() override; - void writeVec(core::ConstVecId v, std::ostream &out) override; - void readVec(core::VecId v, std::istream &in) override; - SReal compareVec(core::ConstVecId v, std::istream &in) override; + void writeVec(core::ConstVecId vecId, std::ostream &out) override; + void readVec(core::VecId vecId, std::istream &in) override; + SReal compareVec(core::ConstVecId vecId, std::istream &in) override; void writeState( std::ostream& out ) override; /// @name New vectors access API based on VecId /// @{ - Data< VecCoord >* write(core::VecCoordId v) override; - const Data< VecCoord >* read(core::ConstVecCoordId v) const override; + Data< VecCoord >* write(core::VecCoordId vecId) override; + const Data< VecCoord >* read(core::ConstVecCoordId vecId) const override; - Data< VecDeriv >* write(core::VecDerivId v) override; - const Data< VecDeriv >* read(core::ConstVecDerivId v) const override; + Data< VecDeriv >* write(core::VecDerivId vecId) override; + const Data< VecDeriv >* read(core::ConstVecDerivId vecId) const override; - Data< MatrixDeriv >* write(core::MatrixDerivId v) override; - const Data< MatrixDeriv >* read(core::ConstMatrixDerivId v) const override; + Data< MatrixDeriv >* write(core::MatrixDerivId vecId) override; + const Data< MatrixDeriv >* read(core::ConstMatrixDerivId vecId) const override; /// @} @@ -147,14 +147,13 @@ class MechanicalObject : public sofa::core::behavior::MechanicalState Size getSize() const override { return d_size.getValue(); } - SReal getPX(sofa::Index i) const override { Real x=0.0,y=0.0,z=0.0; DataTypes::get(x,y,z,(read(core::vec_id::read_access::position)->getValue())[i]); return (SReal)x; } - SReal getPY(sofa::Index i) const override { Real x=0.0,y=0.0,z=0.0; DataTypes::get(x,y,z,(read(core::vec_id::read_access::position)->getValue())[i]); return (SReal)y; } - SReal getPZ(sofa::Index i) const override { Real x=0.0,y=0.0,z=0.0; DataTypes::get(x,y,z,(read(core::vec_id::read_access::position)->getValue())[i]); return (SReal)z; } - - SReal getVX(sofa::Index i) const { Real x=0.0,y=0.0,z=0.0; DataTypes::get(x,y,z, read(core::vec_id::read_access::velocity)->getValue()[i]); return (SReal)x; } - SReal getVY(sofa::Index i) const { Real x=0.0,y=0.0,z=0.0; DataTypes::get(x,y,z, read(core::vec_id::read_access::velocity)->getValue()[i]); return (SReal)y; } - SReal getVZ(sofa::Index i) const { Real x=0.0,y=0.0,z=0.0; DataTypes::get(x,y,z, read(core::vec_id::read_access::velocity)->getValue()[i]); return (SReal)z; } + SReal getPX(sofa::Index i) const override; + SReal getPY(sofa::Index i) const override; + SReal getPZ(sofa::Index i) const override; + SReal getVX(sofa::Index i) const; + SReal getVY(sofa::Index i) const; + SReal getVZ(sofa::Index i) const; /** \brief Overwrite values at index outputIndex by the ones at inputIndex. * @@ -186,7 +185,7 @@ class MechanicalObject : public sofa::core::behavior::MechanicalState /// @{ /// Apply translation vector to the position. - void applyTranslation (const SReal dx, const SReal dy, const SReal dz) override; + void applyTranslation (const SReal translationVectorX, const SReal translationVectorY, const SReal translationVectorZ) override; /// Rotation using Euler Angles in degree. void applyRotation (const SReal rx, const SReal ry, const SReal rz) override; @@ -243,7 +242,7 @@ class MechanicalObject : public sofa::core::behavior::MechanicalState /// @name Initial transformations accessors. /// @{ - void setTranslation(SReal dx, SReal dy, SReal dz) {translation.setValue(type::Vec3(dx,dy,dz));} + void setTranslation(SReal translationVectorX, SReal translationVectorY, SReal translationVectorZ); void setRotation(SReal rx, SReal ry, SReal rz) {rotation.setValue(type::Vec3(rx,ry,rz));} void setScale(SReal sx, SReal sy, SReal sz) {scale.setValue(type::Vec3(sx,sy,sz));} @@ -263,23 +262,23 @@ class MechanicalObject : public sofa::core::behavior::MechanicalState void accumulateForce(const core::ExecParams* params, core::VecDerivId f = core::vec_id::write_access::force) override; // see BaseMechanicalState::accumulateForce(const ExecParams*, VecId) override /// Increment the index of the given VecCoordId, so that all 'allocated' vectors in this state have a lower index - void vAvail(const core::ExecParams* params, core::VecCoordId& v) override; + void vAvail(const core::ExecParams* params, core::VecCoordId& vecId) override; /// Increment the index of the given VecDerivId, so that all 'allocated' vectors in this state have a lower index - void vAvail(const core::ExecParams* params, core::VecDerivId& v) override; + void vAvail(const core::ExecParams* params, core::VecDerivId& vecId) override; /// Increment the index of the given MatrixDerivId, so that all 'allocated' vectors in this state have a lower index //virtual void vAvail(core::MatrixDerivId& v); /// Allocate a new temporary vector - void vAlloc(const core::ExecParams* params, core::VecCoordId v, const core::VecIdProperties& properties = {}) override; + void vAlloc(const core::ExecParams* params, core::VecCoordId vecId, const core::VecIdProperties& properties = {}) override; /// Allocate a new temporary vector - void vAlloc(const core::ExecParams* params, core::VecDerivId v, const core::VecIdProperties& properties = {}) override; + void vAlloc(const core::ExecParams* params, core::VecDerivId vecId, const core::VecIdProperties& properties = {}) override; /// Allocate a new temporary vector //virtual void vAlloc(core::MatrixDerivId v); /// Reallocate a new temporary vector - void vRealloc(const core::ExecParams* params, core::VecCoordId v, const core::VecIdProperties& properties = {}) override; + void vRealloc(const core::ExecParams* params, core::VecCoordId vecId, const core::VecIdProperties& properties = {}) override; /// Reallocate a new temporary vector - void vRealloc(const core::ExecParams* params, core::VecDerivId v, const core::VecIdProperties& properties = {}) override; + void vRealloc(const core::ExecParams* params, core::VecDerivId vecId, const core::VecIdProperties& properties = {}) override; /// Free a temporary vector @@ -296,7 +295,9 @@ class MechanicalObject : public sofa::core::behavior::MechanicalState /// Initialize an unset vector //virtual void vInit(const core::ExecParams* params, core::MatrixDerivId v, core::ConstMatrixDerivId vSrc); - void vOp(const core::ExecParams* params, core::VecId v, core::ConstVecId a = core::ConstVecId::null(), core::ConstVecId b = core::ConstVecId::null(), SReal f=1.0) override; + void vOp(const core::ExecParams* params, core::VecId r, + core::ConstVecId a = core::ConstVecId::null(), + core::ConstVecId b = core::ConstVecId::null(), SReal f = 1.0) override; void vMultiOp(const core::ExecParams* params, const VMultiOp& ops) override; @@ -310,7 +311,7 @@ class MechanicalObject : public sofa::core::behavior::MechanicalState /// Maximum of the absolute values of the entries of state vector a. This is used to compute the infinite-norm of the vector. SReal vMax(const core::ExecParams* params, core::ConstVecId a) override; - Size vSize( const core::ExecParams* params, core::ConstVecId v ) override; + Size vSize( const core::ExecParams* params, core::ConstVecId vecId ) override; void resetForce(const core::ExecParams* params, core::VecDerivId f = core::vec_id::write_access::force) override; @@ -445,7 +446,7 @@ protected : /// Given the number of a constraint Equation, find the index in the MatrixDeriv C, where the constraint is actually stored // unsigned int getIdxConstraintFromId(unsigned int id) const; - MechanicalObjectInternalData data; + MechanicalObjectInternalData m_data; template static void setVecIdProperties(core::TVecId v, const core::VecIdProperties& properties, core::BaseData* vec_d); diff --git a/Sofa/Component/StateContainer/src/sofa/component/statecontainer/MechanicalObject.inl b/Sofa/Component/StateContainer/src/sofa/component/statecontainer/MechanicalObject.inl index 278f489da96..910a49e8214 100644 --- a/Sofa/Component/StateContainer/src/sofa/component/statecontainer/MechanicalObject.inl +++ b/Sofa/Component/StateContainer/src/sofa/component/statecontainer/MechanicalObject.inl @@ -169,7 +169,7 @@ MechanicalObject::MechanicalObject() { m_initialized = false; - data = MechanicalObjectInternalData(this); + m_data = MechanicalObjectInternalData(this); x .setGroup("Vector"); @@ -670,14 +670,60 @@ void MechanicalObject::reserve(const Size size) reserveFunction(vectorsDeriv); } +template +SReal getPi(const MechanicalObject& self, const VecId& id, sofa::Index i) +{ + sofa::type::Vec<3, sofa::Real_t > xyz; + DataTypes::get(xyz[0], xyz[1], xyz[2], self.read(id)->getValue()[i]); + return static_cast(xyz[dim]); +} + +template +SReal MechanicalObject::getPX(sofa::Index i) const +{ + return getPi<0>(*this, sofa::core::vec_id::read_access::position, i); +} + +template +SReal MechanicalObject::getPY(sofa::Index i) const +{ + return getPi<1>(*this, sofa::core::vec_id::read_access::position, i); +} + +template +SReal MechanicalObject::getPZ(sofa::Index i) const +{ + return getPi<2>(*this, sofa::core::vec_id::read_access::position, i); +} + +template +SReal MechanicalObject::getVX(sofa::Index i) const +{ + return getPi<0>(*this, sofa::core::vec_id::read_access::velocity, i); +} + +template +SReal MechanicalObject::getVY(sofa::Index i) const +{ + return getPi<1>(*this, sofa::core::vec_id::read_access::velocity, i); +} + +template +SReal MechanicalObject::getVZ(sofa::Index i) const +{ + return getPi<2>(*this, sofa::core::vec_id::read_access::velocity, i); +} + template -void MechanicalObject::applyTranslation (const SReal dx, const SReal dy, const SReal dz) +void MechanicalObject::applyTranslation (const SReal translationVectorX, + const SReal translationVectorY, + const SReal translationVectorZ) { helper::WriteAccessor< Data > x_wA = *this->write(core::vec_id::write_access::position); for (unsigned int i = 0; i < x_wA.size(); i++) { - DataTypes::add(x_wA[i], dx, dy, dz); + DataTypes::add(x_wA[i], translationVectorX, translationVectorY, translationVectorZ); } } @@ -722,11 +768,14 @@ void MechanicalObject::getIndicesInSpace(sofa::type::vector > x_rA = this->readPositions(); - for( unsigned i=0; i= xmin && x <= xmax && y >= ymin && y <= ymax && z >= zmin && z <= zmax ) + sofa::type::Vec<3, Real> pos; + DataTypes::get(pos[0], pos[1], pos[2], x_rA[i]); + + if (box.contains(pos)) { indices.push_back(i); } @@ -1137,11 +1186,11 @@ void MechanicalObject::storeResetState() vOp(core::execparams::defaultInstance(), sofa::core::vec_id::write_access::resetPosition, sofa::core::vec_id::read_access::position); // we only store a resetVelocity if the velocity is not zero - helper::ReadAccessor< Data > v = *this->read(core::vec_id::read_access::velocity); + helper::ReadAccessor< Data > v_ra = *this->read(core::vec_id::read_access::velocity); bool zero = true; - for (unsigned int i=0; i::reset() template -void MechanicalObject::writeVec(core::ConstVecId v, std::ostream &out) +void MechanicalObject::writeVec(core::ConstVecId vecId, std::ostream &out) { - switch (v.type) + switch (vecId.type) { case sofa::core::V_COORD: - out << this->read(core::ConstVecCoordId(v))->getValue(); + out << this->read(core::ConstVecCoordId(vecId))->getValue(); break; case sofa::core::V_DERIV: - out << this->read(core::ConstVecDerivId(v))->getValue(); + out << this->read(core::ConstVecDerivId(vecId))->getValue(); break; case sofa::core::V_MATDERIV: - out << this->read(core::ConstMatrixDerivId(v))->getValue(); + out << this->read(core::ConstMatrixDerivId(vecId))->getValue(); break; default: break; @@ -1196,16 +1245,16 @@ void MechanicalObject::writeVec(core::ConstVecId v, std::ostream &out } template -void MechanicalObject::readVec(core::VecId v, std::istream &in) +void MechanicalObject::readVec(core::VecId vecId, std::istream &in) { Size i = 0; - switch (v.type) + switch (vecId.type) { case sofa::core::V_COORD: { Coord coord; - helper::WriteOnlyAccessor< Data< VecCoord > > vec = *this->write(core::VecCoordId(v)); + helper::WriteOnlyAccessor< Data< VecCoord > > vec = *this->write(core::VecCoordId(vecId)); while (in >> coord) { @@ -1219,7 +1268,7 @@ void MechanicalObject::readVec(core::VecId v, std::istream &in) case sofa::core::V_DERIV: { Deriv deriv; - helper::WriteOnlyAccessor< Data< VecDeriv > > vec = *this->write(core::VecDerivId(v)); + helper::WriteOnlyAccessor< Data< VecDeriv > > vec = *this->write(core::VecDerivId(vecId)); while (in >> deriv) { @@ -1242,23 +1291,23 @@ void MechanicalObject::readVec(core::VecId v, std::istream &in) } template -SReal MechanicalObject::compareVec(core::ConstVecId v, std::istream &in) +SReal MechanicalObject::compareVec(core::ConstVecId vecId, std::istream &in) { std::string ref,cur; getline(in, ref); std::ostringstream out; - switch (v.type) + switch (vecId.type) { case sofa::core::V_COORD: - out << this->read(core::ConstVecCoordId(v))->getValue(); + out << this->read(core::ConstVecCoordId(vecId))->getValue(); break; case sofa::core::V_DERIV: - out << this->read(core::ConstVecDerivId(v))->getValue(); + out << this->read(core::ConstVecDerivId(vecId))->getValue(); break; case sofa::core::V_MATDERIV: - out << this->read(core::ConstMatrixDerivId(v))->getValue(); + out << this->read(core::ConstMatrixDerivId(vecId))->getValue(); break; default: break; @@ -1326,44 +1375,44 @@ void MechanicalObject::accumulateForce(const core::ExecParams* params } template -Data::VecCoord>* MechanicalObject::write(core::VecCoordId v) +Data::VecCoord>* MechanicalObject::write(core::VecCoordId vecId) { - if (v.index >= vectorsCoord.size()) + if (vecId.index >= vectorsCoord.size()) { - vectorsCoord.resize(v.index + 1, 0); + vectorsCoord.resize(vecId.index + 1, 0); } - if (vectorsCoord[v.index] == nullptr) + if (vectorsCoord[vecId.index] == nullptr) { - vectorsCoord[v.index] = new Data< VecCoord >; - vectorsCoord[v.index]->setName(v.getName()); - const auto group = v.getGroup(); + vectorsCoord[vecId.index] = new Data< VecCoord >; + vectorsCoord[vecId.index]->setName(vecId.getName()); + const auto group = vecId.getGroup(); if (!group.empty()) { - vectorsCoord[v.index]->setGroup(group); + vectorsCoord[vecId.index]->setGroup(group); } else { - vectorsCoord[v.index]->setGroup("Vector"); + vectorsCoord[vecId.index]->setGroup("Vector"); } - this->addData(vectorsCoord[v.index]); + this->addData(vectorsCoord[vecId.index]); if (f_reserve.getValue() > 0) { - vectorsCoord[v.index]->beginWriteOnly()->reserve(f_reserve.getValue()); - vectorsCoord[v.index]->endEdit(); + vectorsCoord[vecId.index]->beginWriteOnly()->reserve(f_reserve.getValue()); + vectorsCoord[vecId.index]->endEdit(); } - if (vectorsCoord[v.index]->getValue().size() != getSize()) + if (vectorsCoord[vecId.index]->getValue().size() != getSize()) { - vectorsCoord[v.index]->beginWriteOnly()->resize( getSize() ); - vectorsCoord[v.index]->endEdit(); + vectorsCoord[vecId.index]->beginWriteOnly()->resize( getSize() ); + vectorsCoord[vecId.index]->endEdit(); } } - Data::VecCoord>* d = vectorsCoord[v.index]; + Data::VecCoord>* d = vectorsCoord[vecId.index]; #if !defined(NDEBUG) const typename MechanicalObject::VecCoord& val = d->getValue(); if (!val.empty() && val.size() != (unsigned int)this->getSize()) { - msg_error() << "Writing to State vector " << v << " with incorrect size : " << val.size() << " != " << this->getSize(); + msg_error() << "Writing to State vector " << vecId << " with incorrect size : " << val.size() << " != " << this->getSize(); } #endif return d; @@ -1372,85 +1421,85 @@ Data::VecCoord>* MechanicalObject -const Data::VecCoord>* MechanicalObject::read(core::ConstVecCoordId v) const +const Data::VecCoord>* MechanicalObject::read(core::ConstVecCoordId vecId) const { - if (v.isNull()) + if (vecId.isNull()) { msg_error() << "Accessing null VecCoord"; } - if (v.index < vectorsCoord.size() && vectorsCoord[v.index] != nullptr) + if (vecId.index < vectorsCoord.size() && vectorsCoord[vecId.index] != nullptr) { - const Data::VecCoord>* d = vectorsCoord[v.index]; + const Data::VecCoord>* d = vectorsCoord[vecId.index]; #if !defined(NDEBUG) const typename MechanicalObject::VecCoord& val = d->getValue(); if (!val.empty() && val.size() != (unsigned int)this->getSize()) { - msg_error() << "Accessing State vector " << v << " with incorrect size : " << val.size() << " != " << this->getSize(); + msg_error() << "Accessing State vector " << vecId << " with incorrect size : " << val.size() << " != " << this->getSize(); } #endif return d; } else { - msg_error() << "Vector " << v << " does not exist"; + msg_error() << "Vector " << vecId << " does not exist"; return nullptr; } } template -Data::VecDeriv>* MechanicalObject::write(core::VecDerivId v) +Data::VecDeriv>* MechanicalObject::write(core::VecDerivId vecId) { - if (v.index >= vectorsDeriv.size()) + if (vecId.index >= vectorsDeriv.size()) { - vectorsDeriv.resize(v.index + 1, 0); + vectorsDeriv.resize(vecId.index + 1, 0); } - if (vectorsDeriv[v.index] == nullptr) + if (vectorsDeriv[vecId.index] == nullptr) { - vectorsDeriv[v.index] = new Data< VecDeriv >; - vectorsDeriv[v.index]->setName(v.getName()); - const auto group = v.getGroup(); + vectorsDeriv[vecId.index] = new Data< VecDeriv >; + vectorsDeriv[vecId.index]->setName(vecId.getName()); + const auto group = vecId.getGroup(); if (!group.empty()) { - vectorsDeriv[v.index]->setGroup(group); + vectorsDeriv[vecId.index]->setGroup(group); } else { - vectorsDeriv[v.index]->setGroup("Vector"); + vectorsDeriv[vecId.index]->setGroup("Vector"); } - this->addData(vectorsDeriv[v.index]); + this->addData(vectorsDeriv[vecId.index]); if (f_reserve.getValue() > 0) { - vectorsDeriv[v.index]->beginWriteOnly()->reserve(f_reserve.getValue()); - vectorsDeriv[v.index]->endEdit(); + vectorsDeriv[vecId.index]->beginWriteOnly()->reserve(f_reserve.getValue()); + vectorsDeriv[vecId.index]->endEdit(); } - if (vectorsDeriv[v.index]->getValue().size() != getSize()) + if (vectorsDeriv[vecId.index]->getValue().size() != getSize()) { - vectorsDeriv[v.index]->beginWriteOnly()->resize( getSize() ); - vectorsDeriv[v.index]->endEdit(); + vectorsDeriv[vecId.index]->beginWriteOnly()->resize( getSize() ); + vectorsDeriv[vecId.index]->endEdit(); } } - Data::VecDeriv>* d = vectorsDeriv[v.index]; + Data::VecDeriv>* d = vectorsDeriv[vecId.index]; #if !defined(NDEBUG) const typename MechanicalObject::VecDeriv& val = d->getValue(); if (!val.empty() && val.size() != (unsigned int)this->getSize()) { - msg_error() << "Writing to State vector " << v << " with incorrect size : " << val.size() << " != " << this->getSize(); + msg_error() << "Writing to State vector " << vecId << " with incorrect size : " << val.size() << " != " << this->getSize(); } #endif return d; } template -const Data::VecDeriv>* MechanicalObject::read(core::ConstVecDerivId v) const +const Data::VecDeriv>* MechanicalObject::read(core::ConstVecDerivId vecId) const { - if (v.index < vectorsDeriv.size()) + if (vecId.index < vectorsDeriv.size()) { - const Data::VecDeriv>* d = vectorsDeriv[v.index]; + const Data::VecDeriv>* d = vectorsDeriv[vecId.index]; #if !defined(NDEBUG) if(d!=NULL) @@ -1458,7 +1507,7 @@ const Data::VecDeriv>* MechanicalObject::VecDeriv& val = d->getValue(); if (!val.empty() && val.size() != (unsigned int)this->getSize()) { - msg_error() << "Accessing State vector " << v << " with incorrect size : " << val.size() << " != " << this->getSize(); + msg_error() << "Accessing State vector " << vecId << " with incorrect size : " << val.size() << " != " << this->getSize(); } } #endif // !defined(NDEBUG) @@ -1467,54 +1516,54 @@ const Data::VecDeriv>* MechanicalObject -Data::MatrixDeriv>* MechanicalObject::write(core::MatrixDerivId v) +Data::MatrixDeriv>* MechanicalObject::write(core::MatrixDerivId vecId) { - if (v.index >= vectorsMatrixDeriv.size()) + if (vecId.index >= vectorsMatrixDeriv.size()) { - vectorsMatrixDeriv.resize(v.index + 1, 0); + vectorsMatrixDeriv.resize(vecId.index + 1, 0); } - if (vectorsMatrixDeriv[v.index] == nullptr) + if (vectorsMatrixDeriv[vecId.index] == nullptr) { - vectorsMatrixDeriv[v.index] = new Data< MatrixDeriv >; - vectorsMatrixDeriv[v.index]->setName(v.getName()); - const auto group = v.getGroup(); + vectorsMatrixDeriv[vecId.index] = new Data< MatrixDeriv >; + vectorsMatrixDeriv[vecId.index]->setName(vecId.getName()); + const auto group = vecId.getGroup(); if (!group.empty()) { - vectorsMatrixDeriv[v.index]->setGroup(group); + vectorsMatrixDeriv[vecId.index]->setGroup(group); } else { - vectorsMatrixDeriv[v.index]->setGroup("Vector"); + vectorsMatrixDeriv[vecId.index]->setGroup("Vector"); } - this->addData(vectorsMatrixDeriv[v.index]); + this->addData(vectorsMatrixDeriv[vecId.index]); } - return vectorsMatrixDeriv[v.index]; + return vectorsMatrixDeriv[vecId.index]; } template -const Data::MatrixDeriv>* MechanicalObject::read(core::ConstMatrixDerivId v) const +const Data::MatrixDeriv>* MechanicalObject::read(core::ConstMatrixDerivId vecId) const { - if (v.index < vectorsMatrixDeriv.size()) - return vectorsMatrixDeriv[v.index]; + if (vecId.index < vectorsMatrixDeriv.size()) + return vectorsMatrixDeriv[vecId.index]; else { - msg_error() << "Vector " << v << "does not exist"; + msg_error() << "Vector " << vecId << "does not exist"; return nullptr; } } template -void MechanicalObject::setVecCoord(core::ConstVecCoordId vecId, Data< VecCoord > *v) +void MechanicalObject::setVecCoord(core::ConstVecCoordId vecId, Data< VecCoord > *vecData) { const auto index = vecId.getIndex(); if (index >= vectorsCoord.size()) @@ -1522,17 +1571,17 @@ void MechanicalObject::setVecCoord(core::ConstVecCoordId vecId, Data< vectorsCoord.resize(index + 1, 0); } - vectorsCoord[index] = v; + vectorsCoord[index] = vecData; const auto group = vecId.getGroup(); if (!group.empty()) { - v->setGroup(group); + vecData->setGroup(group); } } template -void MechanicalObject::setVecDeriv(core::ConstVecDerivId vecId, Data< VecDeriv > *v) +void MechanicalObject::setVecDeriv(core::ConstVecDerivId vecId, Data< VecDeriv > *vecData) { const auto index = vecId.getIndex(); if (index >= vectorsDeriv.size()) @@ -1540,18 +1589,18 @@ void MechanicalObject::setVecDeriv(core::ConstVecDerivId vecId, Data< vectorsDeriv.resize(index + 1, 0); } - vectorsDeriv[index] = v; + vectorsDeriv[index] = vecData; const auto group = vecId.getGroup(); if (!group.empty()) { - v->setGroup(group); + vecData->setGroup(group); } } template -void MechanicalObject::setVecMatrixDeriv(core::ConstMatrixDerivId vecId, Data < MatrixDeriv > *m) +void MechanicalObject::setVecMatrixDeriv(core::ConstMatrixDerivId vecId, Data < MatrixDeriv > *vecData) { const auto index = vecId.getIndex(); if (index >= vectorsMatrixDeriv.size()) @@ -1559,12 +1608,12 @@ void MechanicalObject::setVecMatrixDeriv(core::ConstMatrixDerivId vec vectorsMatrixDeriv.resize(index + 1, 0); } - vectorsMatrixDeriv[index] = m; + vectorsMatrixDeriv[index] = vecData; const auto group = vecId.getGroup(); if (!group.empty()) { - m->setGroup(group); + vecData->setGroup(group); } } @@ -1583,17 +1632,17 @@ void MechanicalObject::vAvailImpl(core::TVecId& } template -void MechanicalObject::vAvail(const core::ExecParams* params, core::VecCoordId& v) +void MechanicalObject::vAvail(const core::ExecParams* params, core::VecCoordId& vecId) { SOFA_UNUSED(params); - vAvailImpl(v, vectorsCoord); + vAvailImpl(vecId, vectorsCoord); } template -void MechanicalObject::vAvail(const core::ExecParams* params, core::VecDerivId& v) +void MechanicalObject::vAvail(const core::ExecParams* params, core::VecDerivId& vecId) { SOFA_UNUSED(params); - vAvailImpl(v, vectorsDeriv); + vAvailImpl(vecId, vectorsDeriv); } template @@ -1615,17 +1664,17 @@ void MechanicalObject::vAllocImpl( } template -void MechanicalObject::vAlloc(const core::ExecParams* params, core::VecCoordId v, const core::VecIdProperties& properties) +void MechanicalObject::vAlloc(const core::ExecParams* params, core::VecCoordId vecId, const core::VecIdProperties& properties) { SOFA_UNUSED(params); - vAllocImpl(v, properties); + vAllocImpl(vecId, properties); } template -void MechanicalObject::vAlloc(const core::ExecParams* params, core::VecDerivId v, const core::VecIdProperties& properties) +void MechanicalObject::vAlloc(const core::ExecParams* params, core::VecDerivId vecId, const core::VecIdProperties& properties) { SOFA_UNUSED(params); - vAllocImpl(v, properties); + vAllocImpl(vecId, properties); } template @@ -1646,17 +1695,17 @@ void MechanicalObject::vReallocImpl( } template -void MechanicalObject::vRealloc(const core::ExecParams* params, core::VecCoordId v, const core::VecIdProperties& properties) +void MechanicalObject::vRealloc(const core::ExecParams* params, core::VecCoordId vecId, const core::VecIdProperties& properties) { SOFA_UNUSED(params); - vReallocImpl(v, properties); + vReallocImpl(vecId, properties); } template -void MechanicalObject::vRealloc(const core::ExecParams* params, core::VecDerivId v, const core::VecIdProperties& properties) +void MechanicalObject::vRealloc(const core::ExecParams* params, core::VecDerivId vecId, const core::VecIdProperties& properties) { SOFA_UNUSED(params); - vReallocImpl(v, properties); + vReallocImpl(vecId, properties); } template @@ -1788,11 +1837,11 @@ DataTypes>::getReadAccessor(core::ConstVecId v) bool canApplyPredicate = false;\ if (v.type == core::V_COORD)\ {\ - canApplyPredicate = PRED(*this, v, f);\ + canApplyPredicate = PRED(*this, v, k);\ }\ else if (v.type == core::V_DERIV)\ {\ - canApplyPredicate = PRED(*this, v, f);\ + canApplyPredicate = PRED(*this, v, k);\ } #define APPLY_PREDICATE_TWOPARAMS(PRED, v, b)\ @@ -1801,22 +1850,22 @@ DataTypes>::getReadAccessor(core::ConstVecId v) {\ if (b.type == core::V_COORD)\ {\ - canApplyPredicate = PRED(*this, v, b, f);\ + canApplyPredicate = PRED(*this, v, b, k);\ }\ else if (b.type == core::V_DERIV)\ {\ - canApplyPredicate = PRED(*this, v, b, f);\ + canApplyPredicate = PRED(*this, v, b, k);\ }\ }\ else if (v.type == core::V_DERIV)\ {\ if (b.type == core::V_COORD)\ {\ - canApplyPredicate = PRED(*this, v, b, f);\ + canApplyPredicate = PRED(*this, v, b, k);\ }\ else if (b.type == core::V_DERIV)\ {\ - canApplyPredicate = PRED(*this, v, b, f);\ + canApplyPredicate = PRED(*this, v, b, k);\ }\ } @@ -1828,22 +1877,22 @@ DataTypes>::getReadAccessor(core::ConstVecId v) {\ if (b.type == core::V_COORD)\ {\ - canApplyPredicate = PRED(*this, v, a, b, f);\ + canApplyPredicate = PRED(*this, v, a, b, k);\ }\ else if (b.type == core::V_DERIV)\ {\ - canApplyPredicate = PRED(*this, v, a, b, f);\ + canApplyPredicate = PRED(*this, v, a, b, k);\ }\ }\ else if (a.type == core::V_DERIV)\ {\ if (b.type == core::V_COORD)\ {\ - canApplyPredicate = PRED(*this, v, a, b, f);\ + canApplyPredicate = PRED(*this, v, a, b, k);\ }\ else if (b.type == core::V_DERIV)\ {\ - canApplyPredicate = PRED(*this, v, a, b, f);\ + canApplyPredicate = PRED(*this, v, a, b, k);\ }\ }\ }\ @@ -1853,35 +1902,35 @@ DataTypes>::getReadAccessor(core::ConstVecId v) {\ if (b.type == core::V_COORD)\ {\ - canApplyPredicate = PRED(*this, v, a, b, f);\ + canApplyPredicate = PRED(*this, v, a, b, k);\ }\ else if (b.type == core::V_DERIV)\ {\ - canApplyPredicate = PRED(*this, v, a, b, f);\ + canApplyPredicate = PRED(*this, v, a, b, k);\ }\ }\ else if (a.type == core::V_DERIV)\ {\ if (b.type == core::V_COORD)\ {\ - canApplyPredicate = PRED(*this, v, a, b, f);\ + canApplyPredicate = PRED(*this, v, a, b, k);\ }\ else if (b.type == core::V_DERIV)\ {\ - canApplyPredicate = PRED(*this, v, a, b, f);\ + canApplyPredicate = PRED(*this, v, a, b, k);\ }\ }\ } template -bool vOp_vf(MechanicalObject& self, core::VecId v, Real_t f) +bool vOp_vf(MechanicalObject& self, core::VecId v, Real_t k) { if constexpr (requires(core::StateVecType_t vc, Real_t fc){vc[0] *= fc;}) { auto vv = helper::getWriteOnlyAccessor(*self.write(core::TVecId(v))); for (unsigned int i = 0; i < vv.size(); ++i) { - vv[i] *= f; + vv[i] *= k; } return true; } @@ -1889,7 +1938,7 @@ bool vOp_vf(MechanicalObject& self, core::VecId v, Real_t } template -bool vOp_vbf(MechanicalObject& self, core::VecId v, core::ConstVecId b, Real_t f) +bool vOp_vbf(MechanicalObject& self, core::VecId v, core::ConstVecId b, Real_t k) { if constexpr (requires(core::StateVecType_t vc, core::StateVecType_t bc, Real_t fc){vc[0] = bc[0] * fc;}) { @@ -1898,7 +1947,7 @@ bool vOp_vbf(MechanicalObject& self, core::VecId v, core::ConstVecId vv.resize(vb.size()); for (unsigned int i = 0; i < vv.size(); ++i) { - vv[i] = vb[i] * f; + vv[i] = vb[i] * k; } return true; } @@ -1906,9 +1955,9 @@ bool vOp_vbf(MechanicalObject& self, core::VecId v, core::ConstVecId } template -bool vOp_va(MechanicalObject& self, core::VecId v, core::ConstVecId b, Real_t f) +bool vOp_va(MechanicalObject& self, core::VecId v, core::ConstVecId b, Real_t k) { - SOFA_UNUSED(f); + SOFA_UNUSED(k); if constexpr (requires(core::StateVecType_t vc, core::StateVecType_t bc){vc[0] = bc[0];}) { auto vv = helper::getWriteOnlyAccessor(*self.write(core::TVecId(v))); @@ -1920,9 +1969,9 @@ bool vOp_va(MechanicalObject& self, core::VecId v, core::ConstVecId b } template -bool vOp_vb(MechanicalObject& self, core::VecId v, core::ConstVecId b, Real_t f) +bool vOp_vb(MechanicalObject& self, core::VecId v, core::ConstVecId b, Real_t k) { - SOFA_UNUSED(f); + SOFA_UNUSED(k); if constexpr (requires(core::StateVecType_t vc, core::StateVecType_t bc){vc[0] += bc[0];}) { auto vv = helper::getWriteOnlyAccessor(*self.write(core::TVecId(v))); @@ -1938,16 +1987,16 @@ bool vOp_vb(MechanicalObject& self, core::VecId v, core::ConstVecId b } template -bool vOp_avf(MechanicalObject& self, core::VecId v, core::ConstVecId b, Real_t f) +bool vOp_avf(MechanicalObject& self, core::VecId v, core::ConstVecId b, Real_t k) { - if constexpr (requires(core::StateVecType_t vc, core::StateVecType_t bc){vc[0] *= f; vc[0] += bc[0];}) + if constexpr (requires(core::StateVecType_t vc, core::StateVecType_t bc){vc[0] *= k; vc[0] += bc[0];}) { auto vv = helper::getWriteOnlyAccessor(*self.write(core::TVecId(v))); auto vb = helper::getReadAccessor(*self.read(core::TVecId(b))); vv.resize(vb.size()); for (unsigned int i = 0; i < vv.size(); ++i) { - vv[i] *= f; + vv[i] *= k; vv[i] += vb[i]; } return true; @@ -1956,7 +2005,7 @@ bool vOp_avf(MechanicalObject& self, core::VecId v, core::ConstVecId } template -bool vOp_v_inc_bf(MechanicalObject& self, core::VecId v, core::ConstVecId b, Real_t f) +bool vOp_v_inc_bf(MechanicalObject& self, core::VecId v, core::ConstVecId b, Real_t k) { if constexpr (requires(core::StateVecType_t vc, core::StateVecType_t bc, Real_t fc){vc[0] += bc[0] * fc;}) { @@ -1965,7 +2014,7 @@ bool vOp_v_inc_bf(MechanicalObject& self, core::VecId v, core::ConstV vv.resize(vb.size()); for (unsigned int i = 0; i < vv.size(); ++i) { - vv[i] += vb[i] * f; + vv[i] += vb[i] * k; } return true; } @@ -1973,9 +2022,9 @@ bool vOp_v_inc_bf(MechanicalObject& self, core::VecId v, core::ConstV } template -bool vOp_vab(MechanicalObject& self, core::VecId v, core::ConstVecId a, core::ConstVecId b, Real_t f) +bool vOp_vab(MechanicalObject& self, core::VecId v, core::ConstVecId a, core::ConstVecId b, Real_t k) { - SOFA_UNUSED(f); + SOFA_UNUSED(k); if constexpr (requires(core::StateVecType_t vc, core::StateVecType_t ac, core::StateVecType_t bc){vc[0] = ac[0] + bc[0];}) { @@ -1993,7 +2042,7 @@ bool vOp_vab(MechanicalObject& self, core::VecId v, core::ConstVecId } template -bool vOp_vabf(MechanicalObject& self, core::VecId v, core::ConstVecId a, core::ConstVecId b, Real_t f) +bool vOp_vabf(MechanicalObject& self, core::VecId v, core::ConstVecId a, core::ConstVecId b, Real_t k) { if constexpr (requires(core::StateVecType_t vc, core::StateVecType_t ac, core::StateVecType_t bc, Real_t fc){vc[0] = ac[0] + bc[0] * fc;}) { @@ -2003,7 +2052,7 @@ bool vOp_vabf(MechanicalObject& self, core::VecId v, core::ConstVecId vv.resize(vb.size()); for (unsigned int i = 0; i < vv.size(); ++i) { - vv[i] = va[i] + vb[i] * f; + vv[i] = va[i] + vb[i] * k; } return true; } @@ -2011,16 +2060,15 @@ bool vOp_vabf(MechanicalObject& self, core::VecId v, core::ConstVecId } template -void MechanicalObject::vOp(const core::ExecParams* params, core::VecId v, - core::ConstVecId a, - core::ConstVecId b, SReal f) +void MechanicalObject::vOp(const core::ExecParams* params, core::VecId r, + core::ConstVecId a, core::ConstVecId b, SReal k) { SOFA_UNUSED(params); - if(v.isNull()) + if(r.isNull()) { // ERROR - msg_error() << "Invalid vOp operation 1 ("<::vOp(const core::ExecParams* params, core::VecI { if (b.isNull()) { - // v = 0 - applyPredicateIfCoordOrDeriv(v.type, [this, &v](auto vec) + // r = 0 + applyPredicateIfCoordOrDeriv(r.type, [this, &r](auto vec) { - auto vv = getWriteOnlyAccessor(v); + auto vv = getWriteOnlyAccessor(r); vv.resize(d_size.getValue()); sofa::defaulttype::resetDataTypeVec(vv.wref()); }); } else { - if (v == b) + if (r == b) { - // v *= f - APPLY_PREDICATE_ONEPARAMS(vOp_vf, v) - msg_error_when(!canApplyPredicate) << "Cannot apply vector operation v *= f (" << v << ',' << a << ',' << b << ',' << f << ")"; + // r *= k + APPLY_PREDICATE_ONEPARAMS(vOp_vf, r) + msg_error_when(!canApplyPredicate) << "Cannot apply vector operation r *= k (" << r << ',' << a << ',' << b << ',' << k << ")"; } else { - // v = b*f - APPLY_PREDICATE_TWOPARAMS(vOp_vbf, v, b) - msg_error_when(!canApplyPredicate) << "Cannot apply vector operation v = b*f (" << v << ',' << a << ',' << b << ',' << f << ")"; + // r = b*k + APPLY_PREDICATE_TWOPARAMS(vOp_vbf, r, b) + msg_error_when(!canApplyPredicate) << "Cannot apply vector operation r = b*k (" << r << ',' << a << ',' << b << ',' << k << ")"; } } } @@ -2056,66 +2104,66 @@ void MechanicalObject::vOp(const core::ExecParams* params, core::VecI { if (b.isNull()) { - // v = a - APPLY_PREDICATE_TWOPARAMS(vOp_va, v, a) - msg_error_when(!canApplyPredicate) << "Cannot apply vector operation v = a (" << v << ',' << a << ',' << b << ',' << f << ")"; + // r = a + APPLY_PREDICATE_TWOPARAMS(vOp_va, r, a) + msg_error_when(!canApplyPredicate) << "Cannot apply vector operation r = a (" << r << ',' << a << ',' << b << ',' << k << ")"; } else { - if (v == a) + if (r == a) { - if (f == 1._sreal) + if (k == 1._sreal) { - // v += b - APPLY_PREDICATE_TWOPARAMS(vOp_vb, v, b) - msg_error_when(!canApplyPredicate) << "Cannot apply vector operation v += b (" << v << ',' << a << ',' << b << ',' << f << ")"; + // r += b + APPLY_PREDICATE_TWOPARAMS(vOp_vb, r, b) + msg_error_when(!canApplyPredicate) << "Cannot apply vector operation r += b (" << r << ',' << a << ',' << b << ',' << k << ")"; } else { - // v += b*f - APPLY_PREDICATE_TWOPARAMS(vOp_v_inc_bf, v, b) - msg_error_when(!canApplyPredicate) << "Cannot apply vector operation v += b*f (" << v << ',' << a << ',' << b << ',' << f << ")"; + // r += b*k + APPLY_PREDICATE_TWOPARAMS(vOp_v_inc_bf, r, b) + msg_error_when(!canApplyPredicate) << "Cannot apply vector operation r += b*k (" << r << ',' << a << ',' << b << ',' << k << ")"; } } - else if (v == b) + else if (r == b) { - if (f == 1._sreal) + if (k == 1._sreal) { - // v += a - APPLY_PREDICATE_TWOPARAMS(vOp_vb, v, a) - msg_error_when(!canApplyPredicate) << "Cannot apply vector operation v += a (" << v << ',' << a << ',' << b << ',' << f << ")"; + // r += a + APPLY_PREDICATE_TWOPARAMS(vOp_vb, r, a) + msg_error_when(!canApplyPredicate) << "Cannot apply vector operation r += a (" << r << ',' << a << ',' << b << ',' << k << ")"; } else { - // v = a+v*f - APPLY_PREDICATE_TWOPARAMS(vOp_avf, v, a) - msg_error_when(!canApplyPredicate) << "Cannot apply vector operation v = a+v*f (" << v << ',' << a << ',' << b << ',' << f << ")"; + // r = a+r*k + APPLY_PREDICATE_TWOPARAMS(vOp_avf, r, a) + msg_error_when(!canApplyPredicate) << "Cannot apply vector operation r = a+r*k (" << r << ',' << a << ',' << b << ',' << k << ")"; } } else { - if (f == 1._sreal) + if (k == 1._sreal) { - // v = a+b - APPLY_PREDICATE_THREEPARAMS(vOp_vab, v, a, b) - msg_error_when(!canApplyPredicate) << "Cannot apply vector operation v = a+b (" << v << ',' << a << ',' << b << ',' << f << ")"; + // r = a+b + APPLY_PREDICATE_THREEPARAMS(vOp_vab, r, a, b) + msg_error_when(!canApplyPredicate) << "Cannot apply vector operation r = a+b (" << r << ',' << a << ',' << b << ',' << k << ")"; } else { const auto generalCase = [&]() { - // v = a+b*f - APPLY_PREDICATE_THREEPARAMS(vOp_vabf, v, a, b) - msg_error_when(!canApplyPredicate) << "Cannot apply vector operation v = a+b*f (" << v << ',' << a << ',' << b << ',' << f << ")"; + // r = a+b*k + APPLY_PREDICATE_THREEPARAMS(vOp_vabf, r, a, b) + msg_error_when(!canApplyPredicate) << "Cannot apply vector operation r = a+b*k (" << r << ',' << a << ',' << b << ',' << k << ")"; }; if constexpr (requires (Coord ca, Coord cb) {DataTypes::coordDifference(ca, cb);}) { - if (f == -1._sreal && - (v.type == core::V_DERIV && a.type == core::V_COORD && b.type == core::V_COORD)) + if (k == -1._sreal && + (r.type == core::V_DERIV && a.type == core::V_COORD && b.type == core::V_COORD)) { - // v = a-b - auto vv = this->getWriteOnlyAccessor(v); + // r = a-b + auto vv = this->getWriteOnlyAccessor(r); auto va = this->getReadAccessor(a); auto vb = this->getReadAccessor(b); vv.resize(vb.size()); @@ -2355,34 +2403,34 @@ SReal MechanicalObject::vMax(const core::ExecParams* params, core::Co } template -Size MechanicalObject::vSize(const core::ExecParams* params, core::ConstVecId v) +Size MechanicalObject::vSize(const core::ExecParams* params, core::ConstVecId vecId) { SOFA_UNUSED(params); Size size = 0; - const bool isApplied = applyPredicateIfCoordOrDeriv(v.type, [this, &size, &v](auto vtype) + const bool isApplied = applyPredicateIfCoordOrDeriv(vecId.type, [this, &size, &vecId](auto vtype) { - auto va = this->getReadAccessor(v); + auto va = this->getReadAccessor(vecId); size = va.size() * core::StateTypeSize_v; }); - msg_error_when(!isApplied) << "Invalid size operation (" << v << ")"; + msg_error_when(!isApplied) << "Invalid size operation (" << vecId << ")"; return size; } template -void MechanicalObject::printDOF( core::ConstVecId v, std::ostream& out, int firstIndex, int range) const +void MechanicalObject::printDOF( core::ConstVecId vecId, std::ostream& out, int firstIndex, int range) const { const unsigned int size=this->getSize(); if ((unsigned int) (abs(firstIndex)) >= size) return; const unsigned int first=((firstIndex>=0)?firstIndex:size+firstIndex); const unsigned int max=( ( (range >= 0) && ( (range+first)read(core::TVecId(v)); + const auto* d_x = this->read(core::TVecId(vecId)); if (d_x == nullptr) return; auto xa = sofa::helper::getReadAccessor(*d_x); @@ -2400,18 +2448,18 @@ void MechanicalObject::printDOF( core::ConstVecId v, std::ostream& ou if (!isApplied) { - out<<"MechanicalObject::printDOF, unknown v.type = "<::printDOF, unknown v.type = "< -unsigned MechanicalObject::printDOFWithElapsedTime(core::ConstVecId v, unsigned count, unsigned time, std::ostream& out) +unsigned MechanicalObject::printDOFWithElapsedTime(core::ConstVecId vecId, unsigned count, unsigned time, std::ostream& out) { unsigned xSize {}; - const bool isApplied = applyPredicateIfCoordOrDeriv(v.type, [&](auto vtype) + const bool isApplied = applyPredicateIfCoordOrDeriv(vecId.type, [&](auto vtype) { - const auto* d_x = this->read(core::TVecId(v)); + const auto* d_x = this->read(core::TVecId(vecId)); if (d_x == nullptr) return; auto xa = sofa::helper::getReadAccessor(*d_x); @@ -2426,7 +2474,7 @@ unsigned MechanicalObject::printDOFWithElapsedTime(core::ConstVecId v }); if (!isApplied) - out << "MechanicalObject::printDOFWithElapsedTime, unknown v.type = " << v.type << std::endl; + out << "MechanicalObject::printDOFWithElapsedTime, unknown v.type = " << vecId.type << std::endl; return xSize; } @@ -2437,8 +2485,8 @@ void MechanicalObject::resetForce(const core::ExecParams* params, cor SOFA_UNUSED(params); { - helper::WriteOnlyAccessor< Data > f( *this->write(fid) ); - sofa::defaulttype::resetDataTypeVec(f.wref()); + helper::WriteOnlyAccessor< Data > f_wa( *this->write(fid) ); + sofa::defaulttype::resetDataTypeVec(f_wa.wref()); } } @@ -2461,8 +2509,8 @@ void MechanicalObject::resetConstraint(const core::ConstraintParams* sofa::helper::getWriteOnlyAccessor(c_data)->clear(); //reset the mapping jacobian matrix - Data& m_data = *this->write(core::vec_id::write_access::mappingJacobian); - sofa::helper::getWriteOnlyAccessor(m_data)->clear(); + Data& matrix = *this->write(core::vec_id::write_access::mappingJacobian); + sofa::helper::getWriteOnlyAccessor(matrix)->clear(); } template @@ -2470,11 +2518,11 @@ void MechanicalObject::getConstraintJacobian(const core::ConstraintPa { // Compute J const auto N = Deriv::size(); - const MatrixDeriv& c = cParams->readJ(this)->getValue(); + const MatrixDeriv& matrix = cParams->readJ(this)->getValue(); - MatrixDerivRowConstIterator rowItEnd = c.end(); + MatrixDerivRowConstIterator rowItEnd = matrix.end(); - for (MatrixDerivRowConstIterator rowIt = c.begin(); rowIt != rowItEnd; ++rowIt) + for (MatrixDerivRowConstIterator rowIt = matrix.begin(); rowIt != rowItEnd; ++rowIt) { const int cid = rowIt.index(); @@ -2598,7 +2646,7 @@ SReal MechanicalObject::getConstraintJacobianTimesVecDeriv(unsigned i if (rowIterator == constraints.end()) return 0; - const VecDeriv *data = 0; + const VecDeriv *data = nullptr; // Maybe we should extend this to restvelocity if (id == sofa::core::vec_id::read_access::velocity) @@ -2627,6 +2675,13 @@ SReal MechanicalObject::getConstraintJacobianTimesVecDeriv(unsigned i return result; } +template +void MechanicalObject::setTranslation(SReal translationVectorX, SReal translationVectorY, + SReal translationVectorZ) +{ + translation.setValue(type::Vec3(translationVectorX, translationVectorY, translationVectorZ)); +} + template inline void MechanicalObject::drawIndices(const core::visual::VisualParams* vparams) { @@ -2641,7 +2696,7 @@ inline void MechanicalObject::drawIndices(const core::visual::VisualP template inline void MechanicalObject::drawVectors(const core::visual::VisualParams* vparams) { - float scale = showVectorsScale.getValue(); + float scaleValue = showVectorsScale.getValue(); sofa::helper::ReadAccessor< Data > v_rA = *this->read(core::vec_id::read_access::velocity); type::vector points; points.resize(2); @@ -2650,7 +2705,7 @@ inline void MechanicalObject::drawVectors(const core::visual::VisualP Real vx=0.0,vy=0.0,vz=0.0; DataTypes::get(vx,vy,vz,v_rA[i]); type::Vec3 p1 = type::Vec3(getPX(i), getPY(i), getPZ(i)); - type::Vec3 p2 = type::Vec3(getPX(i)+scale*vx, getPY(i)+scale*vy, getPZ(i)+scale*vz); + type::Vec3 p2 = type::Vec3(getPX(i)+scaleValue*vx, getPY(i)+scaleValue*vy, getPZ(i)+scaleValue*vz); const float rad = (float)( (p1-p2).norm()/20.0 ); switch (drawMode.getValue()) @@ -2691,7 +2746,7 @@ inline void MechanicalObject::draw(const core::visual::VisualParams* if (showObject.getValue()) { - const float& scale = showObjectScale.getValue(); + const float& scaleValue = showObjectScale.getValue(); type::vector positions(d_size.getValue()); for (sofa::Index i = 0; i < Size(d_size.getValue()); ++i) positions[i] = type::Vec3(getPX(i), getPY(i), getPZ(i)); @@ -2699,23 +2754,23 @@ inline void MechanicalObject::draw(const core::visual::VisualParams* switch (drawMode.getValue()) { case 0: - vparams->drawTool()->drawPoints(positions,scale, d_color.getValue()); + vparams->drawTool()->drawPoints(positions,scaleValue, d_color.getValue()); break; case 1: vparams->drawTool()->setLightingEnabled(true); - vparams->drawTool()->drawSpheres(positions,scale, d_color.getValue()); + vparams->drawTool()->drawSpheres(positions,scaleValue, d_color.getValue()); break; case 2: vparams->drawTool()->setLightingEnabled(true); - vparams->drawTool()->drawSpheres(positions,scale, sofa::type::RGBAColor::red()); + vparams->drawTool()->drawSpheres(positions,scaleValue, sofa::type::RGBAColor::red()); break; case 3: vparams->drawTool()->setLightingEnabled(true); - vparams->drawTool()->drawSpheres(positions,scale, sofa::type::RGBAColor::green()); + vparams->drawTool()->drawSpheres(positions,scaleValue, sofa::type::RGBAColor::green()); break; case 4: vparams->drawTool()->setLightingEnabled(true); - vparams->drawTool()->drawSpheres(positions,scale, sofa::type::RGBAColor::blue()); + vparams->drawTool()->drawSpheres(positions,scaleValue, sofa::type::RGBAColor::blue()); break; default: msg_error() << "No proper drawing mode found!"; @@ -2738,14 +2793,14 @@ bool MechanicalObject::pickParticles(const core::ExecParams* /* param // TODO: this verification is awful and should be done by template specialization { // seems to be valid DOFs - const VecCoord& x =this->read(core::vec_id::read_access::position)->getValue(); + const VecCoord& position = this->read(core::vec_id::read_access::position)->getValue(); type::Vec<3,Real> origin((Real)rayOx, (Real)rayOy, (Real)rayOz); type::Vec<3,Real> direction((Real)rayDx, (Real)rayDy, (Real)rayDz); for (int i=0; i< d_size.getValue(); ++i) { - type::Vec<3,Real> pos; - DataTypes::get(pos[0],pos[1],pos[2],x[i]); + type::Vec<3, Real> pos; + DataTypes::get(pos[0], pos[1], pos[2], position[i]); if (pos == origin) continue; SReal dist = (pos-origin)*direction; @@ -2774,16 +2829,15 @@ bool MechanicalObject::addBBox(SReal* minBBox, SReal* maxBBox) static const unsigned spatial_dimensions = std::min( (unsigned)DataTypes::spatial_dimensions, 3u ); - const VecCoord& x = read(core::vec_id::read_access::position)->getValue(); - for(Size i=0; igetValue()) { type::Vec<3,Real> p; - DataTypes::get( p[0], p[1], p[2], x[i] ); + DataTypes::get( p[0], p[1], p[2], position ); - for( unsigned int j=0 ; jmaxBBox[j]) maxBBox[j]=p[j]; + if (p[j] < minBBox[j]) minBBox[j] = p[j]; + if (p[j] > maxBBox[j]) maxBBox[j] = p[j]; } } return true; diff --git a/Sofa/framework/Core/src/sofa/core/behavior/BaseMechanicalState.h b/Sofa/framework/Core/src/sofa/core/behavior/BaseMechanicalState.h index 462bb36afc9..1870ddb8b9c 100644 --- a/Sofa/framework/Core/src/sofa/core/behavior/BaseMechanicalState.h +++ b/Sofa/framework/Core/src/sofa/core/behavior/BaseMechanicalState.h @@ -118,17 +118,15 @@ class SOFA_CORE_API BaseMechanicalState : public virtual BaseState /// Initialize an unset vector //virtual void vInit(const ExecParams* params, MatrixDerivId v, ConstMatrixDerivId vSrc) = 0; - /// \brief Compute a linear operation on vectors : v = a + b * f. + /// \brief Compute a linear operation on vectors : r = a + b * k. /// /// This generic operation can be used for many simpler cases : - /// \li v = 0 - /// \li v = a - /// \li v = a + b - /// \li v = b * f - virtual void vOp(const ExecParams* params, VecId v, - ConstVecId a = ConstVecId::null(), - ConstVecId b = ConstVecId::null(), - SReal f = 1.0 ) = 0; + /// \li r = 0 + /// \li r = a + /// \li r = a + b + /// \li r = b * k + virtual void vOp(const ExecParams* params, VecId r, ConstVecId a = ConstVecId::null(), + ConstVecId b = ConstVecId::null(), SReal k = 1.0) = 0; /// Data structure describing a set of linear operation on vectors /// \see vMultiOp @@ -269,7 +267,7 @@ class SOFA_CORE_API BaseMechanicalState : public virtual BaseState virtual Size getDerivDimension() const { return 0; } /// Translate the current state - virtual void applyTranslation(const SReal dx, const SReal dy, const SReal dz)=0; + virtual void applyTranslation(const SReal translationVectorX, const SReal translationVectorY, const SReal translationVectorZ) =0; /// \brief Rotate the current state /// diff --git a/applications/plugins/SofaCUDA/Component/src/SofaCUDA/component/statecontainer/CudaMechanicalObject.inl b/applications/plugins/SofaCUDA/Component/src/SofaCUDA/component/statecontainer/CudaMechanicalObject.inl index de1d4428dca..6728cc405aa 100644 --- a/applications/plugins/SofaCUDA/Component/src/SofaCUDA/component/statecontainer/CudaMechanicalObject.inl +++ b/applications/plugins/SofaCUDA/Component/src/SofaCUDA/component/statecontainer/CudaMechanicalObject.inl @@ -1401,8 +1401,8 @@ double MechanicalObjectInternalData< gpu::cuda::CudaVectorTypesdata.tmpdot.recreate(tmpsize); - Kernels::vDot(va->size(), &r, va->deviceRead(), vb->deviceRead(), m->data.tmpdot.deviceWrite(), (Real*)(&(m->data.tmpdot.getCached(0)))); + m->m_data.tmpdot.recreate(tmpsize); + Kernels::vDot(va->size(), &r, va->deviceRead(), vb->deviceRead(), m->m_data.tmpdot.deviceWrite(), (Real*)(&(m->m_data.tmpdot.getCached(0)))); } } else if (a.type == sofa::core::V_DERIV && b.type == sofa::core::V_DERIV) @@ -1416,8 +1416,8 @@ double MechanicalObjectInternalData< gpu::cuda::CudaVectorTypesdata.tmpdot.recreate(tmpsize); - Kernels::vDot(va->size(), &r, va->deviceRead(), vb->deviceRead(), m->data.tmpdot.deviceWrite(), (Real*)(&(m->data.tmpdot.getCached(0)))); + m->m_data.tmpdot.recreate(tmpsize); + Kernels::vDot(va->size(), &r, va->deviceRead(), vb->deviceRead(), m->m_data.tmpdot.deviceWrite(), (Real*)(&(m->m_data.tmpdot.getCached(0)))); } #ifndef NDEBUG // Check the result @@ -2091,8 +2091,8 @@ double MechanicalObjectInternalData< gpu::cuda::CudaRigidTypes >::vDot( } else { - m->data.tmpdot.recreate(tmpsize); - Kernels::vDot(va->size(), &r, va->deviceRead(), vb->deviceRead(), m->data.tmpdot.deviceWrite(), (Real*)(&(m->data.tmpdot.getCached(0)))); + m->m_data.tmpdot.recreate(tmpsize); + Kernels::vDot(va->size(), &r, va->deviceRead(), vb->deviceRead(), m->m_data.tmpdot.deviceWrite(), (Real*)(&(m->m_data.tmpdot.getCached(0)))); } } else if (a.type == sofa::core::V_DERIV && b.type == sofa::core::V_DERIV) @@ -2106,8 +2106,8 @@ double MechanicalObjectInternalData< gpu::cuda::CudaRigidTypes >::vDot( } else { - m->data.tmpdot.recreate(tmpsize); - Kernels::vDot(va->size(), &r, va->deviceRead(), vb->deviceRead(), m->data.tmpdot.deviceWrite(), (Real*)(&(m->data.tmpdot.getCached(0)))); + m->m_data.tmpdot.recreate(tmpsize); + Kernels::vDot(va->size(), &r, va->deviceRead(), vb->deviceRead(), m->m_data.tmpdot.deviceWrite(), (Real*)(&(m->m_data.tmpdot.getCached(0)))); } #ifndef NDEBUG // Check the result @@ -2359,24 +2359,24 @@ void MechanicalObjectInternalData< gpu::cuda::CudaRigidTypes >::addFrom // I know using macros is bad design but this is the only way not to repeat the code for all CUDA types #define CudaMechanicalObject_ImplMethods(T) \ template<> void MechanicalObject< T >::accumulateForce(const core::ExecParams* params, core::VecDerivId fid) \ -{ if( fid==core::vec_id::write_access::force ) data.accumulateForce(this); else core::behavior::BaseMechanicalState::accumulateForce(params,fid);} \ +{ if( fid==core::vec_id::write_access::force ) m_data.accumulateForce(this); else core::behavior::BaseMechanicalState::accumulateForce(params,fid);} \ template<> void MechanicalObject< T >::vOp(const core::ExecParams* /* params */, core::VecId v, core::ConstVecId a, core::ConstVecId b, SReal f) \ -{ data.vOp(this, v, a, b, f); } \ +{ m_data.vOp(this, v, a, b, f); } \ template<> void MechanicalObject< T >::vMultiOp(const core::ExecParams* params, const VMultiOp& ops) \ -{ data.vMultiOp(this, params, ops); } \ +{ m_data.vMultiOp(this, params, ops); } \ template<> SReal MechanicalObject< T >::vDot(const core::ExecParams* /* params */, core::ConstVecId a, core::ConstVecId b) \ -{ return data.vDot(this, a, b); } \ +{ return m_data.vDot(this, a, b); } \ template<> void MechanicalObject< T >::resetForce(const core::ExecParams* params, core::VecDerivId fid) \ -{ if( fid==core::vec_id::write_access::force ) data.resetForce(this); else core::behavior::BaseMechanicalState::resetForce(params,fid); } \ +{ if( fid==core::vec_id::write_access::force ) m_data.resetForce(this); else core::behavior::BaseMechanicalState::resetForce(params,fid); } \ template<> void MechanicalObject< T >::copyToBaseVector(linearalgebra::BaseVector * dest, core::ConstVecId src, unsigned int &offset) \ -{ if (CudaBaseVectorType * vec = dynamic_cast *>(dest)) data.copyToCudaBaseVector(this, vec,src,offset); \ -else data.copyToBaseVector(this, dest,src,offset); } \ +{ if (CudaBaseVectorType * vec = dynamic_cast *>(dest)) m_data.copyToCudaBaseVector(this, vec,src,offset); \ +else m_data.copyToBaseVector(this, dest,src,offset); } \ template<> void MechanicalObject< T >::copyFromBaseVector(core::VecId dest, const linearalgebra::BaseVector * src, unsigned int &offset) \ -{ if (const CudaBaseVectorType * vec = dynamic_cast *>(src)) data.copyFromCudaBaseVector(this, dest,vec,offset); \ -else data.copyFromBaseVector(this, dest,src,offset); } \ +{ if (const CudaBaseVectorType * vec = dynamic_cast *>(src)) m_data.copyFromCudaBaseVector(this, dest,vec,offset); \ +else m_data.copyFromBaseVector(this, dest,src,offset); } \ template<> void MechanicalObject< T >::addFromBaseVectorSameSize(core::VecId dest, const linearalgebra::BaseVector *src, unsigned int &offset) \ -{ if (const CudaBaseVectorType * vec = dynamic_cast *>(src)) data.addFromCudaBaseVectorSameSize(this, dest,vec,offset); \ -else data.addFromBaseVectorSameSize(this, dest,src,offset); } +{ if (const CudaBaseVectorType * vec = dynamic_cast *>(src)) m_data.addFromCudaBaseVectorSameSize(this, dest,vec,offset); \ +else m_data.addFromBaseVectorSameSize(this, dest,src,offset); } CudaMechanicalObject_ImplMethods(gpu::cuda::CudaVec1fTypes) CudaMechanicalObject_ImplMethods(gpu::cuda::CudaVec2fTypes)