From 251fd89b99379aa2f7693095f9399368acdc72ee Mon Sep 17 00:00:00 2001 From: Alex Bilger Date: Wed, 28 Feb 2024 00:40:24 +0100 Subject: [PATCH] [DefaultType] Extract inner classes from SolidTypes in files (#4513) * Move SpatialVector in its own file * Move Transform in its own file * Move both files in Sofa.Type * Change types from inner class of SolidTypes to its true type --- .../sofa/component/haptics/ForceFeedback.cpp | 2 +- .../sofa/component/haptics/ForceFeedback.h | 10 +- .../component/haptics/LCPForceFeedback.cpp | 6 +- .../sofa/component/haptics/LCPForceFeedback.h | 6 +- .../component/haptics/LCPForceFeedback.inl | 12 +- .../haptics/MechanicalStateForceFeedback.h | 4 +- .../component/haptics/NullForceFeedback.cpp | 2 +- .../component/haptics/NullForceFeedback.h | 2 +- .../component/haptics/NullForceFeedbackT.h | 2 +- .../src/sofa/component/visual/BaseCamera.cpp | 16 +- .../sofa/gl/component/shader/LightManager.h | 1 - .../src/sofa/defaulttype/SolidTypes.h | 263 +-------------- .../src/sofa/defaulttype/SolidTypes.inl | 318 ------------------ Sofa/framework/Type/CMakeLists.txt | 54 +-- .../Type/src/sofa/type/SpatialVector.cpp | 29 ++ .../Type/src/sofa/type/SpatialVector.h | 158 +++++++++ .../Type/src/sofa/type/SpatialVector.inl | 101 ++++++ .../Type/src/sofa/type/Transform.cpp | 29 ++ Sofa/framework/Type/src/sofa/type/Transform.h | 180 ++++++++++ .../Type/src/sofa/type/Transform.inl | 275 +++++++++++++++ .../ArticulatedHierarchyContainer.h | 4 +- 21 files changed, 839 insertions(+), 635 deletions(-) create mode 100644 Sofa/framework/Type/src/sofa/type/SpatialVector.cpp create mode 100644 Sofa/framework/Type/src/sofa/type/SpatialVector.h create mode 100644 Sofa/framework/Type/src/sofa/type/SpatialVector.inl create mode 100644 Sofa/framework/Type/src/sofa/type/Transform.cpp create mode 100644 Sofa/framework/Type/src/sofa/type/Transform.h create mode 100644 Sofa/framework/Type/src/sofa/type/Transform.inl diff --git a/Sofa/Component/Haptics/src/sofa/component/haptics/ForceFeedback.cpp b/Sofa/Component/Haptics/src/sofa/component/haptics/ForceFeedback.cpp index 729a1d53b73..5888d5b3b37 100644 --- a/Sofa/Component/Haptics/src/sofa/component/haptics/ForceFeedback.cpp +++ b/Sofa/Component/Haptics/src/sofa/component/haptics/ForceFeedback.cpp @@ -36,7 +36,7 @@ void ForceFeedback::init() context = sofa::simulation::node::getNodeFrom(getContext()); } -void ForceFeedback::setReferencePosition(sofa::defaulttype::SolidTypes::Transform& referencePosition) +void ForceFeedback::setReferencePosition(sofa::type::Transform& referencePosition) { SOFA_UNUSED(referencePosition); } diff --git a/Sofa/Component/Haptics/src/sofa/component/haptics/ForceFeedback.h b/Sofa/Component/Haptics/src/sofa/component/haptics/ForceFeedback.h index 43aee2b5ad3..ce5cc3f35bd 100644 --- a/Sofa/Component/Haptics/src/sofa/component/haptics/ForceFeedback.h +++ b/Sofa/Component/Haptics/src/sofa/component/haptics/ForceFeedback.h @@ -25,7 +25,7 @@ #include #include -#include +#include #include namespace sofa::component::haptics @@ -48,11 +48,11 @@ class SOFA_COMPONENT_HAPTICS_API ForceFeedback : public virtual core::behavior:: SReal u, SReal v, SReal w, SReal q, SReal& fx, SReal& fy, SReal& fz) = 0; - virtual void computeWrench(const sofa::defaulttype::SolidTypes::Transform &, - const sofa::defaulttype::SolidTypes::SpatialVector &, - sofa::defaulttype::SolidTypes::SpatialVector & )=0; + virtual void computeWrench(const sofa::type::Transform &, + const sofa::type::SpatialVector &, + sofa::type::SpatialVector & )=0; - virtual void setReferencePosition(sofa::defaulttype::SolidTypes::Transform& referencePosition); + virtual void setReferencePosition(sofa::type::Transform& referencePosition); virtual bool isEnabled(); /// Abstract method to lock or unlock the force feedback computation. To be implemented by child class if needed diff --git a/Sofa/Component/Haptics/src/sofa/component/haptics/LCPForceFeedback.cpp b/Sofa/Component/Haptics/src/sofa/component/haptics/LCPForceFeedback.cpp index 615eac76567..d751ec301c2 100644 --- a/Sofa/Component/Haptics/src/sofa/component/haptics/LCPForceFeedback.cpp +++ b/Sofa/Component/Haptics/src/sofa/component/haptics/LCPForceFeedback.cpp @@ -45,9 +45,9 @@ void LCPForceFeedback< Rigid3Types >::computeForce(SReal x, SReal y, SReal z, SR template <> -void LCPForceFeedback< Rigid3Types >::computeWrench(const sofa::defaulttype::SolidTypes::Transform &world_H_tool, - const sofa::defaulttype::SolidTypes::SpatialVector &/*V_tool_world*/, - sofa::defaulttype::SolidTypes::SpatialVector &W_tool_world ) +void LCPForceFeedback< Rigid3Types >::computeWrench(const sofa::type::Transform &world_H_tool, + const sofa::type::SpatialVector &/*V_tool_world*/, + sofa::type::SpatialVector &W_tool_world ) { if (!this->d_activate.getValue()) { diff --git a/Sofa/Component/Haptics/src/sofa/component/haptics/LCPForceFeedback.h b/Sofa/Component/Haptics/src/sofa/component/haptics/LCPForceFeedback.h index bb984801931..2f0c4f73dc3 100644 --- a/Sofa/Component/Haptics/src/sofa/component/haptics/LCPForceFeedback.h +++ b/Sofa/Component/Haptics/src/sofa/component/haptics/LCPForceFeedback.h @@ -75,9 +75,9 @@ class LCPForceFeedback : public MechanicalStateForceFeedback void computeForce(SReal x, SReal y, SReal z, SReal u, SReal v, SReal w, SReal q, SReal& fx, SReal& fy, SReal& fz) override; - void computeWrench(const sofa::defaulttype::SolidTypes::Transform &world_H_tool, - const sofa::defaulttype::SolidTypes::SpatialVector &V_tool_world, - sofa::defaulttype::SolidTypes::SpatialVector &W_tool_world ) override; + void computeWrench(const sofa::type::Transform &world_H_tool, + const sofa::type::SpatialVector &V_tool_world, + sofa::type::SpatialVector &W_tool_world ) override; void computeForce(const VecCoord& state, VecDeriv& forces) override; protected: diff --git a/Sofa/Component/Haptics/src/sofa/component/haptics/LCPForceFeedback.inl b/Sofa/Component/Haptics/src/sofa/component/haptics/LCPForceFeedback.inl index 20090fa04cb..011b03282ec 100644 --- a/Sofa/Component/Haptics/src/sofa/component/haptics/LCPForceFeedback.inl +++ b/Sofa/Component/Haptics/src/sofa/component/haptics/LCPForceFeedback.inl @@ -412,9 +412,9 @@ void LCPForceFeedback::computeForce(SReal , SReal, SReal, SReal, SRea template -void LCPForceFeedback::computeWrench(const sofa::defaulttype::SolidTypes::Transform &, - const sofa::defaulttype::SolidTypes::SpatialVector &, - sofa::defaulttype::SolidTypes::SpatialVector & ) +void LCPForceFeedback::computeWrench(const sofa::type::Transform &, + const sofa::type::SpatialVector &, + sofa::type::SpatialVector & ) { } @@ -425,9 +425,9 @@ template <> void SOFA_COMPONENT_HAPTICS_API LCPForceFeedback< sofa::defaulttype::Rigid3Types >::computeForce(SReal x, SReal y, SReal z, SReal, SReal, SReal, SReal, SReal& fx, SReal& fy, SReal& fz); template <> -void SOFA_COMPONENT_HAPTICS_API LCPForceFeedback< sofa::defaulttype::Rigid3Types >::computeWrench(const sofa::defaulttype::SolidTypes::Transform &world_H_tool, - const sofa::defaulttype::SolidTypes::SpatialVector &/*V_tool_world*/, - sofa::defaulttype::SolidTypes::SpatialVector &W_tool_world ); +void SOFA_COMPONENT_HAPTICS_API LCPForceFeedback< sofa::defaulttype::Rigid3Types >::computeWrench(const sofa::type::Transform &world_H_tool, + const sofa::type::SpatialVector &/*V_tool_world*/, + sofa::type::SpatialVector &W_tool_world ); diff --git a/Sofa/Component/Haptics/src/sofa/component/haptics/MechanicalStateForceFeedback.h b/Sofa/Component/Haptics/src/sofa/component/haptics/MechanicalStateForceFeedback.h index 3931cacf0cb..49fec18f073 100644 --- a/Sofa/Component/Haptics/src/sofa/component/haptics/MechanicalStateForceFeedback.h +++ b/Sofa/Component/Haptics/src/sofa/component/haptics/MechanicalStateForceFeedback.h @@ -44,8 +44,8 @@ class SOFA_COMPONENT_HAPTICS_API MechanicalStateForceFeedback : public ForceFeed void init() override {context = sofa::simulation::node::getNodeFrom(getContext());} void computeForce(SReal x, SReal y, SReal z, SReal u, SReal v, SReal w, SReal q, SReal& fx, SReal& fy, SReal& fz) override = 0; - void computeWrench(const sofa::defaulttype::SolidTypes::Transform &, const sofa::defaulttype::SolidTypes::SpatialVector &, sofa::defaulttype::SolidTypes::SpatialVector & ) override = 0; - void setReferencePosition(sofa::defaulttype::SolidTypes::Transform& /*referencePosition*/) override {} + void computeWrench(const sofa::type::Transform &, const sofa::type::SpatialVector &, sofa::type::SpatialVector & ) override = 0; + void setReferencePosition(sofa::type::Transform& /*referencePosition*/) override {} protected: MechanicalStateForceFeedback(void) {} diff --git a/Sofa/Component/Haptics/src/sofa/component/haptics/NullForceFeedback.cpp b/Sofa/Component/Haptics/src/sofa/component/haptics/NullForceFeedback.cpp index 8eaf91c710c..b18c50307a0 100644 --- a/Sofa/Component/Haptics/src/sofa/component/haptics/NullForceFeedback.cpp +++ b/Sofa/Component/Haptics/src/sofa/component/haptics/NullForceFeedback.cpp @@ -36,7 +36,7 @@ void NullForceFeedback::computeForce(SReal /*x*/, SReal /*y*/, SReal /*z*/, SRea fx = fy = fz = 0.0; } -void NullForceFeedback::computeWrench(const sofa::defaulttype::SolidTypes::Transform &/*world_H_tool*/, const sofa::defaulttype::SolidTypes::SpatialVector &/*V_tool_world*/, sofa::defaulttype::SolidTypes::SpatialVector &W_tool_world ) +void NullForceFeedback::computeWrench(const sofa::type::Transform &/*world_H_tool*/, const sofa::type::SpatialVector &/*V_tool_world*/, sofa::type::SpatialVector &W_tool_world ) { W_tool_world.clear(); } diff --git a/Sofa/Component/Haptics/src/sofa/component/haptics/NullForceFeedback.h b/Sofa/Component/Haptics/src/sofa/component/haptics/NullForceFeedback.h index 309d1e9ec9b..4f9bc49231f 100644 --- a/Sofa/Component/Haptics/src/sofa/component/haptics/NullForceFeedback.h +++ b/Sofa/Component/Haptics/src/sofa/component/haptics/NullForceFeedback.h @@ -37,7 +37,7 @@ class SOFA_COMPONENT_HAPTICS_API NullForceFeedback : public ForceFeedback void init() override; void computeForce(SReal x, SReal y, SReal z, SReal u, SReal v, SReal w, SReal q, SReal& fx, SReal& fy, SReal& fz) override; - void computeWrench(const sofa::defaulttype::SolidTypes::Transform &world_H_tool, const sofa::defaulttype::SolidTypes::SpatialVector &V_tool_world, sofa::defaulttype::SolidTypes::SpatialVector &W_tool_world ) override; + void computeWrench(const sofa::type::Transform &world_H_tool, const sofa::type::SpatialVector &V_tool_world, sofa::type::SpatialVector &W_tool_world ) override; }; } // namespace sofa::component::haptics diff --git a/Sofa/Component/Haptics/src/sofa/component/haptics/NullForceFeedbackT.h b/Sofa/Component/Haptics/src/sofa/component/haptics/NullForceFeedbackT.h index 5467c97dfb4..bf91e977a49 100644 --- a/Sofa/Component/Haptics/src/sofa/component/haptics/NullForceFeedbackT.h +++ b/Sofa/Component/Haptics/src/sofa/component/haptics/NullForceFeedbackT.h @@ -43,7 +43,7 @@ class SOFA_COMPONENT_HAPTICS_API NullForceFeedbackT : public MechanicalStateForc fx = fy = fz = 0.0; } void computeForce(const VecCoord &, VecDeriv &) override {} - void computeWrench(const sofa::defaulttype::SolidTypes::Transform &, const sofa::defaulttype::SolidTypes::SpatialVector &, sofa::defaulttype::SolidTypes::SpatialVector &W_tool_world ) override {W_tool_world.clear();} + void computeWrench(const sofa::type::Transform &, const sofa::type::SpatialVector &, sofa::type::SpatialVector &W_tool_world ) override {W_tool_world.clear();} }; } // namespace sofa::component::haptics diff --git a/Sofa/Component/Visual/src/sofa/component/visual/BaseCamera.cpp b/Sofa/Component/Visual/src/sofa/component/visual/BaseCamera.cpp index ea658e27654..4918b7810fd 100644 --- a/Sofa/Component/Visual/src/sofa/component/visual/BaseCamera.cpp +++ b/Sofa/Component/Visual/src/sofa/component/visual/BaseCamera.cpp @@ -29,7 +29,7 @@ using Mat3 = sofa::type::Mat3x3; using Mat4 = sofa::type::Mat4x4; -#include +#include #include #include @@ -369,7 +369,7 @@ type::Vec2 BaseCamera::worldToScreenCoordinates(const type::Vec3& pos) void BaseCamera::getModelViewMatrix(double mat[16]) { - const defaulttype::SolidTypes::Transform world_H_cam(p_position.getValue(), this->getOrientation()); + const sofa::type::Transform world_H_cam(p_position.getValue(), this->getOrientation()); Mat3 rot = world_H_cam.inversed().getRotationMatrix(); //rotation @@ -392,7 +392,7 @@ void BaseCamera::getModelViewMatrix(double mat[16]) void BaseCamera::getOpenGLModelViewMatrix(double mat[16]) { - const defaulttype::SolidTypes::Transform world_H_cam(p_position.getValue(), this->getOrientation()); + const sofa::type::Transform world_H_cam(p_position.getValue(), this->getOrientation()); world_H_cam.inversed().writeOpenGlMatrix(mat); } @@ -548,10 +548,10 @@ void BaseCamera::rotateWorldAroundPoint(Quat &rotation, const type::Vec3 &point, rotation.quatToAxis(tempAxis, tempAngle); const Quat tempQuat (orientationCam.rotate(-tempAxis), tempAngle); - const defaulttype::SolidTypes::Transform world_H_cam(positionCam, orientationCam); - const defaulttype::SolidTypes::Transform world_H_pivot(point, Quat()); - const defaulttype::SolidTypes::Transform pivotBefore_R_pivotAfter(type::Vec3(0.0,0.0,0.0), tempQuat); - const defaulttype::SolidTypes::Transform camera_H_WorldAfter = world_H_cam.inversed() * world_H_pivot * pivotBefore_R_pivotAfter * world_H_pivot.inversed(); + const sofa::type::Transform world_H_cam(positionCam, orientationCam); + const sofa::type::Transform world_H_pivot(point, Quat()); + const sofa::type::Transform pivotBefore_R_pivotAfter(type::Vec3(0.0,0.0,0.0), tempQuat); + const sofa::type::Transform camera_H_WorldAfter = world_H_cam.inversed() * world_H_pivot * pivotBefore_R_pivotAfter * world_H_pivot.inversed(); //defaulttype::SolidTypes::Transform camera_H_WorldAfter = worldBefore_H_cam.inversed()*worldBefore_R_worldAfter; positionCam = camera_H_WorldAfter.inversed().getOrigin(); @@ -662,7 +662,7 @@ void BaseCamera::computeZ() if (p_computeZClip.getValue()) { //modelview transform - defaulttype::SolidTypes::Transform world_H_cam(p_position.getValue(), this->getOrientation()); + sofa::type::Transform world_H_cam(p_position.getValue(), this->getOrientation()); //double distanceCamToCenter = fabs((world_H_cam.inversed().projectPoint(sceneCenter))[2]); const double distanceCamToCenter = (p_position.getValue() - sceneCenter).norm(); diff --git a/Sofa/GL/Component/Shader/src/sofa/gl/component/shader/LightManager.h b/Sofa/GL/Component/Shader/src/sofa/gl/component/shader/LightManager.h index e2876069211..bac70cadad3 100644 --- a/Sofa/GL/Component/Shader/src/sofa/gl/component/shader/LightManager.h +++ b/Sofa/GL/Component/Shader/src/sofa/gl/component/shader/LightManager.h @@ -22,7 +22,6 @@ #pragma once #include -#include #include #include #include diff --git a/Sofa/framework/DefaultType/src/sofa/defaulttype/SolidTypes.h b/Sofa/framework/DefaultType/src/sofa/defaulttype/SolidTypes.h index e726ad5225c..72b92178766 100644 --- a/Sofa/framework/DefaultType/src/sofa/defaulttype/SolidTypes.h +++ b/Sofa/framework/DefaultType/src/sofa/defaulttype/SolidTypes.h @@ -31,6 +31,8 @@ #include #include #include +#include + namespace sofa::defaulttype @@ -41,10 +43,6 @@ Base types for the ArticulatedSolid: position, orientation, velocity, angular ve @author François Faure, INRIA-UJF, 2006 */ - -class SOFA_DEFAULTTYPE_API Transform; - - template< class R=float > class SOFA_DEFAULTTYPE_API SolidTypes { @@ -60,122 +58,8 @@ class SOFA_DEFAULTTYPE_API SolidTypes typedef type::Vec<6,Real> Vec6; typedef Vec6 DOF; ///< For compatibility - - /** A spatial vector. - When representing a velocity, lineVec is the angular velocity and freeVec is the linear velocity. - When representing a spatial force, lineVec is the force and freeVec is the torque. */ - class SOFA_DEFAULTTYPE_API SpatialVector - { - public: - Vec lineVec{ type::NOINIT }; - Vec freeVec{ type::NOINIT }; - - void clear(); - SpatialVector() = default; - /** - \param l The line vector: angular velocity, or force - \param f The free vector: linear velocity, or torque - */ - SpatialVector( const Vec& l, const Vec& f ); - - - SpatialVector& operator += (const SpatialVector& v); - - //template - SpatialVector operator * ( Real a ) const - { - return SpatialVector( lineVec *a, freeVec * a); - } - - SpatialVector& operator *= ( Real a ) - { - lineVec *=a; - freeVec *= a; - return *this; - } - - SpatialVector operator + ( const SpatialVector& v ) const; - SpatialVector operator - ( const SpatialVector& v ) const; - SpatialVector operator - ( ) const; - /// Spatial dot product (cross terms) - Real operator * ( const SpatialVector& v ) const; - /// Spatial cross product - SpatialVector cross( const SpatialVector& v ) const; - /// product with a dense matrix - SpatialVector operator * (const Mat66&) const; - - /// write to an output stream - inline friend std::ostream& operator << (std::ostream& out, const SpatialVector& t ) - { - out << t.lineVec << " " << t.freeVec; - return out; - } - - /// read from an input stream - inline friend std::istream& operator >> ( std::istream& in, SpatialVector& t ) - { - in >> t.lineVec >> t.freeVec; - return in; - } - - /// If the SpatialVector models a spatial velocity, then the linear velocity is the freeVec. - /// Otherwise, the SpatialVector models a spatial force, and this method returns a torque. - Vec& getLinearVelocity() - { - return freeVec; - } - const Vec& getLinearVelocity() const - { - return freeVec; - } - void setLinearVelocity(const Vec& v) - { - freeVec = v; - } - /// If the SpatialVector models a spatial velocity, then the angular velocity is the lineVec. - /// Otherwise, the SpatialVector models a spatial force, and this method returns a force. - Vec& getAngularVelocity() - { - return lineVec; - } - const Vec& getAngularVelocity() const - { - return lineVec; - } - void setAngularVelocity(const Vec& v) - { - lineVec = v; - } - - /// If the SpatialVector models a spatial force, then the torque is the freeVec. - /// Otherwise, the SpatialVector models a spatial velocity, and this method returns a linear velocity. - Vec& getTorque() - { - return freeVec; - } - const Vec& getTorque() const - { - return freeVec; - } - void setTorque(const Vec& v) - { - freeVec = v; - } - /// If the SpatialVector models a spatial force, then the torque is the lineVec. - /// Otherwise, the SpatialVector models a spatial velocity, and this method returns an angular velocity. - Vec& getForce() - { - return lineVec; - } - const Vec& getForce() const - { - return lineVec; - } - void setForce(const Vec& v) - { - lineVec = v; - } - }; + using SpatialVector = sofa::type::SpatialVector; + using Transform = sofa::type::Transform; /** * \brief A twist aka a SpatialVector representing a velocity @@ -203,144 +87,6 @@ class SOFA_DEFAULTTYPE_API SolidTypes : SpatialVector(force, torque) {} }; - /** Define a frame (child) whith respect to another (parent). A frame represents a local coordinate system. - - Internal data represents the orientation of the child wrt the parent, BUT the translation vector represents the origin of the parent with respect to the child. For example, the coordinates M_p of point M in parent given the coordinates M_c of the same point in child are given by: M_p = orientation * ( M_c - origin ). This is due to Featherstone's conventions. Use method setTranslationRotation( const Vec& t, const Rot& q ) to model the Transform the standard way (i.e. translation givne in the parent frame). - - - */ - class SOFA_DEFAULTTYPE_API Transform - { - public: - /// The default constructor does not initialize the transform - Transform(); - /// Origin of the child in parent coordinates, orientation of the child wrt to parent - Transform( const Vec& origin, const Rot& orientation ); - /// WARNING: using Featherstone's conventions (see class documentation) - Transform( const Rot& q, const Vec& o ); - /// Origin of the child in the parent coordinate system and the orientation of the child wrt the parent (i.e. standard way) - void set( const Vec& t, const Rot& q ); - /// Reset this to identity - void clear(); - /// The identity transform (child = parent) - static Transform identity(); - /// Origin of the child in the parent coordinate system and the orientation of the child wrt the parent (i.e. standard way) - //static Transform inParent(const Vec& t, const Rot& r); - /// Define child as a given SpatialVector integrated during one second, starting from the parent (used for time integration). The spatial vector is given in parent coordinates. - Transform( const SpatialVector& v ); - /// The inverse transform i.e. parent wrt child - Transform inversed() const; - /// Parent origin in child coordinates (the way it is actually stored internally) - const Vec& getOriginOfParentInChild() const; - /// Origin of child in parent coordinates - Vec getOrigin() const; - /// Origin of child in parent coordinates - void setOrigin( const Vec& ); - /// Orientation of the child coordinate axes wrt the parent coordinate axes - const Rot& getOrientation() const; - /// Orientation of the child coordinate axes wrt the parent coordinate axes - void setOrientation( const Rot& ); - /// Matrix which projects vectors from child coordinates to parent coordinates. The columns of the matrix are the axes of the child base axes in the parent coordinate system. - Mat3x3 getRotationMatrix() const; - - - - - - - /** - * \brief Adjoint matrix to the transform - * This matrix transports velocities in twist coordinates from the child frame to the parent frame. - * Its inverse transpose does the same for the wrenches - */ - Mat6x6 getAdjointMatrix() const; - - /// Project a vector (i.e. a direction or a displacement) from child coordinates to parent coordinates - Vec projectVector( const Vec& vectorInChild ) const; - /// Project a point from child coordinates to parent coordinates - Vec projectPoint( const Vec& pointInChild ) const; - /// Projected a vector (i.e. a direction or a displacement) from parent coordinates to child coordinates - Vec backProjectVector( const Vec& vectorInParent ) const; - /// Project point from parent coordinates to this coordinates - Vec backProjectPoint( const Vec& pointInParent ) const; - /// Combine two transforms. If (*this) locates frame B (child) wrt frame A (parent) and if f2 locates frame C (child) wrt frame B (parent) then the result locates frame C wrt to Frame A. - Transform operator * (const Transform& f2) const; - /// Combine two transforms. If (*this) locates frame B (child) wrt frame A (parent) and if f2 locates frame C (child) wrt frame B (parent) then the result locates frame C wrt to Frame A. - Transform& operator *= (const Transform& f2); - - /** Project a spatial vector from child to parent - * TODO One should handle differently the transformation of a twist and a wrench ! - * This applying the adjoint to velocities or its transpose to wrench : - * V_parent = Ad . V_child or W_child = Ad^T . W_parent - * To project a wrench in the child frame to the parent frame you need to do - * parent_wrench = this->inversed * child_wrench - * (this doc needs to be douv-ble checked !) - */ - // create a spatial Vector from a small transformation - SpatialVector CreateSpatialVector(); - SpatialVector DTrans(); - - SpatialVector operator * (const SpatialVector& sv ) const; - /// Project a spatial vector from parent to child (the inverse of operator *). This method computes (*this).inversed()*sv without inverting (*this). - SpatialVector operator / (const SpatialVector& sv ) const; - /// Write an OpenGL matrix encoding the transformation of the coordinate system of the child wrt the coordinate system of the parent. - void writeOpenGlMatrix( double *m ) const; - /// Draw the axes of the child coordinate system in the parent coordinate system - /// Print the origin of the child in the parent coordinate system and the quaternion defining the orientation of the child wrt the parent - inline friend std::ostream& operator << (std::ostream& out, const Transform& t ) - { - out << t.getOrigin() << " " << t.getOrientation(); - return out; - } - - /// read from an input stream - inline friend std::istream& operator >> ( std::istream& in, Transform& t ) - { - Vec origin; - Rot orientation; - - in >> origin >> orientation; - - t.set(origin, orientation); - - return in; - } - - /// Print the internal values (i.e. using Featherstone's conventions, see class documentation) - void printInternal( std::ostream&) const; - - /** @name Time integration - * Methods used in time integration - */ - ///@{ - /// (*this) *= Transform(v) Used for time integration. SHOULD WE RATHER APPLY (*this)=Transform(v)*(*this) ??? - Transform& operator +=(const SpatialVector& a); - - Transform& operator +=(const Transform& a); - - template - Transform& operator*=(Real2 a) - { - origin_ *= a; - return *this; - } - - template - Transform operator*(Real2 a) const - { - Transform r = *this; - r*=a; - return r; - } - ///@} - - - protected: - Rot orientation_; ///< child wrt parent - Vec origin_; ///< parent wrt child - - }; - class SOFA_DEFAULTTYPE_API RigidInertia { @@ -412,7 +158,6 @@ class SOFA_DEFAULTTYPE_API SolidTypes #if !defined(SOFA_DEFAULTTYPE_SOLIDTYPES_CPP) extern template class SOFA_DEFAULTTYPE_API SolidTypes; extern template class SOFA_DEFAULTTYPE_API SolidTypes; - #endif } diff --git a/Sofa/framework/DefaultType/src/sofa/defaulttype/SolidTypes.inl b/Sofa/framework/DefaultType/src/sofa/defaulttype/SolidTypes.inl index d24b55715df..2306827c805 100644 --- a/Sofa/framework/DefaultType/src/sofa/defaulttype/SolidTypes.inl +++ b/Sofa/framework/DefaultType/src/sofa/defaulttype/SolidTypes.inl @@ -31,324 +31,6 @@ namespace sofa::defaulttype { -template -SolidTypes::SpatialVector::SpatialVector( const Vec& l, const Vec& f ):lineVec(l),freeVec(f) -{} - -template -void SolidTypes::SpatialVector::clear() -{ - lineVec = freeVec = Vec(0,0,0); -} - -template -typename SolidTypes::SpatialVector& SolidTypes::SpatialVector::operator += (const SpatialVector& v) -{ - lineVec += v.lineVec; - freeVec += v.freeVec; - return *this; -} - -template -typename SolidTypes::SpatialVector SolidTypes::SpatialVector::operator + ( const SpatialVector& v ) const -{ - return SpatialVector(lineVec+v.lineVec,freeVec+v.freeVec); -} - -template -typename SolidTypes::SpatialVector SolidTypes::SpatialVector::operator - ( const SpatialVector& v ) const -{ - return SpatialVector(lineVec-v.lineVec,freeVec-v.freeVec); -} - -template -typename SolidTypes::SpatialVector SolidTypes::SpatialVector::operator - ( ) const -{ - return SpatialVector(-lineVec,-freeVec); -} - -/// Spatial dot product (cross terms) -template -typename SolidTypes::Real SolidTypes::SpatialVector::operator * ( const SpatialVector& v ) const -{ - return lineVec * v.freeVec + freeVec * v.lineVec; -} - -/// Spatial cross product -template -typename SolidTypes::SpatialVector SolidTypes::SpatialVector::cross( const SpatialVector& v ) const -{ - return SpatialVector( - type::cross(lineVec,v.lineVec), - type::cross(freeVec,v.lineVec) + type::cross(lineVec,v.freeVec) - ); -} - -template -typename SolidTypes::SpatialVector SolidTypes::SpatialVector::operator * (const Mat66& m) const -{ - SpatialVector result; - for( int i=0; i<3; i++ ) - { - result.lineVec[i]=0; - result.freeVec[i]=0; - for( int j=0; j<3; j++ ) - { - result.lineVec[i] += lineVec[j]*m[i][j] + freeVec[j]*m[i][j+3]; - result.freeVec[i] += lineVec[j]*m[i+3][j] + freeVec[j]*m[i+3][j+3]; - } - } - return result; -} -//====================================================================================================== - -template -SolidTypes::Transform::Transform() - : orientation_() // default constructor set to identity - , origin_() // default constructor set to {0, 0, 0} -{ - -} - -/// Define using Featherstone's conventions -template -SolidTypes::Transform::Transform( const Rot& q, const Vec& o ):orientation_(q),origin_(o) -{} - -/// Define using standard conventions -template -SolidTypes::Transform::Transform( const Vec& t, const Rot& q ) - :orientation_(q),origin_(-(q.inverseRotate(t))) -{} - -/// Define given the origin of the child wrt the parent and the orientation of the child wrt the parent (i.e. standard way) -template -void SolidTypes::Transform::set -( const Vec& t, const Rot& q ) -{ - orientation_ =q, origin_ = -(q.inverseRotate(t)); -} - -/// Define given the origin of the child wrt the parent and the orientation of the child wrt the parent (i.e. standard way) -template -typename SolidTypes::Transform SolidTypes::Transform::identity() -{ - return Transform( Rot::identity(), Vec(0,0,0) ); -} - -/// Define as a given SpatialVector integrated during one second. The spatial vector is given in parent coordinates. -template -SolidTypes::Transform::Transform( const SpatialVector& v ) -{ - orientation_ = Rot::createFromRotationVector( v.lineVec ); - origin_ = - orientation_.inverseRotate( v.freeVec ); -} - -template -const typename SolidTypes::Vec& SolidTypes::Transform::getOriginOfParentInChild() const -{ - return origin_; -} - -template -typename SolidTypes::Vec SolidTypes::Transform::getOrigin() const -{ - return -orientation_.rotate(origin_); -} - -template -void SolidTypes::Transform::setOrigin( const Vec& op ) -{ - origin_ = -orientation_.inverseRotate(op); -} - -template -const typename SolidTypes::Rot& SolidTypes::Transform::getOrientation() const -{ - return orientation_; -} - -template -void SolidTypes::Transform::setOrientation( const Rot& q ) -{ - orientation_=q; -} -template -typename SolidTypes::SpatialVector SolidTypes::Transform::DTrans() -{ - - return SpatialVector(orientation_.quatToRotationVector(), this->getOrigin()); - // Use of quatToRotationVector instead of toEulerVector: - // this is done to keep the old behavior (before the - // correction of the toEulerVector function). If the - // purpose was to obtain the Eulerian vector and not the - // rotation vector please use the following line instead - //return SpatialVector(orientation_.toEulerVector(), this->getOrigin()); -} - -template -typename SolidTypes::Vec SolidTypes::Transform::projectVector( const Vec& v ) const -{ - return orientation_.rotate( v ); -} - -template -typename SolidTypes::Vec SolidTypes::Transform::projectPoint( const Vec& p ) const -{ - return orientation_.rotate( p - origin_ ); -} - -template -typename SolidTypes::Vec SolidTypes::Transform::backProjectVector( const Vec& v ) const -{ - return orientation_.inverseRotate( v ); -} - -template -typename SolidTypes::Vec SolidTypes::Transform::backProjectPoint( const Vec& p ) const -{ - return orientation_.inverseRotate( p ) + origin_; -} - -template -typename SolidTypes::Mat3x3 SolidTypes::Transform::getRotationMatrix() const -{ - Mat3x3 m; - m[0][0] = (1.0f - 2.0f * (orientation_[1] * orientation_[1] + orientation_[2] * orientation_[2])); - m[0][1] = (2.0f * (orientation_[0] * orientation_[1] - orientation_[2] * orientation_[3])); - m[0][2] = (2.0f * (orientation_[2] * orientation_[0] + orientation_[1] * orientation_[3])); - - m[1][0] = (2.0f * (orientation_[0] * orientation_[1] + orientation_[2] * orientation_[3])); - m[1][1] = (1.0f - 2.0f * (orientation_[2] * orientation_[2] + orientation_[0] * orientation_[0])); - m[1][2] = (2.0f * (orientation_[1] * orientation_[2] - orientation_[0] * orientation_[3])); - - m[2][0] = (2.0f * (orientation_[2] * orientation_[0] - orientation_[1] * orientation_[3])); - m[2][1] = (2.0f * (orientation_[1] * orientation_[2] + orientation_[0] * orientation_[3])); - m[2][2] = (1.0f - 2.0f * (orientation_[1] * orientation_[1] + orientation_[0] * orientation_[0])); - return m; -} - -template -typename SolidTypes::Mat6x6 SolidTypes::Transform::getAdjointMatrix() const -{ - /// TODO - Mat6x6 Adj; - Mat3x3 Rot; - Rot = this->getRotationMatrix(); - // correspond au produit vectoriel v^origin - Mat3x3 Origin; - Origin[0][0]=(Real)0.0; Origin[0][1]=origin_[2]; Origin[0][2]=-origin_[1]; - Origin[1][0]=-origin_[2]; Origin[1][1]=(Real)0.0; Origin[1][2]=origin_[0]; - Origin[2][0]=origin_[1]; Origin[2][1]=-origin_[0]; Origin[2][2]=(Real)0.0; - - Mat3x3 R_Origin = Rot*Origin; - - for (int i=0; i<3; i++) - { - for(int j=0; j<3; j++) - { - Adj[i][j] = Rot[i][j]; - Adj[i+3][j+3] = Rot[i][j]; - Adj[i][j+3] = R_Origin[i][j]; - Adj[i+3][j] = 0.0; - } - } - - - return Adj; -} - -template -void SolidTypes::Transform::clear() -{ - orientation_.clear(); - origin_=Vec(0,0,0); -} - -template -typename SolidTypes::Transform SolidTypes::Transform::operator * (const Transform& f2) const -{ - return Transform( orientation_ * f2.getOrientation(), f2.getOriginOfParentInChild() + f2.getOrientation().inverseRotate(origin_)) ; -} - -template -typename SolidTypes::Transform& SolidTypes::Transform::operator *= (const Transform& f2) -{ - orientation_ *= f2.getOrientation(); - origin_ = f2.getOriginOfParentInChild() + f2.getOrientation().inverseRotate(origin_); - return (*this); -} - - -template -typename SolidTypes::SpatialVector SolidTypes::Transform::CreateSpatialVector() -{ - return SpatialVector(this->getOrientation().quatToRotationVector(), this->getOrigin() ); -} - -template -typename SolidTypes::SpatialVector SolidTypes::Transform::operator * (const SpatialVector& sv ) const -{ - return SpatialVector( - orientation_.rotate(sv.lineVec), - orientation_.rotate( cross(sv.lineVec, origin_ ) + sv.freeVec) - ); -} - -template -typename SolidTypes::SpatialVector SolidTypes::Transform::operator / (const SpatialVector& sv ) const -{ - return inversed()*sv; -} - - - - -template -typename SolidTypes::Transform SolidTypes::Transform::inversed() const -{ - return Transform( orientation_.inverse(), -(orientation_.rotate(origin_)) ); -} - -template -void SolidTypes::Transform::writeOpenGlMatrix( double *m ) const -{ - orientation_.writeOpenGlMatrix(m); - Vec t = getOrigin(); - m[12] = t[0]; - m[13] = t[1]; - m[14] = t[2]; -} - -template -void SolidTypes::Transform::printInternal( std::ostream& out ) const -{ - out<<"internal t= "< SolidTypes::RigidInertia::RigidInertia() diff --git a/Sofa/framework/Type/CMakeLists.txt b/Sofa/framework/Type/CMakeLists.txt index 9371b860db3..a73965467b1 100644 --- a/Sofa/framework/Type/CMakeLists.txt +++ b/Sofa/framework/Type/CMakeLists.txt @@ -6,61 +6,67 @@ set(SOFATYPESRC_ROOT "src/sofa/type") set(HEADER_FILES ${SOFATYPESRC_ROOT}/config.h.in ${SOFATYPESRC_ROOT}/init.h - ${SOFATYPESRC_ROOT}/Vec.h + ${SOFATYPESRC_ROOT}/BoundingBox.h + ${SOFATYPESRC_ROOT}/Color.h + ${SOFATYPESRC_ROOT}/DualQuat.h + ${SOFATYPESRC_ROOT}/DualQuat.inl + ${SOFATYPESRC_ROOT}/Frame.h ${SOFATYPESRC_ROOT}/Mat.h ${SOFATYPESRC_ROOT}/MatSym.h ${SOFATYPESRC_ROOT}/Mat_solve_Cholesky.h + ${SOFATYPESRC_ROOT}/Mat_solve_LCP.h ${SOFATYPESRC_ROOT}/Mat_solve_LU.h ${SOFATYPESRC_ROOT}/Mat_solve_SVD.h - ${SOFATYPESRC_ROOT}/Mat_solve_LCP.h - ${SOFATYPESRC_ROOT}/trait/is_container.h - ${SOFATYPESRC_ROOT}/trait/is_vector.h - ${SOFATYPESRC_ROOT}/trait/Rebind.h - ${SOFATYPESRC_ROOT}/fwd.h - ${SOFATYPESRC_ROOT}/Quat.h - ${SOFATYPESRC_ROOT}/Quat.inl - ${SOFATYPESRC_ROOT}/DualQuat.h - ${SOFATYPESRC_ROOT}/DualQuat.inl - ${SOFATYPESRC_ROOT}/BoundingBox.h - ${SOFATYPESRC_ROOT}/Color.h - ${SOFATYPESRC_ROOT}/Frame.h ${SOFATYPESRC_ROOT}/Material.h ${SOFATYPESRC_ROOT}/PrimitiveGroup.h - ${SOFATYPESRC_ROOT}/Ray.h + ${SOFATYPESRC_ROOT}/Quat.h + ${SOFATYPESRC_ROOT}/Quat.inl ${SOFATYPESRC_ROOT}/RGBAColor.h ${SOFATYPESRC_ROOT}/RGBAColor_fwd.h + ${SOFATYPESRC_ROOT}/Ray.h + ${SOFATYPESRC_ROOT}/SVector.h + ${SOFATYPESRC_ROOT}/SpatialVector.h + ${SOFATYPESRC_ROOT}/SpatialVector.inl + ${SOFATYPESRC_ROOT}/Transform.h + ${SOFATYPESRC_ROOT}/Transform.inl + ${SOFATYPESRC_ROOT}/Vec.h ${SOFATYPESRC_ROOT}/fixed_array.h ${SOFATYPESRC_ROOT}/fixed_array_algorithms.h + ${SOFATYPESRC_ROOT}/fwd.h + ${SOFATYPESRC_ROOT}/isRigidType.h ${SOFATYPESRC_ROOT}/stable_vector.h + ${SOFATYPESRC_ROOT}/trait/Rebind.h + ${SOFATYPESRC_ROOT}/trait/is_container.h + ${SOFATYPESRC_ROOT}/trait/is_vector.h ${SOFATYPESRC_ROOT}/vector.h - ${SOFATYPESRC_ROOT}/vector_T.h - ${SOFATYPESRC_ROOT}/vector_T.inl - ${SOFATYPESRC_ROOT}/vector_Real.h ${SOFATYPESRC_ROOT}/vector_Integral.h + ${SOFATYPESRC_ROOT}/vector_Real.h ${SOFATYPESRC_ROOT}/vector_String.h + ${SOFATYPESRC_ROOT}/vector_T.h + ${SOFATYPESRC_ROOT}/vector_T.inl ${SOFATYPESRC_ROOT}/vector_algebra.h ${SOFATYPESRC_ROOT}/vector_algorithm.h ${SOFATYPESRC_ROOT}/vector_device.h - ${SOFATYPESRC_ROOT}/SVector.h ) set(SOURCE_FILES ${SOFATYPESRC_ROOT}/init.cpp - ${SOFATYPESRC_ROOT}/Vec.cpp - ${SOFATYPESRC_ROOT}/Quat.cpp - ${SOFATYPESRC_ROOT}/DualQuat.cpp ${SOFATYPESRC_ROOT}/BoundingBox.cpp + ${SOFATYPESRC_ROOT}/DualQuat.cpp ${SOFATYPESRC_ROOT}/Frame.cpp ${SOFATYPESRC_ROOT}/Material.cpp ${SOFATYPESRC_ROOT}/PrimitiveGroup.cpp + ${SOFATYPESRC_ROOT}/Quat.cpp ${SOFATYPESRC_ROOT}/RGBAColor.cpp + ${SOFATYPESRC_ROOT}/SVector.cpp + ${SOFATYPESRC_ROOT}/SpatialVector.cpp + ${SOFATYPESRC_ROOT}/Transform.cpp + ${SOFATYPESRC_ROOT}/Vec.cpp ${SOFATYPESRC_ROOT}/fixed_array.cpp ${SOFATYPESRC_ROOT}/vector.cpp - ${SOFATYPESRC_ROOT}/vector_Real.cpp ${SOFATYPESRC_ROOT}/vector_Integral.cpp + ${SOFATYPESRC_ROOT}/vector_Real.cpp ${SOFATYPESRC_ROOT}/vector_String.cpp - ${SOFATYPESRC_ROOT}/SVector.cpp - ${SOFATYPESRC_ROOT}/isRigidType.h ) sofa_find_package(Sofa.Config REQUIRED) diff --git a/Sofa/framework/Type/src/sofa/type/SpatialVector.cpp b/Sofa/framework/Type/src/sofa/type/SpatialVector.cpp new file mode 100644 index 00000000000..f145c294f7c --- /dev/null +++ b/Sofa/framework/Type/src/sofa/type/SpatialVector.cpp @@ -0,0 +1,29 @@ +/****************************************************************************** +* SOFA, Simulation Open-Framework Architecture * +* (c) 2006 INRIA, USTL, UJF, CNRS, MGH * +* * +* This program is free software; you can redistribute it and/or modify it * +* under the terms of the GNU Lesser General Public License as published by * +* the Free Software Foundation; either version 2.1 of the License, or (at * +* your option) any later version. * +* * +* This program is distributed in the hope that it will be useful, but WITHOUT * +* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or * +* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License * +* for more details. * +* * +* You should have received a copy of the GNU Lesser General Public License * +* along with this program. If not, see . * +******************************************************************************* +* Authors: The SOFA Team and external contributors (see Authors.txt) * +* * +* Contact information: contact@sofa-framework.org * +******************************************************************************/ +#define SOFA_TYPE_SPATIALVECTOR_CPP +#include + +namespace sofa::type +{ +template class SOFA_TYPE_API SpatialVector; +template class SOFA_TYPE_API SpatialVector; +} diff --git a/Sofa/framework/Type/src/sofa/type/SpatialVector.h b/Sofa/framework/Type/src/sofa/type/SpatialVector.h new file mode 100644 index 00000000000..06ff949afe4 --- /dev/null +++ b/Sofa/framework/Type/src/sofa/type/SpatialVector.h @@ -0,0 +1,158 @@ +/****************************************************************************** +* SOFA, Simulation Open-Framework Architecture * +* (c) 2006 INRIA, USTL, UJF, CNRS, MGH * +* * +* This program is free software; you can redistribute it and/or modify it * +* under the terms of the GNU Lesser General Public License as published by * +* the Free Software Foundation; either version 2.1 of the License, or (at * +* your option) any later version. * +* * +* This program is distributed in the hope that it will be useful, but WITHOUT * +* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or * +* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License * +* for more details. * +* * +* You should have received a copy of the GNU Lesser General Public License * +* along with this program. If not, see . * +******************************************************************************* +* Authors: The SOFA Team and external contributors (see Authors.txt) * +* * +* Contact information: contact@sofa-framework.org * +******************************************************************************/ +#pragma once + +#include +#include + +namespace sofa::type +{ + +/** + * A spatial vector. + * When representing a velocity, lineVec is the angular velocity and freeVec is the linear velocity. + * When representing a spatial force, lineVec is the force and freeVec is the torque. + * */ +template +class SpatialVector +{ +public: + using Real = TReal; + using Vec = sofa::type::Vec<3, TReal>; + using Mat66 = sofa::type::Mat<6, 6, TReal>; + + Vec lineVec{ type::NOINIT }; + Vec freeVec{ type::NOINIT }; + + void clear(); + SpatialVector() = default; + /** + \param l The line vector: angular velocity, or force + \param f The free vector: linear velocity, or torque + */ + SpatialVector( const Vec& l, const Vec& f ); + + + SpatialVector& operator+= (const SpatialVector& v); + + //template + SpatialVector operator* ( Real a ) const + { + return SpatialVector( lineVec *a, freeVec * a); + } + + SpatialVector& operator*= ( Real a ) + { + lineVec *=a; + freeVec *= a; + return *this; + } + + SpatialVector operator + ( const SpatialVector& v ) const; + SpatialVector operator - ( const SpatialVector& v ) const; + SpatialVector operator - ( ) const; + /// Spatial dot product (cross terms) + Real operator* ( const SpatialVector& v ) const; + /// Spatial cross product + SpatialVector cross( const SpatialVector& v ) const; + /// product with a dense matrix + SpatialVector operator* (const Mat66&) const; + + /// write to an output stream + inline friend std::ostream& operator << (std::ostream& out, const SpatialVector& t ) + { + out << t.lineVec << " " << t.freeVec; + return out; + } + + /// read from an input stream + inline friend std::istream& operator >> ( std::istream& in, SpatialVector& t ) + { + in >> t.lineVec >> t.freeVec; + return in; + } + + /// If the SpatialVector models a spatial velocity, then the linear velocity is the freeVec. + /// Otherwise, the SpatialVector models a spatial force, and this method returns a torque. + Vec& getLinearVelocity() + { + return freeVec; + } + const Vec& getLinearVelocity() const + { + return freeVec; + } + void setLinearVelocity(const Vec& v) + { + freeVec = v; + } + /// If the SpatialVector models a spatial velocity, then the angular velocity is the lineVec. + /// Otherwise, the SpatialVector models a spatial force, and this method returns a force. + Vec& getAngularVelocity() + { + return lineVec; + } + const Vec& getAngularVelocity() const + { + return lineVec; + } + void setAngularVelocity(const Vec& v) + { + lineVec = v; + } + + /// If the SpatialVector models a spatial force, then the torque is the freeVec. + /// Otherwise, the SpatialVector models a spatial velocity, and this method returns a linear velocity. + Vec& getTorque() + { + return freeVec; + } + const Vec& getTorque() const + { + return freeVec; + } + void setTorque(const Vec& v) + { + freeVec = v; + } + /// If the SpatialVector models a spatial force, then the torque is the lineVec. + /// Otherwise, the SpatialVector models a spatial velocity, and this method returns an angular velocity. + Vec& getForce() + { + return lineVec; + } + const Vec& getForce() const + { + return lineVec; + } + void setForce(const Vec& v) + { + lineVec = v; + } +}; + +#if !defined(SOFA_TYPE_SPATIALVECTOR_CPP) +extern template class SOFA_TYPE_API SpatialVector; +extern template class SOFA_TYPE_API SpatialVector; +#endif + +} diff --git a/Sofa/framework/Type/src/sofa/type/SpatialVector.inl b/Sofa/framework/Type/src/sofa/type/SpatialVector.inl new file mode 100644 index 00000000000..af2a5376cb9 --- /dev/null +++ b/Sofa/framework/Type/src/sofa/type/SpatialVector.inl @@ -0,0 +1,101 @@ +/****************************************************************************** +* SOFA, Simulation Open-Framework Architecture * +* (c) 2006 INRIA, USTL, UJF, CNRS, MGH * +* * +* This program is free software; you can redistribute it and/or modify it * +* under the terms of the GNU Lesser General Public License as published by * +* the Free Software Foundation; either version 2.1 of the License, or (at * +* your option) any later version. * +* * +* This program is distributed in the hope that it will be useful, but WITHOUT * +* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or * +* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License * +* for more details. * +* * +* You should have received a copy of the GNU Lesser General Public License * +* along with this program. If not, see . * +******************************************************************************* +* Authors: The SOFA Team and external contributors (see Authors.txt) * +* * +* Contact information: contact@sofa-framework.org * +******************************************************************************/ +#pragma once + +#include + +namespace sofa::type +{ + +template +SpatialVector::SpatialVector( const Vec& l, const Vec& f ) + : lineVec(l) + , freeVec(f) +{} + +template +void SpatialVector::clear() +{ + lineVec = freeVec = Vec(0, 0, 0); +} + +template +SpatialVector& SpatialVector::operator+= (const SpatialVector& v) +{ + lineVec += v.lineVec; + freeVec += v.freeVec; + return *this; +} + +template +SpatialVector SpatialVector::operator+( const SpatialVector& v ) const +{ + return SpatialVector(lineVec + v.lineVec, freeVec + v.freeVec); +} + +template +SpatialVector SpatialVector::operator-( const SpatialVector& v ) const +{ + return SpatialVector(lineVec - v.lineVec, freeVec - v.freeVec); +} + +template +SpatialVector SpatialVector::operator- ( ) const +{ + return SpatialVector(-lineVec, -freeVec); +} + +/// Spatial dot product (cross terms) +template +TReal SpatialVector::operator* ( const SpatialVector& v ) const +{ + return lineVec * v.freeVec + freeVec * v.lineVec; +} + +/// Spatial cross product +template +SpatialVector SpatialVector::cross( const SpatialVector& v ) const +{ + return SpatialVector( + type::cross(lineVec,v.lineVec), + type::cross(freeVec,v.lineVec) + type::cross(lineVec,v.freeVec) + ); +} + +template +SpatialVector SpatialVector::operator* (const Mat66& m) const +{ + SpatialVector result; + for (int i = 0; i < 3; i++) + { + result.lineVec[i] = 0; + result.freeVec[i] = 0; + for (int j = 0; j < 3; j++) + { + result.lineVec[i] += lineVec[j] * m[i][j] + freeVec[j] * m[i][j + 3]; + result.freeVec[i] += lineVec[j] * m[i + 3][j] + freeVec[j] * m[i + 3][j + 3]; + } + } + return result; +} + +} diff --git a/Sofa/framework/Type/src/sofa/type/Transform.cpp b/Sofa/framework/Type/src/sofa/type/Transform.cpp new file mode 100644 index 00000000000..2f00857d766 --- /dev/null +++ b/Sofa/framework/Type/src/sofa/type/Transform.cpp @@ -0,0 +1,29 @@ +/****************************************************************************** +* SOFA, Simulation Open-Framework Architecture * +* (c) 2006 INRIA, USTL, UJF, CNRS, MGH * +* * +* This program is free software; you can redistribute it and/or modify it * +* under the terms of the GNU Lesser General Public License as published by * +* the Free Software Foundation; either version 2.1 of the License, or (at * +* your option) any later version. * +* * +* This program is distributed in the hope that it will be useful, but WITHOUT * +* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or * +* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License * +* for more details. * +* * +* You should have received a copy of the GNU Lesser General Public License * +* along with this program. If not, see . * +******************************************************************************* +* Authors: The SOFA Team and external contributors (see Authors.txt) * +* * +* Contact information: contact@sofa-framework.org * +******************************************************************************/ +#define SOFA_TYPE_TRANSFORM_CPP +#include + +namespace sofa::type +{ +template class SOFA_TYPE_API Transform; +template class SOFA_TYPE_API Transform; +} diff --git a/Sofa/framework/Type/src/sofa/type/Transform.h b/Sofa/framework/Type/src/sofa/type/Transform.h new file mode 100644 index 00000000000..6303fbfec65 --- /dev/null +++ b/Sofa/framework/Type/src/sofa/type/Transform.h @@ -0,0 +1,180 @@ +/****************************************************************************** +* SOFA, Simulation Open-Framework Architecture * +* (c) 2006 INRIA, USTL, UJF, CNRS, MGH * +* * +* This program is free software; you can redistribute it and/or modify it * +* under the terms of the GNU Lesser General Public License as published by * +* the Free Software Foundation; either version 2.1 of the License, or (at * +* your option) any later version. * +* * +* This program is distributed in the hope that it will be useful, but WITHOUT * +* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or * +* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License * +* for more details. * +* * +* You should have received a copy of the GNU Lesser General Public License * +* along with this program. If not, see . * +******************************************************************************* +* Authors: The SOFA Team and external contributors (see Authors.txt) * +* * +* Contact information: contact@sofa-framework.org * +******************************************************************************/ +#pragma once +#include +#include + +namespace sofa::type +{ + +/** + * Define a frame (child) whith respect to another (parent). A frame represents a local coordinate system. + * + * Internal data represents the orientation of the child wrt the parent, BUT the + * translation vector represents the origin of the parent with respect to the + * child. For example, the coordinates M_p of point M in parent given the + * coordinates M_c of the same point in child are given by: + * M_p = orientation * ( M_c - origin ). This is due to Featherstone's + * conventions. Use method setTranslationRotation( const Vec& t, const Rot& q ) + * to model the Transform the standard way (i.e. translation givne in the parent frame). + **/ +template +class Transform +{ +public: + using Real = TReal; + using Vec = sofa::type::Vec<3, TReal>; + using Rot = type::Quat; + using Mat3x3 = type::Mat<3,3,Real>; + using Mat6x6 = sofa::type::Mat<6, 6, TReal>; + + /// The default constructor does not initialize the transform + Transform(); + /// Origin of the child in parent coordinates, orientation of the child wrt to parent + Transform( const Vec& origin, const Rot& orientation ); + /// WARNING: using Featherstone's conventions (see class documentation) + Transform( const Rot& q, const Vec& o ); + /// Origin of the child in the parent coordinate system and the orientation of the child wrt the parent (i.e. standard way) + void set( const Vec& t, const Rot& q ); + /// Reset this to identity + void clear(); + /// The identity transform (child = parent) + static Transform identity(); + /// Origin of the child in the parent coordinate system and the orientation of the child wrt the parent (i.e. standard way) + //static Transform inParent(const Vec& t, const Rot& r); + /// Define child as a given SpatialVector integrated during one second, starting from the parent (used for time integration). The spatial vector is given in parent coordinates. + Transform( const SpatialVector& v ); + /// The inverse transform i.e. parent wrt child + Transform inversed() const; + /// Parent origin in child coordinates (the way it is actually stored internally) + const Vec& getOriginOfParentInChild() const; + /// Origin of child in parent coordinates + Vec getOrigin() const; + /// Origin of child in parent coordinates + void setOrigin( const Vec& ); + /// Orientation of the child coordinate axes wrt the parent coordinate axes + const Rot& getOrientation() const; + /// Orientation of the child coordinate axes wrt the parent coordinate axes + void setOrientation( const Rot& ); + /// Matrix which projects vectors from child coordinates to parent coordinates. The columns of the matrix are the axes of the child base axes in the parent coordinate system. + Mat3x3 getRotationMatrix() const; + + + /** + * \brief Adjoint matrix to the transform + * This matrix transports velocities in twist coordinates from the child frame to the parent frame. + * Its inverse transpose does the same for the wrenches + */ + Mat6x6 getAdjointMatrix() const; + + /// Project a vector (i.e. a direction or a displacement) from child coordinates to parent coordinates + Vec projectVector( const Vec& vectorInChild ) const; + /// Project a point from child coordinates to parent coordinates + Vec projectPoint( const Vec& pointInChild ) const; + /// Projected a vector (i.e. a direction or a displacement) from parent coordinates to child coordinates + Vec backProjectVector( const Vec& vectorInParent ) const; + /// Project point from parent coordinates to this coordinates + Vec backProjectPoint( const Vec& pointInParent ) const; + /// Combine two transforms. If (*this) locates frame B (child) wrt frame A (parent) and if f2 locates frame C (child) wrt frame B (parent) then the result locates frame C wrt to Frame A. + Transform operator * (const Transform& f2) const; + /// Combine two transforms. If (*this) locates frame B (child) wrt frame A (parent) and if f2 locates frame C (child) wrt frame B (parent) then the result locates frame C wrt to Frame A. + Transform& operator *= (const Transform& f2); + + /** Project a spatial vector from child to parent + * TODO One should handle differently the transformation of a twist and a wrench ! + * This applying the adjoint to velocities or its transpose to wrench : + * V_parent = Ad . V_child or W_child = Ad^T . W_parent + * To project a wrench in the child frame to the parent frame you need to do + * parent_wrench = this->inversed * child_wrench + * (this doc needs to be douv-ble checked !) + */ + // create a spatial Vector from a small transformation + SpatialVector CreateSpatialVector(); + SpatialVector DTrans(); + + SpatialVector operator * (const SpatialVector& sv ) const; + /// Project a spatial vector from parent to child (the inverse of operator *). This method computes (*this).inversed()*sv without inverting (*this). + SpatialVector operator / (const SpatialVector& sv ) const; + /// Write an OpenGL matrix encoding the transformation of the coordinate system of the child wrt the coordinate system of the parent. + void writeOpenGlMatrix( double *m ) const; + /// Draw the axes of the child coordinate system in the parent coordinate system + /// Print the origin of the child in the parent coordinate system and the quaternion defining the orientation of the child wrt the parent + inline friend std::ostream& operator << (std::ostream& out, const Transform& t ) + { + out << t.getOrigin() << " " << t.getOrientation(); + return out; + } + + /// read from an input stream + inline friend std::istream& operator >> ( std::istream& in, Transform& t ) + { + Vec origin; + Rot orientation; + + in >> origin >> orientation; + + t.set(origin, orientation); + + return in; + } + + /// Print the internal values (i.e. using Featherstone's conventions, see class documentation) + void printInternal( std::ostream&) const; + + /** @name Time integration + * Methods used in time integration + */ + ///@{ + /// (*this) *= Transform(v) Used for time integration. SHOULD WE RATHER APPLY (*this)=Transform(v)*(*this) ??? + Transform& operator +=(const SpatialVector& a); + + Transform& operator +=(const Transform& a); + + template + Transform& operator*=(Real2 a) + { + origin_ *= a; + return *this; + } + + template + Transform operator*(Real2 a) const + { + Transform r = *this; + r*=a; + return r; + } + ///@} + + +protected: + Rot orientation_; ///< child wrt parent + Vec origin_; ///< parent wrt child + +}; + +#if !defined(SOFA_TYPE_TRANSFORM_CPP) +extern template class SOFA_TYPE_API Transform; +extern template class SOFA_TYPE_API Transform; +#endif + +} diff --git a/Sofa/framework/Type/src/sofa/type/Transform.inl b/Sofa/framework/Type/src/sofa/type/Transform.inl new file mode 100644 index 00000000000..bee352c9dc2 --- /dev/null +++ b/Sofa/framework/Type/src/sofa/type/Transform.inl @@ -0,0 +1,275 @@ +/****************************************************************************** +* SOFA, Simulation Open-Framework Architecture * +* (c) 2006 INRIA, USTL, UJF, CNRS, MGH * +* * +* This program is free software; you can redistribute it and/or modify it * +* under the terms of the GNU Lesser General Public License as published by * +* the Free Software Foundation; either version 2.1 of the License, or (at * +* your option) any later version. * +* * +* This program is distributed in the hope that it will be useful, but WITHOUT * +* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or * +* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License * +* for more details. * +* * +* You should have received a copy of the GNU Lesser General Public License * +* along with this program. If not, see . * +******************************************************************************* +* Authors: The SOFA Team and external contributors (see Authors.txt) * +* * +* Contact information: contact@sofa-framework.org * +******************************************************************************/ +#pragma once +#include + +namespace sofa::type +{ + +template +Transform::Transform() + : orientation_() // default constructor set to identity + , origin_() // default constructor set to {0, 0, 0} +{ + +} + +/// Define using Featherstone's conventions +template +Transform::Transform( const Rot& q, const Vec& o ) + : orientation_(q) + , origin_(o) +{} + +/// Define using standard conventions +template +Transform::Transform( const Vec& t, const Rot& q ) + : orientation_(q) + , origin_(-(q.inverseRotate(t))) +{} + +/// Define given the origin of the child wrt the parent and the orientation of the child wrt the parent (i.e. standard way) +template +void Transform::set +( const Vec& t, const Rot& q ) +{ + orientation_ =q, origin_ = -(q.inverseRotate(t)); +} + +/// Define given the origin of the child wrt the parent and the orientation of the child wrt the parent (i.e. standard way) +template +Transform Transform::identity() +{ + return Transform( Rot::identity(), Vec(0,0,0) ); +} + +/// Define as a given SpatialVector integrated during one second. The spatial vector is given in parent coordinates. +template +Transform::Transform( const SpatialVector& v ) +{ + orientation_ = Rot::createFromRotationVector( v.lineVec ); + origin_ = - orientation_.inverseRotate( v.freeVec ); +} + +template +const typename Transform::Vec& Transform::getOriginOfParentInChild() const +{ + return origin_; +} + +template +typename Transform::Vec Transform::getOrigin() const +{ + return -orientation_.rotate(origin_); +} + +template +void Transform::setOrigin( const Vec& op ) +{ + origin_ = -orientation_.inverseRotate(op); +} + +template +const typename Transform::Rot& Transform::getOrientation() const +{ + return orientation_; +} + +template +void Transform::setOrientation( const Rot& q ) +{ + orientation_=q; +} +template +SpatialVector Transform::DTrans() +{ + + return SpatialVector(orientation_.quatToRotationVector(), this->getOrigin()); + // Use of quatToRotationVector instead of toEulerVector: + // this is done to keep the old behavior (before the + // correction of the toEulerVector function). If the + // purpose was to obtain the Eulerian vector and not the + // rotation vector please use the following line instead + //return SpatialVector(orientation_.toEulerVector(), this->getOrigin()); +} + +template +typename Transform::Vec Transform::projectVector( const Vec& v ) const +{ + return orientation_.rotate( v ); +} + +template +auto Transform::projectPoint(const Vec& p) const -> Transform::Vec +{ + return orientation_.rotate( p - origin_ ); +} + +template +auto Transform::backProjectVector(const Vec& v) const -> Transform::Vec +{ + return orientation_.inverseRotate( v ); +} + +template +auto Transform::backProjectPoint(const Vec& p) const -> Transform::Vec +{ + return orientation_.inverseRotate( p ) + origin_; +} + +template +auto Transform::getRotationMatrix() const -> Transform::Mat3x3 +{ + Mat3x3 m; + m[0][0] = (1.0f - 2.0f * (orientation_[1] * orientation_[1] + orientation_[2] * orientation_[2])); + m[0][1] = (2.0f * (orientation_[0] * orientation_[1] - orientation_[2] * orientation_[3])); + m[0][2] = (2.0f * (orientation_[2] * orientation_[0] + orientation_[1] * orientation_[3])); + + m[1][0] = (2.0f * (orientation_[0] * orientation_[1] + orientation_[2] * orientation_[3])); + m[1][1] = (1.0f - 2.0f * (orientation_[2] * orientation_[2] + orientation_[0] * orientation_[0])); + m[1][2] = (2.0f * (orientation_[1] * orientation_[2] - orientation_[0] * orientation_[3])); + + m[2][0] = (2.0f * (orientation_[2] * orientation_[0] - orientation_[1] * orientation_[3])); + m[2][1] = (2.0f * (orientation_[1] * orientation_[2] + orientation_[0] * orientation_[3])); + m[2][2] = (1.0f - 2.0f * (orientation_[1] * orientation_[1] + orientation_[0] * orientation_[0])); + return m; +} + +template +auto Transform::getAdjointMatrix() const -> Transform::Mat6x6 +{ + /// TODO + Mat6x6 Adj; + Mat3x3 Rot; + Rot = this->getRotationMatrix(); + // correspond au produit vectoriel v^origin + Mat3x3 Origin; + Origin[0][0]=(Real)0.0; Origin[0][1]=origin_[2]; Origin[0][2]=-origin_[1]; + Origin[1][0]=-origin_[2]; Origin[1][1]=(Real)0.0; Origin[1][2]=origin_[0]; + Origin[2][0]=origin_[1]; Origin[2][1]=-origin_[0]; Origin[2][2]=(Real)0.0; + + Mat3x3 R_Origin = Rot*Origin; + + for (int i=0; i<3; i++) + { + for(int j=0; j<3; j++) + { + Adj[i][j] = Rot[i][j]; + Adj[i+3][j+3] = Rot[i][j]; + Adj[i][j+3] = R_Origin[i][j]; + Adj[i+3][j] = 0.0; + } + } + + + return Adj; +} + +template +void Transform::clear() +{ + orientation_.clear(); + origin_=Vec(0,0,0); +} + +template +Transform Transform::operator * (const Transform& f2) const +{ + return Transform( orientation_ * f2.getOrientation(), f2.getOriginOfParentInChild() + f2.getOrientation().inverseRotate(origin_)) ; +} + +template +Transform& Transform::operator *= (const Transform& f2) +{ + orientation_ *= f2.getOrientation(); + origin_ = f2.getOriginOfParentInChild() + f2.getOrientation().inverseRotate(origin_); + return (*this); +} + + +template +SpatialVector Transform::CreateSpatialVector() +{ + return SpatialVector(this->getOrientation().quatToRotationVector(), this->getOrigin() ); +} + +template +SpatialVector Transform::operator * (const SpatialVector& sv ) const +{ + return SpatialVector( + orientation_.rotate(sv.lineVec), + orientation_.rotate( cross(sv.lineVec, origin_ ) + sv.freeVec) + ); +} + +template +SpatialVector Transform::operator / (const SpatialVector& sv ) const +{ + return inversed()*sv; +} + + + + +template +Transform Transform::inversed() const +{ + return Transform( orientation_.inverse(), -(orientation_.rotate(origin_)) ); +} + +template +void Transform::writeOpenGlMatrix( double *m ) const +{ + orientation_.writeOpenGlMatrix(m); + Vec t = getOrigin(); + m[12] = t[0]; + m[13] = t[1]; + m[14] = t[2]; +} + +template +void Transform::printInternal( std::ostream& out ) const +{ + out<<"internal t= "<