diff --git a/include/RigidBody/Markers.h b/include/RigidBody/Markers.h index b53ccba9..217167c6 100644 --- a/include/RigidBody/Markers.h +++ b/include/RigidBody/Markers.h @@ -11,6 +11,7 @@ namespace utils { class String; class Matrix; +class Vector3d; } namespace rigidbody @@ -74,6 +75,16 @@ class BIORBD_API Markers int id = -1 ); + /// + /// \brief Change the values of a specific marker without changing other parameters + /// \param index The index of the marker to change. If biorbd is compiled with SKIP_ASSERT, then no check is performed to ensure index is within bounds + /// \param pos The new position of the marker + /// + void setMarker( + size_t index, + const utils::Vector3d &pos + ); + /// /// \brief Return the marker of index idx /// \param idx The marker we want to return @@ -159,7 +170,7 @@ class BIORBD_API Markers bool updateKin = true); /// - /// \brief Return all the markers in their respective parent reference frame + /// \brief Return a copy of all the markers in their respective parent reference frame /// \param removeAxis If there are axis to remove from the position variables /// \return All the markers /// diff --git a/include/RigidBody/NodeSegment.h b/include/RigidBody/NodeSegment.h index 7c2e60dd..77865416 100644 --- a/include/RigidBody/NodeSegment.h +++ b/include/RigidBody/NodeSegment.h @@ -119,6 +119,12 @@ class BIORBD_API NodeSegment : public utils::Vector3d // Get and Set + /// + /// \brief Set internal values of the node without changing other parameters + /// \param values The new values + /// + void setValues(const Vector3d& values); + /// /// \brief Return if node is technical /// \return If node is technical diff --git a/src/RigidBody/Markers.cpp b/src/RigidBody/Markers.cpp index 6faf9e6e..c3943f31 100644 --- a/src/RigidBody/Markers.cpp +++ b/src/RigidBody/Markers.cpp @@ -5,6 +5,7 @@ #include #include "Utils/String.h" #include "Utils/Matrix.h" +#include "Utils/Error.h" #include "RigidBody/GeneralizedCoordinates.h" #include "RigidBody/GeneralizedVelocity.h" #include "RigidBody/GeneralizedAcceleration.h" @@ -57,11 +58,22 @@ void rigidbody::Markers::addMarker( const utils::String& axesToRemove, int id) { - rigidbody::NodeSegment tp(pos, name, parentName, technical, anatomical, - axesToRemove, id); + rigidbody::NodeSegment tp( + pos, name, parentName, technical, anatomical, axesToRemove, id); m_marks->push_back(tp); } + +void rigidbody::Markers::setMarker( + size_t index, + const utils::Vector3d& pos) +{ +#ifndef SKIP_ASSERT + utils::Error::check(index < m_marks->size(), utils::String("The index ") + index + " is larger than the number of markers (" + m_marks->size() + ")"); +#endif // SKIP_ASSERT + (*m_marks)[index].setValues(pos); +} + const rigidbody::NodeSegment &rigidbody::Markers::marker( size_t idx) const { @@ -161,7 +173,7 @@ std::vector rigidbody::Markers::markers( { std::vector pos; for (size_t i=0; i