From bef44782ec78bbfa6f70cc2a4694a9384f6b17da Mon Sep 17 00:00:00 2001 From: Damien Marchal Date: Tue, 11 Jun 2024 10:59:39 +0200 Subject: [PATCH 01/26] Rename BaseCosserat into BaseCosseratMapping --- src/Cosserat/mapping/BaseCosserat.cpp | 26 +++++---- src/Cosserat/mapping/BaseCosserat.h | 20 +++---- src/Cosserat/mapping/BaseCosserat.inl | 55 ++++++++++--------- src/Cosserat/mapping/DifferenceMultiMapping.h | 2 +- .../mapping/DifferenceMultiMapping.inl | 2 +- .../mapping/DiscreteCosseratMapping.h | 34 ++++++------ .../mapping/DiscreteDynamicCosseratMapping.h | 30 +++++----- src/Cosserat/mapping/RigidDistanceMapping.h | 2 +- 8 files changed, 89 insertions(+), 82 deletions(-) diff --git a/src/Cosserat/mapping/BaseCosserat.cpp b/src/Cosserat/mapping/BaseCosserat.cpp index 6e08e40..852484c 100644 --- a/src/Cosserat/mapping/BaseCosserat.cpp +++ b/src/Cosserat/mapping/BaseCosserat.cpp @@ -19,20 +19,21 @@ * * * Contact information: contact@sofa-framework.org * ******************************************************************************/ -#define SOFA_COSSERAT_CPP_BaseCosserat +#define SOFA_COSSERAT_CPP_BaseCosseratMapping #include #include #include #include -namespace Cosserat::mapping { +namespace Cosserat::mapping +{ using namespace sofa::defaulttype; template <> -BaseCosserat::se3 -BaseCosserat::buildXiHat( - const Coord1 &strain_i) { +BaseCosseratMapping::se3 +BaseCosseratMapping::buildXiHat(const Coord1 &strain_i) +{ se3 Xi; Xi[0][1] = -strain_i(2); @@ -58,7 +59,7 @@ BaseCosserat::buildXiHat( } template <> -void BaseCosserat::computeExponentialSE3( +void BaseCosseratMapping::computeExponentialSE3( const double &curv_abs_x_n, const Coord1 &strain_n, Transform &Trans) { Matrix4 I4; I4.identity(); @@ -95,7 +96,7 @@ void BaseCosserat::computeExponentialSE3( } template <> -void BaseCosserat::computeTangExp( +void BaseCosseratMapping::computeTangExp( double &curv_abs_n, const Coord1 &strain_i, Mat6x6 &TgX) { SReal theta = Vec3(strain_i(0), strain_i(1), strain_i(2)) @@ -145,22 +146,23 @@ void BaseCosserat::computeTangExp( } } + // Register in the Factory -int BaseCosseratClass = +int BaseCosseratMappingClass = sofa::core::RegisterObject( "Set the positions and velocities of points attached to a rigid parent") - .add>() - .add>(); template class SOFA_COSSERAT_API - BaseCosserat; template class SOFA_COSSERAT_API - BaseCosserat; } // namespace cosserat::mapping diff --git a/src/Cosserat/mapping/BaseCosserat.h b/src/Cosserat/mapping/BaseCosserat.h index 09c77c3..02ce144 100644 --- a/src/Cosserat/mapping/BaseCosserat.h +++ b/src/Cosserat/mapping/BaseCosserat.h @@ -75,9 +75,9 @@ typedef typename Eigen::Matrix4d _SE3; // TODO(dmarchal: 2024/06/07) This component looks like a mapping but inherit // from BaseObject * can you clarify why is is not inhering from BaseMapping template -class BaseCosserat : public virtual sofa::core::objectmodel::BaseObject { +class BaseCosseratMapping : public virtual sofa::core::objectmodel::BaseObject { public: - SOFA_CLASS(SOFA_TEMPLATE3(BaseCosserat, TIn1, TIn2, TOut), BaseObject); + SOFA_CLASS(SOFA_TEMPLATE3(BaseCosseratMapping, TIn1, TIn2, TOut), BaseObject); typedef BaseObject Inherit; /// Input Model Type @@ -118,14 +118,14 @@ class BaseCosserat : public virtual sofa::core::objectmodel::BaseObject { typedef sofa::Data OutDataVecDeriv; typedef sofa::Data OutDataMatrixDeriv; - typedef sofa::MultiLink, sofa::core::State, + typedef sofa::MultiLink, sofa::core::State, sofa::BaseLink::FLAG_STOREPATH | sofa::BaseLink::FLAG_STRONGLINK> LinkFromModels1; - typedef sofa::MultiLink, sofa::core::State, + typedef sofa::MultiLink, sofa::core::State, sofa::BaseLink::FLAG_STOREPATH | sofa::BaseLink::FLAG_STRONGLINK> LinkFromModels2; [[maybe_unused]] typedef sofa::MultiLink< - BaseCosserat, sofa::core::State, + BaseCosseratMapping, sofa::core::State, sofa::BaseLink::FLAG_STOREPATH | sofa::BaseLink::FLAG_STRONGLINK> LinkToModels; @@ -241,9 +241,9 @@ class BaseCosserat : public virtual sofa::core::objectmodel::BaseObject { protected: /// Constructor - BaseCosserat(); + BaseCosseratMapping(); /// Destructor - ~BaseCosserat() override = default; + ~BaseCosseratMapping() override = default; void computeExponentialSE3(const double &x, const Coord1 &k, Transform &Trans); @@ -266,12 +266,12 @@ class BaseCosserat : public virtual sofa::core::objectmodel::BaseObject { Matrix4 computeLogarithm(const double &x, const Mat4x4 &gX); }; -#if !defined(SOFA_COSSERAT_CPP_BaseCosserat) +#if !defined(SOFA_COSSERAT_CPP_BaseCosseratMapping) extern template class SOFA_COSSERAT_API - BaseCosserat; extern template class SOFA_COSSERAT_API - BaseCosserat; #endif diff --git a/src/Cosserat/mapping/BaseCosserat.inl b/src/Cosserat/mapping/BaseCosserat.inl index 507aedb..e272948 100644 --- a/src/Cosserat/mapping/BaseCosserat.inl +++ b/src/Cosserat/mapping/BaseCosserat.inl @@ -39,15 +39,17 @@ namespace Cosserat::mapping { -namespace { +namespace +{ +using namespace sofa::defaulttype; using sofa::core::objectmodel::BaseContext; using sofa::helper::AdvancedTimer; using sofa::helper::WriteAccessor; using sofa::type::vector; -} // namespace +} template -BaseCosserat::BaseCosserat() +BaseCosseratMapping::BaseCosseratMapping() : d_curv_abs_section(initData(&d_curv_abs_section, "curv_abs_input", " need to be com....")), d_curv_abs_frames(initData(&d_curv_abs_frames, "curv_abs_output", @@ -59,7 +61,7 @@ BaseCosserat::BaseCosserat() // _________________________________________________________________________________________ template -void BaseCosserat::init() { +void BaseCosseratMapping::init() { Inherit1::init(); // Fill the initial vector @@ -69,7 +71,7 @@ void BaseCosserat::init() { } template -void BaseCosserat::computeExponentialSE3( +void BaseCosseratMapping::computeExponentialSE3( const double &curv_abs_x_n, const Coord1 &strain_n, Transform &g_X_n) { Matrix4 I4; I4.identity(); @@ -107,7 +109,7 @@ void BaseCosserat::computeExponentialSE3( // Fill exponential vectors template -void BaseCosserat::updateExponentialSE3( +void BaseCosseratMapping::updateExponentialSE3( const In1VecCoord &inDeform) { // helper::ReadAccessor>> curv_abs_input = // d_curv_abs_section; @@ -175,7 +177,7 @@ void BaseCosserat::updateExponentialSE3( // m_nodesLogarithmSE3Vectors.push_back(log_gX); template -void BaseCosserat::computeAdjoint(const Transform &frame, +void BaseCosseratMapping::computeAdjoint(const Transform &frame, Tangent &adjoint) { Matrix3 R = extractRotMatrix(frame); Vec3 u = frame.getOrigin(); @@ -185,7 +187,7 @@ void BaseCosserat::computeAdjoint(const Transform &frame, } template -void BaseCosserat::computeCoAdjoint(const Transform &frame, +void BaseCosseratMapping::computeCoAdjoint(const Transform &frame, Mat6x6 &co_adjoint) { Matrix3 R = extractRotMatrix(frame); Vec3 u = frame.getOrigin(); @@ -195,7 +197,7 @@ void BaseCosserat::computeCoAdjoint(const Transform &frame, } template -void BaseCosserat::computeAdjoint(const Vec6 &eta, +void BaseCosseratMapping::computeAdjoint(const Vec6 &eta, Mat6x6 &adjoint) { Matrix3 tildeMat = getTildeMatrix(Vec3(eta[0], eta[1], eta[2])); adjoint.setsub(0, 0, tildeMat); @@ -204,7 +206,7 @@ void BaseCosserat::computeAdjoint(const Vec6 &eta, } template -Matrix4 BaseCosserat::computeLogarithm(const double &x, +Matrix4 BaseCosseratMapping::computeLogarithm(const double &x, const Mat4x4 &gX) { // Compute theta before everything const double theta = computeTheta(x, gX); @@ -238,7 +240,7 @@ Matrix4 BaseCosserat::computeLogarithm(const double &x, } template -void BaseCosserat::updateTangExpSE3( +void BaseCosseratMapping::updateTangExpSE3( const In1VecCoord &inDeform) { // Curv abscissa of nodes and frames @@ -289,7 +291,7 @@ void BaseCosserat::updateTangExpSE3( } template -void BaseCosserat::computeTangExp(double &curv_abs_n, +void BaseCosseratMapping::computeTangExp(double &curv_abs_n, const Coord1 &strain_i, Mat6x6 &TgX) { @@ -340,7 +342,7 @@ void BaseCosserat::computeTangExp(double &curv_abs_n, template [[maybe_unused]] Vec6 -BaseCosserat::computeETA(const Vec6 &baseEta, +BaseCosseratMapping::computeETA(const Vec6 &baseEta, const In1VecDeriv &k_dot, const double abs_input) { @@ -382,7 +384,7 @@ BaseCosserat::computeETA(const Vec6 &baseEta, //___________________________________________________________________________ template -void BaseCosserat::initialize() { +void BaseCosseratMapping::initialize() { // For each frame in the global frame, find the segment of the beam to which // it is attached. Here we only use the information from the curvilinear // abscissa of each frame. @@ -442,10 +444,10 @@ void BaseCosserat::initialize() { } template -void BaseCosserat::draw(const sofa::core::visual::VisualParams *) {} +void BaseCosseratMapping::draw(const sofa::core::visual::VisualParams *) {} template -double BaseCosserat::computeTheta(const double &x, +double BaseCosseratMapping::computeTheta(const double &x, const Mat4x4 &gX) { double Tr_gx = 0.0; for (int i = 0; i < 4; i++) { @@ -462,7 +464,7 @@ double BaseCosserat::computeTheta(const double &x, } template -void BaseCosserat::printMatrix(const Mat6x6 R) { +void BaseCosseratMapping::printMatrix(const Mat6x6 R) { // TODO(dmarchal: 2024/06/07): Remove the use of printf in addition to // reconsider the implementation of common utility functions in instance // method. @@ -475,7 +477,7 @@ void BaseCosserat::printMatrix(const Mat6x6 R) { template Matrix3 -BaseCosserat::extractRotMatrix(const Transform &frame) { +BaseCosseratMapping::extractRotMatrix(const Transform &frame) { sofa::type::Quat q = frame.getOrientation(); @@ -492,7 +494,7 @@ BaseCosserat::extractRotMatrix(const Transform &frame) { } template -auto BaseCosserat::buildProjector(const Transform &T) +auto BaseCosseratMapping::buildProjector(const Transform &T) -> Tangent { Mat6x6 P; @@ -511,7 +513,7 @@ auto BaseCosserat::buildProjector(const Transform &T) } template -auto BaseCosserat::buildXiHat(const Coord1 &strain_i) -> se3 { +auto BaseCosseratMapping::buildXiHat(const Coord1 &strain_i) -> se3 { se3 Xi_hat; Xi_hat[0][1] = -strain_i(2); @@ -528,7 +530,7 @@ auto BaseCosserat::buildXiHat(const Coord1 &strain_i) -> se3 { } template -auto BaseCosserat::getTildeMatrix(const sofa::type::Vec3 &u) +auto BaseCosseratMapping::getTildeMatrix(const sofa::type::Vec3 &u) -> Matrix3 { sofa::type::Matrix3 tild; tild[0][1] = -u[2]; @@ -542,7 +544,7 @@ auto BaseCosserat::getTildeMatrix(const sofa::type::Vec3 &u) } template -auto BaseCosserat::buildAdjoint(const Matrix3 &A, +auto BaseCosseratMapping::buildAdjoint(const Matrix3 &A, const Matrix3 &B, Mat6x6 &Adjoint) -> void { Adjoint.clear(); @@ -556,7 +558,7 @@ auto BaseCosserat::buildAdjoint(const Matrix3 &A, } template -auto BaseCosserat::buildCoAdjoint(const Matrix3 &A, +auto BaseCosseratMapping::buildCoAdjoint(const Matrix3 &A, const Matrix3 &B, Mat6x6 &coAdjoint) -> void { coAdjoint.clear(); @@ -573,7 +575,7 @@ auto BaseCosserat::buildCoAdjoint(const Matrix3 &A, } template -auto BaseCosserat::convertTransformToMatrix4x4( +auto BaseCosseratMapping::convertTransformToMatrix4x4( const Transform &T) -> Matrix4 { Matrix4 M; M.identity(); @@ -590,7 +592,7 @@ auto BaseCosserat::convertTransformToMatrix4x4( } template -auto BaseCosserat::piecewiseLogmap(const _SE3 &g_x) -> Vec6 { +auto BaseCosseratMapping::piecewiseLogmap(const _SE3 &g_x) -> Vec6 { _SE3 Xi_hat; double x = 1.0; @@ -625,4 +627,7 @@ auto BaseCosserat::piecewiseLogmap(const _SE3 &g_x) -> Vec6 { return xci; } + + + } // namespace cosserat::mapping diff --git a/src/Cosserat/mapping/DifferenceMultiMapping.h b/src/Cosserat/mapping/DifferenceMultiMapping.h index bf94fe5..7ec2b65 100644 --- a/src/Cosserat/mapping/DifferenceMultiMapping.h +++ b/src/Cosserat/mapping/DifferenceMultiMapping.h @@ -41,7 +41,7 @@ using sofa::type::Vec6; using sofa::type::Quat; using std::get; using sofa::type::vector; -using Cosserat::mapping::BaseCosserat; +using Cosserat::mapping::BaseCosseratMapping; /*! * \class DifferenceMultiMapping diff --git a/src/Cosserat/mapping/DifferenceMultiMapping.inl b/src/Cosserat/mapping/DifferenceMultiMapping.inl index d86f5b7..fec6b9d 100644 --- a/src/Cosserat/mapping/DifferenceMultiMapping.inl +++ b/src/Cosserat/mapping/DifferenceMultiMapping.inl @@ -20,7 +20,7 @@ * Contact information: contact@sofa-framework.org * ******************************************************************************/ #pragma once -#include +#include #include #include diff --git a/src/Cosserat/mapping/DiscreteCosseratMapping.h b/src/Cosserat/mapping/DiscreteCosseratMapping.h index ae0ff83..d3eb3ed 100644 --- a/src/Cosserat/mapping/DiscreteCosseratMapping.h +++ b/src/Cosserat/mapping/DiscreteCosseratMapping.h @@ -56,11 +56,11 @@ using sofa::Data; template class DiscreteCosseratMapping : public sofa::core::Multi2Mapping, - public Cosserat::mapping::BaseCosserat { + public Cosserat::mapping::BaseCosseratMapping { public: SOFA_CLASS2(SOFA_TEMPLATE3(DiscreteCosseratMapping, TIn1, TIn2, TOut), SOFA_TEMPLATE3(sofa::core::Multi2Mapping, TIn1, TIn2, TOut), - SOFA_TEMPLATE3(Cosserat::mapping::BaseCosserat, TIn1, TIn2, TOut)); + SOFA_TEMPLATE3(Cosserat::mapping::BaseCosseratMapping, TIn1, TIn2, TOut)); /// Input Model Type typedef TIn1 In1; @@ -136,21 +136,21 @@ class DiscreteCosseratMapping /// otherwise any access to the base::attribute would require /// the "this->" approach. /// - using BaseCosserat::m_indicesVectors; - using BaseCosserat::d_curv_abs_section; - using BaseCosserat::d_curv_abs_frames; - using BaseCosserat::m_nodesTangExpVectors; - using BaseCosserat::m_nodesVelocityVectors; - using BaseCosserat::m_framesExponentialSE3Vectors; - using BaseCosserat::m_framesTangExpVectors; - using BaseCosserat::m_totalBeamForceVectors; - using BaseCosserat::m_nodesExponentialSE3Vectors; - using BaseCosserat::d_debug; - using BaseCosserat::m_vecTransform; - using BaseCosserat::m_nodeAdjointVectors; - using BaseCosserat::m_index_input; - using BaseCosserat::m_indicesVectorsDraw; - using BaseCosserat::computeTheta; + using BaseCosseratMapping::m_indicesVectors; + using BaseCosseratMapping::d_curv_abs_section; + using BaseCosseratMapping::d_curv_abs_frames; + using BaseCosseratMapping::m_nodesTangExpVectors; + using BaseCosseratMapping::m_nodesVelocityVectors; + using BaseCosseratMapping::m_framesExponentialSE3Vectors; + using BaseCosseratMapping::m_framesTangExpVectors; + using BaseCosseratMapping::m_totalBeamForceVectors; + using BaseCosseratMapping::m_nodesExponentialSE3Vectors; + using BaseCosseratMapping::d_debug; + using BaseCosseratMapping::m_vecTransform; + using BaseCosseratMapping::m_nodeAdjointVectors; + using BaseCosseratMapping::m_index_input; + using BaseCosseratMapping::m_indicesVectorsDraw; + using BaseCosseratMapping::computeTheta; protected: /// Constructor diff --git a/src/Cosserat/mapping/DiscreteDynamicCosseratMapping.h b/src/Cosserat/mapping/DiscreteDynamicCosseratMapping.h index 25eb3a0..349748b 100644 --- a/src/Cosserat/mapping/DiscreteDynamicCosseratMapping.h +++ b/src/Cosserat/mapping/DiscreteDynamicCosseratMapping.h @@ -39,7 +39,7 @@ using sofa::type::Vec6; using std::get; using sofa::Data; using sofa::type::Mat; -using Cosserat::mapping::BaseCosserat; +using Cosserat::mapping::BaseCosseratMapping; /*! * \class DiscretDynamicCosseratMapping @@ -52,7 +52,7 @@ using Cosserat::mapping::BaseCosserat; template class DiscreteDynamicCosseratMapping : public sofa::core::Multi2Mapping, - BaseCosserat + BaseCosseratMapping { public: SOFA_CLASS(SOFA_TEMPLATE3(DiscreteDynamicCosseratMapping, TIn1,TIn2, TOut), @@ -131,19 +131,19 @@ class DiscreteDynamicCosseratMapping : public sofa::core::Multi2Mapping" approach. /// - using BaseCosserat::m_indicesVectors ; - using BaseCosserat::d_curv_abs_section ; - using BaseCosserat::d_curv_abs_frames ; - using BaseCosserat::m_nodesTangExpVectors; - using BaseCosserat::m_nodesVelocityVectors; - using BaseCosserat::m_framesExponentialSE3Vectors; - using BaseCosserat::m_framesTangExpVectors ; - using BaseCosserat::m_totalBeamForceVectors ; - using BaseCosserat::m_nodesExponentialSE3Vectors ; - using BaseCosserat::d_debug; - using BaseCosserat::m_vecTransform ; - using BaseCosserat::m_nodeAdjointVectors; - using BaseCosserat::m_index_input; + using BaseCosseratMapping::m_indicesVectors ; + using BaseCosseratMapping::d_curv_abs_section ; + using BaseCosseratMapping::d_curv_abs_frames ; + using BaseCosseratMapping::m_nodesTangExpVectors; + using BaseCosseratMapping::m_nodesVelocityVectors; + using BaseCosseratMapping::m_framesExponentialSE3Vectors; + using BaseCosseratMapping::m_framesTangExpVectors ; + using BaseCosseratMapping::m_totalBeamForceVectors ; + using BaseCosseratMapping::m_nodesExponentialSE3Vectors ; + using BaseCosseratMapping::d_debug; + using BaseCosseratMapping::m_vecTransform ; + using BaseCosseratMapping::m_nodeAdjointVectors; + using BaseCosseratMapping::m_index_input; public: diff --git a/src/Cosserat/mapping/RigidDistanceMapping.h b/src/Cosserat/mapping/RigidDistanceMapping.h index 3595281..c75c2ae 100644 --- a/src/Cosserat/mapping/RigidDistanceMapping.h +++ b/src/Cosserat/mapping/RigidDistanceMapping.h @@ -51,7 +51,7 @@ using sofa::type::Vec4f; * This is a component: * https://www.sofa-framework.org/community/doc/programming-with-sofa/create-your-component/ */ -using Cosserat::mapping::BaseCosserat; +using Cosserat::mapping::BaseCosseratMapping; // template class RigidDistanceMapping : public sofa::core::Multi2Mapping From 6b170ea3c0ae8fb55d5e37f990e6517d56c8ca08 Mon Sep 17 00:00:00 2001 From: Damien Marchal Date: Tue, 11 Jun 2024 11:22:38 +0200 Subject: [PATCH 02/26] Rename BaseCosserat files into BaseCosseratMapping --- CMakeLists.txt | 6 +++--- .../mapping/{BaseCosserat.cpp => BaseCosseratMapping.cpp} | 2 +- .../mapping/{BaseCosserat.h => BaseCosseratMapping.h} | 0 .../mapping/{BaseCosserat.inl => BaseCosseratMapping.inl} | 2 +- src/Cosserat/mapping/DifferenceMultiMapping.h | 2 +- src/Cosserat/mapping/DiscreteCosseratMapping.h | 2 +- src/Cosserat/mapping/DiscreteDynamicCosseratMapping.h | 2 +- src/Cosserat/mapping/LegendrePolynomialsMapping.h | 2 +- src/Cosserat/mapping/RigidDistanceMapping.h | 2 +- 9 files changed, 10 insertions(+), 10 deletions(-) rename src/Cosserat/mapping/{BaseCosserat.cpp => BaseCosseratMapping.cpp} (99%) rename src/Cosserat/mapping/{BaseCosserat.h => BaseCosseratMapping.h} (100%) rename src/Cosserat/mapping/{BaseCosserat.inl => BaseCosseratMapping.inl} (99%) diff --git a/CMakeLists.txt b/CMakeLists.txt index 9dcb01c..906ebfe 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -21,8 +21,8 @@ set(SRC_ROOT_DIR src/${PROJECT_NAME}) set(HEADER_FILES ${SRC_ROOT_DIR}/initCosserat.h.in - ${SRC_ROOT_DIR}/mapping/BaseCosserat.h - ${SRC_ROOT_DIR}/mapping/BaseCosserat.inl + ${SRC_ROOT_DIR}/mapping/BaseCosseratMapping.h + ${SRC_ROOT_DIR}/mapping/BaseCosseratMapping.inl ${SRC_ROOT_DIR}/mapping/DiscreteCosseratMapping.h ${SRC_ROOT_DIR}/mapping/DiscreteCosseratMapping.inl ${SRC_ROOT_DIR}/mapping/DiscreteDynamicCosseratMapping.h @@ -50,7 +50,7 @@ set(HEADER_FILES ) set(SOURCE_FILES ${SRC_ROOT_DIR}/initCosserat.cpp - ${SRC_ROOT_DIR}/mapping/BaseCosserat.cpp + ${SRC_ROOT_DIR}/mapping/BaseCosseratMapping.cpp ${SRC_ROOT_DIR}/mapping/DiscreteCosseratMapping.cpp ${SRC_ROOT_DIR}/mapping/DiscreteDynamicCosseratMapping.cpp ${SRC_ROOT_DIR}/engine/ProjectionEngine.cpp diff --git a/src/Cosserat/mapping/BaseCosserat.cpp b/src/Cosserat/mapping/BaseCosseratMapping.cpp similarity index 99% rename from src/Cosserat/mapping/BaseCosserat.cpp rename to src/Cosserat/mapping/BaseCosseratMapping.cpp index 852484c..5c6c87b 100644 --- a/src/Cosserat/mapping/BaseCosserat.cpp +++ b/src/Cosserat/mapping/BaseCosseratMapping.cpp @@ -20,7 +20,7 @@ * Contact information: contact@sofa-framework.org * ******************************************************************************/ #define SOFA_COSSERAT_CPP_BaseCosseratMapping -#include +#include #include #include diff --git a/src/Cosserat/mapping/BaseCosserat.h b/src/Cosserat/mapping/BaseCosseratMapping.h similarity index 100% rename from src/Cosserat/mapping/BaseCosserat.h rename to src/Cosserat/mapping/BaseCosseratMapping.h diff --git a/src/Cosserat/mapping/BaseCosserat.inl b/src/Cosserat/mapping/BaseCosseratMapping.inl similarity index 99% rename from src/Cosserat/mapping/BaseCosserat.inl rename to src/Cosserat/mapping/BaseCosseratMapping.inl index e272948..fdf25e8 100644 --- a/src/Cosserat/mapping/BaseCosserat.inl +++ b/src/Cosserat/mapping/BaseCosseratMapping.inl @@ -22,7 +22,7 @@ #pragma once #include -#include +#include #include #include diff --git a/src/Cosserat/mapping/DifferenceMultiMapping.h b/src/Cosserat/mapping/DifferenceMultiMapping.h index 7ec2b65..f4567d0 100644 --- a/src/Cosserat/mapping/DifferenceMultiMapping.h +++ b/src/Cosserat/mapping/DifferenceMultiMapping.h @@ -21,7 +21,7 @@ ******************************************************************************/ #pragma once #include -#include +#include #include #include diff --git a/src/Cosserat/mapping/DiscreteCosseratMapping.h b/src/Cosserat/mapping/DiscreteCosseratMapping.h index d3eb3ed..cb05746 100644 --- a/src/Cosserat/mapping/DiscreteCosseratMapping.h +++ b/src/Cosserat/mapping/DiscreteCosseratMapping.h @@ -23,7 +23,7 @@ #include #include -#include +#include #include #include diff --git a/src/Cosserat/mapping/DiscreteDynamicCosseratMapping.h b/src/Cosserat/mapping/DiscreteDynamicCosseratMapping.h index 349748b..bf30dc1 100644 --- a/src/Cosserat/mapping/DiscreteDynamicCosseratMapping.h +++ b/src/Cosserat/mapping/DiscreteDynamicCosseratMapping.h @@ -21,7 +21,7 @@ ******************************************************************************/ #pragma once #include -#include +#include #include #include diff --git a/src/Cosserat/mapping/LegendrePolynomialsMapping.h b/src/Cosserat/mapping/LegendrePolynomialsMapping.h index 5a1571b..4a72683 100644 --- a/src/Cosserat/mapping/LegendrePolynomialsMapping.h +++ b/src/Cosserat/mapping/LegendrePolynomialsMapping.h @@ -3,7 +3,7 @@ // #pragma once #include -#include +#include #include #include diff --git a/src/Cosserat/mapping/RigidDistanceMapping.h b/src/Cosserat/mapping/RigidDistanceMapping.h index c75c2ae..8258e66 100644 --- a/src/Cosserat/mapping/RigidDistanceMapping.h +++ b/src/Cosserat/mapping/RigidDistanceMapping.h @@ -21,7 +21,7 @@ ******************************************************************************/ #pragma once #include -#include +#include #include #include From 15f60377e81535291426f8609854e53943d9c6a9 Mon Sep 17 00:00:00 2001 From: Damien Marchal Date: Tue, 11 Jun 2024 11:39:47 +0200 Subject: [PATCH 03/26] Refactor the code so that BaseCosseratMapping is the base class of CosseratMapping --- src/Cosserat/mapping/BaseCosseratMapping.cpp | 12 ------------ src/Cosserat/mapping/BaseCosseratMapping.h | 3 ++- src/Cosserat/mapping/DiscreteCosseratMapping.h | 9 +++------ src/Cosserat/mapping/DiscreteCosseratMapping.inl | 1 - .../mapping/DiscreteDynamicCosseratMapping.h | 5 ++--- 5 files changed, 7 insertions(+), 23 deletions(-) diff --git a/src/Cosserat/mapping/BaseCosseratMapping.cpp b/src/Cosserat/mapping/BaseCosseratMapping.cpp index 5c6c87b..0dfbb4b 100644 --- a/src/Cosserat/mapping/BaseCosseratMapping.cpp +++ b/src/Cosserat/mapping/BaseCosseratMapping.cpp @@ -146,18 +146,6 @@ void BaseCosseratMapping::computeTangExp( } } - -// Register in the Factory -int BaseCosseratMappingClass = - sofa::core::RegisterObject( - "Set the positions and velocities of points attached to a rigid parent") - .add>() - .add>(); - template class SOFA_COSSERAT_API BaseCosseratMapping; diff --git a/src/Cosserat/mapping/BaseCosseratMapping.h b/src/Cosserat/mapping/BaseCosseratMapping.h index 02ce144..0a7ba35 100644 --- a/src/Cosserat/mapping/BaseCosseratMapping.h +++ b/src/Cosserat/mapping/BaseCosseratMapping.h @@ -75,7 +75,8 @@ typedef typename Eigen::Matrix4d _SE3; // TODO(dmarchal: 2024/06/07) This component looks like a mapping but inherit // from BaseObject * can you clarify why is is not inhering from BaseMapping template -class BaseCosseratMapping : public virtual sofa::core::objectmodel::BaseObject { +class BaseCosseratMapping : public sofa::core::Multi2Mapping +{ public: SOFA_CLASS(SOFA_TEMPLATE3(BaseCosseratMapping, TIn1, TIn2, TOut), BaseObject); typedef BaseObject Inherit; diff --git a/src/Cosserat/mapping/DiscreteCosseratMapping.h b/src/Cosserat/mapping/DiscreteCosseratMapping.h index cb05746..c5547b8 100644 --- a/src/Cosserat/mapping/DiscreteCosseratMapping.h +++ b/src/Cosserat/mapping/DiscreteCosseratMapping.h @@ -54,13 +54,10 @@ using sofa::Data; // using Cosserat::mapping::BaseCosserat; template -class DiscreteCosseratMapping - : public sofa::core::Multi2Mapping, - public Cosserat::mapping::BaseCosseratMapping { +class DiscreteCosseratMapping : public BaseCosseratMapping { public: - SOFA_CLASS2(SOFA_TEMPLATE3(DiscreteCosseratMapping, TIn1, TIn2, TOut), - SOFA_TEMPLATE3(sofa::core::Multi2Mapping, TIn1, TIn2, TOut), - SOFA_TEMPLATE3(Cosserat::mapping::BaseCosseratMapping, TIn1, TIn2, TOut)); + SOFA_CLASS(SOFA_TEMPLATE3(DiscreteCosseratMapping, TIn1, TIn2, TOut), + SOFA_TEMPLATE3(Cosserat::mapping::BaseCosseratMapping, TIn1, TIn2, TOut)); /// Input Model Type typedef TIn1 In1; diff --git a/src/Cosserat/mapping/DiscreteCosseratMapping.inl b/src/Cosserat/mapping/DiscreteCosseratMapping.inl index 6046a24..df957bd 100644 --- a/src/Cosserat/mapping/DiscreteCosseratMapping.inl +++ b/src/Cosserat/mapping/DiscreteCosseratMapping.inl @@ -85,7 +85,6 @@ void DiscreteCosseratMapping::init() { reinit(); // I call Init here since we build the mechanics only in the Inherit1::init(); - Inherit2::init(); m_colorMap.setColorScheme("Blue to Red"); m_colorMap.reinit(); diff --git a/src/Cosserat/mapping/DiscreteDynamicCosseratMapping.h b/src/Cosserat/mapping/DiscreteDynamicCosseratMapping.h index bf30dc1..ed8d6c1 100644 --- a/src/Cosserat/mapping/DiscreteDynamicCosseratMapping.h +++ b/src/Cosserat/mapping/DiscreteDynamicCosseratMapping.h @@ -51,12 +51,11 @@ using Cosserat::mapping::BaseCosseratMapping; template -class DiscreteDynamicCosseratMapping : public sofa::core::Multi2Mapping, - BaseCosseratMapping +class DiscreteDynamicCosseratMapping : public BaseCosseratMapping { public: SOFA_CLASS(SOFA_TEMPLATE3(DiscreteDynamicCosseratMapping, TIn1,TIn2, TOut), - SOFA_TEMPLATE3(sofa::core::Multi2Mapping, TIn1, TIn2, TOut) ); + SOFA_TEMPLATE3(Cosserat::mapping::BaseCosseratMapping, TIn1, TIn2, TOut) ); typedef sofa::core::Multi2Mapping Inherit; /// Input Model Type From 6f463c9d021a915ffc4dff659c60b954484cf3c6 Mon Sep 17 00:00:00 2001 From: Damien Marchal Date: Wed, 12 Jun 2024 15:03:27 +0200 Subject: [PATCH 04/26] Add a Cosserat/fwd.h file to declare new types in Cosserat plugin Put in this fwd.h file all forward declaration of c++ types. In our case this is where we will declare the SE3 from Matrix4. --- CMakeLists.txt | 1 + src/Cosserat/fwd.h | 41 ++++++++++++++++++++++ src/Cosserat/initCosserat.h.in | 21 ++++------- src/Cosserat/mapping/BaseCosseratMapping.h | 36 +++++++------------ 4 files changed, 61 insertions(+), 38 deletions(-) create mode 100644 src/Cosserat/fwd.h diff --git a/CMakeLists.txt b/CMakeLists.txt index 906ebfe..ab03c20 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -21,6 +21,7 @@ set(SRC_ROOT_DIR src/${PROJECT_NAME}) set(HEADER_FILES ${SRC_ROOT_DIR}/initCosserat.h.in + ${SRC_ROOT_DIR}/fwd.h ${SRC_ROOT_DIR}/mapping/BaseCosseratMapping.h ${SRC_ROOT_DIR}/mapping/BaseCosseratMapping.inl ${SRC_ROOT_DIR}/mapping/DiscreteCosseratMapping.h diff --git a/src/Cosserat/fwd.h b/src/Cosserat/fwd.h new file mode 100644 index 0000000..b800535 --- /dev/null +++ b/src/Cosserat/fwd.h @@ -0,0 +1,41 @@ +/****************************************************************************** + * SOFA, Simulation Open-Framework Architecture * + * (c) 2006-2018 INRIA, USTL, UJF, CNRS, MGH * + * * + * This library 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 library 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 library; if not, write to the Free Software Foundation, * + * Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. * + ******************************************************************************* + * Plugin Cosserat v1.0 * + * * + * This plugin is also distributed under the GNU LGPL (Lesser General * + * Public License) license with the same conditions than SOFA. * + * * + * Contributors: Defrost team (INRIA, University of Lille, CNRS, * + * Ecole Centrale de Lille) * + * * + * Contact information: https://project.inria.fr/softrobot/contact/ * + * * + ******************************************************************************/ +#pragma once + +#include +#include +namespace Cosserat +{ + namespace type + { + using SE3 = sofa::type::Matrix4; + } +} + diff --git a/src/Cosserat/initCosserat.h.in b/src/Cosserat/initCosserat.h.in index 1f4dd37..cbf8f17 100644 --- a/src/Cosserat/initCosserat.h.in +++ b/src/Cosserat/initCosserat.h.in @@ -27,8 +27,7 @@ * Contact information: https://project.inria.fr/softrobot/contact/ * * * ******************************************************************************/ -#ifndef SOFA_COSSERAT_INIT_H -#define SOFA_COSSERAT_INIT_H +#pragma once #include @@ -45,17 +44,9 @@ This is the plugin for the Discret Cosserat Plugin */ -namespace Cosserat { - -namespace type { -//using Vec3 = sofa::defaulttype::Vec3; -//using cosserat::type::SE3; +namespace Cosserat +{ + SOFA_COSSERAT_API void init(); + constexpr const char *MODULE_NAME = "@PROJECT_NAME@"; + constexpr const char *MODULE_VERSION = "@PROJECT_VERSION@"; } - - -SOFA_COSSERAT_API void init(); -constexpr const char *MODULE_NAME = "@PROJECT_NAME@"; -constexpr const char *MODULE_VERSION = "@PROJECT_VERSION@"; -} // namespace Cosserat - -#endif /* SOFA_COSSERAT_INIT_H */ diff --git a/src/Cosserat/mapping/BaseCosseratMapping.h b/src/Cosserat/mapping/BaseCosseratMapping.h index 0a7ba35..4f60e73 100644 --- a/src/Cosserat/mapping/BaseCosseratMapping.h +++ b/src/Cosserat/mapping/BaseCosseratMapping.h @@ -21,35 +21,27 @@ ******************************************************************************/ #pragma once #include +#include -#include #include #include #include #include #include #include - -#include #include - #include #include -using namespace std; -using namespace Eigen; - -#include - -// TODO(dmarchal, 2024/06/07): This is polluating the namespace of -// sofa::components -// plugins should be in their own namespace like -// eg: cosserat::component::mapping -namespace Cosserat::mapping { +namespace Cosserat::mapping +{ // with a private namespace the used named are not polluating the whole // sofa::component::mapping ones. -namespace { +namespace +{ +using namespace std; +using namespace Eigen; using sofa::core::objectmodel::BaseContext; using sofa::core::objectmodel::BaseObject; using sofa::defaulttype::SolidTypes; @@ -61,19 +53,17 @@ using sofa::type::Vec3; using sofa::type::Vec6; using sofa::type::Mat; -typedef typename Eigen::Matrix4d _SE3; -} // namespace +using Cosserat::type::SE3; +using _SE3 = Eigen::Matrix4d; + +} +// TODO(dmarchal: 2024/10/07) Is the description valid ? /*! - * \class BaseCosserat + * \class BaseCosseratMapping * @brief Computes and map the length of the beams * - * This is a component: - * https://www.sofa-framework.org/community/doc/programming-with-sofa/create-your-component/ */ - -// TODO(dmarchal: 2024/06/07) This component looks like a mapping but inherit -// from BaseObject * can you clarify why is is not inhering from BaseMapping template class BaseCosseratMapping : public sofa::core::Multi2Mapping { From 87a30a7da0599214228b332badfdd9f923b9181c Mon Sep 17 00:00:00 2001 From: Damien Marchal Date: Wed, 12 Jun 2024 15:10:16 +0200 Subject: [PATCH 05/26] Move SE3, se3, _SE3, _se3 out of the class. --- src/Cosserat/mapping/BaseCosseratMapping.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/Cosserat/mapping/BaseCosseratMapping.cpp b/src/Cosserat/mapping/BaseCosseratMapping.cpp index 0dfbb4b..70a3c21 100644 --- a/src/Cosserat/mapping/BaseCosseratMapping.cpp +++ b/src/Cosserat/mapping/BaseCosseratMapping.cpp @@ -31,8 +31,7 @@ namespace Cosserat::mapping using namespace sofa::defaulttype; template <> -BaseCosseratMapping::se3 -BaseCosseratMapping::buildXiHat(const Coord1 &strain_i) +auto BaseCosseratMapping::buildXiHat(const Coord1 &strain_i) -> se3 { se3 Xi; From 18d833574e59b9d25829031196c23860df9f482c Mon Sep 17 00:00:00 2001 From: Damien Marchal Date: Wed, 12 Jun 2024 15:10:38 +0200 Subject: [PATCH 06/26] FIXUP --- src/Cosserat/mapping/BaseCosseratMapping.h | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) diff --git a/src/Cosserat/mapping/BaseCosseratMapping.h b/src/Cosserat/mapping/BaseCosseratMapping.h index 4f60e73..ac4bb9f 100644 --- a/src/Cosserat/mapping/BaseCosseratMapping.h +++ b/src/Cosserat/mapping/BaseCosseratMapping.h @@ -53,7 +53,9 @@ using sofa::type::Vec3; using sofa::type::Vec6; using sofa::type::Mat; -using Cosserat::type::SE3; +using se3 = sofa::type::Matrix4; +using SE3 = sofa::type::Matrix4; +using _se3 = Eigen::Matrix4d; using _SE3 = Eigen::Matrix4d; } @@ -123,10 +125,6 @@ class BaseCosseratMapping : public sofa::core::Multi2Mapping typedef typename SolidTypes::Transform Transform; typedef typename sofa::type::vector List; - typedef typename sofa::type::Matrix4 se3; - typedef typename sofa::type::Matrix4 SE3; - - typedef typename Eigen::Matrix4d _se3; typedef typename sofa::type::Mat<6, 6, SReal> Tangent; typedef typename Eigen::Matrix3d RotMat; typedef typename Eigen::Matrix Vector6d; From 3e736395b81c6211e6f762d08660becda2ee1c6a Mon Sep 17 00:00:00 2001 From: Damien Marchal Date: Wed, 12 Jun 2024 16:00:56 +0200 Subject: [PATCH 07/26] Add a types.h file to factorize all types declaration in cosserat plugins --- CMakeLists.txt | 1 + src/Cosserat/mapping/BaseCosseratMapping.h | 26 ++++----- src/Cosserat/mapping/BaseCosseratMapping.inl | 3 +- .../mapping/DiscreteCosseratMapping.cpp | 4 +- src/Cosserat/types.h | 53 +++++++++++++++++++ 5 files changed, 69 insertions(+), 18 deletions(-) create mode 100644 src/Cosserat/types.h diff --git a/CMakeLists.txt b/CMakeLists.txt index ab03c20..162ee19 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -22,6 +22,7 @@ set(SRC_ROOT_DIR src/${PROJECT_NAME}) set(HEADER_FILES ${SRC_ROOT_DIR}/initCosserat.h.in ${SRC_ROOT_DIR}/fwd.h + ${SRC_ROOT_DIR}/types.h ${SRC_ROOT_DIR}/mapping/BaseCosseratMapping.h ${SRC_ROOT_DIR}/mapping/BaseCosseratMapping.inl ${SRC_ROOT_DIR}/mapping/DiscreteCosseratMapping.h diff --git a/src/Cosserat/mapping/BaseCosseratMapping.h b/src/Cosserat/mapping/BaseCosseratMapping.h index ac4bb9f..17b90db 100644 --- a/src/Cosserat/mapping/BaseCosseratMapping.h +++ b/src/Cosserat/mapping/BaseCosseratMapping.h @@ -21,7 +21,7 @@ ******************************************************************************/ #pragma once #include -#include +#include #include #include @@ -47,6 +47,9 @@ using sofa::core::objectmodel::BaseObject; using sofa::defaulttype::SolidTypes; using sofa::type::Matrix3; using sofa::type::Matrix4; +using sofa::type::Mat6x6; +using sofa::type::Mat4x4; + using std::get; using sofa::type::vector; using sofa::type::Vec3; @@ -58,6 +61,10 @@ using SE3 = sofa::type::Matrix4; using _se3 = Eigen::Matrix4d; using _SE3 = Eigen::Matrix4d; +using Cosserat::type::Transform; +using Cosserat::type::Tangent; +using Cosserat::type::RotMat; + } // TODO(dmarchal: 2024/10/07) Is the description valid ? @@ -70,14 +77,11 @@ template class BaseCosseratMapping : public sofa::core::Multi2Mapping { public: - SOFA_CLASS(SOFA_TEMPLATE3(BaseCosseratMapping, TIn1, TIn2, TOut), BaseObject); - typedef BaseObject Inherit; + SOFA_ABSTRACT_CLASS(SOFA_TEMPLATE3(BaseCosseratMapping, TIn1, TIn2, TOut), + SOFA_TEMPLATE3(sofa::core::Multi2Mapping,TIn1, TIn2, TOut)); - /// Input Model Type typedef TIn1 In1; typedef TIn2 In2; - - /// Output Model Type typedef TOut Out; // TODO(dmarchal:2024/06/07) This very long list of public typedefs looks @@ -99,8 +103,6 @@ class BaseCosseratMapping : public sofa::core::Multi2Mapping [[maybe_unused]] typedef typename In2::MatrixDeriv In2MatrixDeriv; typedef sofa::Data In2DataVecCoord; typedef sofa::Data In2DataVecDeriv; - typedef Mat<6, 6, SReal> Mat6x6; - typedef Mat<4, 4, SReal> Mat4x4; typedef typename Out::VecCoord OutVecCoord; typedef typename Out::Coord OutCoord; @@ -117,18 +119,12 @@ class BaseCosseratMapping : public sofa::core::Multi2Mapping typedef sofa::MultiLink, sofa::core::State, sofa::BaseLink::FLAG_STOREPATH | sofa::BaseLink::FLAG_STRONGLINK> LinkFromModels2; + [[maybe_unused]] typedef sofa::MultiLink< BaseCosseratMapping, sofa::core::State, sofa::BaseLink::FLAG_STOREPATH | sofa::BaseLink::FLAG_STRONGLINK> LinkToModels; - typedef typename SolidTypes::Transform Transform; - typedef typename sofa::type::vector List; - - typedef typename sofa::type::Mat<6, 6, SReal> Tangent; - typedef typename Eigen::Matrix3d RotMat; - typedef typename Eigen::Matrix Vector6d; - public: // TODO(dmarchal: 2024/06/07): There is a lot of public attributes is this // really needed ? diff --git a/src/Cosserat/mapping/BaseCosseratMapping.inl b/src/Cosserat/mapping/BaseCosseratMapping.inl index fdf25e8..0bb59b0 100644 --- a/src/Cosserat/mapping/BaseCosseratMapping.inl +++ b/src/Cosserat/mapping/BaseCosseratMapping.inl @@ -61,7 +61,8 @@ BaseCosseratMapping::BaseCosseratMapping() // _________________________________________________________________________________________ template -void BaseCosseratMapping::init() { +void BaseCosseratMapping::init() +{ Inherit1::init(); // Fill the initial vector diff --git a/src/Cosserat/mapping/DiscreteCosseratMapping.cpp b/src/Cosserat/mapping/DiscreteCosseratMapping.cpp index 85fef06..ef389af 100644 --- a/src/Cosserat/mapping/DiscreteCosseratMapping.cpp +++ b/src/Cosserat/mapping/DiscreteCosseratMapping.cpp @@ -48,8 +48,8 @@ void DiscreteCosseratMapping:: applyJ( const auto baseIndex = d_baseIndex.getValue(); // Curv abscissa of nodes and frames - sofa::helper::ReadAccessor> curv_abs_section = d_curv_abs_section; - sofa::helper::ReadAccessor> curv_abs_frames = d_curv_abs_frames; + sofa::helper::ReadAccessor>> curv_abs_section = d_curv_abs_section; + sofa::helper::ReadAccessor>> curv_abs_frames = d_curv_abs_frames; const In1VecCoord& inDeform = m_fromModel1->read(sofa::core::ConstVecCoordId::position())->getValue(); //Strains // Compute the tangent Exponential SE3 vectors diff --git a/src/Cosserat/types.h b/src/Cosserat/types.h new file mode 100644 index 0000000..cf5c720 --- /dev/null +++ b/src/Cosserat/types.h @@ -0,0 +1,53 @@ +/****************************************************************************** + * SOFA, Simulation Open-Framework Architecture * + * (c) 2006-2018 INRIA, USTL, UJF, CNRS, MGH * + * * + * This library 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 library 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 library; if not, write to the Free Software Foundation, * + * Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. * + ******************************************************************************* + * Plugin Cosserat v1.0 * + * * + * This plugin is also distributed under the GNU LGPL (Lesser General * + * Public License) license with the same conditions than SOFA. * + * * + * Contributors: Defrost team (INRIA, University of Lille, CNRS, * + * Ecole Centrale de Lille) * + * * + * Contact information: https://project.inria.fr/softrobot/contact/ * + * * + ******************************************************************************/ +#pragma once + +#include +#include +#include +#include +#include +namespace Cosserat +{ + + namespace type + { + using SE3 = sofa::type::Matrix4; + + typedef typename sofa::defaulttype::SolidTypes::Transform Transform; + typedef typename sofa::type::vector List; + + typedef typename sofa::type::Mat<6, 6, SReal> Tangent; + typedef typename Eigen::Matrix3d RotMat; + typedef typename Eigen::Matrix Vector6d; + + } +} + From dc496d8fe897a75685aaa81f75638bb14012b123 Mon Sep 17 00:00:00 2001 From: Damien Marchal Date: Wed, 12 Jun 2024 16:04:07 +0200 Subject: [PATCH 08/26] Add a comment in BaseCOsseratMapping. --- src/Cosserat/mapping/BaseCosseratMapping.inl | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/Cosserat/mapping/BaseCosseratMapping.inl b/src/Cosserat/mapping/BaseCosseratMapping.inl index 0bb59b0..56c226d 100644 --- a/src/Cosserat/mapping/BaseCosseratMapping.inl +++ b/src/Cosserat/mapping/BaseCosseratMapping.inl @@ -68,6 +68,9 @@ void BaseCosseratMapping::init() // Fill the initial vector const OutDataVecCoord *xfromData = m_toModel->read(sofa::core::ConstVecCoordId::position()); + + //TODO(dmarchal, 2024/07/12): is this line really needed ? + // it initialize a local variable, is it to force a xfromData updates ? const OutVecCoord xfrom = xfromData->getValue(); } @@ -83,8 +86,7 @@ void BaseCosseratMapping::computeExponentialSE3( SE3 _g_X; se3 Xi_hat_n = buildXiHat(strain_n); - if (d_debug.getValue()) - msg_info("BaseCosserat: ") << "matrix Xi : " << Xi_hat_n; + msg_info() << "matrix Xi : " << Xi_hat_n; if (theta <= std::numeric_limits::epsilon()) { _g_X = I4 + curv_abs_x_n * Xi_hat_n; From 837c3bfea0a3dd8585bf4f359759ad03e635c218 Mon Sep 17 00:00:00 2001 From: Damien Marchal Date: Wed, 12 Jun 2024 16:14:11 +0200 Subject: [PATCH 09/26] Replace Matrix3 by Mat3x3 and Matrix4 by Mat4x4. --- src/Cosserat/mapping/BaseCosseratMapping.cpp | 6 +-- src/Cosserat/mapping/BaseCosseratMapping.h | 22 ++++---- src/Cosserat/mapping/BaseCosseratMapping.inl | 57 +++++++++----------- 3 files changed, 39 insertions(+), 46 deletions(-) diff --git a/src/Cosserat/mapping/BaseCosseratMapping.cpp b/src/Cosserat/mapping/BaseCosseratMapping.cpp index 70a3c21..370865b 100644 --- a/src/Cosserat/mapping/BaseCosseratMapping.cpp +++ b/src/Cosserat/mapping/BaseCosseratMapping.cpp @@ -60,7 +60,7 @@ auto BaseCosseratMapping::buildXiHat(const template <> void BaseCosseratMapping::computeExponentialSE3( const double &curv_abs_x_n, const Coord1 &strain_n, Transform &Trans) { - Matrix4 I4; + Mat4x4 I4; I4.identity(); // Get the angular part of the @@ -100,7 +100,7 @@ void BaseCosseratMapping::computeTangExp( SReal theta = Vec3(strain_i(0), strain_i(1), strain_i(2)) .norm(); // Sometimes this is computed over all strain - Matrix3 tilde_k = + Mat3x3 tilde_k = getTildeMatrix(Vec3(strain_i(0), strain_i(1), strain_i(2))); /* Younes @23-11-27 @@ -112,7 +112,7 @@ void BaseCosseratMapping::computeTangExp( #TECHNICAL_DEBT */ // TODO(dmarchal: 2024/06/07) could the debt by solved ? - Matrix3 tilde_q = + Mat3x3 tilde_q = getTildeMatrix(Vec3(strain_i(3), strain_i(4), strain_i(5))); Mat6x6 ad_Xi; diff --git a/src/Cosserat/mapping/BaseCosseratMapping.h b/src/Cosserat/mapping/BaseCosseratMapping.h index 17b90db..2aeabbf 100644 --- a/src/Cosserat/mapping/BaseCosseratMapping.h +++ b/src/Cosserat/mapping/BaseCosseratMapping.h @@ -45,10 +45,9 @@ using namespace Eigen; using sofa::core::objectmodel::BaseContext; using sofa::core::objectmodel::BaseObject; using sofa::defaulttype::SolidTypes; -using sofa::type::Matrix3; -using sofa::type::Matrix4; using sofa::type::Mat6x6; using sofa::type::Mat4x4; +using sofa::type::Mat3x3; using std::get; using sofa::type::vector; @@ -135,7 +134,7 @@ class BaseCosseratMapping : public sofa::core::Multi2Mapping vector m_framesExponentialSE3Vectors; vector m_nodesExponentialSE3Vectors; - vector m_nodesLogarithmeSE3Vectors; + vector m_nodesLogarithmeSE3Vectors; // @todo comment or explain more vectors vector m_indicesVectors; @@ -152,8 +151,9 @@ class BaseCosseratMapping : public sofa::core::Multi2Mapping vector m_nodeAdjointVectors; // TODO(dmarchal:2024/06/07): explain why these attributes are unused - // TODO : yadagolo: Need for the dynamic function, which is not working yet. - // But the component is in this folder + // : yadagolo: Need for the dynamic function, which is not working yet. But the component is in this folder + // : dmarchal: don't add something that will be used "one day" + // : dmarchal: it look like as if you should be working in a branch for making new feature and merge it when it is ready. [[maybe_unused]] vector m_nodeAdjointEtaVectors; [[maybe_unused]] vector m_frameAdjointEtaVectors; [[maybe_unused]] vector m_node_coAdjointEtaVectors; @@ -175,15 +175,15 @@ class BaseCosseratMapping : public sofa::core::Multi2Mapping double computeTheta(const double &x, const Mat4x4 &gX); void printMatrix(const Mat6x6 R); - Matrix3 extractRotMatrix(const Transform &frame); + sofa::type::Mat3x3 extractRotMatrix(const Transform &frame); Tangent buildProjector(const Transform &T); se3 buildXiHat(const Coord1 &strain_i); - Matrix3 getTildeMatrix(const Vec3 &u); + Mat3x3 getTildeMatrix(const Vec3 &u); - void buildAdjoint(const Matrix3 &A, const Matrix3 &B, Mat6x6 &Adjoint); - void buildCoAdjoint(const Matrix3 &A, const Matrix3 &B, Mat6x6 &coAdjoint); + void buildAdjoint(const Mat3x3 &A, const Mat3x3 &B, Mat6x6 &Adjoint); + void buildCoAdjoint(const Mat3x3 &A, const Mat3x3 &B, Mat6x6 &coAdjoint); - Matrix4 convertTransformToMatrix4x4(const Transform &T); + Mat4x4 convertTransformToMatrix4x4(const Transform &T); Vec6 piecewiseLogmap(const _SE3 &g_x); // TODO(dmarchal: 2024/06/07), this looks like a very common utility @@ -248,7 +248,7 @@ class BaseCosseratMapping : public sofa::core::Multi2Mapping [[maybe_unused]] Vec6 computeETA(const Vec6 &baseEta, const In1VecDeriv &k_dot, double abs_input); - Matrix4 computeLogarithm(const double &x, const Mat4x4 &gX); + Mat4x4 computeLogarithm(const double &x, const Mat4x4 &gX); }; #if !defined(SOFA_COSSERAT_CPP_BaseCosseratMapping) diff --git a/src/Cosserat/mapping/BaseCosseratMapping.inl b/src/Cosserat/mapping/BaseCosseratMapping.inl index 56c226d..cdd8a43 100644 --- a/src/Cosserat/mapping/BaseCosseratMapping.inl +++ b/src/Cosserat/mapping/BaseCosseratMapping.inl @@ -77,7 +77,7 @@ void BaseCosseratMapping::init() template void BaseCosseratMapping::computeExponentialSE3( const double &curv_abs_x_n, const Coord1 &strain_n, Transform &g_X_n) { - Matrix4 I4; + Mat4x4 I4; I4.identity(); // Get the angular part of the sofa::type::Vec3 k = Vec3(strain_n(0), strain_n(1), strain_n(2)); @@ -99,8 +99,8 @@ void BaseCosseratMapping::computeExponentialSE3( scalar2 * Xi_hat_n * Xi_hat_n * Xi_hat_n; } - if (d_debug.getValue()) - msg_info("BaseCosserat: ") << "matrix _g_X : " << _g_X; + msg_info() << "matrix _g_X : " << _g_X; + sofa::type::Mat3x3 M; _g_X.getsub(0, 0, M); // get the rotation matrix @@ -172,45 +172,39 @@ void BaseCosseratMapping::updateExponentialSE3( } } -////////////////// Test the logarithm code -// Eigen::Matrix4d gX = convertTransformToMatrix4x4(T); -// Eigen::Matrix4d log_gX= (1.0/x) * computeLogarithm(x, gX); -// std::cout << "k : \n"<< k << std::endl; -// std::cout << "The logarithm : \n"<< log_gX << std::endl; -// m_nodesLogarithmSE3Vectors.push_back(log_gX); - template void BaseCosseratMapping::computeAdjoint(const Transform &frame, Tangent &adjoint) { - Matrix3 R = extractRotMatrix(frame); + Mat3x3 R = extractRotMatrix(frame); Vec3 u = frame.getOrigin(); - Matrix3 tilde_u = getTildeMatrix(u); - Matrix3 tilde_u_R = tilde_u * R; + Mat3x3 tilde_u = getTildeMatrix(u); + Mat3x3 tilde_u_R = tilde_u * R; buildAdjoint(R, tilde_u_R, adjoint); } template void BaseCosseratMapping::computeCoAdjoint(const Transform &frame, Mat6x6 &co_adjoint) { - Matrix3 R = extractRotMatrix(frame); + Mat3x3 R = extractRotMatrix(frame); Vec3 u = frame.getOrigin(); - Matrix3 tilde_u = getTildeMatrix(u); - Matrix3 tilde_u_R = tilde_u * R; + Mat3x3 tilde_u = getTildeMatrix(u); + Mat3x3 tilde_u_R = tilde_u * R; buildCoAdjoint(R, tilde_u_R, co_adjoint); } template void BaseCosseratMapping::computeAdjoint(const Vec6 &eta, Mat6x6 &adjoint) { - Matrix3 tildeMat = getTildeMatrix(Vec3(eta[0], eta[1], eta[2])); + Mat3x3 tildeMat = getTildeMatrix(Vec3(eta[0], eta[1], eta[2])); adjoint.setsub(0, 0, tildeMat); adjoint.setsub(3, 3, tildeMat); adjoint.setsub(3, 0, getTildeMatrix(Vec3(eta[3], eta[4], eta[5]))); } template -Matrix4 BaseCosseratMapping::computeLogarithm(const double &x, - const Mat4x4 &gX) { +auto BaseCosseratMapping::computeLogarithm(const double &x, + const Mat4x4 &gX) -> Mat4x4 +{ // Compute theta before everything const double theta = computeTheta(x, gX); Mat4x4 I4; @@ -300,7 +294,7 @@ void BaseCosseratMapping::computeTangExp(double &curv_abs_n, SReal theta = Vec3(strain_i(0), strain_i(1), strain_i(2)) .norm(); // Sometimes this is computed over all strain - Matrix3 tilde_k = + Mat3x3 tilde_k = getTildeMatrix(Vec3(strain_i(0), strain_i(1), strain_i(2))); /* Younes @23-11-27 old version @@ -310,7 +304,7 @@ void BaseCosseratMapping::computeTangExp(double &curv_abs_n, bring any difference in my three reference scenes, but need more investogation #TECHNICAL_DEBT */ - Matrix3 tilde_q = getTildeMatrix(Vec3(0.0, 0.0, 0.0)); + Mat3x3 tilde_q = getTildeMatrix(Vec3(0.0, 0.0, 0.0)); Mat6x6 ad_Xi; buildAdjoint(tilde_k, tilde_q, ad_Xi); @@ -479,8 +473,7 @@ void BaseCosseratMapping::printMatrix(const Mat6x6 R) { } template -Matrix3 -BaseCosseratMapping::extractRotMatrix(const Transform &frame) { +Mat3x3 BaseCosseratMapping::extractRotMatrix(const Transform &frame) { sofa::type::Quat q = frame.getOrientation(); @@ -489,7 +482,7 @@ BaseCosseratMapping::extractRotMatrix(const Transform &frame) // does not need this amount of code. Real R[4][4]; q.buildRotationMatrix(R); - Matrix3 mat; + Mat3x3 mat; for (unsigned int k = 0; k < 3; k++) for (unsigned int i = 0; i < 3; i++) mat[k][i] = R[k][i]; @@ -534,7 +527,7 @@ auto BaseCosseratMapping::buildXiHat(const Coord1 &strain_i) - template auto BaseCosseratMapping::getTildeMatrix(const sofa::type::Vec3 &u) - -> Matrix3 { + -> Mat3x3 { sofa::type::Matrix3 tild; tild[0][1] = -u[2]; tild[0][2] = u[1]; @@ -547,8 +540,8 @@ auto BaseCosseratMapping::getTildeMatrix(const sofa::type::Vec } template -auto BaseCosseratMapping::buildAdjoint(const Matrix3 &A, - const Matrix3 &B, +auto BaseCosseratMapping::buildAdjoint(const Mat3x3 &A, + const Mat3x3 &B, Mat6x6 &Adjoint) -> void { Adjoint.clear(); for (unsigned int i = 0; i < 3; i++) { @@ -561,8 +554,8 @@ auto BaseCosseratMapping::buildAdjoint(const Matrix3 &A, } template -auto BaseCosseratMapping::buildCoAdjoint(const Matrix3 &A, - const Matrix3 &B, +auto BaseCosseratMapping::buildCoAdjoint(const Mat3x3 &A, + const Mat3x3 &B, Mat6x6 &coAdjoint) -> void { coAdjoint.clear(); for (unsigned int i = 0; i < 3; i++) { @@ -579,10 +572,10 @@ auto BaseCosseratMapping::buildCoAdjoint(const Matrix3 &A, template auto BaseCosseratMapping::convertTransformToMatrix4x4( - const Transform &T) -> Matrix4 { - Matrix4 M; + const Transform &T) -> Mat4x4 { + Mat4x4 M; M.identity(); - Matrix3 R = extractRotMatrix(T); + Mat3x3 R = extractRotMatrix(T); sofa::type::Vec3 trans = T.getOrigin(); for (unsigned int i = 0; i < 3; i++) { From f4b88f22df97cd5b9563d545ad6d608ef34259d2 Mon Sep 17 00:00:00 2001 From: Damien Marchal Date: Wed, 12 Jun 2024 16:23:45 +0200 Subject: [PATCH 10/26] Replace if(d_debug) msg_info("YOLO") by msg_info() Because msg_info already contains a test on wether we should print a message and this is connected by the printLog data field. More generally "debug" datafield is questionnable. --- src/Cosserat/mapping/BaseCosseratMapping.cpp | 6 - src/Cosserat/mapping/BaseCosseratMapping.inl | 153 +++++++++---------- 2 files changed, 68 insertions(+), 91 deletions(-) diff --git a/src/Cosserat/mapping/BaseCosseratMapping.cpp b/src/Cosserat/mapping/BaseCosseratMapping.cpp index 370865b..dc81312 100644 --- a/src/Cosserat/mapping/BaseCosseratMapping.cpp +++ b/src/Cosserat/mapping/BaseCosseratMapping.cpp @@ -48,12 +48,6 @@ auto BaseCosseratMapping::buildXiHat(const for (unsigned int i = 0; i < 3; i++) Xi[i][3] += strain_i(i + 3); - // se3 = [ - // 0 -screw(3) screw(2) screw(4); - // screw(3) 0 -screw(1) screw(5); - // -screw(2) screw(1) 0 screw(6); - // 0 0 0 0]; - return Xi; } diff --git a/src/Cosserat/mapping/BaseCosseratMapping.inl b/src/Cosserat/mapping/BaseCosseratMapping.inl index cdd8a43..48a775d 100644 --- a/src/Cosserat/mapping/BaseCosseratMapping.inl +++ b/src/Cosserat/mapping/BaseCosseratMapping.inl @@ -50,13 +50,14 @@ using sofa::type::vector; template BaseCosseratMapping::BaseCosseratMapping() + // TODO(dmarchal: 2024/06/12): please add the help comments ! : d_curv_abs_section(initData(&d_curv_abs_section, "curv_abs_input", " need to be com....")), - d_curv_abs_frames(initData(&d_curv_abs_frames, "curv_abs_output", - " need to be com....")), - d_debug(initData(&d_debug, false, "debug", "printf for the debug")), - m_fromModel1(NULL), m_fromModel2(NULL), m_toModel(NULL), - m_index_input(0) {} + d_curv_abs_frames(initData(&d_curv_abs_frames, "curv_abs_output", + " need to be com....")), + d_debug(initData(&d_debug, false, "debug", "printf for the debug")), + m_fromModel1(NULL), m_fromModel2(NULL), m_toModel(NULL), + m_index_input(0) {} // _________________________________________________________________________________________ @@ -67,7 +68,7 @@ void BaseCosseratMapping::init() // Fill the initial vector const OutDataVecCoord *xfromData = - m_toModel->read(sofa::core::ConstVecCoordId::position()); + m_toModel->read(sofa::core::ConstVecCoordId::position()); //TODO(dmarchal, 2024/07/12): is this line really needed ? // it initialize a local variable, is it to force a xfromData updates ? @@ -76,7 +77,7 @@ void BaseCosseratMapping::init() template void BaseCosseratMapping::computeExponentialSE3( - const double &curv_abs_x_n, const Coord1 &strain_n, Transform &g_X_n) { + const double &curv_abs_x_n, const Coord1 &strain_n, Transform &g_X_n) { Mat4x4 I4; I4.identity(); // Get the angular part of the @@ -92,7 +93,7 @@ void BaseCosseratMapping::computeExponentialSE3( _g_X = I4 + curv_abs_x_n * Xi_hat_n; } else { double scalar1 = - (1.0 - std::cos(curv_abs_x_n * theta)) / std::pow(theta, 2); + (1.0 - std::cos(curv_abs_x_n * theta)) / std::pow(theta, 2); double scalar2 = (curv_abs_x_n * theta - std::sin(curv_abs_x_n * theta)) / std::pow(theta, 3); _g_X = I4 + curv_abs_x_n * Xi_hat_n + scalar1 * Xi_hat_n * Xi_hat_n + @@ -113,11 +114,11 @@ void BaseCosseratMapping::computeExponentialSE3( // Fill exponential vectors template void BaseCosseratMapping::updateExponentialSE3( - const In1VecCoord &inDeform) { + const In1VecCoord &inDeform) { // helper::ReadAccessor>> curv_abs_input = // d_curv_abs_section; sofa::helper::ReadAccessor>> curv_abs_frames = - d_curv_abs_frames; + d_curv_abs_frames; m_framesExponentialSE3Vectors.clear(); m_nodesExponentialSE3Vectors.clear(); @@ -128,53 +129,46 @@ void BaseCosseratMapping::updateExponentialSE3( for (size_t i = 0; i < sz; i++) { Transform g_X_frame_i; - const Coord1 strain_n = inDeform[m_indicesVectors[i] - - 1]; // Cosserat reduce coordinates (strain) - // the size varies from 1 to 6 - // The distance between the frame and the closest beam node toward the base + const Coord1 strain_n = inDeform[m_indicesVectors[i] - 1]; // Cosserat reduce coordinates (strain) + + // the size varies from 1 to 6 + // The distance between the frame and the closest beam node toward the base const SReal curv_abs_x = - m_framesLengthVectors[i]; // curv_abs_x = frame_curv_abs - L_(n-1) + m_framesLengthVectors[i]; // curv_abs_x = frame_curv_abs - L_(n-1) computeExponentialSE3(curv_abs_x, strain_n, g_X_frame_i); m_framesExponentialSE3Vectors.push_back(g_X_frame_i); - if (d_debug.getValue()) { - msg_info("BaseCosserat:") - << "_________________" << i << "_________________________"; - msg_info("BaseCosserat:") - << "x :" << curv_abs_x << "; strain :" << strain_n; - msg_info("BaseCosserat:") + msg_info() + << "_________________" << i << "_________________________" << msgendl + << "x :" << curv_abs_x << "; strain :" << strain_n << msgendl << "m_framesExponentialSE3Vectors :" << g_X_frame_i; - } } // Compute the exponential on the nodes m_nodesExponentialSE3Vectors.push_back( - Transform(sofa::type::Vec3(0.0, 0.0, 0.0), - sofa::type::Quat(0., 0., 0., 1.))); // The first node. + Transform(sofa::type::Vec3(0.0, 0.0, 0.0), + sofa::type::Quat(0., 0., 0., 1.))); // The first node. for (unsigned int j = 0; j < inDeform.size(); j++) { Coord1 strain_n = inDeform[j]; // Strain_n const SReal curv_abs_x = - m_BeamLengthVectors[j]; // curv_abs_x = L_n - L_(n-1) + m_BeamLengthVectors[j]; // curv_abs_x = L_n - L_(n-1) Transform g_X_node_j; computeExponentialSE3(curv_abs_x, strain_n, g_X_node_j); m_nodesExponentialSE3Vectors.push_back(g_X_node_j); - if (d_debug.getValue()) { - msg_info("BaseCosserat:") - << "_________________Beam Node Expo___________________" << j; - msg_info("BaseCosserat:") - << "Node m_framesExponentialSE3Vectors :" << g_X_node_j; - msg_info("BaseCosserat:") + msg_info() + << "_________________Beam Node Expo___________________" << msgendl + << "Node m_framesExponentialSE3Vectors :" << g_X_node_j << msgendl << "_________________Beam Node Expo___________________"; - } + } } template void BaseCosseratMapping::computeAdjoint(const Transform &frame, - Tangent &adjoint) { + Tangent &adjoint) { Mat3x3 R = extractRotMatrix(frame); Vec3 u = frame.getOrigin(); Mat3x3 tilde_u = getTildeMatrix(u); @@ -184,7 +178,7 @@ void BaseCosseratMapping::computeAdjoint(const Transform &fram template void BaseCosseratMapping::computeCoAdjoint(const Transform &frame, - Mat6x6 &co_adjoint) { + Mat6x6 &co_adjoint) { Mat3x3 R = extractRotMatrix(frame); Vec3 u = frame.getOrigin(); Mat3x3 tilde_u = getTildeMatrix(u); @@ -194,7 +188,7 @@ void BaseCosseratMapping::computeCoAdjoint(const Transform &fr template void BaseCosseratMapping::computeAdjoint(const Vec6 &eta, - Mat6x6 &adjoint) { + Mat6x6 &adjoint) { Mat3x3 tildeMat = getTildeMatrix(Vec3(eta[0], eta[1], eta[2])); adjoint.setsub(0, 0, tildeMat); adjoint.setsub(3, 3, tildeMat); @@ -203,7 +197,7 @@ void BaseCosseratMapping::computeAdjoint(const Vec6 &eta, template auto BaseCosseratMapping::computeLogarithm(const double &x, - const Mat4x4 &gX) -> Mat4x4 + const Mat4x4 &gX) -> Mat4x4 { // Compute theta before everything const double theta = computeTheta(x, gX); @@ -226,10 +220,10 @@ auto BaseCosseratMapping::computeLogarithm(const double &x, log_gX = cst * ((x_theta * cos_2XTheta - sin_XTheta) * I4 - (x_theta * cos_XTheta + 2.0 * x_theta * cos_2XTheta - sin_XTheta - sin_2XTheta) * - gX + + gX + (2.0 * x_theta * cos_XTheta + x_theta * cos_2XTheta - sin_XTheta - sin_2XTheta) * - (gX * gX) - + (gX * gX) - (x_theta * cos_XTheta - sin_XTheta) * (gX * gX * gX)); } @@ -238,18 +232,20 @@ auto BaseCosseratMapping::computeLogarithm(const double &x, template void BaseCosseratMapping::updateTangExpSE3( - const In1VecCoord &inDeform) { + const In1VecCoord &inDeform) { // Curv abscissa of nodes and frames sofa::helper::ReadAccessor>> curv_abs_section = - d_curv_abs_section; + d_curv_abs_section; sofa::helper::ReadAccessor>> curv_abs_frames = - d_curv_abs_frames; + d_curv_abs_frames; m_framesTangExpVectors.clear(); unsigned int sz = curv_abs_frames.size(); + // Compute tangExpo at frame points - for (unsigned int i = 0; i < sz; i++) { + for (unsigned int i = 0; i < sz; i++) + { Tangent temp; Coord1 strain_frame_i = inDeform[m_indicesVectors[i] - 1]; @@ -258,13 +254,9 @@ void BaseCosseratMapping::updateTangExpSE3( m_framesTangExpVectors.push_back(temp); - if (d_debug.getValue()) { - printf("__________________________________________\n"); - std::cout << "x :" << curv_abs_x_i << "; k :" << strain_frame_i - << std::endl; - std::cout << "m_framesTangExpVectors :" << m_framesTangExpVectors[i] - << std::endl; - } + msg_info() + << "x :" << curv_abs_x_i << "; k :" << strain_frame_i << msgendl + << "m_framesTangExpVectors :" << m_framesTangExpVectors[i]; } // Compute the TangExpSE3 at the nodes @@ -281,21 +273,18 @@ void BaseCosseratMapping::updateTangExpSE3( computeTangExp(x, strain_node_i, temp); m_nodesTangExpVectors.push_back(temp); } - if (d_debug.getValue()) { - printf("_________________Node TangExpo___________________\n"); - std::cout << "Node TangExpo : " << m_nodesTangExpVectors << std::endl; - } + msg_info() << "Node TangExpo : " << m_nodesTangExpVectors; } template void BaseCosseratMapping::computeTangExp(double &curv_abs_n, - const Coord1 &strain_i, - Mat6x6 &TgX) { + const Coord1 &strain_i, + Mat6x6 &TgX) { SReal theta = Vec3(strain_i(0), strain_i(1), strain_i(2)) - .norm(); // Sometimes this is computed over all strain + .norm(); // Sometimes this is computed over all strain Mat3x3 tilde_k = - getTildeMatrix(Vec3(strain_i(0), strain_i(1), strain_i(2))); + getTildeMatrix(Vec3(strain_i(0), strain_i(1), strain_i(2))); /* Younes @23-11-27 old version @Todo ???? is p the linear deformation? If so, why didn't I just put a zero @@ -340,18 +329,16 @@ void BaseCosseratMapping::computeTangExp(double &curv_abs_n, template [[maybe_unused]] Vec6 BaseCosseratMapping::computeETA(const Vec6 &baseEta, - const In1VecDeriv &k_dot, - const double abs_input) { + const In1VecDeriv &k_dot, + const double abs_input) { // Fill the initial vector const In1DataVecCoord *x1fromData = - m_fromModel1->read(sofa::core::ConstVecCoordId::position()); + m_fromModel1->read(sofa::core::ConstVecCoordId::position()); const In1VecCoord x1from = x1fromData->getValue(); sofa::helper::ReadAccessor>> curv_abs_input = - d_curv_abs_section; - // helper::ReadAccessor>> curv_abs_output = - // d_curv_abs_frames; + d_curv_abs_section; Transform out_Trans; Mat6x6 Adjoint, Tg; @@ -386,14 +373,13 @@ void BaseCosseratMapping::initialize() { // it is attached. Here we only use the information from the curvilinear // abscissa of each frame. sofa::helper::ReadAccessor>> curv_abs_section = - d_curv_abs_section; + d_curv_abs_section; sofa::helper::ReadAccessor>> curv_abs_frames = - d_curv_abs_frames; + d_curv_abs_frames; size_t sz = d_curv_abs_frames.getValue().size(); - if (d_debug.getValue()) - msg_info("BaseCosserat:") + msg_info() << " curv_abs_section " << d_curv_abs_frames.getValue().size() << "; curv_abs_frames: " << d_curv_abs_frames.getValue().size(); @@ -408,7 +394,7 @@ void BaseCosseratMapping::initialize() { if (curv_abs_section[input_index] > curv_abs_frames[i]) { m_indicesVectors.emplace_back(input_index); m_indicesVectorsDraw.emplace_back( - input_index); // maybe I shouldn't do this here !!! + input_index); // maybe I shouldn't do this here !!! } else if (curv_abs_section[input_index] == curv_abs_frames[i]) { m_indicesVectors.emplace_back(input_index); input_index++; @@ -423,21 +409,18 @@ void BaseCosseratMapping::initialize() { // m_framesLengthVectors.push_back(curv_abs_frames[i] - // curv_abs_section[m_indicesVectors[i] - 1]); m_framesLengthVectors.emplace_back( - curv_abs_frames[i] - curv_abs_section[m_indicesVectors.back() - 1]); + curv_abs_frames[i] - curv_abs_section[m_indicesVectors.back() - 1]); } for (size_t j = 0; j < curv_abs_section.size() - 1; j++) { m_BeamLengthVectors.emplace_back(curv_abs_section[j + 1] - - curv_abs_section[j]); + curv_abs_section[j]); } - if (d_debug.getValue()) { - msg_info("BaseCosserat:") << "m_indicesVectors : " << m_indicesVectors; - msg_info("BaseCosserat:") - << "m_framesLengthVectors : " << m_framesLengthVectors; - msg_info("BaseCosserat:") - << "m_BeamLengthVectors : " << m_BeamLengthVectors; - } + msg_info() + << "m_indicesVectors : " << m_indicesVectors << msgendl + << "m_framesLengthVectors : " << msgendl + << "m_BeamLengthVectors : " << msgendl; } template @@ -445,7 +428,7 @@ void BaseCosseratMapping::draw(const sofa::core::visual::Visua template double BaseCosseratMapping::computeTheta(const double &x, - const Mat4x4 &gX) { + const Mat4x4 &gX) { double Tr_gx = 0.0; for (int i = 0; i < 4; i++) { Tr_gx += gX[i][i]; @@ -491,7 +474,7 @@ Mat3x3 BaseCosseratMapping::extractRotMatrix(const Transform & template auto BaseCosseratMapping::buildProjector(const Transform &T) - -> Tangent { +-> Tangent { Mat6x6 P; // TODO(dmarchal: 2024/06/07) The following code should probably become @@ -527,7 +510,7 @@ auto BaseCosseratMapping::buildXiHat(const Coord1 &strain_i) - template auto BaseCosseratMapping::getTildeMatrix(const sofa::type::Vec3 &u) - -> Mat3x3 { +-> Mat3x3 { sofa::type::Matrix3 tild; tild[0][1] = -u[2]; tild[0][2] = u[1]; @@ -541,8 +524,8 @@ auto BaseCosseratMapping::getTildeMatrix(const sofa::type::Vec template auto BaseCosseratMapping::buildAdjoint(const Mat3x3 &A, - const Mat3x3 &B, - Mat6x6 &Adjoint) -> void { + const Mat3x3 &B, + Mat6x6 &Adjoint) -> void { Adjoint.clear(); for (unsigned int i = 0; i < 3; i++) { for (int j = 0; j < 3; ++j) { // TODO(dmarchal:2024/06/07) unify i++ and ++j @@ -555,8 +538,8 @@ auto BaseCosseratMapping::buildAdjoint(const Mat3x3 &A, template auto BaseCosseratMapping::buildCoAdjoint(const Mat3x3 &A, - const Mat3x3 &B, - Mat6x6 &coAdjoint) -> void { + const Mat3x3 &B, + Mat6x6 &coAdjoint) -> void { coAdjoint.clear(); for (unsigned int i = 0; i < 3; i++) { for (int j = 0; j < 3; ++j) { // TODO(dmarchal:2024/06/07) unify i++ and ++j @@ -572,7 +555,7 @@ auto BaseCosseratMapping::buildCoAdjoint(const Mat3x3 &A, template auto BaseCosseratMapping::convertTransformToMatrix4x4( - const Transform &T) -> Mat4x4 { + const Transform &T) -> Mat4x4 { Mat4x4 M; M.identity(); Mat3x3 R = extractRotMatrix(T); From 9aa7ac6a42e826c3806c7a62d573d1287f1b5dde Mon Sep 17 00:00:00 2001 From: Damien Marchal Date: Wed, 12 Jun 2024 17:08:09 +0200 Subject: [PATCH 11/26] Replace ReadAccessor with sofa::helper::getReadAccessor; The sofa::helper::getReadAccessor function is to be used on type free expression this way: auto a = getReadAccessor(m_data); --- src/Cosserat/mapping/BaseCosseratMapping.h | 3 ++- src/Cosserat/mapping/BaseCosseratMapping.inl | 27 ++++++++------------ 2 files changed, 12 insertions(+), 18 deletions(-) diff --git a/src/Cosserat/mapping/BaseCosseratMapping.h b/src/Cosserat/mapping/BaseCosseratMapping.h index 2aeabbf..9dfdf8b 100644 --- a/src/Cosserat/mapping/BaseCosseratMapping.h +++ b/src/Cosserat/mapping/BaseCosseratMapping.h @@ -29,7 +29,7 @@ #include #include #include -#include +//#include #include #include @@ -64,6 +64,7 @@ using Cosserat::type::Transform; using Cosserat::type::Tangent; using Cosserat::type::RotMat; + } // TODO(dmarchal: 2024/10/07) Is the description valid ? diff --git a/src/Cosserat/mapping/BaseCosseratMapping.inl b/src/Cosserat/mapping/BaseCosseratMapping.inl index 48a775d..a72e0b7 100644 --- a/src/Cosserat/mapping/BaseCosseratMapping.inl +++ b/src/Cosserat/mapping/BaseCosseratMapping.inl @@ -114,11 +114,9 @@ void BaseCosseratMapping::computeExponentialSE3( // Fill exponential vectors template void BaseCosseratMapping::updateExponentialSE3( - const In1VecCoord &inDeform) { - // helper::ReadAccessor>> curv_abs_input = - // d_curv_abs_section; - sofa::helper::ReadAccessor>> curv_abs_frames = - d_curv_abs_frames; + const In1VecCoord &inDeform) +{ + auto curv_abs_frames = sofa::helper::getReadAccessor(d_curv_abs_frames); m_framesExponentialSE3Vectors.clear(); m_nodesExponentialSE3Vectors.clear(); @@ -235,13 +233,11 @@ void BaseCosseratMapping::updateTangExpSE3( const In1VecCoord &inDeform) { // Curv abscissa of nodes and frames - sofa::helper::ReadAccessor>> curv_abs_section = - d_curv_abs_section; - sofa::helper::ReadAccessor>> curv_abs_frames = - d_curv_abs_frames; + auto curv_abs_section = sofa::helper::getReadAccessor(d_curv_abs_section); + auto curv_abs_frames = sofa::helper::getReadAccessor(d_curv_abs_frames); - m_framesTangExpVectors.clear(); unsigned int sz = curv_abs_frames.size(); + m_framesTangExpVectors.resize(sz); // Compute tangExpo at frame points for (unsigned int i = 0; i < sz; i++) @@ -252,7 +248,7 @@ void BaseCosseratMapping::updateTangExpSE3( double curv_abs_x_i = m_framesLengthVectors[i]; computeTangExp(curv_abs_x_i, strain_frame_i, temp); - m_framesTangExpVectors.push_back(temp); + m_framesTangExpVectors[i] = temp; msg_info() << "x :" << curv_abs_x_i << "; k :" << strain_frame_i << msgendl @@ -337,8 +333,7 @@ BaseCosseratMapping::computeETA(const Vec6 &baseEta, m_fromModel1->read(sofa::core::ConstVecCoordId::position()); const In1VecCoord x1from = x1fromData->getValue(); - sofa::helper::ReadAccessor>> curv_abs_input = - d_curv_abs_section; + auto curv_abs_input = sofa::helper::getReadAccessor(d_curv_abs_section); Transform out_Trans; Mat6x6 Adjoint, Tg; @@ -372,10 +367,8 @@ void BaseCosseratMapping::initialize() { // For each frame in the global frame, find the segment of the beam to which // it is attached. Here we only use the information from the curvilinear // abscissa of each frame. - sofa::helper::ReadAccessor>> curv_abs_section = - d_curv_abs_section; - sofa::helper::ReadAccessor>> curv_abs_frames = - d_curv_abs_frames; + auto curv_abs_section = sofa::helper::getReadAccessor(d_curv_abs_section); + auto curv_abs_frames = sofa::helper::getReadAccessor(d_curv_abs_frames); size_t sz = d_curv_abs_frames.getValue().size(); From 077e98438b1af08459acc875d7f5d7833596a11c Mon Sep 17 00:00:00 2001 From: Damien Marchal Date: Wed, 12 Jun 2024 17:19:58 +0200 Subject: [PATCH 12/26] remove toModel, fromModel1, fromModel2 to use he inherited attributes from MultiMapping --- src/Cosserat/mapping/BaseCosseratMapping.h | 10 +++------- src/Cosserat/mapping/BaseCosseratMapping.inl | 5 ++--- 2 files changed, 5 insertions(+), 10 deletions(-) diff --git a/src/Cosserat/mapping/BaseCosseratMapping.h b/src/Cosserat/mapping/BaseCosseratMapping.h index 9dfdf8b..f2d7f66 100644 --- a/src/Cosserat/mapping/BaseCosseratMapping.h +++ b/src/Cosserat/mapping/BaseCosseratMapping.h @@ -217,13 +217,9 @@ class BaseCosseratMapping : public sofa::core::Multi2Mapping sofa::Data> d_curv_abs_frames; sofa::Data d_debug; - /// Input Models container. New inputs are added through addInputModel(In* ). - sofa::core::State *m_fromModel1; - - // TODO(dmarchal): why this maybe_unused on a data field ? - [[maybe_unused]] sofa::core::State *m_fromModel2; - - sofa::core::State *m_toModel; + using Inherit1::fromModels1; + using Inherit1::fromModels2; + using Inherit1::toModels; protected: /// Constructor diff --git a/src/Cosserat/mapping/BaseCosseratMapping.inl b/src/Cosserat/mapping/BaseCosseratMapping.inl index a72e0b7..e592228 100644 --- a/src/Cosserat/mapping/BaseCosseratMapping.inl +++ b/src/Cosserat/mapping/BaseCosseratMapping.inl @@ -56,7 +56,6 @@ BaseCosseratMapping::BaseCosseratMapping() d_curv_abs_frames(initData(&d_curv_abs_frames, "curv_abs_output", " need to be com....")), d_debug(initData(&d_debug, false, "debug", "printf for the debug")), - m_fromModel1(NULL), m_fromModel2(NULL), m_toModel(NULL), m_index_input(0) {} // _________________________________________________________________________________________ @@ -68,7 +67,7 @@ void BaseCosseratMapping::init() // Fill the initial vector const OutDataVecCoord *xfromData = - m_toModel->read(sofa::core::ConstVecCoordId::position()); + toModels[0]->read(sofa::core::ConstVecCoordId::position()); //TODO(dmarchal, 2024/07/12): is this line really needed ? // it initialize a local variable, is it to force a xfromData updates ? @@ -330,7 +329,7 @@ BaseCosseratMapping::computeETA(const Vec6 &baseEta, // Fill the initial vector const In1DataVecCoord *x1fromData = - m_fromModel1->read(sofa::core::ConstVecCoordId::position()); + fromModels1[0]->read(sofa::core::ConstVecCoordId::position()); const In1VecCoord x1from = x1fromData->getValue(); auto curv_abs_input = sofa::helper::getReadAccessor(d_curv_abs_section); From 2d44aef63cfc2b847c7b6a40bf513376629bbc41 Mon Sep 17 00:00:00 2001 From: Damien Marchal Date: Wed, 12 Jun 2024 17:33:37 +0200 Subject: [PATCH 13/26] Several cleaning BaseCosseratMapping --- src/Cosserat/mapping/BaseCosseratMapping.h | 2 +- src/Cosserat/mapping/BaseCosseratMapping.inl | 66 ++++++++++---------- 2 files changed, 33 insertions(+), 35 deletions(-) diff --git a/src/Cosserat/mapping/BaseCosseratMapping.h b/src/Cosserat/mapping/BaseCosseratMapping.h index f2d7f66..7314f59 100644 --- a/src/Cosserat/mapping/BaseCosseratMapping.h +++ b/src/Cosserat/mapping/BaseCosseratMapping.h @@ -163,7 +163,6 @@ class BaseCosseratMapping : public sofa::core::Multi2Mapping public: /********************** Inhertited from BaseObject **************/ void init() override; - void draw(const sofa::core::visual::VisualParams *) override; /************************* BaseCosserat **************************/ // TODO(dmarchal:2024/06/07), so we have "initialize" and "init" @@ -224,6 +223,7 @@ class BaseCosseratMapping : public sofa::core::Multi2Mapping protected: /// Constructor BaseCosseratMapping(); + /// Destructor ~BaseCosseratMapping() override = default; diff --git a/src/Cosserat/mapping/BaseCosseratMapping.inl b/src/Cosserat/mapping/BaseCosseratMapping.inl index e592228..de58c0e 100644 --- a/src/Cosserat/mapping/BaseCosseratMapping.inl +++ b/src/Cosserat/mapping/BaseCosseratMapping.inl @@ -37,16 +37,9 @@ // To go further => // https://www.mathworks.com/matlabcentral/fileexchange/83038-sorosim/ -namespace Cosserat::mapping { - -namespace +namespace Cosserat::mapping { -using namespace sofa::defaulttype; -using sofa::core::objectmodel::BaseContext; -using sofa::helper::AdvancedTimer; -using sofa::helper::WriteAccessor; -using sofa::type::vector; -} + template BaseCosseratMapping::BaseCosseratMapping() @@ -65,20 +58,20 @@ void BaseCosseratMapping::init() { Inherit1::init(); - // Fill the initial vector - const OutDataVecCoord *xfromData = - toModels[0]->read(sofa::core::ConstVecCoordId::position()); - - //TODO(dmarchal, 2024/07/12): is this line really needed ? - // it initialize a local variable, is it to force a xfromData updates ? + //TODO(dmarchal, 2024/07/12): is the following line really needed ? + // it initialize a local variable which initialize another data. + // is it to force a xfromData updates through the use of getValue(). In addition + // this is puzzleing as toModel is the destination of the computation of the mapping. + // to me it should be safe to remove the two line.. and thus the init :) + const OutDataVecCoord *xfromData = toModels[0]->read(sofa::core::ConstVecCoordId::position()); const OutVecCoord xfrom = xfromData->getValue(); } template void BaseCosseratMapping::computeExponentialSE3( const double &curv_abs_x_n, const Coord1 &strain_n, Transform &g_X_n) { - Mat4x4 I4; - I4.identity(); + const auto I4 = Mat4x4::Identity(); + // Get the angular part of the sofa::type::Vec3 k = Vec3(strain_n(0), strain_n(1), strain_n(2)); SReal theta = k.norm(); // @@ -120,6 +113,7 @@ void BaseCosseratMapping::updateExponentialSE3( m_framesExponentialSE3Vectors.clear(); m_nodesExponentialSE3Vectors.clear(); m_nodesLogarithmeSE3Vectors.clear(); + const unsigned int sz = curv_abs_frames.size(); // Compute exponential at each frame point @@ -369,11 +363,11 @@ void BaseCosseratMapping::initialize() { auto curv_abs_section = sofa::helper::getReadAccessor(d_curv_abs_section); auto curv_abs_frames = sofa::helper::getReadAccessor(d_curv_abs_frames); - size_t sz = d_curv_abs_frames.getValue().size(); + size_t sz = curv_abs_frames.size(); msg_info() - << " curv_abs_section " << d_curv_abs_frames.getValue().size() - << "; curv_abs_frames: " << d_curv_abs_frames.getValue().size(); + << " curv_abs_section " << curv_abs_frames.size() + << "; curv_abs_frames: " << curv_abs_frames.size(); m_indicesVectors.clear(); m_framesLengthVectors.clear(); @@ -381,13 +375,14 @@ void BaseCosseratMapping::initialize() { m_indicesVectorsDraw.clear(); size_t input_index = 1; - - for (size_t i = 0; i < sz; i++) { - if (curv_abs_section[input_index] > curv_abs_frames[i]) { + for (size_t i = 0; i < sz; ++i) + { + if (curv_abs_section[input_index] > curv_abs_frames[i]) + { m_indicesVectors.emplace_back(input_index); - m_indicesVectorsDraw.emplace_back( - input_index); // maybe I shouldn't do this here !!! - } else if (curv_abs_section[input_index] == curv_abs_frames[i]) { + m_indicesVectorsDraw.emplace_back(input_index); // maybe I shouldn't do this here !!! + } else if (curv_abs_section[input_index] == curv_abs_frames[i]) + { m_indicesVectors.emplace_back(input_index); input_index++; m_indicesVectorsDraw.emplace_back(input_index); @@ -396,6 +391,7 @@ void BaseCosseratMapping::initialize() { m_indicesVectors.emplace_back(input_index); m_indicesVectorsDraw.emplace_back(input_index); } + // Fill the vector m_framesLengthVectors with the distance // between frame(output) and the closest beam node toward the base // m_framesLengthVectors.push_back(curv_abs_frames[i] - @@ -404,7 +400,8 @@ void BaseCosseratMapping::initialize() { curv_abs_frames[i] - curv_abs_section[m_indicesVectors.back() - 1]); } - for (size_t j = 0; j < curv_abs_section.size() - 1; j++) { + for (size_t j = 0; j < sz - 1; j++) + { m_BeamLengthVectors.emplace_back(curv_abs_section[j + 1] - curv_abs_section[j]); } @@ -415,9 +412,6 @@ void BaseCosseratMapping::initialize() { << "m_BeamLengthVectors : " << msgendl; } -template -void BaseCosseratMapping::draw(const sofa::core::visual::VisualParams *) {} - template double BaseCosseratMapping::computeTheta(const double &x, const Mat4x4 &gX) { @@ -519,8 +513,10 @@ auto BaseCosseratMapping::buildAdjoint(const Mat3x3 &A, const Mat3x3 &B, Mat6x6 &Adjoint) -> void { Adjoint.clear(); - for (unsigned int i = 0; i < 3; i++) { - for (int j = 0; j < 3; ++j) { // TODO(dmarchal:2024/06/07) unify i++ and ++j + for (unsigned int i = 0; i < 3; ++i) + { + for (int j = 0; j < 3; ++j) + { Adjoint[i][j] = A[i][j]; Adjoint[i + 3][j + 3] = A[i][j]; Adjoint[i + 3][j] = B[i][j]; @@ -533,8 +529,10 @@ auto BaseCosseratMapping::buildCoAdjoint(const Mat3x3 &A, const Mat3x3 &B, Mat6x6 &coAdjoint) -> void { coAdjoint.clear(); - for (unsigned int i = 0; i < 3; i++) { - for (int j = 0; j < 3; ++j) { // TODO(dmarchal:2024/06/07) unify i++ and ++j + for (unsigned int i = 0; i < 3; ++i) + { + for (int j = 0; j < 3; ++j) + { coAdjoint[i][j] = A[i][j]; coAdjoint[i + 3][j + 3] = A[i][j]; From e7392fed682f56c347e178d28cc463018e51a2f9 Mon Sep 17 00:00:00 2001 From: Damien Marchal Date: Wed, 12 Jun 2024 18:00:12 +0200 Subject: [PATCH 14/26] Clean Vec6,Mat4x4 usage in BaseCosseratMapping for more readability (should't be breaking) --- src/Cosserat/mapping/BaseCosseratMapping.inl | 109 +++++++++---------- 1 file changed, 51 insertions(+), 58 deletions(-) diff --git a/src/Cosserat/mapping/BaseCosseratMapping.inl b/src/Cosserat/mapping/BaseCosseratMapping.inl index de58c0e..60b0b92 100644 --- a/src/Cosserat/mapping/BaseCosseratMapping.inl +++ b/src/Cosserat/mapping/BaseCosseratMapping.inl @@ -40,6 +40,10 @@ namespace Cosserat::mapping { +using sofa::helper::getReadAccessor; +using sofa::type::Vec6; +using sofa::type::Vec3; +using sofa::type::Quat; template BaseCosseratMapping::BaseCosseratMapping() @@ -73,7 +77,7 @@ void BaseCosseratMapping::computeExponentialSE3( const auto I4 = Mat4x4::Identity(); // Get the angular part of the - sofa::type::Vec3 k = Vec3(strain_n(0), strain_n(1), strain_n(2)); + Vec3 k = Vec3(strain_n(0), strain_n(1), strain_n(2)); SReal theta = k.norm(); // SE3 _g_X; @@ -94,11 +98,11 @@ void BaseCosseratMapping::computeExponentialSE3( msg_info() << "matrix _g_X : " << _g_X; - sofa::type::Mat3x3 M; + Mat3x3 M; _g_X.getsub(0, 0, M); // get the rotation matrix // convert the rotation 3x3 matrix to a quaternion - sofa::type::Quat R; + Quat R; R.fromMatrix(M); g_X_n = Transform(Vec3(_g_X(0, 3), _g_X(1, 3), _g_X(2, 3)), R); } @@ -124,8 +128,7 @@ void BaseCosseratMapping::updateExponentialSE3( // the size varies from 1 to 6 // The distance between the frame and the closest beam node toward the base - const SReal curv_abs_x = - m_framesLengthVectors[i]; // curv_abs_x = frame_curv_abs - L_(n-1) + const SReal curv_abs_x = m_framesLengthVectors[i]; computeExponentialSE3(curv_abs_x, strain_n, g_X_frame_i); m_framesExponentialSE3Vectors.push_back(g_X_frame_i); @@ -137,13 +140,13 @@ void BaseCosseratMapping::updateExponentialSE3( // Compute the exponential on the nodes m_nodesExponentialSE3Vectors.push_back( - Transform(sofa::type::Vec3(0.0, 0.0, 0.0), - sofa::type::Quat(0., 0., 0., 1.))); // The first node. + Transform(Vec3(0.0, 0.0, 0.0), + Quat(0., 0., 0., 1.))); // The first node. - for (unsigned int j = 0; j < inDeform.size(); j++) { - Coord1 strain_n = inDeform[j]; // Strain_n - const SReal curv_abs_x = - m_BeamLengthVectors[j]; // curv_abs_x = L_n - L_(n-1) + for (unsigned int j = 0; j < inDeform.size(); ++j) + { + Coord1 strain_n = inDeform[j]; + const SReal curv_abs_x = m_BeamLengthVectors[j]; Transform g_X_node_j; computeExponentialSE3(curv_abs_x, strain_n, g_X_node_j); @@ -226,8 +229,8 @@ void BaseCosseratMapping::updateTangExpSE3( const In1VecCoord &inDeform) { // Curv abscissa of nodes and frames - auto curv_abs_section = sofa::helper::getReadAccessor(d_curv_abs_section); - auto curv_abs_frames = sofa::helper::getReadAccessor(d_curv_abs_frames); + auto curv_abs_section = getReadAccessor(d_curv_abs_section); + auto curv_abs_frames = getReadAccessor(d_curv_abs_frames); unsigned int sz = curv_abs_frames.size(); m_framesTangExpVectors.resize(sz); @@ -288,7 +291,6 @@ void BaseCosseratMapping::computeTangExp(double &curv_abs_n, buildAdjoint(tilde_k, tilde_q, ad_Xi); Mat6x6 Id6 = Mat6x6::Identity(); - // for (unsigned int i =0; i< 6;i++) Id6[i][i]=1.0; //define identity 6x6 if (theta <= std::numeric_limits::epsilon()) { double scalar0 = std::pow(curv_abs_n, 2) / 2.0; @@ -319,37 +321,38 @@ template [[maybe_unused]] Vec6 BaseCosseratMapping::computeETA(const Vec6 &baseEta, const In1VecDeriv &k_dot, - const double abs_input) { + const double abs_input) +{ - // Fill the initial vector - const In1DataVecCoord *x1fromData = - fromModels1[0]->read(sofa::core::ConstVecCoordId::position()); - const In1VecCoord x1from = x1fromData->getValue(); + // Get the positions from model 0. This function returns the position wrapped in a Data<> + auto d_x1 = fromModels1[0]->read(sofa::core::ConstVecCoordId::position()); - auto curv_abs_input = sofa::helper::getReadAccessor(d_curv_abs_section); + // To access the actual content (in this case position) from a data, we have to use + // a read accessor that insures the data is updated according to DDGNode state + auto x1 = getReadAccessor(*d_x1); + + // Same as for x1, query a read accessor so we can access the content of d_curv_abs_section + auto curv_abs_input = getReadAccessor(d_curv_abs_section); Transform out_Trans; Mat6x6 Adjoint, Tg; + Vec6 Xi_dot; - sofa::type::Vec6 Xi_dot; - for (unsigned int i = 0; i < 3; i++) + for (unsigned int i = 0; i < 3; ++i) Xi_dot[i] = k_dot[m_index_input][i]; - double diff0; - double _diff0; + double diff0 = abs_input; + double _diff0 = -abs_input; - if (m_index_input == 0) { - diff0 = abs_input; // - _diff0 = -abs_input; - } else { + if (m_index_input != 0) + { diff0 = abs_input - curv_abs_input[m_index_input - 1]; _diff0 = curv_abs_input[m_index_input - 1] - abs_input; } - computeExponentialSE3(_diff0, x1from[m_index_input], out_Trans); + computeExponentialSE3(_diff0, x1[m_index_input], out_Trans); computeAdjoint(out_Trans, Adjoint); - - computeTangExp(diff0, x1from[m_index_input], Tg); + computeTangExp(diff0, x1[m_index_input], Tg); return Adjoint * (baseEta + Tg * Xi_dot); } @@ -360,20 +363,18 @@ void BaseCosseratMapping::initialize() { // For each frame in the global frame, find the segment of the beam to which // it is attached. Here we only use the information from the curvilinear // abscissa of each frame. - auto curv_abs_section = sofa::helper::getReadAccessor(d_curv_abs_section); - auto curv_abs_frames = sofa::helper::getReadAccessor(d_curv_abs_frames); - - size_t sz = curv_abs_frames.size(); + auto curv_abs_section = getReadAccessor(d_curv_abs_section); + auto curv_abs_frames = getReadAccessor(d_curv_abs_frames); msg_info() - << " curv_abs_section " << curv_abs_frames.size() - << "; curv_abs_frames: " << curv_abs_frames.size(); + << " curv_abs_section " << curv_abs_frames.size() << "; curv_abs_frames: " << curv_abs_frames.size(); m_indicesVectors.clear(); m_framesLengthVectors.clear(); m_BeamLengthVectors.clear(); m_indicesVectorsDraw.clear(); + size_t sz = curv_abs_frames.size(); size_t input_index = 1; for (size_t i = 0; i < sz; ++i) { @@ -394,16 +395,13 @@ void BaseCosseratMapping::initialize() { // Fill the vector m_framesLengthVectors with the distance // between frame(output) and the closest beam node toward the base - // m_framesLengthVectors.push_back(curv_abs_frames[i] - - // curv_abs_section[m_indicesVectors[i] - 1]); m_framesLengthVectors.emplace_back( curv_abs_frames[i] - curv_abs_section[m_indicesVectors.back() - 1]); } - for (size_t j = 0; j < sz - 1; j++) + for (size_t j = 0; j < sz - 1; ++j) { - m_BeamLengthVectors.emplace_back(curv_abs_section[j + 1] - - curv_abs_section[j]); + m_BeamLengthVectors.emplace_back(curv_abs_section[j + 1] - curv_abs_section[j]); } msg_info() @@ -415,18 +413,12 @@ void BaseCosseratMapping::initialize() { template double BaseCosseratMapping::computeTheta(const double &x, const Mat4x4 &gX) { - double Tr_gx = 0.0; - for (int i = 0; i < 4; i++) { - Tr_gx += gX[i][i]; - } + double Tr_gx = sofa::type::trace(gX); - double theta; - if (x <= std::numeric_limits::epsilon()) - theta = 0.0; - else - theta = (1.0 / x) * std::acos((Tr_gx / 2.0) - 1); + if (x > std::numeric_limits::epsilon()) + return (1.0 / x) * std::acos((Tr_gx / 2.0) - 1); - return theta; + return 0.0; } template @@ -444,7 +436,7 @@ void BaseCosseratMapping::printMatrix(const Mat6x6 R) { template Mat3x3 BaseCosseratMapping::extractRotMatrix(const Transform &frame) { - sofa::type::Quat q = frame.getOrientation(); + Quat q = frame.getOrientation(); // TODO(dmarchal: 2024/06/07) The following code should probably become // utility function building a 3x3 matix from a quaternion should probably @@ -546,13 +538,14 @@ auto BaseCosseratMapping::buildCoAdjoint(const Mat3x3 &A, template auto BaseCosseratMapping::convertTransformToMatrix4x4( const Transform &T) -> Mat4x4 { - Mat4x4 M; - M.identity(); + Mat4x4 M = Mat4x4::Identity(); Mat3x3 R = extractRotMatrix(T); - sofa::type::Vec3 trans = T.getOrigin(); + Vec3 trans = T.getOrigin(); - for (unsigned int i = 0; i < 3; i++) { - for (unsigned int j = 0; j < 3; j++) { + for (unsigned int i = 0; i < 3; i++) + { + for (unsigned int j = 0; j < 3; j++) + { M(i, j) = R[i][j]; M(i, 3) = trans[i]; } From ee8b742a4b2351ccca29b254b637cc72fa48e63b Mon Sep 17 00:00:00 2001 From: Damien Marchal Date: Thu, 13 Jun 2024 11:38:33 +0200 Subject: [PATCH 15/26] Cosmetic cleaning in BaseCosserat & DiscreteCorreseratMapping.h --- src/Cosserat/mapping/BaseCosseratMapping.h | 17 +-- src/Cosserat/mapping/BaseCosseratMapping.inl | 25 ++-- .../mapping/DiscreteCosseratMapping.h | 119 +++++++++--------- 3 files changed, 72 insertions(+), 89 deletions(-) diff --git a/src/Cosserat/mapping/BaseCosseratMapping.h b/src/Cosserat/mapping/BaseCosseratMapping.h index 7314f59..f380eea 100644 --- a/src/Cosserat/mapping/BaseCosseratMapping.h +++ b/src/Cosserat/mapping/BaseCosseratMapping.h @@ -113,18 +113,6 @@ class BaseCosseratMapping : public sofa::core::Multi2Mapping typedef sofa::Data OutDataVecDeriv; typedef sofa::Data OutDataMatrixDeriv; - typedef sofa::MultiLink, sofa::core::State, - sofa::BaseLink::FLAG_STOREPATH | sofa::BaseLink::FLAG_STRONGLINK> - LinkFromModels1; - typedef sofa::MultiLink, sofa::core::State, - sofa::BaseLink::FLAG_STOREPATH | sofa::BaseLink::FLAG_STRONGLINK> - LinkFromModels2; - - [[maybe_unused]] typedef sofa::MultiLink< - BaseCosseratMapping, sofa::core::State, - sofa::BaseLink::FLAG_STOREPATH | sofa::BaseLink::FLAG_STRONGLINK> - LinkToModels; - public: // TODO(dmarchal: 2024/06/07): There is a lot of public attributes is this // really needed ? @@ -231,9 +219,8 @@ class BaseCosseratMapping : public sofa::core::Multi2Mapping Transform &Trans); // TODO(dmarchal: 2024/06/07): - // - two naming convention - // - unclear the difference between computeAdjoing and buildAdjoint ... is - // there room for factoring things ? + // - clarify the difference between computeAdjoing and buildAdjoint ... + // - clarify why we need Transform and Vec6. void computeAdjoint(const Transform &frame, Mat6x6 &adjoint); void computeAdjoint(const Vec6 &frame, Mat6x6 &adjoint); diff --git a/src/Cosserat/mapping/BaseCosseratMapping.inl b/src/Cosserat/mapping/BaseCosseratMapping.inl index 60b0b92..e310406 100644 --- a/src/Cosserat/mapping/BaseCosseratMapping.inl +++ b/src/Cosserat/mapping/BaseCosseratMapping.inl @@ -121,7 +121,8 @@ void BaseCosseratMapping::updateExponentialSE3( const unsigned int sz = curv_abs_frames.size(); // Compute exponential at each frame point - for (size_t i = 0; i < sz; i++) { + for (size_t i = 0; i < sz; ++i) + { Transform g_X_frame_i; const Coord1 strain_n = inDeform[m_indicesVectors[i] - 1]; // Cosserat reduce coordinates (strain) @@ -334,12 +335,9 @@ BaseCosseratMapping::computeETA(const Vec6 &baseEta, // Same as for x1, query a read accessor so we can access the content of d_curv_abs_section auto curv_abs_input = getReadAccessor(d_curv_abs_section); - Transform out_Trans; - Mat6x6 Adjoint, Tg; - Vec6 Xi_dot; - - for (unsigned int i = 0; i < 3; ++i) - Xi_dot[i] = k_dot[m_index_input][i]; + auto& kdot = k_dot[m_index_input]; + Vec6 Xi_dot {kdot[0], kdot[1], kdot[2], + 0,0,0}; double diff0 = abs_input; double _diff0 = -abs_input; @@ -350,11 +348,16 @@ BaseCosseratMapping::computeETA(const Vec6 &baseEta, _diff0 = curv_abs_input[m_index_input - 1] - abs_input; } - computeExponentialSE3(_diff0, x1[m_index_input], out_Trans); - computeAdjoint(out_Trans, Adjoint); - computeTangExp(diff0, x1[m_index_input], Tg); + Transform outTransform; + computeExponentialSE3(_diff0, x1[m_index_input], outTransform); + + Mat6x6 adjointMatrix; + computeAdjoint(outTransform, adjointMatrix); + + Tangent tangentMatrix; + computeTangExp(diff0, x1[m_index_input], tangentMatrix); - return Adjoint * (baseEta + Tg * Xi_dot); + return adjointMatrix * (baseEta + tangentMatrix * Xi_dot); } //___________________________________________________________________________ diff --git a/src/Cosserat/mapping/DiscreteCosseratMapping.h b/src/Cosserat/mapping/DiscreteCosseratMapping.h index c5547b8..5cd0398 100644 --- a/src/Cosserat/mapping/DiscreteCosseratMapping.h +++ b/src/Cosserat/mapping/DiscreteCosseratMapping.h @@ -44,15 +44,12 @@ using sofa::type::Quat; using std::get; using sofa::Data; + /*! * \class DiscreteCosseratMapping * @brief Computes and map the length of the beams * - * This is a component: - * https://www.sofa-framework.org/community/doc/programming-with-sofa/create-your-component/ */ -// using Cosserat::mapping::BaseCosserat; - template class DiscreteCosseratMapping : public BaseCosseratMapping { public: @@ -99,10 +96,9 @@ class DiscreteCosseratMapping : public BaseCosseratMapping { typedef Data OutDataMatrixDeriv; typedef typename SolidTypes::Transform Transform; -protected: - sofa::core::State *m_fromModel1; - sofa::core::State *m_fromModel2; - sofa::core::State *m_toModel; + ////////////////////////////////////////////////////////////////////// + /// @name Data Fields + /// @{ Data d_deformationAxis; Data d_max; Data d_min; @@ -111,92 +107,89 @@ class DiscreteCosseratMapping : public BaseCosseratMapping { Data d_color; Data> d_index; Data d_baseIndex; + /// @} + ////////////////////////////////////////////////////////////////////// public: - typedef sofa::MultiLink, - sofa::core::State, - sofa::BaseLink::FLAG_STOREPATH | sofa::BaseLink::FLAG_STRONGLINK> - LinkFromModels1; - typedef sofa::MultiLink, - sofa::core::State, - sofa::BaseLink::FLAG_STOREPATH | sofa::BaseLink::FLAG_STRONGLINK> - LinkFromModels2; - typedef sofa::MultiLink, - sofa::core::State, - sofa::BaseLink::FLAG_STOREPATH | sofa::BaseLink::FLAG_STRONGLINK> - LinkToModels; - -protected: - ////////////////////////// Inherited attributes //////////////////////////// - /// https://gcc.gnu.org/onlinedocs/gcc/Name-lookup.html - /// Bring inherited attributes and function in the current lookup context. - /// otherwise any access to the base::attribute would require - /// the "this->" approach. - /// - using BaseCosseratMapping::m_indicesVectors; - using BaseCosseratMapping::d_curv_abs_section; - using BaseCosseratMapping::d_curv_abs_frames; - using BaseCosseratMapping::m_nodesTangExpVectors; - using BaseCosseratMapping::m_nodesVelocityVectors; - using BaseCosseratMapping::m_framesExponentialSE3Vectors; - using BaseCosseratMapping::m_framesTangExpVectors; - using BaseCosseratMapping::m_totalBeamForceVectors; - using BaseCosseratMapping::m_nodesExponentialSE3Vectors; - using BaseCosseratMapping::d_debug; - using BaseCosseratMapping::m_vecTransform; - using BaseCosseratMapping::m_nodeAdjointVectors; - using BaseCosseratMapping::m_index_input; - using BaseCosseratMapping::m_indicesVectorsDraw; - using BaseCosseratMapping::computeTheta; - -protected: - /// Constructor - DiscreteCosseratMapping(); - /// Destructor - ~DiscreteCosseratMapping() override {} - -public: - /**********************SOFA METHODS**************************/ + ////////////////////////////////////////////////////////////////////// + /// The following methods are inherited from BaseObject + /// @{ void init() override; void reinit() override; void draw(const sofa::core::visual::VisualParams *vparams) override; + /// @} + ////////////////////////////////////////////////////////////////////// + - /**********************MAPPING METHODS**************************/ - void - apply(const sofa::core::MechanicalParams * /* mparams */, + ////////////////////////////////////////////////////////////////////// + /// The following method are inherited from MultiMapping + /// @{ + void apply(const sofa::core::MechanicalParams * /* mparams */, const vector &dataVecOutPos, const vector &dataVecIn1Pos, const vector &dataVecIn2Pos) override; - void - applyJ(const sofa::core::MechanicalParams * /* mparams */, + void applyJ(const sofa::core::MechanicalParams * /* mparams */, const vector &dataVecOutVel, const vector &dataVecIn1Vel, const vector &dataVecIn2Vel) override; - // ApplyJT Force - void - applyJT(const sofa::core::MechanicalParams * /* mparams */, + void applyJT(const sofa::core::MechanicalParams * /* mparams */, const vector &dataVecOut1Force, const vector &dataVecOut2RootForce, const vector &dataVecInForce) override; + // TODO(dmarchal:2024/06/13): Override with an empty function is a rare code pattern + // to make it clear this is the intented and not just an "I'm too lazy to implement it" + // please always have a precise code comment explaning with details why it is empty. void applyDJT(const sofa::core::MechanicalParams * /*mparams*/, sofa::core::MultiVecDerivId /*inForce*/, sofa::core::ConstMultiVecDerivId /*outForce*/) override {} - /// This method must be reimplemented by all mappings if they need to support - /// constraints. - virtual void applyJT( + /// Support for constraints. + void applyJT( const sofa::core::ConstraintParams *cparams, const vector &dataMatOut1Const, const vector &dataMatOut2Const, const vector &dataMatInConst) override; + /// @} + ///////////////////////////////////////////////////////////////////////////// + + void computeBBox(const sofa::core::ExecParams *params, bool onlyVisible) override; void computeLogarithm(const double &x, const Matrix4 &gX, Matrix4 &log_gX); protected: + ////////////////////////// Inherited attributes //////////////////////////// + /// https://gcc.gnu.org/onlinedocs/gcc/Name-lookup.html + /// Bring inherited attributes and function in the current lookup context. + /// otherwise any access to the base::attribute would require + /// the "this->" approach. + using BaseCosseratMapping::m_indicesVectors; + using BaseCosseratMapping::d_curv_abs_section; + using BaseCosseratMapping::d_curv_abs_frames; + using BaseCosseratMapping::m_nodesTangExpVectors; + using BaseCosseratMapping::m_nodesVelocityVectors; + using BaseCosseratMapping::m_framesExponentialSE3Vectors; + using BaseCosseratMapping::m_framesTangExpVectors; + using BaseCosseratMapping::m_totalBeamForceVectors; + using BaseCosseratMapping::m_nodesExponentialSE3Vectors; + using BaseCosseratMapping::d_debug; + using BaseCosseratMapping::m_vecTransform; + using BaseCosseratMapping::m_nodeAdjointVectors; + using BaseCosseratMapping::m_index_input; + using BaseCosseratMapping::m_indicesVectorsDraw; + using BaseCosseratMapping::computeTheta; + ////////////////////////////////////////////////////////////////////////////// + sofa::helper::ColorMap m_colorMap; + sofa::core::State *m_fromModel1; + sofa::core::State *m_fromModel2; + sofa::core::State *m_toModel; + +protected: + DiscreteCosseratMapping(); + ~DiscreteCosseratMapping() override {} }; #if !defined(SOFA_COSSERAT_CPP_DiscreteCosseratMapping) From 293f3d70ec7f960df0ae4d329bedae76f1931b29 Mon Sep 17 00:00:00 2001 From: Damien Marchal Date: Thu, 13 Jun 2024 11:44:38 +0200 Subject: [PATCH 16/26] Removes un-needed includes in BaseCosseratMapping.h --- src/Cosserat/mapping/BaseCosseratMapping.h | 18 ++++-------------- 1 file changed, 4 insertions(+), 14 deletions(-) diff --git a/src/Cosserat/mapping/BaseCosseratMapping.h b/src/Cosserat/mapping/BaseCosseratMapping.h index f380eea..13aa59a 100644 --- a/src/Cosserat/mapping/BaseCosseratMapping.h +++ b/src/Cosserat/mapping/BaseCosseratMapping.h @@ -24,26 +24,15 @@ #include #include -#include -#include -#include -#include -#include -//#include -#include -#include namespace Cosserat::mapping { -// with a private namespace the used named are not polluating the whole -// sofa::component::mapping ones. +// Use a private namespace so we are not polluating the Cosserat::mapping. namespace { using namespace std; using namespace Eigen; -using sofa::core::objectmodel::BaseContext; -using sofa::core::objectmodel::BaseObject; using sofa::defaulttype::SolidTypes; using sofa::type::Mat6x6; using sofa::type::Mat4x4; @@ -55,8 +44,9 @@ using sofa::type::Vec3; using sofa::type::Vec6; using sofa::type::Mat; -using se3 = sofa::type::Matrix4; -using SE3 = sofa::type::Matrix4; +// TODO(dmarchal: 2024/06/12): please check the comment to confirme this is true +using SE3 = sofa::type::Matrix4; ///< The "coordinate" in SE3 +using se3 = sofa::type::Matrix4; ///< The "speed" of change of SE3. using _se3 = Eigen::Matrix4d; using _SE3 = Eigen::Matrix4d; From 6a6f10d514f9397dadfba3d6f269c8efd852ad56 Mon Sep 17 00:00:00 2001 From: Damien Marchal Date: Thu, 13 Jun 2024 15:17:35 +0200 Subject: [PATCH 17/26] Factorize toModel, fromModel1, fromModel2 in BaseCosseratMapping --- src/Cosserat/mapping/BaseCosseratMapping.h | 8 ++++++-- src/Cosserat/mapping/BaseCosseratMapping.inl | 4 ++++ .../mapping/DiscreteCosseratMapping.cpp | 1 + .../mapping/DiscreteCosseratMapping.h | 9 +++++---- .../mapping/DiscreteCosseratMapping.inl | 19 ++++++++++--------- .../mapping/DiscreteDynamicCosseratMapping.h | 8 +++----- .../DiscreteDynamicCosseratMapping.inl | 12 ++---------- src/Cosserat/types.h | 4 +++- 8 files changed, 34 insertions(+), 31 deletions(-) diff --git a/src/Cosserat/mapping/BaseCosseratMapping.h b/src/Cosserat/mapping/BaseCosseratMapping.h index 13aa59a..7224e92 100644 --- a/src/Cosserat/mapping/BaseCosseratMapping.h +++ b/src/Cosserat/mapping/BaseCosseratMapping.h @@ -198,6 +198,10 @@ class BaseCosseratMapping : public sofa::core::Multi2Mapping using Inherit1::fromModels2; using Inherit1::toModels; + sofa::core::State* m_fromModel1; + sofa::core::State* m_fromModel2; + sofa::core::State* m_toModel; + protected: /// Constructor BaseCosseratMapping(); @@ -210,8 +214,8 @@ class BaseCosseratMapping : public sofa::core::Multi2Mapping // TODO(dmarchal: 2024/06/07): // - clarify the difference between computeAdjoing and buildAdjoint ... - // - clarify why we need Transform and Vec6. - void computeAdjoint(const Transform &frame, Mat6x6 &adjoint); + // - clarify why we need Transform and Vec6 and Tangent & Mat6x6 + void computeAdjoint(const Transform &frame, Tangent &adjoint); void computeAdjoint(const Vec6 &frame, Mat6x6 &adjoint); void computeCoAdjoint(const Transform &frame, Mat6x6 &coAdjoint); diff --git a/src/Cosserat/mapping/BaseCosseratMapping.inl b/src/Cosserat/mapping/BaseCosseratMapping.inl index e310406..cef60be 100644 --- a/src/Cosserat/mapping/BaseCosseratMapping.inl +++ b/src/Cosserat/mapping/BaseCosseratMapping.inl @@ -69,6 +69,10 @@ void BaseCosseratMapping::init() // to me it should be safe to remove the two line.. and thus the init :) const OutDataVecCoord *xfromData = toModels[0]->read(sofa::core::ConstVecCoordId::position()); const OutVecCoord xfrom = xfromData->getValue(); + + m_fromModel1 = fromModels1[0]; + m_fromModel2 = fromModels2[0]; + m_toModel = toModels[0]; } template diff --git a/src/Cosserat/mapping/DiscreteCosseratMapping.cpp b/src/Cosserat/mapping/DiscreteCosseratMapping.cpp index ef389af..5b2ef2b 100644 --- a/src/Cosserat/mapping/DiscreteCosseratMapping.cpp +++ b/src/Cosserat/mapping/DiscreteCosseratMapping.cpp @@ -52,6 +52,7 @@ void DiscreteCosseratMapping:: applyJ( sofa::helper::ReadAccessor>> curv_abs_frames = d_curv_abs_frames; const In1VecCoord& inDeform = m_fromModel1->read(sofa::core::ConstVecCoordId::position())->getValue(); //Strains + // Compute the tangent Exponential SE3 vectors this->updateTangExpSE3(inDeform); diff --git a/src/Cosserat/mapping/DiscreteCosseratMapping.h b/src/Cosserat/mapping/DiscreteCosseratMapping.h index 5cd0398..8083f6d 100644 --- a/src/Cosserat/mapping/DiscreteCosseratMapping.h +++ b/src/Cosserat/mapping/DiscreteCosseratMapping.h @@ -180,13 +180,14 @@ class DiscreteCosseratMapping : public BaseCosseratMapping { using BaseCosseratMapping::m_index_input; using BaseCosseratMapping::m_indicesVectorsDraw; using BaseCosseratMapping::computeTheta; + + using BaseCosseratMapping::m_toModel; + using BaseCosseratMapping::m_fromModel1; + using BaseCosseratMapping::m_fromModel2; + ////////////////////////////////////////////////////////////////////////////// sofa::helper::ColorMap m_colorMap; - sofa::core::State *m_fromModel1; - sofa::core::State *m_fromModel2; - sofa::core::State *m_toModel; - protected: DiscreteCosseratMapping(); ~DiscreteCosseratMapping() override {} diff --git a/src/Cosserat/mapping/DiscreteCosseratMapping.inl b/src/Cosserat/mapping/DiscreteCosseratMapping.inl index df957bd..25cd62e 100644 --- a/src/Cosserat/mapping/DiscreteCosseratMapping.inl +++ b/src/Cosserat/mapping/DiscreteCosseratMapping.inl @@ -43,8 +43,7 @@ using sofa::helper::WriteAccessor; using sofa::type::RGBAColor; template -DiscreteCosseratMapping::DiscreteCosseratMapping() - : m_fromModel1(NULL), m_fromModel2(NULL), m_toModel(NULL), +DiscreteCosseratMapping::DiscreteCosseratMapping() : d_deformationAxis( initData(&d_deformationAxis, (int)1, "deformationAxis", "the axis in which we want to show the deformation.\n")), @@ -74,7 +73,9 @@ DiscreteCosseratMapping::DiscreteCosseratMapping() "by another body.")) {} template -void DiscreteCosseratMapping::init() { +void DiscreteCosseratMapping::init() +{ + if (this->getFromModels1().empty() || this->getFromModels2().empty() || this->getToModels().empty()) { msg_error() << "Error while initializing ; input " @@ -91,12 +92,12 @@ void DiscreteCosseratMapping::init() { } template -void DiscreteCosseratMapping::reinit() { - m_fromModel1 = this->getFromModels1()[0]; // Cosserat deformations (torsion, - // bending_y/_z, elongation and - // shear_y/_z), in local frame - m_fromModel2 = this->getFromModels2()[0]; // Cosserat base, in global frame - m_toModel = this->getToModels()[0]; // Cosserat rigid frames, in global frame +void DiscreteCosseratMapping::reinit() +{ + + m_fromModel1 = this->getFromModels1()[0]; /// Cosserat deformations (torsion, bending_y/_z, elongation and shear_y/_z), in local frame + m_fromModel2 = this->getFromModels2()[0]; ///< Cosserat base, in global frame + m_toModel = this->getToModels()[0]; ///< Cosserat rigid frames, in global frame // Fill the initial vector void init() override; const OutDataVecCoord *xFromData = diff --git a/src/Cosserat/mapping/DiscreteDynamicCosseratMapping.h b/src/Cosserat/mapping/DiscreteDynamicCosseratMapping.h index ed8d6c1..64b685c 100644 --- a/src/Cosserat/mapping/DiscreteDynamicCosseratMapping.h +++ b/src/Cosserat/mapping/DiscreteDynamicCosseratMapping.h @@ -106,11 +106,6 @@ class DiscreteDynamicCosseratMapping : public BaseCosseratMapping::Transform Transform ; -protected: - sofa::core::State* m_fromModel1; - sofa::core::State* m_fromModel2; - sofa::core::State* m_toModel; - protected: /// Constructor DiscreteDynamicCosseratMapping() ; @@ -143,6 +138,9 @@ class DiscreteDynamicCosseratMapping : public BaseCosseratMapping::m_vecTransform ; using BaseCosseratMapping::m_nodeAdjointVectors; using BaseCosseratMapping::m_index_input; + using BaseCosseratMapping::m_toModel; + using BaseCosseratMapping::m_fromModel1; + using BaseCosseratMapping::m_fromModel2; public: diff --git a/src/Cosserat/mapping/DiscreteDynamicCosseratMapping.inl b/src/Cosserat/mapping/DiscreteDynamicCosseratMapping.inl index cfff43a..2478a2a 100644 --- a/src/Cosserat/mapping/DiscreteDynamicCosseratMapping.inl +++ b/src/Cosserat/mapping/DiscreteDynamicCosseratMapping.inl @@ -43,11 +43,7 @@ using sofa::helper::WriteAccessor; template DiscreteDynamicCosseratMapping::DiscreteDynamicCosseratMapping() - : m_fromModel1(NULL) - , m_fromModel2(NULL) - , m_toModel(NULL) -{ -} +{} // _________________________________________________________________________________________ @@ -77,10 +73,6 @@ void DiscreteDynamicCosseratMapping::init() printf("=================================> Init from the DiscretDynamicCosseratMapping component \n"); - m_fromModel1 = this->getFromModels1()[0]; - m_fromModel2 = this->getFromModels2()[0]; - m_toModel = this->getToModels()[0]; - // Fill the initial vector const OutDataVecCoord* xfromData = m_toModel->read(sofa::core::ConstVecCoordId::position()); const OutVecCoord xfrom = xfromData->getValue(); @@ -208,7 +200,7 @@ void DiscreteDynamicCosseratMapping:: applyJ( for (size_t i = 1 ; i < curv_abs_input.size(); i++) { Transform t= m_nodesExponentialSE3Vectors[i].inversed(); - Mat6x6 Adjoint; Adjoint.clear(); + Tangent Adjoint; Adjoint.clear(); this->computeAdjoint(t,Adjoint); //Add this line because need for the jacobian computation m_nodeAdjointVectors.push_back(Adjoint); diff --git a/src/Cosserat/types.h b/src/Cosserat/types.h index cf5c720..282c78c 100644 --- a/src/Cosserat/types.h +++ b/src/Cosserat/types.h @@ -44,10 +44,12 @@ namespace Cosserat typedef typename sofa::defaulttype::SolidTypes::Transform Transform; typedef typename sofa::type::vector List; - typedef typename sofa::type::Mat<6, 6, SReal> Tangent; typedef typename Eigen::Matrix3d RotMat; typedef typename Eigen::Matrix Vector6d; + using Tangent = sofa::type::Mat6x6; + //class Tangent : public sofa::type::Mat<6, 6, SReal>{}; + } } From 005994265c2bcb9e9877aab9a5279373fce61661 Mon Sep 17 00:00:00 2001 From: Damien Marchal Date: Thu, 13 Jun 2024 16:05:46 +0200 Subject: [PATCH 18/26] Refactor the init() between BaseCosseratMapping & its child classes (breaking) --- src/Cosserat/mapping/BaseCosseratMapping.h | 3 +- src/Cosserat/mapping/BaseCosseratMapping.inl | 18 +++++++ .../mapping/DiscreteCosseratMapping.h | 3 +- .../mapping/DiscreteCosseratMapping.inl | 38 +++------------ .../mapping/DiscreteDynamicCosseratMapping.h | 9 +--- .../DiscreteDynamicCosseratMapping.inl | 48 +------------------ 6 files changed, 29 insertions(+), 90 deletions(-) diff --git a/src/Cosserat/mapping/BaseCosseratMapping.h b/src/Cosserat/mapping/BaseCosseratMapping.h index 7224e92..426d286 100644 --- a/src/Cosserat/mapping/BaseCosseratMapping.h +++ b/src/Cosserat/mapping/BaseCosseratMapping.h @@ -140,7 +140,8 @@ class BaseCosseratMapping : public sofa::core::Multi2Mapping public: /********************** Inhertited from BaseObject **************/ - void init() override; + void init() final override; + virtual void doBaseCosseratInit() = 0; /************************* BaseCosserat **************************/ // TODO(dmarchal:2024/06/07), so we have "initialize" and "init" diff --git a/src/Cosserat/mapping/BaseCosseratMapping.inl b/src/Cosserat/mapping/BaseCosseratMapping.inl index cef60be..50f5aff 100644 --- a/src/Cosserat/mapping/BaseCosseratMapping.inl +++ b/src/Cosserat/mapping/BaseCosseratMapping.inl @@ -62,6 +62,24 @@ void BaseCosseratMapping::init() { Inherit1::init(); + if(fromModels1.empty()) + { + msg_error() << "Error while initializing ; input getFromModels1 not found" ; + return; + } + + if(fromModels2.empty()) + { + msg_error() << "Error while initializing ; output getFromModels2 not found" ; + return; + } + + if(toModels.empty()) + { + msg_error() << "Error while initializing ; output Model not found" ; + return; + } + //TODO(dmarchal, 2024/07/12): is the following line really needed ? // it initialize a local variable which initialize another data. // is it to force a xfromData updates through the use of getValue(). In addition diff --git a/src/Cosserat/mapping/DiscreteCosseratMapping.h b/src/Cosserat/mapping/DiscreteCosseratMapping.h index 8083f6d..2319255 100644 --- a/src/Cosserat/mapping/DiscreteCosseratMapping.h +++ b/src/Cosserat/mapping/DiscreteCosseratMapping.h @@ -114,8 +114,7 @@ class DiscreteCosseratMapping : public BaseCosseratMapping { ////////////////////////////////////////////////////////////////////// /// The following methods are inherited from BaseObject /// @{ - void init() override; - void reinit() override; + void doBaseCosseratInit() final override; void draw(const sofa::core::visual::VisualParams *vparams) override; /// @} ////////////////////////////////////////////////////////////////////// diff --git a/src/Cosserat/mapping/DiscreteCosseratMapping.inl b/src/Cosserat/mapping/DiscreteCosseratMapping.inl index 25cd62e..0e2ebee 100644 --- a/src/Cosserat/mapping/DiscreteCosseratMapping.inl +++ b/src/Cosserat/mapping/DiscreteCosseratMapping.inl @@ -73,32 +73,8 @@ DiscreteCosseratMapping::DiscreteCosseratMapping() : "by another body.")) {} template -void DiscreteCosseratMapping::init() +void DiscreteCosseratMapping::doBaseCosseratInit() { - - if (this->getFromModels1().empty() || this->getFromModels2().empty() || - this->getToModels().empty()) { - msg_error() << "Error while initializing ; input " - "getFromModels1/getFromModels2/output not found"; - return; - } - - reinit(); - // I call Init here since we build the mechanics only in the - Inherit1::init(); - - m_colorMap.setColorScheme("Blue to Red"); - m_colorMap.reinit(); -} - -template -void DiscreteCosseratMapping::reinit() -{ - - m_fromModel1 = this->getFromModels1()[0]; /// Cosserat deformations (torsion, bending_y/_z, elongation and shear_y/_z), in local frame - m_fromModel2 = this->getFromModels2()[0]; ///< Cosserat base, in global frame - m_toModel = this->getToModels()[0]; ///< Cosserat rigid frames, in global frame - // Fill the initial vector void init() override; const OutDataVecCoord *xFromData = m_toModel->read(sofa::core::ConstVecCoordId::position()); @@ -109,14 +85,12 @@ void DiscreteCosseratMapping::reinit() m_vecTransform.push_back(xFrom[i]); } - if (d_debug.getValue()) { - msg_info("DiscreteCosseratMapping") << " =================" - "============================ "; - msg_info("DiscreteCosseratMapping") - << " m_vecTransform : " << m_vecTransform; - } - this->initialize(); + + m_colorMap.setColorScheme("Blue to Red"); + m_colorMap.reinit(); + + msg_info() << " m_vecTransform : " << m_vecTransform; } template diff --git a/src/Cosserat/mapping/DiscreteDynamicCosseratMapping.h b/src/Cosserat/mapping/DiscreteDynamicCosseratMapping.h index 64b685c..428a8b3 100644 --- a/src/Cosserat/mapping/DiscreteDynamicCosseratMapping.h +++ b/src/Cosserat/mapping/DiscreteDynamicCosseratMapping.h @@ -142,17 +142,10 @@ class DiscreteDynamicCosseratMapping : public BaseCosseratMapping::m_fromModel1; using BaseCosseratMapping::m_fromModel2; - public: - - /**********************SOFA METHODS**************************/ - void init() override; - virtual void bwdInit() override; // get the points - virtual void reset() override; - virtual void reinit() override; + void doBaseCosseratInit() final override; void draw(const sofa::core::visual::VisualParams* vparams) override; - /**********************MAPPING METHODS**************************/ void apply( const sofa::core::MechanicalParams* /* mparams */, diff --git a/src/Cosserat/mapping/DiscreteDynamicCosseratMapping.inl b/src/Cosserat/mapping/DiscreteDynamicCosseratMapping.inl index 2478a2a..918c032 100644 --- a/src/Cosserat/mapping/DiscreteDynamicCosseratMapping.inl +++ b/src/Cosserat/mapping/DiscreteDynamicCosseratMapping.inl @@ -49,68 +49,22 @@ DiscreteDynamicCosseratMapping::DiscreteDynamicCosseratMapping // _________________________________________________________________________________________ template -void DiscreteDynamicCosseratMapping::init() +void DiscreteDynamicCosseratMapping::doBaseCosseratInit() { - Inherit1::init(); - - if(this->getFromModels1().empty()) - { - msg_error() << "Error while initializing ; input getFromModels1 not found" ; - return; - } - - if(this->getFromModels2().empty()) - { - msg_error() << "Error while initializing ; output getFromModels2 not found" ; - return; - } - - if(this->getToModels().empty()) - { - msg_error() << "Error while initializing ; output Model not found" ; - return; - } - - printf("=================================> Init from the DiscretDynamicCosseratMapping component \n"); - // Fill the initial vector const OutDataVecCoord* xfromData = m_toModel->read(sofa::core::ConstVecCoordId::position()); const OutVecCoord xfrom = xfromData->getValue(); - // WriteAccessor>> curv_abs_output = d_curv_abs_frames; - // curv_abs_output.clear(); m_vecTransform.clear(); for (unsigned int i = 0; i < xfrom.size(); i++) { m_vecTransform.push_back(xfrom[i]); } - //initialize the constant matrix m_matrixBi[0][0] = 1.0; for(size_t i = 0 ; i < 3; i++) m_matrixBi[i][i] = 1.0; this->initialize(); } - -template -void DiscreteDynamicCosseratMapping::bwdInit() -{ - -} - -template -void DiscreteDynamicCosseratMapping::reinit() -{ - -} - -template -void DiscreteDynamicCosseratMapping::reset() -{ - reinit(); -} - - - template void DiscreteDynamicCosseratMapping::apply( const sofa::core::MechanicalParams* /* mparams */, From bda1ff11c7c8b10ea823a3070c7b48d84759e533 Mon Sep 17 00:00:00 2001 From: Damien Marchal Date: Thu, 13 Jun 2024 16:37:20 +0200 Subject: [PATCH 19/26] Factorize initialization in BaseCosseratMapping. --- src/Cosserat/mapping/BaseCosseratMapping.inl | 27 ++++++++++--------- .../mapping/DiscreteCosseratMapping.inl | 14 ---------- .../DiscreteDynamicCosseratMapping.inl | 18 +++---------- 3 files changed, 18 insertions(+), 41 deletions(-) diff --git a/src/Cosserat/mapping/BaseCosseratMapping.inl b/src/Cosserat/mapping/BaseCosseratMapping.inl index 50f5aff..da5da5c 100644 --- a/src/Cosserat/mapping/BaseCosseratMapping.inl +++ b/src/Cosserat/mapping/BaseCosseratMapping.inl @@ -64,33 +64,36 @@ void BaseCosseratMapping::init() if(fromModels1.empty()) { - msg_error() << "Error while initializing ; input getFromModels1 not found" ; + msg_error() << "input1 not found" ; return; } if(fromModels2.empty()) { - msg_error() << "Error while initializing ; output getFromModels2 not found" ; + msg_error() << "input2 not found" ; return; } if(toModels.empty()) { - msg_error() << "Error while initializing ; output Model not found" ; + msg_error() << "output missing" ; return; } - //TODO(dmarchal, 2024/07/12): is the following line really needed ? - // it initialize a local variable which initialize another data. - // is it to force a xfromData updates through the use of getValue(). In addition - // this is puzzleing as toModel is the destination of the computation of the mapping. - // to me it should be safe to remove the two line.. and thus the init :) - const OutDataVecCoord *xfromData = toModels[0]->read(sofa::core::ConstVecCoordId::position()); - const OutVecCoord xfrom = xfromData->getValue(); - m_fromModel1 = fromModels1[0]; m_fromModel2 = fromModels2[0]; m_toModel = toModels[0]; + + // Fill the initial vector + const OutDataVecCoord* xfromData = m_toModel->read(sofa::core::ConstVecCoordId::position()); + const OutVecCoord xfrom = xfromData->getValue(); + + m_vecTransform.clear(); + for (unsigned int i = 0; i < xfrom.size(); i++) { + m_vecTransform.push_back(xfrom[i]); + } + + doBaseCosseratInit(); } template @@ -392,7 +395,7 @@ void BaseCosseratMapping::initialize() { auto curv_abs_frames = getReadAccessor(d_curv_abs_frames); msg_info() - << " curv_abs_section " << curv_abs_frames.size() << "; curv_abs_frames: " << curv_abs_frames.size(); + << " curv_abs_section " << curv_abs_section.size() << "; curv_abs_frames: " << curv_abs_frames.size(); m_indicesVectors.clear(); m_framesLengthVectors.clear(); diff --git a/src/Cosserat/mapping/DiscreteCosseratMapping.inl b/src/Cosserat/mapping/DiscreteCosseratMapping.inl index 0e2ebee..78b39d9 100644 --- a/src/Cosserat/mapping/DiscreteCosseratMapping.inl +++ b/src/Cosserat/mapping/DiscreteCosseratMapping.inl @@ -75,22 +75,8 @@ DiscreteCosseratMapping::DiscreteCosseratMapping() : template void DiscreteCosseratMapping::doBaseCosseratInit() { - // Fill the initial vector void init() override; - const OutDataVecCoord *xFromData = - m_toModel->read(sofa::core::ConstVecCoordId::position()); - const OutVecCoord xFrom = xFromData->getValue(); - - m_vecTransform.clear(); // initialise with the rigids frames - for (unsigned int i = 0; i < xFrom.size(); i++) { - m_vecTransform.push_back(xFrom[i]); - } - - this->initialize(); - m_colorMap.setColorScheme("Blue to Red"); m_colorMap.reinit(); - - msg_info() << " m_vecTransform : " << m_vecTransform; } template diff --git a/src/Cosserat/mapping/DiscreteDynamicCosseratMapping.inl b/src/Cosserat/mapping/DiscreteDynamicCosseratMapping.inl index 918c032..44b9204 100644 --- a/src/Cosserat/mapping/DiscreteDynamicCosseratMapping.inl +++ b/src/Cosserat/mapping/DiscreteDynamicCosseratMapping.inl @@ -45,26 +45,14 @@ template DiscreteDynamicCosseratMapping::DiscreteDynamicCosseratMapping() {} - -// _________________________________________________________________________________________ - template void DiscreteDynamicCosseratMapping::doBaseCosseratInit() { - // Fill the initial vector - const OutDataVecCoord* xfromData = m_toModel->read(sofa::core::ConstVecCoordId::position()); - const OutVecCoord xfrom = xfromData->getValue(); - - m_vecTransform.clear(); - for (unsigned int i = 0; i < xfrom.size(); i++) { - m_vecTransform.push_back(xfrom[i]); - } - - for(size_t i = 0 ; i < 3; i++) m_matrixBi[i][i] = 1.0; - - this->initialize(); + for(size_t i = 0 ; i < 3; i++) + m_matrixBi[i][i] = 1.0; } + template void DiscreteDynamicCosseratMapping::apply( const sofa::core::MechanicalParams* /* mparams */, From 334cf6b8dea45131d03171e430c1a028fe5765c9 Mon Sep 17 00:00:00 2001 From: Damien Marchal Date: Thu, 13 Jun 2024 17:09:05 +0200 Subject: [PATCH 20/26] FIXUP initialization. --- docs/testScene/tuto_compare_2.py | 8 +- src/Cosserat/mapping/BaseCosseratMapping.inl | 133 +++++++++++------- .../mapping/DiscreteCosseratMapping.inl | 20 ++- 3 files changed, 98 insertions(+), 63 deletions(-) diff --git a/docs/testScene/tuto_compare_2.py b/docs/testScene/tuto_compare_2.py index 6fdbee4..faf1261 100644 --- a/docs/testScene/tuto_compare_2.py +++ b/docs/testScene/tuto_compare_2.py @@ -49,11 +49,13 @@ def _add_cosserat_frame(p_node, _bending_node, framesF, _section_curv_abs, _fram cosserat_in_Sofa_frame_node.addObject('UniformMass', totalMass=_beam_mass) - cosserat_in_Sofa_frame_node.addObject('DiscreteCosseratMapping', curv_abs_input=_section_curv_abs, - curv_abs_output=_frame_curv_abs, name='cosseratMapping', + cosserat_in_Sofa_frame_node.addObject('DiscreteCosseratMapping', + curv_abs_input=_section_curv_abs, + curv_abs_output=_frame_curv_abs, + name='cosseratMapping', input1=_bending_node.cosserat_state.getLinkPath(), input2=p_node.cosserat_base_mo.getLinkPath(), - output=frames_mo.getLinkPath(), debug=0, radius=_radius) + output=frames_mo.getLinkPath(), printLog=True, radius=_radius) return cosserat_in_Sofa_frame_node diff --git a/src/Cosserat/mapping/BaseCosseratMapping.inl b/src/Cosserat/mapping/BaseCosseratMapping.inl index da5da5c..0629087 100644 --- a/src/Cosserat/mapping/BaseCosseratMapping.inl +++ b/src/Cosserat/mapping/BaseCosseratMapping.inl @@ -60,7 +60,9 @@ BaseCosseratMapping::BaseCosseratMapping() template void BaseCosseratMapping::init() { - Inherit1::init(); + m_fromModel1 = nullptr; + m_fromModel2 = nullptr; + m_toModel = nullptr; if(fromModels1.empty()) { @@ -84,6 +86,22 @@ void BaseCosseratMapping::init() m_fromModel2 = fromModels2[0]; m_toModel = toModels[0]; + if(m_fromModel1==nullptr) + { + msg_error() << "input1 not found" ; + return; + } + if(m_fromModel2==nullptr) + { + msg_error() << "input1 not found" ; + return; + } + if(m_toModel==nullptr) + { + msg_error() << "input1 not found" ; + return; + } + // Fill the initial vector const OutDataVecCoord* xfromData = m_toModel->read(sofa::core::ConstVecCoordId::position()); const OutVecCoord xfrom = xfromData->getValue(); @@ -93,9 +111,65 @@ void BaseCosseratMapping::init() m_vecTransform.push_back(xfrom[i]); } + initialize(); doBaseCosseratInit(); + Inherit1::init(); +} + +//___________________________________________________________________________ +template +void BaseCosseratMapping::initialize() { + // For each frame in the global frame, find the segment of the beam to which + // it is attached. Here we only use the information from the curvilinear + // abscissa of each frame. + auto curv_abs_section = getReadAccessor(d_curv_abs_section); + auto curv_abs_frames = getReadAccessor(d_curv_abs_frames); + + msg_info() + << " curv_abs_section " << curv_abs_section.size() << "; curv_abs_frames: " << curv_abs_frames.size(); + + m_indicesVectors.clear(); + m_framesLengthVectors.clear(); + m_BeamLengthVectors.clear(); + m_indicesVectorsDraw.clear(); + + size_t sz = curv_abs_frames.size(); + size_t input_index = 1; + for (size_t i = 0; i < sz; ++i) + { + if (curv_abs_section[input_index] > curv_abs_frames[i]) + { + m_indicesVectors.emplace_back(input_index); + m_indicesVectorsDraw.emplace_back(input_index); // maybe I shouldn't do this here !!! + } else if (curv_abs_section[input_index] == curv_abs_frames[i]) + { + m_indicesVectors.emplace_back(input_index); + input_index++; + m_indicesVectorsDraw.emplace_back(input_index); + } else { + input_index++; + m_indicesVectors.emplace_back(input_index); + m_indicesVectorsDraw.emplace_back(input_index); + } + + // Fill the vector m_framesLengthVectors with the distance + // between frame(output) and the closest beam node toward the base + m_framesLengthVectors.emplace_back( + curv_abs_frames[i] - curv_abs_section[m_indicesVectors.back() - 1]); + } + + for (size_t j = 0; j < sz - 1; ++j) + { + m_BeamLengthVectors.emplace_back(curv_abs_section[j + 1] - curv_abs_section[j]); + } + + msg_info() + << "m_indicesVectors : " << m_indicesVectors << msgendl + << "m_framesLengthVectors : " << msgendl + << "m_BeamLengthVectors : " << msgendl; } + template void BaseCosseratMapping::computeExponentialSE3( const double &curv_abs_x_n, const Coord1 &strain_n, Transform &g_X_n) { @@ -137,6 +211,7 @@ template void BaseCosseratMapping::updateExponentialSE3( const In1VecCoord &inDeform) { + msg_info() << " ########## updateMap 2########"; auto curv_abs_frames = sofa::helper::getReadAccessor(d_curv_abs_frames); m_framesExponentialSE3Vectors.clear(); @@ -145,10 +220,14 @@ void BaseCosseratMapping::updateExponentialSE3( const unsigned int sz = curv_abs_frames.size(); + msg_info() << " ########## updateMap 3 , "<< sz << " et " << inDeform.size() << "########"; + // Compute exponential at each frame point for (size_t i = 0; i < sz; ++i) { Transform g_X_frame_i; + msg_info() << " ########## updateMap 4, "<< i << "-> " << "########"; + msg_info() << " ########## updateMap 5, "<< i << "-> " << m_indicesVectors[i] << "########"; const Coord1 strain_n = inDeform[m_indicesVectors[i] - 1]; // Cosserat reduce coordinates (strain) @@ -385,58 +464,6 @@ BaseCosseratMapping::computeETA(const Vec6 &baseEta, return adjointMatrix * (baseEta + tangentMatrix * Xi_dot); } -//___________________________________________________________________________ -template -void BaseCosseratMapping::initialize() { - // For each frame in the global frame, find the segment of the beam to which - // it is attached. Here we only use the information from the curvilinear - // abscissa of each frame. - auto curv_abs_section = getReadAccessor(d_curv_abs_section); - auto curv_abs_frames = getReadAccessor(d_curv_abs_frames); - - msg_info() - << " curv_abs_section " << curv_abs_section.size() << "; curv_abs_frames: " << curv_abs_frames.size(); - - m_indicesVectors.clear(); - m_framesLengthVectors.clear(); - m_BeamLengthVectors.clear(); - m_indicesVectorsDraw.clear(); - - size_t sz = curv_abs_frames.size(); - size_t input_index = 1; - for (size_t i = 0; i < sz; ++i) - { - if (curv_abs_section[input_index] > curv_abs_frames[i]) - { - m_indicesVectors.emplace_back(input_index); - m_indicesVectorsDraw.emplace_back(input_index); // maybe I shouldn't do this here !!! - } else if (curv_abs_section[input_index] == curv_abs_frames[i]) - { - m_indicesVectors.emplace_back(input_index); - input_index++; - m_indicesVectorsDraw.emplace_back(input_index); - } else { - input_index++; - m_indicesVectors.emplace_back(input_index); - m_indicesVectorsDraw.emplace_back(input_index); - } - - // Fill the vector m_framesLengthVectors with the distance - // between frame(output) and the closest beam node toward the base - m_framesLengthVectors.emplace_back( - curv_abs_frames[i] - curv_abs_section[m_indicesVectors.back() - 1]); - } - - for (size_t j = 0; j < sz - 1; ++j) - { - m_BeamLengthVectors.emplace_back(curv_abs_section[j + 1] - curv_abs_section[j]); - } - - msg_info() - << "m_indicesVectors : " << m_indicesVectors << msgendl - << "m_framesLengthVectors : " << msgendl - << "m_BeamLengthVectors : " << msgendl; -} template double BaseCosseratMapping::computeTheta(const double &x, diff --git a/src/Cosserat/mapping/DiscreteCosseratMapping.inl b/src/Cosserat/mapping/DiscreteCosseratMapping.inl index 78b39d9..cb535ab 100644 --- a/src/Cosserat/mapping/DiscreteCosseratMapping.inl +++ b/src/Cosserat/mapping/DiscreteCosseratMapping.inl @@ -85,16 +85,19 @@ void DiscreteCosseratMapping::apply( const vector &dataVecOutPos, const vector &dataVecIn1Pos, const vector &dataVecIn2Pos) { + if (dataVecOutPos.empty() || dataVecIn1Pos.empty() || dataVecIn2Pos.empty()) return; - if (d_debug.getValue()) - std::cout << " ########## Apply Function ########" << std::endl; + msg_info() << " ########## Apply Function ########"; + /// Do Apply // We need only one input In model and input Root model (if present) const In1VecCoord &in1 = dataVecIn1Pos[0]->getValue(); const In2VecCoord &in2 = dataVecIn2Pos[0]->getValue(); + msg_info() << " ########## Apply Function 2########"; + const auto sz = d_curv_abs_frames.getValue().size(); OutVecCoord &out = *dataVecOutPos[0]->beginEdit(); // frames states out.resize(sz); @@ -105,6 +108,7 @@ void DiscreteCosseratMapping::apply( // Which are the homogeneous matrices of the frames and the nodes in local // coordinate. this->updateExponentialSE3(in1); + /* Apply the transformation to go from cossserat to SOFA frame*/ Transform frame0 = Transform(In2::getCPos(in2[baseIndex]), In2::getCRot(in2[baseIndex])); @@ -116,19 +120,21 @@ void DiscreteCosseratMapping::apply( } frame *= m_framesExponentialSE3Vectors[i]; // frame*gX(x) - if (d_debug.getValue()) - std::cout << "Frame : " << i << " = " << frame << std::endl; + msg_info() << "Frame : " << i << " = " << frame; Vec3 v = frame.getOrigin(); Quat q = frame.getOrientation(); out[i] = OutCoord(v, q); } - // - if (d_debug.getValue()) { + + if (this->f_printLog.getValue()) + { + std::stringstream tmp; for (unsigned int i = 0; i < out.size() - 1; i++) { Vec3 diff = out[i + 1].getCenter() - out[i].getCenter(); - std::cout << "dist " << i << " : " << diff.norm() << std::endl; + tmp << "dist " << i << " : " << diff.norm() << msgendl; } + msg_info() << tmp.str(); } m_index_input = 0; dataVecOutPos[0]->endEdit(); From aa20251363a2f26a7670b5f9808e9086dc29b1ef Mon Sep 17 00:00:00 2001 From: Damien Marchal Date: Thu, 13 Jun 2024 21:08:20 +0200 Subject: [PATCH 21/26] Factorizing in init funciton. --- src/Cosserat/mapping/BaseCosseratMapping.inl | 16 ------------ .../mapping/DiscreteCosseratMapping.inl | 25 ------------------- 2 files changed, 41 deletions(-) diff --git a/src/Cosserat/mapping/BaseCosseratMapping.inl b/src/Cosserat/mapping/BaseCosseratMapping.inl index 0629087..48ebac5 100644 --- a/src/Cosserat/mapping/BaseCosseratMapping.inl +++ b/src/Cosserat/mapping/BaseCosseratMapping.inl @@ -86,22 +86,6 @@ void BaseCosseratMapping::init() m_fromModel2 = fromModels2[0]; m_toModel = toModels[0]; - if(m_fromModel1==nullptr) - { - msg_error() << "input1 not found" ; - return; - } - if(m_fromModel2==nullptr) - { - msg_error() << "input1 not found" ; - return; - } - if(m_toModel==nullptr) - { - msg_error() << "input1 not found" ; - return; - } - // Fill the initial vector const OutDataVecCoord* xfromData = m_toModel->read(sofa::core::ConstVecCoordId::position()); const OutVecCoord xfrom = xfromData->getValue(); diff --git a/src/Cosserat/mapping/DiscreteCosseratMapping.inl b/src/Cosserat/mapping/DiscreteCosseratMapping.inl index cb535ab..ad6d7e7 100644 --- a/src/Cosserat/mapping/DiscreteCosseratMapping.inl +++ b/src/Cosserat/mapping/DiscreteCosseratMapping.inl @@ -89,15 +89,11 @@ void DiscreteCosseratMapping::apply( if (dataVecOutPos.empty() || dataVecIn1Pos.empty() || dataVecIn2Pos.empty()) return; - msg_info() << " ########## Apply Function ########"; - /// Do Apply // We need only one input In model and input Root model (if present) const In1VecCoord &in1 = dataVecIn1Pos[0]->getValue(); const In2VecCoord &in2 = dataVecIn2Pos[0]->getValue(); - msg_info() << " ########## Apply Function 2########"; - const auto sz = d_curv_abs_frames.getValue().size(); OutVecCoord &out = *dataVecOutPos[0]->beginEdit(); // frames states out.resize(sz); @@ -174,27 +170,6 @@ void DiscreteCosseratMapping::computeLogarithm( } } -// template -// void computeViolation(In1VecCoord& inDeform, const helper::vector -// m_framesLengthVectors, const -// size_t sz, std::function f) -//{ -// for (std::size_t i = 0; i < sz; i++) { -// Mat6x6 temp ; - -// type::Vec3 k = inDeform[m_indicesVectors[i]-1]; -// double x = m_framesLengthVectors[i]; -// compute_Tang_Exp(x,k,temp) ; -// m_framesTangExpVectors.push_back(temp); - -//// if (d_debug.getValue()){ -//// printf("__________________________________________\n"); -//// std::cout << "x :"<< x << "; k :"<< k << std::endl; -//// std::cout<< "m_framesTangExpVectors :"<< -///m_framesTangExpVectors[i] << std::endl; / } -// } -//} - template void DiscreteCosseratMapping::applyJ( const sofa::core::MechanicalParams * /* mparams */, From 60446a581f6caa522fac41e4cdb32ccd2e35176e Mon Sep 17 00:00:00 2001 From: Damien Marchal Date: Thu, 13 Jun 2024 21:26:17 +0200 Subject: [PATCH 22/26] Types declaration in BaseCosseratMapping removed. --- src/Cosserat/mapping/BaseCosseratMapping.cpp | 2 +- src/Cosserat/mapping/BaseCosseratMapping.h | 35 ++++++---------- src/Cosserat/mapping/BaseCosseratMapping.inl | 44 ++++++++------------ 3 files changed, 31 insertions(+), 50 deletions(-) diff --git a/src/Cosserat/mapping/BaseCosseratMapping.cpp b/src/Cosserat/mapping/BaseCosseratMapping.cpp index dc81312..ed591d1 100644 --- a/src/Cosserat/mapping/BaseCosseratMapping.cpp +++ b/src/Cosserat/mapping/BaseCosseratMapping.cpp @@ -83,7 +83,7 @@ void BaseCosseratMapping::computeExponentia g_X_n.getsub(0, 0, M); // get the rotation matrix // convert the rotation 3x3 matrix to a quaternion - sofa::type::Quat R; + sofa::type::Quat R; R.fromMatrix(M); Trans = Transform(Vec3(g_X_n(0, 3), g_X_n(1, 3), g_X_n(2, 3)), R); } diff --git a/src/Cosserat/mapping/BaseCosseratMapping.h b/src/Cosserat/mapping/BaseCosseratMapping.h index 426d286..ecd478e 100644 --- a/src/Cosserat/mapping/BaseCosseratMapping.h +++ b/src/Cosserat/mapping/BaseCosseratMapping.h @@ -79,29 +79,18 @@ class BaseCosseratMapping : public sofa::core::Multi2Mapping // typedefs are public typedef typename In1::Coord Coord1; typedef typename In1::Deriv Deriv1; - typedef typename In1::VecCoord In1VecCoord; - typedef typename In1::VecDeriv In1VecDeriv; - [[maybe_unused]] typedef typename In1::MatrixDeriv In1MatrixDeriv; - typedef sofa::Data In1DataVecCoord; - typedef sofa::Data In1DataVecDeriv; + //typedef typename In1::VecCoord In1VecCoord; + //typedef typename In1::VecDeriv In1VecDeriv; - typedef typename In2::Coord::value_type Real; typedef typename In2::Coord Coord2; typedef typename In2::Deriv Deriv2; - typedef typename In2::VecCoord In2VecCoord; - typedef typename In2::VecDeriv In2VecDeriv; - [[maybe_unused]] typedef typename In2::MatrixDeriv In2MatrixDeriv; - typedef sofa::Data In2DataVecCoord; - typedef sofa::Data In2DataVecDeriv; + //typedef typename In2::VecCoord In2VecCoord; + //typedef typename In2::VecDeriv In2VecDeriv; - typedef typename Out::VecCoord OutVecCoord; typedef typename Out::Coord OutCoord; typedef typename Out::Deriv OutDeriv; - typedef typename Out::VecDeriv OutVecDeriv; - typedef typename Out::MatrixDeriv OutMatrixDeriv; - typedef sofa::Data OutDataVecCoord; - typedef sofa::Data OutDataVecDeriv; - typedef sofa::Data OutDataMatrixDeriv; + //typedef typename Out::VecCoord OutVecCoord; + //typedef typename Out::VecDeriv OutVecDeriv; public: // TODO(dmarchal: 2024/06/07): There is a lot of public attributes is this @@ -109,7 +98,7 @@ class BaseCosseratMapping : public sofa::core::Multi2Mapping /*===========COSSERAT VECTORS ======================*/ unsigned int m_index_input; - OutVecCoord m_vecTransform; + vector m_vecTransform; vector m_framesExponentialSE3Vectors; vector m_nodesExponentialSE3Vectors; @@ -119,7 +108,7 @@ class BaseCosseratMapping : public sofa::core::Multi2Mapping vector m_indicesVectors; vector m_indicesVectorsDraw; - vector m_BeamLengthVectors; + vector m_beamLengthVectors; vector m_framesLengthVectors; vector m_nodesVelocityVectors; @@ -149,7 +138,7 @@ class BaseCosseratMapping : public sofa::core::Multi2Mapping // roles is unclear and generates ambiguities // TODO @yadagolo: Yes, because the function is used by callback, when we // do dynamic meshing. - void initialize(); + void initializeFrames(); double computeTheta(const double &x, const Mat4x4 &gX); void printMatrix(const Mat6x6 R); @@ -221,12 +210,12 @@ class BaseCosseratMapping : public sofa::core::Multi2Mapping void computeCoAdjoint(const Transform &frame, Mat6x6 &coAdjoint); - void updateExponentialSE3(const In1VecCoord &inDeform); - void updateTangExpSE3(const In1VecCoord &inDeform); + void updateExponentialSE3(const vector &inDeform); + void updateTangExpSE3(const vector &inDeform); void computeTangExp(double &x, const Coord1 &k, Mat6x6 &TgX); [[maybe_unused]] Vec6 - computeETA(const Vec6 &baseEta, const In1VecDeriv &k_dot, double abs_input); + computeETA(const Vec6 &baseEta, const vector &k_dot, double abs_input); Mat4x4 computeLogarithm(const double &x, const Mat4x4 &gX); }; diff --git a/src/Cosserat/mapping/BaseCosseratMapping.inl b/src/Cosserat/mapping/BaseCosseratMapping.inl index 48ebac5..48fd82c 100644 --- a/src/Cosserat/mapping/BaseCosseratMapping.inl +++ b/src/Cosserat/mapping/BaseCosseratMapping.inl @@ -55,7 +55,6 @@ BaseCosseratMapping::BaseCosseratMapping() d_debug(initData(&d_debug, false, "debug", "printf for the debug")), m_index_input(0) {} -// _________________________________________________________________________________________ template void BaseCosseratMapping::init() @@ -87,22 +86,23 @@ void BaseCosseratMapping::init() m_toModel = toModels[0]; // Fill the initial vector - const OutDataVecCoord* xfromData = m_toModel->read(sofa::core::ConstVecCoordId::position()); - const OutVecCoord xfrom = xfromData->getValue(); + auto xfromData = m_toModel->read(sofa::core::ConstVecCoordId::position()); + const vector xfrom = xfromData->getValue(); m_vecTransform.clear(); for (unsigned int i = 0; i < xfrom.size(); i++) { m_vecTransform.push_back(xfrom[i]); } - initialize(); + initializeFrames(); doBaseCosseratInit(); Inherit1::init(); } //___________________________________________________________________________ template -void BaseCosseratMapping::initialize() { +void BaseCosseratMapping::initializeFrames() +{ // For each frame in the global frame, find the segment of the beam to which // it is attached. Here we only use the information from the curvilinear // abscissa of each frame. @@ -114,7 +114,7 @@ void BaseCosseratMapping::initialize() { m_indicesVectors.clear(); m_framesLengthVectors.clear(); - m_BeamLengthVectors.clear(); + m_beamLengthVectors.clear(); m_indicesVectorsDraw.clear(); size_t sz = curv_abs_frames.size(); @@ -144,7 +144,7 @@ void BaseCosseratMapping::initialize() { for (size_t j = 0; j < sz - 1; ++j) { - m_BeamLengthVectors.emplace_back(curv_abs_section[j + 1] - curv_abs_section[j]); + m_beamLengthVectors.emplace_back(curv_abs_section[j + 1] - curv_abs_section[j]); } msg_info() @@ -185,7 +185,7 @@ void BaseCosseratMapping::computeExponentialSE3( _g_X.getsub(0, 0, M); // get the rotation matrix // convert the rotation 3x3 matrix to a quaternion - Quat R; + Quat R; R.fromMatrix(M); g_X_n = Transform(Vec3(_g_X(0, 3), _g_X(1, 3), _g_X(2, 3)), R); } @@ -193,10 +193,9 @@ void BaseCosseratMapping::computeExponentialSE3( // Fill exponential vectors template void BaseCosseratMapping::updateExponentialSE3( - const In1VecCoord &inDeform) + const vector &inDeform) { - msg_info() << " ########## updateMap 2########"; - auto curv_abs_frames = sofa::helper::getReadAccessor(d_curv_abs_frames); + auto curv_abs_frames = getReadAccessor(d_curv_abs_frames); m_framesExponentialSE3Vectors.clear(); m_nodesExponentialSE3Vectors.clear(); @@ -204,14 +203,10 @@ void BaseCosseratMapping::updateExponentialSE3( const unsigned int sz = curv_abs_frames.size(); - msg_info() << " ########## updateMap 3 , "<< sz << " et " << inDeform.size() << "########"; - // Compute exponential at each frame point for (size_t i = 0; i < sz; ++i) { Transform g_X_frame_i; - msg_info() << " ########## updateMap 4, "<< i << "-> " << "########"; - msg_info() << " ########## updateMap 5, "<< i << "-> " << m_indicesVectors[i] << "########"; const Coord1 strain_n = inDeform[m_indicesVectors[i] - 1]; // Cosserat reduce coordinates (strain) @@ -235,7 +230,7 @@ void BaseCosseratMapping::updateExponentialSE3( for (unsigned int j = 0; j < inDeform.size(); ++j) { Coord1 strain_n = inDeform[j]; - const SReal curv_abs_x = m_BeamLengthVectors[j]; + const SReal curv_abs_x = m_beamLengthVectors[j]; Transform g_X_node_j; computeExponentialSE3(curv_abs_x, strain_n, g_X_node_j); @@ -315,7 +310,7 @@ auto BaseCosseratMapping::computeLogarithm(const double &x, template void BaseCosseratMapping::updateTangExpSE3( - const In1VecCoord &inDeform) { + const vector &inDeform) { // Curv abscissa of nodes and frames auto curv_abs_section = getReadAccessor(d_curv_abs_section); @@ -348,7 +343,7 @@ void BaseCosseratMapping::updateTangExpSE3( for (size_t j = 1; j < curv_abs_section.size(); j++) { Coord1 strain_node_i = inDeform[j - 1]; - double x = m_BeamLengthVectors[j - 1]; + double x = m_beamLengthVectors[j - 1]; Tangent temp; temp.clear(); computeTangExp(x, strain_node_i, temp); @@ -409,12 +404,12 @@ void BaseCosseratMapping::computeTangExp(double &curv_abs_n, template [[maybe_unused]] Vec6 BaseCosseratMapping::computeETA(const Vec6 &baseEta, - const In1VecDeriv &k_dot, + const vector &k_dot, const double abs_input) { // Get the positions from model 0. This function returns the position wrapped in a Data<> - auto d_x1 = fromModels1[0]->read(sofa::core::ConstVecCoordId::position()); + auto d_x1 = m_fromModel1->read(sofa::core::ConstVecCoordId::position()); // To access the actual content (in this case position) from a data, we have to use // a read accessor that insures the data is updated according to DDGNode state @@ -480,7 +475,7 @@ Mat3x3 BaseCosseratMapping::extractRotMatrix(const Transform & // TODO(dmarchal: 2024/06/07) The following code should probably become // utility function building a 3x3 matix from a quaternion should probably // does not need this amount of code. - Real R[4][4]; + SReal R[4][4]; q.buildRotationMatrix(R); Mat3x3 mat; for (unsigned int k = 0; k < 3; k++) @@ -497,7 +492,7 @@ auto BaseCosseratMapping::buildProjector(const Transform &T) // TODO(dmarchal: 2024/06/07) The following code should probably become // utility function building a 3x3 matix from a quaternion should probably // does not need this amount of code. - Real R[4][4]; + SReal R[4][4]; (T.getOrientation()).buildRotationMatrix(R); for (unsigned int i = 0; i < 3; i++) { for (unsigned int j = 0; j < 3; j++) { @@ -526,7 +521,7 @@ auto BaseCosseratMapping::buildXiHat(const Coord1 &strain_i) - } template -auto BaseCosseratMapping::getTildeMatrix(const sofa::type::Vec3 &u) +auto BaseCosseratMapping::getTildeMatrix(const Vec3 &u) -> Mat3x3 { sofa::type::Matrix3 tild; tild[0][1] = -u[2]; @@ -628,7 +623,4 @@ auto BaseCosseratMapping::piecewiseLogmap(const _SE3 &g_x) -> return xci; } - - - } // namespace cosserat::mapping From 40279f68e77eca9d8fa1134e423b3feaf949cc77 Mon Sep 17 00:00:00 2001 From: Damien Marchal Date: Thu, 13 Jun 2024 21:46:43 +0200 Subject: [PATCH 23/26] Modernize types declaration in DiscreetCosseratMapping Among other things: - instead of re-defining a new type in each class, re-using the one defined in its parent. - remove useless headers - remove type declaration that does not seems to be needed. --- src/Cosserat/mapping/BaseCosseratMapping.h | 6 - src/Cosserat/mapping/DifferenceMultiMapping.h | 5 - .../mapping/DiscreteCosseratMapping.h | 131 ++++++++---------- .../mapping/DiscreteCosseratMapping.inl | 34 ++--- 4 files changed, 74 insertions(+), 102 deletions(-) diff --git a/src/Cosserat/mapping/BaseCosseratMapping.h b/src/Cosserat/mapping/BaseCosseratMapping.h index ecd478e..baa2e7f 100644 --- a/src/Cosserat/mapping/BaseCosseratMapping.h +++ b/src/Cosserat/mapping/BaseCosseratMapping.h @@ -79,18 +79,12 @@ class BaseCosseratMapping : public sofa::core::Multi2Mapping // typedefs are public typedef typename In1::Coord Coord1; typedef typename In1::Deriv Deriv1; - //typedef typename In1::VecCoord In1VecCoord; - //typedef typename In1::VecDeriv In1VecDeriv; typedef typename In2::Coord Coord2; typedef typename In2::Deriv Deriv2; - //typedef typename In2::VecCoord In2VecCoord; - //typedef typename In2::VecDeriv In2VecDeriv; typedef typename Out::Coord OutCoord; typedef typename Out::Deriv OutDeriv; - //typedef typename Out::VecCoord OutVecCoord; - //typedef typename Out::VecDeriv OutVecDeriv; public: // TODO(dmarchal: 2024/06/07): There is a lot of public attributes is this diff --git a/src/Cosserat/mapping/DifferenceMultiMapping.h b/src/Cosserat/mapping/DifferenceMultiMapping.h index f4567d0..c9ff7e3 100644 --- a/src/Cosserat/mapping/DifferenceMultiMapping.h +++ b/src/Cosserat/mapping/DifferenceMultiMapping.h @@ -45,12 +45,7 @@ using Cosserat::mapping::BaseCosseratMapping; /*! * \class DifferenceMultiMapping - * @brief Computes and map the length of the beams - * - * This is a component: - * https://www.sofa-framework.org/community/doc/programming-with-sofa/create-your-component/ */ - template class DifferenceMultiMapping : public sofa::core::Multi2Mapping { diff --git a/src/Cosserat/mapping/DiscreteCosseratMapping.h b/src/Cosserat/mapping/DiscreteCosseratMapping.h index 2319255..4b82ffe 100644 --- a/src/Cosserat/mapping/DiscreteCosseratMapping.h +++ b/src/Cosserat/mapping/DiscreteCosseratMapping.h @@ -22,29 +22,24 @@ #pragma once #include -#include #include - -#include -#include -#include -#include -#include #include -namespace Cosserat::mapping { -using sofa::component::forcefield::BeamHookeLawForceField; -using sofa::core::objectmodel::BaseContext; -using sofa::defaulttype::SolidTypes; -using sofa::type::Matrix3; -using sofa::type::Matrix4; +namespace Cosserat::mapping +{ +namespace +{ +using Mat3x6 = sofa::type::Mat<3, 6, SReal>; +using Mat6x3 = sofa::type::Mat<6, 3, SReal>; +using sofa::type::Mat4x4; +using sofa::type::Mat6x6; using sofa::type::Vec3; using sofa::type::Vec6; using sofa::type::Quat; -using std::get; using sofa::Data; +} - +// TODO(dmarchal: 2024/10/07) Is the description valid ? I don't think so. /*! * \class DiscreteCosseratMapping * @brief Computes and map the length of the beams @@ -59,53 +54,41 @@ class DiscreteCosseratMapping : public BaseCosseratMapping { /// Input Model Type typedef TIn1 In1; typedef TIn2 In2; - - /// Output Model Type typedef TOut Out; - typedef typename In2::Coord::value_type Real1; - typedef typename In1::Coord Coord1; - typedef typename In1::Deriv Deriv1; - typedef typename In1::VecCoord In1VecCoord; - typedef typename In1::VecDeriv In1VecDeriv; - typedef typename In1::MatrixDeriv In1MatrixDeriv; - typedef Data In1DataVecCoord; - typedef Data In1DataVecDeriv; - typedef Data In1DataMatrixDeriv; - - typedef typename In2::Coord::value_type Real2; - typedef typename In2::Coord Coord2; - typedef typename In2::Deriv Deriv2; - typedef typename In2::VecCoord In2VecCoord; - typedef typename In2::VecDeriv In2VecDeriv; - typedef typename In2::MatrixDeriv In2MatrixDeriv; - typedef Data In2DataVecCoord; - typedef Data In2DataVecDeriv; - typedef Data In2DataMatrixDeriv; - typedef sofa::type::Mat<6, 6, Real2> Mat6x6; - typedef sofa::type::Mat<3, 6, Real2> Mat3x6; - typedef sofa::type::Mat<6, 3, Real2> Mat6x3; - typedef sofa::type::Mat<4, 4, Real2> Mat4x4; - - typedef typename Out::VecCoord OutVecCoord; - typedef typename Out::Coord OutCoord; - typedef typename Out::Deriv OutDeriv; - typedef typename Out::VecDeriv OutVecDeriv; - typedef typename Out::MatrixDeriv OutMatrixDeriv; - typedef Data OutDataVecCoord; - typedef Data OutDataVecDeriv; - typedef Data OutDataMatrixDeriv; - typedef typename SolidTypes::Transform Transform; + + using typename Inherit1::In1VecCoord; + using typename Inherit1::In1VecDeriv; + using typename Inherit1::In1MatrixDeriv; + using typename Inherit1::In1DataVecCoord; + using typename Inherit1::In1DataVecDeriv; + using typename Inherit1::In1DataMatrixDeriv; + + using typename Inherit1::In2VecCoord; + using typename Inherit1::In2VecDeriv; + using typename Inherit1::In2MatrixDeriv; + using typename Inherit1::In2DataVecCoord; + using typename Inherit1::In2DataVecDeriv; + using typename Inherit1::In2DataMatrixDeriv; + + using typename Inherit1::OutCoord; + using typename Inherit1::OutDeriv; + using typename Inherit1::OutVecCoord; + using typename Inherit1::OutVecDeriv; + using typename Inherit1::OutMatrixDeriv; + using typename Inherit1::OutDataVecCoord; + using typename Inherit1::OutDataVecDeriv; + using typename Inherit1::OutDataMatrixDeriv; ////////////////////////////////////////////////////////////////////// /// @name Data Fields /// @{ - Data d_deformationAxis; - Data d_max; - Data d_min; - Data d_radius; - Data d_drawMapBeam; + Data d_deformationAxis; + Data d_max; + Data d_min; + Data d_radius; + Data d_drawMapBeam; Data d_color; - Data> d_index; + Data> d_index; Data d_baseIndex; /// @} ////////////////////////////////////////////////////////////////////// @@ -124,19 +107,19 @@ class DiscreteCosseratMapping : public BaseCosseratMapping { /// The following method are inherited from MultiMapping /// @{ void apply(const sofa::core::MechanicalParams * /* mparams */, - const vector &dataVecOutPos, - const vector &dataVecIn1Pos, - const vector &dataVecIn2Pos) override; + const vector &dataVecOutPos, + const vector &dataVecIn1Pos, + const vector &dataVecIn2Pos) override; void applyJ(const sofa::core::MechanicalParams * /* mparams */, - const vector &dataVecOutVel, - const vector &dataVecIn1Vel, - const vector &dataVecIn2Vel) override; + const vector &dataVecOutVel, + const vector &dataVecIn1Vel, + const vector &dataVecIn2Vel) override; void applyJT(const sofa::core::MechanicalParams * /* mparams */, - const vector &dataVecOut1Force, - const vector &dataVecOut2RootForce, - const vector &dataVecInForce) override; + const vector &dataVecOut1Force, + const vector &dataVecOut2RootForce, + const vector &dataVecInForce) override; // TODO(dmarchal:2024/06/13): Override with an empty function is a rare code pattern // to make it clear this is the intented and not just an "I'm too lazy to implement it" @@ -147,16 +130,16 @@ class DiscreteCosseratMapping : public BaseCosseratMapping { /// Support for constraints. void applyJT( - const sofa::core::ConstraintParams *cparams, - const vector &dataMatOut1Const, - const vector &dataMatOut2Const, - const vector &dataMatInConst) override; + const sofa::core::ConstraintParams *cparams, + const vector &dataMatOut1Const, + const vector &dataMatOut2Const, + const vector &dataMatInConst) override; /// @} ///////////////////////////////////////////////////////////////////////////// void computeBBox(const sofa::core::ExecParams *params, bool onlyVisible) override; - void computeLogarithm(const double &x, const Matrix4 &gX, Matrix4 &log_gX); + void computeLogarithm(const double &x, const Mat4x4 &gX, Mat4x4 &log_gX); protected: ////////////////////////// Inherited attributes //////////////////////////// @@ -194,11 +177,11 @@ class DiscreteCosseratMapping : public BaseCosseratMapping { #if !defined(SOFA_COSSERAT_CPP_DiscreteCosseratMapping) extern template class SOFA_COSSERAT_API DiscreteCosseratMapping< - sofa::defaulttype::Vec3Types, sofa::defaulttype::Rigid3Types, - sofa::defaulttype::Rigid3Types>; + sofa::defaulttype::Vec3Types, sofa::defaulttype::Rigid3Types, + sofa::defaulttype::Rigid3Types>; extern template class SOFA_COSSERAT_API DiscreteCosseratMapping< - sofa::defaulttype::Vec6Types, sofa::defaulttype::Rigid3Types, - sofa::defaulttype::Rigid3Types>; + sofa::defaulttype::Vec6Types, sofa::defaulttype::Rigid3Types, + sofa::defaulttype::Rigid3Types>; #endif } // namespace Cosserat::mapping diff --git a/src/Cosserat/mapping/DiscreteCosseratMapping.inl b/src/Cosserat/mapping/DiscreteCosseratMapping.inl index ad6d7e7..0beadc5 100644 --- a/src/Cosserat/mapping/DiscreteCosseratMapping.inl +++ b/src/Cosserat/mapping/DiscreteCosseratMapping.inl @@ -47,12 +47,12 @@ DiscreteCosseratMapping::DiscreteCosseratMapping() : d_deformationAxis( initData(&d_deformationAxis, (int)1, "deformationAxis", "the axis in which we want to show the deformation.\n")), - d_max(initData(&d_max, (Real2)1.0e-2, "max", + d_max(initData(&d_max, (SReal)1.0e-2, "max", "the maximum of the deformation.\n")), - d_min(initData(&d_min, (Real2)0.0, "min", + d_min(initData(&d_min, (SReal)0.0, "min", "the minimum of the deformation.\n")), d_radius( - initData(&d_radius, (Real2)0.05, "radius", + initData(&d_radius, (SReal)0.05, "radius", "the axis in which we want to show the deformation.\n")), d_drawMapBeam( initData(&d_drawMapBeam, true, "nonColored", @@ -138,11 +138,11 @@ void DiscreteCosseratMapping::apply( template void DiscreteCosseratMapping::computeLogarithm( - const double &x, const Matrix4 &gX, Matrix4 &log_gX) { + const double &x, const Mat4x4 &gX, Mat4x4 &log_gX) { // Compute theta before everything double theta = computeTheta(x, gX); - Matrix4 I4; + Mat4x4 I4; I4.clear(); I4.identity(); log_gX.clear(); @@ -564,17 +564,17 @@ void DiscreteCosseratMapping::applyJT( template void DiscreteCosseratMapping::computeBBox( - const sofa::core::ExecParams *, bool) { - // const VecCoord& x = getVertices(); //m_vertices.getValue(); + const sofa::core::ExecParams *, bool) +{ const OutVecCoord &x = m_toModel->read(sofa::core::ConstVecCoordId::position())->getValue(); - SReal minBBox[3] = {std::numeric_limits::max(), - std::numeric_limits::max(), - std::numeric_limits::max()}; - SReal maxBBox[3] = {-std::numeric_limits::max(), - -std::numeric_limits::max(), - -std::numeric_limits::max()}; + SReal minBBox[3] = {std::numeric_limits::max(), + std::numeric_limits::max(), + std::numeric_limits::max()}; + SReal maxBBox[3] = {-std::numeric_limits::max(), + -std::numeric_limits::max(), + -std::numeric_limits::max()}; for (std::size_t i = 0; i < x.size(); i++) { const OutCoord &p = x[i]; for (int c = 0; c < 3; c++) { @@ -602,7 +602,7 @@ void DiscreteCosseratMapping::draw( m_toModel->read(sofa::core::ConstVecCoordId::position()); const OutVecCoord xData = xfromData->getValue(); vector positions; - vector> Orientation; + vector> Orientation; positions.clear(); Orientation.clear(); unsigned int sz = xData.size(); @@ -623,9 +623,9 @@ void DiscreteCosseratMapping::draw( d_radius.getValue(), drawColor); // Define color map - Real2 min = d_min.getValue(); - Real2 max = d_max.getValue(); - sofa::helper::ColorMap::evaluator _eval = m_colorMap.getEvaluator(min, max); + SReal min = d_min.getValue(); + SReal max = d_max.getValue(); + sofa::helper::ColorMap::evaluator _eval = m_colorMap.getEvaluator(min, max); glLineWidth(d_radius.getValue()); glBegin(GL_LINES); From b63fb6d9f5745f006af7e457cc45748f8ea733fa Mon Sep 17 00:00:00 2001 From: Damien Marchal Date: Tue, 18 Jun 2024 10:17:02 +0200 Subject: [PATCH 24/26] Refactor computeAdjoint. --- src/Cosserat/mapping/BaseCosseratMapping.cpp | 111 --------- src/Cosserat/mapping/BaseCosseratMapping.h | 13 +- src/Cosserat/mapping/BaseCosseratMapping.inl | 131 ++++++----- .../mapping/DiscreteCosseratMapping.cpp | 150 ++++++------- .../mapping/DiscreteCosseratMapping.h | 2 +- .../mapping/DiscreteCosseratMapping.inl | 211 ++++++++++-------- .../mapping/DiscreteDynamicCosseratMapping.h | 2 +- .../DiscreteDynamicCosseratMapping.inl | 29 +-- src/Cosserat/types.h | 8 +- 9 files changed, 282 insertions(+), 375 deletions(-) diff --git a/src/Cosserat/mapping/BaseCosseratMapping.cpp b/src/Cosserat/mapping/BaseCosseratMapping.cpp index ed591d1..649828e 100644 --- a/src/Cosserat/mapping/BaseCosseratMapping.cpp +++ b/src/Cosserat/mapping/BaseCosseratMapping.cpp @@ -28,117 +28,6 @@ namespace Cosserat::mapping { -using namespace sofa::defaulttype; - -template <> -auto BaseCosseratMapping::buildXiHat(const Coord1 &strain_i) -> se3 -{ - se3 Xi; - - Xi[0][1] = -strain_i(2); - Xi[0][2] = strain_i[1]; - Xi[1][2] = -strain_i[0]; - - Xi[1][0] = -Xi(0, 1); - Xi[2][0] = -Xi(0, 2); - Xi[2][1] = -Xi(1, 2); - - Xi[0][3] = 1.0; - - for (unsigned int i = 0; i < 3; i++) - Xi[i][3] += strain_i(i + 3); - - return Xi; -} - -template <> -void BaseCosseratMapping::computeExponentialSE3( - const double &curv_abs_x_n, const Coord1 &strain_n, Transform &Trans) { - Mat4x4 I4; - I4.identity(); - - // Get the angular part of the - Vec3 k = Vec3(strain_n(0), strain_n(1), strain_n(2)); - SReal theta = k.norm(); // - - SE3 g_X_n; - se3 Xi_hat_n = buildXiHat(strain_n); - - msg_info() << "matrix Xi : " << Xi_hat_n; - - if (theta <= std::numeric_limits::epsilon()) { - g_X_n = I4 + curv_abs_x_n * Xi_hat_n; - } else { - double scalar1 = - (1.0 - std::cos(curv_abs_x_n * theta)) / std::pow(theta, 2); - double scalar2 = (curv_abs_x_n * theta - std::sin(curv_abs_x_n * theta)) / - std::pow(theta, 3); - g_X_n = I4 + curv_abs_x_n * Xi_hat_n + scalar1 * Xi_hat_n * Xi_hat_n + - scalar2 * Xi_hat_n * Xi_hat_n * Xi_hat_n; - } - - msg_info() << "matrix g_X : " << g_X_n; - - sofa::type::Mat3x3 M; - g_X_n.getsub(0, 0, M); // get the rotation matrix - - // convert the rotation 3x3 matrix to a quaternion - sofa::type::Quat R; - R.fromMatrix(M); - Trans = Transform(Vec3(g_X_n(0, 3), g_X_n(1, 3), g_X_n(2, 3)), R); -} - -template <> -void BaseCosseratMapping::computeTangExp( - double &curv_abs_n, const Coord1 &strain_i, Mat6x6 &TgX) { - - SReal theta = Vec3(strain_i(0), strain_i(1), strain_i(2)) - .norm(); // Sometimes this is computed over all strain - Mat3x3 tilde_k = - getTildeMatrix(Vec3(strain_i(0), strain_i(1), strain_i(2))); - - /* Younes @23-11-27 - old version - @Todo ???? is p the linear deformation? If so, why didn't I just put a zero - vector in place of p and the first element of p is equal to 1? Matrix3 tilde_p - = getTildeMatrix(type::Vec3(1.0, 0.0, 0.0)); Using the new version does not - bring any difference in my three reference scenes, but need more investogation - #TECHNICAL_DEBT - */ - // TODO(dmarchal: 2024/06/07) could the debt by solved ? - Mat3x3 tilde_q = - getTildeMatrix(Vec3(strain_i(3), strain_i(4), strain_i(5))); - - Mat6x6 ad_Xi; - buildAdjoint(tilde_k, tilde_q, ad_Xi); - - Mat6x6 Id6 = Mat6x6::Identity(); - - if (theta <= std::numeric_limits::epsilon()) { - double scalar0 = std::pow(curv_abs_n, 2) / 2.0; - TgX = curv_abs_n * Id6 + scalar0 * ad_Xi; - } else { - double scalar1 = (4.0 - 4.0 * cos(curv_abs_n * theta) - - curv_abs_n * theta * sin(curv_abs_n * theta)) / - (2.0 * theta * theta); - double scalar2 = (4.0 * curv_abs_n * theta + - curv_abs_n * theta * cos(curv_abs_n * theta) - - 5.0 * sin(curv_abs_n * theta)) / - (2.0 * theta * theta * theta); - double scalar3 = (2.0 - 2.0 * cos(curv_abs_n * theta) - - curv_abs_n * theta * sin(curv_abs_n * theta)) / - (2.0 * theta * theta * theta * theta); - double scalar4 = (2.0 * curv_abs_n * theta + - curv_abs_n * theta * cos(curv_abs_n * theta) - - 3.0 * sin(curv_abs_n * theta)) / - (2.0 * theta * theta * theta * theta * theta); - - TgX = curv_abs_n * Id6 + scalar1 * ad_Xi + scalar2 * ad_Xi * ad_Xi + - scalar3 * ad_Xi * ad_Xi * ad_Xi + - scalar4 * ad_Xi * ad_Xi * ad_Xi * ad_Xi; - } -} - template class SOFA_COSSERAT_API BaseCosseratMapping; diff --git a/src/Cosserat/mapping/BaseCosseratMapping.h b/src/Cosserat/mapping/BaseCosseratMapping.h index baa2e7f..c2371c3 100644 --- a/src/Cosserat/mapping/BaseCosseratMapping.h +++ b/src/Cosserat/mapping/BaseCosseratMapping.h @@ -51,7 +51,7 @@ using _se3 = Eigen::Matrix4d; using _SE3 = Eigen::Matrix4d; using Cosserat::type::Transform; -using Cosserat::type::Tangent; +using Cosserat::type::TangentTransform; using Cosserat::type::RotMat; @@ -91,7 +91,7 @@ class BaseCosseratMapping : public sofa::core::Multi2Mapping // really needed ? /*===========COSSERAT VECTORS ======================*/ - unsigned int m_index_input; + unsigned int m_indexInput; vector m_vecTransform; vector m_framesExponentialSE3Vectors; @@ -138,8 +138,7 @@ class BaseCosseratMapping : public sofa::core::Multi2Mapping void printMatrix(const Mat6x6 R); sofa::type::Mat3x3 extractRotMatrix(const Transform &frame); - Tangent buildProjector(const Transform &T); - se3 buildXiHat(const Coord1 &strain_i); + TangentTransform buildProjector(const Transform &T); Mat3x3 getTildeMatrix(const Vec3 &u); void buildAdjoint(const Mat3x3 &A, const Mat3x3 &B, Mat6x6 &Adjoint); @@ -198,15 +197,17 @@ class BaseCosseratMapping : public sofa::core::Multi2Mapping // TODO(dmarchal: 2024/06/07): // - clarify the difference between computeAdjoing and buildAdjoint ... - // - clarify why we need Transform and Vec6 and Tangent & Mat6x6 - void computeAdjoint(const Transform &frame, Tangent &adjoint); + // - clarify why we need Transform and Vec6 and TangentTransform & Mat6x6 + void computeAdjoint(const Transform &frame, TangentTransform &adjoint); void computeAdjoint(const Vec6 &frame, Mat6x6 &adjoint); void computeCoAdjoint(const Transform &frame, Mat6x6 &coAdjoint); void updateExponentialSE3(const vector &inDeform); void updateTangExpSE3(const vector &inDeform); + void computeTangExp(double &x, const Coord1 &k, Mat6x6 &TgX); + void computeTangExpImplementation(double &x, const Vec6 &k, Mat6x6 &TgX); [[maybe_unused]] Vec6 computeETA(const Vec6 &baseEta, const vector &k_dot, double abs_input); diff --git a/src/Cosserat/mapping/BaseCosseratMapping.inl b/src/Cosserat/mapping/BaseCosseratMapping.inl index 48fd82c..4f2cd43 100644 --- a/src/Cosserat/mapping/BaseCosseratMapping.inl +++ b/src/Cosserat/mapping/BaseCosseratMapping.inl @@ -53,7 +53,7 @@ BaseCosseratMapping::BaseCosseratMapping() d_curv_abs_frames(initData(&d_curv_abs_frames, "curv_abs_output", " need to be com....")), d_debug(initData(&d_debug, false, "debug", "printf for the debug")), - m_index_input(0) {} + m_indexInput(0) {} template @@ -153,15 +153,43 @@ void BaseCosseratMapping::initializeFrames() << "m_BeamLengthVectors : " << msgendl; } +auto buildXiHat(const Vec3& strain_i) -> se3 +{ + se3 Xi_hat; + + Xi_hat[0][1] = -strain_i[2]; + Xi_hat[0][2] = strain_i[1]; + Xi_hat[1][2] = -strain_i[0]; + + Xi_hat[1][0] = -Xi_hat(0, 1); + Xi_hat[2][0] = -Xi_hat(0, 2); + Xi_hat[2][1] = -Xi_hat(1, 2); + + //@TODO: Why this , if q = 0 ???? + Xi_hat[0][3] = 1.0; + return Xi_hat; +} + +auto buildXiHat(const Vec6& strain_i) -> se3 +{ + se3 Xi = buildXiHat(Vec3(strain_i(0), strain_i(1), strain_i(2))); + + for (unsigned int i = 0; i < 3; i++) + Xi[i][3] += strain_i(i + 3); + + return Xi; +} template -void BaseCosseratMapping::computeExponentialSE3( - const double &curv_abs_x_n, const Coord1 &strain_n, Transform &g_X_n) { +void BaseCosseratMapping::computeExponentialSE3(const double &curv_abs_x_n, + const Coord1 &strain_n, + Transform &g_X_n) +{ const auto I4 = Mat4x4::Identity(); - // Get the angular part of the + // Get the angular part of the strain Vec3 k = Vec3(strain_n(0), strain_n(1), strain_n(2)); - SReal theta = k.norm(); // + SReal theta = k.norm(); SE3 _g_X; se3 Xi_hat_n = buildXiHat(strain_n); @@ -246,7 +274,8 @@ void BaseCosseratMapping::updateExponentialSE3( template void BaseCosseratMapping::computeAdjoint(const Transform &frame, - Tangent &adjoint) { + TangentTransform &adjoint) +{ Mat3x3 R = extractRotMatrix(frame); Vec3 u = frame.getOrigin(); Mat3x3 tilde_u = getTildeMatrix(u); @@ -266,21 +295,21 @@ void BaseCosseratMapping::computeCoAdjoint(const Transform &fr template void BaseCosseratMapping::computeAdjoint(const Vec6 &eta, - Mat6x6 &adjoint) { - Mat3x3 tildeMat = getTildeMatrix(Vec3(eta[0], eta[1], eta[2])); - adjoint.setsub(0, 0, tildeMat); - adjoint.setsub(3, 3, tildeMat); - adjoint.setsub(3, 0, getTildeMatrix(Vec3(eta[3], eta[4], eta[5]))); + Mat6x6 &adjoint) +{ + Mat3x3 tildeMat1 = getTildeMatrix(Vec3(eta[0], eta[1], eta[2])); + Mat3x3 tildeMat2 = getTildeMatrix(Vec3(eta[3], eta[4], eta[5])); + buildAdjoint(tildeMat1, tildeMat2, adjoint); } + template auto BaseCosseratMapping::computeLogarithm(const double &x, const Mat4x4 &gX) -> Mat4x4 { // Compute theta before everything const double theta = computeTheta(x, gX); - Mat4x4 I4; - I4.identity(); + Mat4x4 I4 = Mat4x4::Identity(); Mat4x4 log_gX; double csc_theta = 1.0 / (sin(x * theta / 2.0)); @@ -322,7 +351,7 @@ void BaseCosseratMapping::updateTangExpSE3( // Compute tangExpo at frame points for (unsigned int i = 0; i < sz; i++) { - Tangent temp; + TangentTransform temp; Coord1 strain_frame_i = inDeform[m_indicesVectors[i] - 1]; double curv_abs_x_i = m_framesLengthVectors[i]; @@ -337,14 +366,14 @@ void BaseCosseratMapping::updateTangExpSE3( // Compute the TangExpSE3 at the nodes m_nodesTangExpVectors.clear(); - Tangent tangExpO; + TangentTransform tangExpO; tangExpO.clear(); m_nodesTangExpVectors.push_back(tangExpO); for (size_t j = 1; j < curv_abs_section.size(); j++) { Coord1 strain_node_i = inDeform[j - 1]; double x = m_beamLengthVectors[j - 1]; - Tangent temp; + TangentTransform temp; temp.clear(); computeTangExp(x, strain_node_i, temp); m_nodesTangExpVectors.push_back(temp); @@ -355,27 +384,27 @@ void BaseCosseratMapping::updateTangExpSE3( template void BaseCosseratMapping::computeTangExp(double &curv_abs_n, const Coord1 &strain_i, - Mat6x6 &TgX) { - - SReal theta = Vec3(strain_i(0), strain_i(1), strain_i(2)) - .norm(); // Sometimes this is computed over all strain - Mat3x3 tilde_k = - getTildeMatrix(Vec3(strain_i(0), strain_i(1), strain_i(2))); - /* Younes @23-11-27 - old version - @Todo ???? is p the linear deformation? If so, why didn't I just put a zero - vector in place of p and the first element of p is equal to 1? Matrix3 tilde_p - = getTildeMatrix(type::Vec3(1.0, 0.0, 0.0)); Using the new version does not - bring any difference in my three reference scenes, but need more investogation - #TECHNICAL_DEBT - */ - Mat3x3 tilde_q = getTildeMatrix(Vec3(0.0, 0.0, 0.0)); + Mat6x6 &TgX) +{ + if constexpr( Coord1::static_size == 3 ) + computeTangExpImplementation(curv_abs_n, Vec6(strain_i(0),strain_i(1),strain_i(2),0,0,0), TgX); + else + computeTangExpImplementation(curv_abs_n, strain_i, TgX); +} + +template +void BaseCosseratMapping::computeTangExpImplementation(double &curv_abs_n, + const Vec6 &strain_i, + Mat6x6 &TgX) +{ + SReal theta = Vec3(strain_i(0), strain_i(1), strain_i(2)).norm(); + Mat3x3 tilde_k = getTildeMatrix(Vec3(strain_i(0), strain_i(1), strain_i(2))); + Mat3x3 tilde_q = getTildeMatrix(Vec3(strain_i(3), strain_i(4), strain_i(5))); Mat6x6 ad_Xi; buildAdjoint(tilde_k, tilde_q, ad_Xi); Mat6x6 Id6 = Mat6x6::Identity(); - if (theta <= std::numeric_limits::epsilon()) { double scalar0 = std::pow(curv_abs_n, 2) / 2.0; TgX = curv_abs_n * Id6 + scalar0 * ad_Xi; @@ -418,27 +447,28 @@ BaseCosseratMapping::computeETA(const Vec6 &baseEta, // Same as for x1, query a read accessor so we can access the content of d_curv_abs_section auto curv_abs_input = getReadAccessor(d_curv_abs_section); - auto& kdot = k_dot[m_index_input]; + auto& kdot = k_dot[m_indexInput]; Vec6 Xi_dot {kdot[0], kdot[1], kdot[2], 0,0,0}; + // if m_indexInput is == 0 double diff0 = abs_input; double _diff0 = -abs_input; - if (m_index_input != 0) + if (m_indexInput != 0) { - diff0 = abs_input - curv_abs_input[m_index_input - 1]; - _diff0 = curv_abs_input[m_index_input - 1] - abs_input; + diff0 = abs_input - curv_abs_input[m_indexInput - 1]; + _diff0 = curv_abs_input[m_indexInput - 1] - abs_input; } Transform outTransform; - computeExponentialSE3(_diff0, x1[m_index_input], outTransform); + computeExponentialSE3(_diff0, x1[m_indexInput], outTransform); - Mat6x6 adjointMatrix; + TangentTransform adjointMatrix; computeAdjoint(outTransform, adjointMatrix); - Tangent tangentMatrix; - computeTangExp(diff0, x1[m_index_input], tangentMatrix); + TangentTransform tangentMatrix; + computeTangExp(diff0, x1[m_indexInput], tangentMatrix); return adjointMatrix * (baseEta + tangentMatrix * Xi_dot); } @@ -486,8 +516,8 @@ Mat3x3 BaseCosseratMapping::extractRotMatrix(const Transform & template auto BaseCosseratMapping::buildProjector(const Transform &T) --> Tangent { - Mat6x6 P; +-> TangentTransform { + TangentTransform P; // TODO(dmarchal: 2024/06/07) The following code should probably become // utility function building a 3x3 matix from a quaternion should probably @@ -503,23 +533,6 @@ auto BaseCosseratMapping::buildProjector(const Transform &T) return P; } -template -auto BaseCosseratMapping::buildXiHat(const Coord1 &strain_i) -> se3 { - se3 Xi_hat; - - Xi_hat[0][1] = -strain_i(2); - Xi_hat[0][2] = strain_i[1]; - Xi_hat[1][2] = -strain_i[0]; - - Xi_hat[1][0] = -Xi_hat(0, 1); - Xi_hat[2][0] = -Xi_hat(0, 2); - Xi_hat[2][1] = -Xi_hat(1, 2); - - //@TODO: Why this , if q = 0 ???? - Xi_hat[0][3] = 1.0; - return Xi_hat; -} - template auto BaseCosseratMapping::getTildeMatrix(const Vec3 &u) -> Mat3x3 { diff --git a/src/Cosserat/mapping/DiscreteCosseratMapping.cpp b/src/Cosserat/mapping/DiscreteCosseratMapping.cpp index 5b2ef2b..554bb99 100644 --- a/src/Cosserat/mapping/DiscreteCosseratMapping.cpp +++ b/src/Cosserat/mapping/DiscreteCosseratMapping.cpp @@ -37,21 +37,23 @@ void DiscreteCosseratMapping:: applyJ( const vector& dataVecIn1Vel, const vector& dataVecIn2Vel) { - if(d_debug.getValue()) - std::cout<< " ########## ApplyJ R Function ########"<< std::endl; - if(dataVecOutVel.empty() || dataVecIn1Vel.empty() ||dataVecIn2Vel.empty() ) return; - const In1VecDeriv& in1_vel = dataVecIn1Vel[0]->getValue(); - const In2VecDeriv& in2_vel = dataVecIn2Vel[0]->getValue(); - OutVecDeriv& out_vel = *dataVecOutVel[0]->beginEdit(); + + msg_info() << " ########## ApplyJ R Function ########"; + + const In1VecDeriv& in1_vel = sofa::helper::getReadAccessor(*dataVecIn1Vel[0]); + const In2VecDeriv& in2_vel = sofa::helper::getReadAccessor(*dataVecIn2Vel[0]); + + auto out_vel = sofa::helper::getWriteOnlyAccessor(*dataVecOutVel[0]); + const auto baseIndex = d_baseIndex.getValue(); // Curv abscissa of nodes and frames - sofa::helper::ReadAccessor>> curv_abs_section = d_curv_abs_section; - sofa::helper::ReadAccessor>> curv_abs_frames = d_curv_abs_frames; + auto curv_abs_section = sofa::helper::getReadAccessor(d_curv_abs_section); + auto curv_abs_frames = sofa::helper::getReadAccessor(d_curv_abs_frames); - const In1VecCoord& inDeform = m_fromModel1->read(sofa::core::ConstVecCoordId::position())->getValue(); //Strains + const auto inDeform = sofa::helper::getReadAccessor(*m_fromModel1->read(sofa::core::ConstVecCoordId::position())); // Compute the tangent Exponential SE3 vectors this->updateTangExpSE3(inDeform); @@ -65,18 +67,19 @@ void DiscreteCosseratMapping:: applyJ( baseVelocity[u] = in2_vel[baseIndex][u]; //Apply the local transform i.e. from SOFA's frame to Cosserat's frame - const In2VecCoord& xfrom2Data = m_fromModel2->read(sofa::core::ConstVecCoordId::position())->getValue(); + const In2VecCoord& xfrom2Data = sofa::helper::getReadAccessor(*m_fromModel2->read(sofa::core::ConstVecCoordId::position())); Transform TInverse = Transform(xfrom2Data[baseIndex].getCenter(), xfrom2Data[baseIndex].getOrientation()).inversed(); Mat6x6 P = this->buildProjector(TInverse); Vec6 baseLocalVelocity = P * baseVelocity; //This is the base velocity in Locale frame m_nodesVelocityVectors.push_back(baseLocalVelocity); - if(d_debug.getValue()) - std::cout << "Base local Velocity :"<< baseLocalVelocity <computeAdjoint(Trans, Adjoint); Vec6 node_Xi_dot; @@ -85,36 +88,28 @@ void DiscreteCosseratMapping:: applyJ( Vec6 eta_node_i = Adjoint * (m_nodesVelocityVectors[i-1] + m_nodesTangExpVectors[i] *node_Xi_dot ); m_nodesVelocityVectors.push_back(eta_node_i); - if(d_debug.getValue()) - std::cout<< "Node velocity : "<< i << " = " << eta_node_i<< std::endl; + msg_info() << "Node velocity : "<< i << " = " << eta_node_i; } - const OutVecCoord& out = m_toModel->read(sofa::core::ConstVecCoordId::position())->getValue(); + const OutVecCoord& out = sofa::helper::getReadAccessor(*m_toModel->read(sofa::core::ConstVecCoordId::position())); + auto sz = curv_abs_frames.size(); out_vel.resize(sz); - for (unsigned int i = 0 ; i < sz; i++) { Transform Trans = m_framesExponentialSE3Vectors[i].inversed(); - Tangent Adjoint; Adjoint.clear(); + TangentTransform Adjoint; Adjoint.clear(); this->computeAdjoint(Trans, Adjoint); Vec6 frame_Xi_dot = in1_vel[m_indicesVectors[i]-1]; -// for (unsigned int u =0; u<6; u++) -// frame_Xi_dot(i) = in1_vel[m_indicesVectors[i]-1][u]; - Vec6 eta_frame_i = Adjoint * (m_nodesVelocityVectors[m_indicesVectors[i]-1] + m_framesTangExpVectors[i] * frame_Xi_dot ); // eta auto T = Transform(out[i].getCenter(), out[i].getOrientation()); - Tangent Proj = this->buildProjector(T); + TangentTransform Proj = this->buildProjector(T); out_vel[i] = Proj * eta_frame_i; - if(d_debug.getValue()) - std::cout<< "Frame velocity : "<< i << " = " << eta_frame_i<< std::endl; + msg_info() << "Frame velocity : "<< i << " = " << eta_frame_i; } - dataVecOutVel[0]->endEdit(); - m_index_input = 0; + m_indexInput = 0; } - - template <> void DiscreteCosseratMapping:: applyJT( const sofa::core::MechanicalParams* /*mparams*/, const vector< In1DataVecDeriv*>& dataVecOut1Force, @@ -124,12 +119,11 @@ void DiscreteCosseratMapping:: applyJT( if(dataVecOut1Force.empty() || dataVecInForce.empty() || dataVecOut2Force.empty()) return; - if(d_debug.getValue()) - std::cout<< " ########## ApplyJT force R Function ########"<< std::endl; + msg_info() << " ########## ApplyJT force R Function ########"; const OutVecDeriv& in = dataVecInForce[0]->getValue(); - In1VecDeriv& out1 = *dataVecOut1Force[0]->beginEdit(); - In2VecDeriv& out2 = *dataVecOut2Force[0]->beginEdit(); + In1VecDeriv out1 = sofa::helper::getWriteAccessor(*dataVecOut1Force[0]); + In2VecDeriv out2 = sofa::helper::getWriteAccessor(*dataVecOut2Force[0]); const auto baseIndex = d_baseIndex.getValue(); const OutVecCoord& frame = m_toModel->read(sofa::core::ConstVecCoordId::position())->getValue(); @@ -159,12 +153,11 @@ void DiscreteCosseratMapping:: applyJT( Vec6 F_tot; F_tot.clear(); m_totalBeamForceVectors.push_back(F_tot); - Tangent matB_trans; matB_trans.clear(); + TangentTransform matB_trans; matB_trans.clear(); for(unsigned int k=0; k<3; k++) matB_trans[k][k] = 1.0; - for (auto s = sz ; s-- ; ) { - Tangent coAdjoint; + TangentTransform coAdjoint; this->computeCoAdjoint(m_framesExponentialSE3Vectors[s], coAdjoint); // m_framesExponentialSE3Vectors[s] computed in apply Vec6 node_F_Vec = coAdjoint * local_F_Vec[s]; @@ -183,9 +176,8 @@ void DiscreteCosseratMapping:: applyJT( Vec6 temp_f = matB_trans * temp * F_tot; out1[index-1] += temp_f; } - if(d_debug.getValue()) - std::cout << "f at s ="<< s <<" and index"<< index << " is : "<< f << std::endl; + msg_info() << "f at s ="<< s <<" and index"<< index << " is : "<< f; //compute F_tot F_tot += node_F_Vec; @@ -196,13 +188,9 @@ void DiscreteCosseratMapping:: applyJT( Mat6x6 M = this->buildProjector(frame0); out2[baseIndex] += M * F_tot; - if(d_debug.getValue()){ - std::cout << "Node forces "<< out1 << std::endl; - std::cout << "base Force: "<< out2[baseIndex] << std::endl; - } - - dataVecOut1Force[0]->endEdit(); - dataVecOut2Force[0]->endEdit(); + msg_info() + << "Node forces "<< out1 << msgendl + << "base Force: "<< out2[baseIndex]; } template <> @@ -214,43 +202,41 @@ void DiscreteCosseratMapping::applyJT( if(dataMatOut1Const.empty() || dataMatOut2Const.empty() || dataMatInConst.empty() ) return; - if(d_debug.getValue()) - std::cout<< " ########## ApplyJT constraint R Function ########"<< std::endl; + msg_info() << " ########## ApplyJT constraint R Function ########"; //We need only one input In model and input Root model (if present) - In1MatrixDeriv& out1 = *dataMatOut1Const[0]->beginEdit(); // constraints on the strain space (reduced coordinate) - In2MatrixDeriv& out2 = *dataMatOut2Const[0]->beginEdit(); // constraints on the reference frame (base frame) + In1MatrixDeriv& out1 = sofa::helper::getWriteAccessor(*dataMatOut1Const[0]); // constraints on the strain space (reduced coordinate) + In2MatrixDeriv& out2 = sofa::helper::getWriteAccessor(*dataMatOut2Const[0]); // constraints on the reference frame (base frame) const OutMatrixDeriv& in = dataMatInConst[0]->getValue(); // input constraints defined on the mapped frames const OutVecCoord& frame = m_toModel->read(sofa::core::ConstVecCoordId::position())->getValue(); const In1DataVecCoord* x1fromData = m_fromModel1->read(sofa::core::ConstVecCoordId::position()); const In1VecCoord x1from = x1fromData->getValue(); - Tangent matB_trans; matB_trans.clear(); + TangentTransform matB_trans; matB_trans.clear(); for(unsigned int k=0; k<3; k++) matB_trans[k][k] = 1.0; vector< std::tuple > NodesInvolved; vector< std::tuple > NodesInvolvedCompressed; - //helper::vector NodesConstraintDirection; typename OutMatrixDeriv::RowConstIterator rowItEnd = in.end(); + bool doPrintLog = f_printLog.getValue(); for (typename OutMatrixDeriv::RowConstIterator rowIt = in.begin(); rowIt != rowItEnd; ++rowIt) { - if (d_debug.getValue()){ - std::cout<<"************* Apply JT (MatrixDeriv) iteration on line "; - std::cout<::applyJT( int indexBeam = m_indicesVectors[childIndex]; Transform _T = Transform(frame[childIndex].getCenter(),frame[childIndex].getOrientation()); - Tangent P_trans =(this->buildProjector(_T)); + TangentTransform P_trans =(this->buildProjector(_T)); P_trans.transpose(); Mat6x6 coAdjoint; this->computeCoAdjoint(m_framesExponentialSE3Vectors[childIndex], coAdjoint); // m_framesExponentialSE3Vectors[s] computed in apply - Tangent temp = m_framesTangExpVectors[childIndex]; // m_framesTangExpVectors[s] computed in applyJ (here we transpose) + Mat6x6 temp = m_framesTangExpVectors[childIndex]; // m_framesTangExpVectors[s] computed in applyJ (here we transpose) temp.transpose(); Vec6 local_F = coAdjoint * P_trans * valueConst; // constraint direction in local frame of the beam. @@ -285,19 +271,20 @@ void DiscreteCosseratMapping::applyJT( NodesInvolved.push_back(node_force); colIt++; - } - if (d_debug.getValue()){ - std::cout<<"==> NodesInvolved : "<(NodesInvolved[i]) << " force :" - << get<1>(NodesInvolved[i]) << "\n "; + tmp << "index :" <(NodesInvolved[i]) << " force :" << get<1>(NodesInvolved[i]) << msgendl; + msg_info() <<"==> NodesInvolved : " << tmp.str(); } - // sort the Nodes Invoved by decreasing order + // sort the Nodes Involved by decreasing order std::sort(begin(NodesInvolved), end(NodesInvolved), [](std::tuple const &t1, std::tuple const &t2) { - return std::get<0>(t1) > std::get<0>(t2); // custom compare function + return std::get<0>(t1) > std::get<0>(t2); } ); NodesInvolvedCompressed.clear(); @@ -316,8 +303,6 @@ void DiscreteCosseratMapping::applyJT( while (numNode_i == numNode_i1) { cumulativeF += std::get<1>(test_i1); - //// This was if ((n!=NodesInvolved.size()-2)||(n==0)) before and I change it to - /// if ((n!=NodesInvolved.size()-1)||(n==0)) since the code can't leave the will loop if ((n!=NodesInvolved.size()-1)||(n==0)){ n++; break; @@ -330,13 +315,17 @@ void DiscreteCosseratMapping::applyJT( NodesInvolvedCompressed.push_back(std::make_tuple(numNode_i, cumulativeF)); } - if (d_debug.getValue()){ - std::cout<<" NodesInvolved after sort and compress"<(NodesInvolvedCompressed[i]) << " force :" - << get<1>(NodesInvolvedCompressed[i]) << "\n "; + tmp << "index :" <(NodesInvolvedCompressed[i]) << " force :" + << get<1>(NodesInvolvedCompressed[i]) << msgendl; + msg_info() << tmp.str(); } + auto baseIndex = d_baseIndex.getValue(); for (unsigned n=0; n test = NodesInvolvedCompressed[n]; @@ -348,7 +337,7 @@ void DiscreteCosseratMapping::applyJT( { //cumulate on beam frame Mat6x6 coAdjoint; - this->computeCoAdjoint(m_nodesExponentialSE3Vectors[i-1],coAdjoint); //m_nodesExponentialSE3Vectors computed in apply + this->computeCoAdjoint(m_nodesExponentialSE3Vectors[i-1],coAdjoint); CumulativeF = coAdjoint * CumulativeF; // transfer to strain space (local coordinates) Mat6x6 temp = m_nodesTangExpVectors[i-1]; @@ -363,22 +352,15 @@ void DiscreteCosseratMapping::applyJT( Mat6x6 M = this->buildProjector(frame0); Vec6 base_force = M * CumulativeF; - o2.addCol(d_baseIndex.getValue(), base_force); + o2.addCol(baseIndex, base_force); } } - - //"""END ARTICULATION SYSTEM MAPPING""" - dataMatOut1Const[0]->endEdit(); - dataMatOut2Const[0]->endEdit(); } - // Register in the Factory int DiscreteCosseratMappingClass = sofa::core::RegisterObject("Set the positions and velocities of points attached to a rigid parent") .add< DiscreteCosseratMapping< sofa::defaulttype::Vec3Types, sofa::defaulttype::Rigid3Types, sofa::defaulttype::Rigid3Types > >(true) - .add< DiscreteCosseratMapping< sofa::defaulttype::Vec6Types, sofa::defaulttype::Rigid3Types, sofa::defaulttype::Rigid3Types > >() - ; - + .add< DiscreteCosseratMapping< sofa::defaulttype::Vec6Types, sofa::defaulttype::Rigid3Types, sofa::defaulttype::Rigid3Types > >(); template class SOFA_COSSERAT_API DiscreteCosseratMapping< sofa::defaulttype::Vec3Types, sofa::defaulttype::Rigid3Types, sofa::defaulttype::Rigid3Types >; template class SOFA_COSSERAT_API DiscreteCosseratMapping< sofa::defaulttype::Vec6Types, sofa::defaulttype::Rigid3Types, sofa::defaulttype::Rigid3Types >; diff --git a/src/Cosserat/mapping/DiscreteCosseratMapping.h b/src/Cosserat/mapping/DiscreteCosseratMapping.h index 4b82ffe..7dc835b 100644 --- a/src/Cosserat/mapping/DiscreteCosseratMapping.h +++ b/src/Cosserat/mapping/DiscreteCosseratMapping.h @@ -159,7 +159,7 @@ class DiscreteCosseratMapping : public BaseCosseratMapping { using BaseCosseratMapping::d_debug; using BaseCosseratMapping::m_vecTransform; using BaseCosseratMapping::m_nodeAdjointVectors; - using BaseCosseratMapping::m_index_input; + using BaseCosseratMapping::m_indexInput; using BaseCosseratMapping::m_indicesVectorsDraw; using BaseCosseratMapping::computeTheta; diff --git a/src/Cosserat/mapping/DiscreteCosseratMapping.inl b/src/Cosserat/mapping/DiscreteCosseratMapping.inl index 0beadc5..9d48634 100644 --- a/src/Cosserat/mapping/DiscreteCosseratMapping.inl +++ b/src/Cosserat/mapping/DiscreteCosseratMapping.inl @@ -59,9 +59,9 @@ DiscreteCosseratMapping::DiscreteCosseratMapping() : "if this parameter is false, you draw the beam with " "color according to the force apply to each beam")), d_color(initData( - &d_color, - sofa::type::RGBAColor(40 / 255.0, 104 / 255.0, 137 / 255.0, 0.8), - "color", "The default beam color")), + &d_color, + sofa::type::RGBAColor(40 / 255.0, 104 / 255.0, 137 / 255.0, 0.8), + "color", "The default beam color")), d_index( initData(&d_index, "index", "if this parameter is false, you draw the beam with color " @@ -81,10 +81,10 @@ void DiscreteCosseratMapping::doBaseCosseratInit() template void DiscreteCosseratMapping::apply( - const sofa::core::MechanicalParams * /* mparams */, - const vector &dataVecOutPos, - const vector &dataVecIn1Pos, - const vector &dataVecIn2Pos) { + const sofa::core::MechanicalParams * /* mparams */, + const vector &dataVecOutPos, + const vector &dataVecIn1Pos, + const vector &dataVecIn2Pos) { if (dataVecOutPos.empty() || dataVecIn1Pos.empty() || dataVecIn2Pos.empty()) return; @@ -107,23 +107,32 @@ void DiscreteCosseratMapping::apply( /* Apply the transformation to go from cossserat to SOFA frame*/ Transform frame0 = - Transform(In2::getCPos(in2[baseIndex]), In2::getCRot(in2[baseIndex])); - for (unsigned int i = 0; i < sz; i++) { + Transform(In2::getCPos(in2[baseIndex]), In2::getCRot(in2[baseIndex])); + + // Cache the printLog value out of the loop, otherwise it will trigger a graph update at + // every iteration. + bool doPrintLog = this->f_printLog.getValue(); + for (unsigned int i = 0; i < sz; i++) + { Transform frame = frame0; for (unsigned int u = 0; u < m_indicesVectors[i]; u++) { frame *= - m_nodesExponentialSE3Vectors[u]; // frame = gX(L_0)*...*gX(L_{n-1}) + m_nodesExponentialSE3Vectors[u]; // frame = gX(L_0)*...*gX(L_{n-1}) } frame *= m_framesExponentialSE3Vectors[i]; // frame*gX(x) - msg_info() << "Frame : " << i << " = " << frame; + // This is a lazy printing approach, so there is no time consuming action in the core + // of the loop. + msg_info_when(doPrintLog) + << "Frame : " << i << " = " << frame; - Vec3 v = frame.getOrigin(); - Quat q = frame.getOrientation(); - out[i] = OutCoord(v, q); + Vec3 origin = frame.getOrigin(); + Quat orientation = frame.getOrientation(); + out[i] = OutCoord(origin, orientation); } - if (this->f_printLog.getValue()) + // If the printLog attribute is checked then print distance between out frames. + if (doPrintLog) { std::stringstream tmp; for (unsigned int i = 0; i < out.size() - 1; i++) { @@ -132,21 +141,27 @@ void DiscreteCosseratMapping::apply( } msg_info() << tmp.str(); } - m_index_input = 0; - dataVecOutPos[0]->endEdit(); + + // TODO(dmarchal:2024/06/13): This looks a suspicious design pattern, elaborate more on the purpose + // of m_indexInput and how to use it. + m_indexInput = 0; } template void DiscreteCosseratMapping::computeLogarithm( - const double &x, const Mat4x4 &gX, Mat4x4 &log_gX) { + const double &x, const Mat4x4 &gX, Mat4x4 &log_gX) { - // Compute theta before everything + // computes theta double theta = computeTheta(x, gX); - Mat4x4 I4; - I4.clear(); - I4.identity(); - log_gX.clear(); + // if theta is very small we retun log_gX as the identity. + if (theta <= std::numeric_limits::epsilon()) + { + log_gX = Mat4x4::Identity(); + return; + } + + // otherwise we compute it double csc_theta = 1.0 / (sin(x * theta / 2.0)); double sec_theta = 1.0 / (cos(x * theta / 2.0)); double cst = (1.0 / 8) * (csc_theta * csc_theta * csc_theta) * sec_theta; @@ -156,26 +171,25 @@ void DiscreteCosseratMapping::computeLogarithm( double sin_2Xtheta = sin(2.0 * x_theta); double sin_Xtheta = sin(x_theta); - if (theta <= std::numeric_limits::epsilon()) - log_gX = I4; - else { - log_gX = cst * ((x_theta * cos_2Xtheta - sin_Xtheta) * I4 - - (x_theta * cos_Xtheta + 2.0 * x_theta * cos_2Xtheta - - sin_Xtheta - sin_2Xtheta) * - gX + - (2.0 * x_theta * cos_Xtheta + x_theta * cos_2Xtheta - - sin_Xtheta - sin_2Xtheta) * - (gX * gX) - - (x_theta * cos_Xtheta - sin_Xtheta) * (gX * gX * gX)); - } + // TODO(dmarchal:2024/06/13): Why is there a clear here. Does not the line + // just after enough ? + log_gX.clear(); + log_gX = cst * ((x_theta * cos_2Xtheta - sin_Xtheta) * Mat4x4::Identity() - + (x_theta * cos_Xtheta + 2.0 * x_theta * cos_2Xtheta - + sin_Xtheta - sin_2Xtheta) * + gX + + (2.0 * x_theta * cos_Xtheta + x_theta * cos_2Xtheta - + sin_Xtheta - sin_2Xtheta) * + (gX * gX) - + (x_theta * cos_Xtheta - sin_Xtheta) * (gX * gX * gX)); } template void DiscreteCosseratMapping::applyJ( - const sofa::core::MechanicalParams * /* mparams */, - const vector &dataVecOutVel, - const vector &dataVecIn1Vel, - const vector &dataVecIn2Vel) { + const sofa::core::MechanicalParams * /* mparams */, + const vector &dataVecOutVel, + const vector &dataVecIn1Vel, + const vector &dataVecIn2Vel) { if (dataVecOutVel.empty() || dataVecIn1Vel.empty() || dataVecIn2Vel.empty()) return; @@ -188,12 +202,12 @@ void DiscreteCosseratMapping::applyJ( // Curv abscissa of nodes and frames sofa::helper::ReadAccessor>> curv_abs_section = - d_curv_abs_section; + d_curv_abs_section; sofa::helper::ReadAccessor>> curv_abs_frames = - d_curv_abs_frames; + d_curv_abs_frames; const In1VecCoord &inDeform = - m_fromModel1->read(sofa::core::ConstVecCoordId::position()) + m_fromModel1->read(sofa::core::ConstVecCoordId::position()) ->getValue(); // Strains // Compute the tangent Exponential SE3 vectors this->updateTangExpSE3(inDeform); @@ -208,13 +222,13 @@ void DiscreteCosseratMapping::applyJ( // Apply the local transform i.e. from SOFA's frame to Cosserat's frame const In2VecCoord &xfrom2Data = - m_fromModel2->read(sofa::core::ConstVecCoordId::position())->getValue(); + m_fromModel2->read(sofa::core::ConstVecCoordId::position())->getValue(); Transform TInverse = Transform(xfrom2Data[baseIndex].getCenter(), xfrom2Data[baseIndex].getOrientation()) - .inversed(); + .inversed(); Mat6x6 P = this->buildProjector(TInverse); Vec6 baseLocalVelocity = - P * baseVelocity; // This is the base velocity in Locale frame + P * baseVelocity; // This is the base velocity in Locale frame m_nodesVelocityVectors.push_back(baseLocalVelocity); if (d_debug.getValue()) std::cout << "Base local Velocity :" << baseLocalVelocity << std::endl; @@ -222,7 +236,7 @@ void DiscreteCosseratMapping::applyJ( // Compute velocity at nodes for (unsigned int i = 1; i < curv_abs_section.size(); i++) { Transform Trans = m_nodesExponentialSE3Vectors[i].inversed(); - Mat6x6 Adjoint; + TangentTransform Adjoint; Adjoint.clear(); this->computeAdjoint(Trans, Adjoint); @@ -230,19 +244,20 @@ void DiscreteCosseratMapping::applyJ( Vec6 Xi_dot = Vec6(in1_vel[i - 1], Vec3(0.0, 0.0, 0.0)); Vec6 eta_node_i = Adjoint * (m_nodesVelocityVectors[i - 1] + - m_nodesTangExpVectors[i] * Xi_dot); + m_nodesTangExpVectors[i] * Xi_dot); m_nodesVelocityVectors.push_back(eta_node_i); if (d_debug.getValue()) std::cout << "Node velocity : " << i << " = " << eta_node_i << std::endl; } const OutVecCoord &out = - m_toModel->read(sofa::core::ConstVecCoordId::position())->getValue(); + m_toModel->read(sofa::core::ConstVecCoordId::position())->getValue(); auto sz = curv_abs_frames.size(); out_vel.resize(sz); - for (unsigned int i = 0; i < sz; i++) { + for (unsigned int i = 0; i < sz; i++) + { Transform Trans = m_framesExponentialSE3Vectors[i].inversed(); - Mat6x6 Adjoint; + TangentTransform Adjoint; ///< the class insure that the constructed adjoint is zeroed. Adjoint.clear(); this->computeAdjoint(Trans, Adjoint); Vec6 frame_Xi_dot; @@ -252,8 +267,8 @@ void DiscreteCosseratMapping::applyJ( frame_Xi_dot(u + 3) = 0.; } Vec6 eta_frame_i = - Adjoint * (m_nodesVelocityVectors[m_indicesVectors[i] - 1] + - m_framesTangExpVectors[i] * frame_Xi_dot); // eta + Adjoint * (m_nodesVelocityVectors[m_indicesVectors[i] - 1] + + m_framesTangExpVectors[i] * frame_Xi_dot); // eta auto T = Transform(out[i].getCenter(), out[i].getOrientation()); Mat6x6 Proj = this->buildProjector(T); @@ -265,15 +280,15 @@ void DiscreteCosseratMapping::applyJ( << std::endl; } dataVecOutVel[0]->endEdit(); - m_index_input = 0; + m_indexInput = 0; } template void DiscreteCosseratMapping::applyJT( - const sofa::core::MechanicalParams * /*mparams*/, - const vector &dataVecOut1Force, - const vector &dataVecOut2Force, - const vector &dataVecInForce) { + const sofa::core::MechanicalParams * /*mparams*/, + const vector &dataVecOut1Force, + const vector &dataVecOut2Force, + const vector &dataVecInForce) { if (dataVecOut1Force.empty() || dataVecInForce.empty() || dataVecOut2Force.empty()) @@ -288,9 +303,9 @@ void DiscreteCosseratMapping::applyJT( const auto baseIndex = d_baseIndex.getValue(); const OutVecCoord &frame = - m_toModel->read(sofa::core::ConstVecCoordId::position())->getValue(); + m_toModel->read(sofa::core::ConstVecCoordId::position())->getValue(); const In1DataVecCoord *x1fromData = - m_fromModel1->read(sofa::core::ConstVecCoordId::position()); + m_fromModel1->read(sofa::core::ConstVecCoordId::position()); const In1VecCoord x1from = x1fromData->getValue(); vector local_F_Vec; local_F_Vec.clear(); @@ -305,7 +320,7 @@ void DiscreteCosseratMapping::applyJT( vec[j] = in[var][j]; // Convert input from global frame(SOFA) to local frame Transform _T = - Transform(frame[var].getCenter(), frame[var].getOrientation()); + Transform(frame[var].getCenter(), frame[var].getOrientation()); Mat6x6 P_trans = (this->buildProjector(_T)); P_trans.transpose(); Vec6 local_F = P_trans * vec; @@ -331,12 +346,12 @@ void DiscreteCosseratMapping::applyJT( Mat6x6 coAdjoint; this->computeCoAdjoint( - m_framesExponentialSE3Vectors[s], - coAdjoint); // m_framesExponentialSE3Vectors[s] computed in apply + m_framesExponentialSE3Vectors[s], + coAdjoint); // m_framesExponentialSE3Vectors[s] computed in apply Vec6 node_F_Vec = coAdjoint * local_F_Vec[s]; Mat6x6 temp = - m_framesTangExpVectors[s]; // m_framesTangExpVectors[s] computed in - // applyJ (here we transpose) + m_framesTangExpVectors[s]; // m_framesTangExpVectors[s] computed in + // applyJ (here we transpose) temp.transpose(); Vec3 f = matB_trans * temp * node_F_Vec; @@ -344,8 +359,8 @@ void DiscreteCosseratMapping::applyJT( index--; // bring F_tot to the reference of the new beam this->computeCoAdjoint( - m_nodesExponentialSE3Vectors[index], - coAdjoint); // m_nodesExponentialSE3Vectors computed in apply + m_nodesExponentialSE3Vectors[index], + coAdjoint); // m_nodesExponentialSE3Vectors computed in apply F_tot = coAdjoint * F_tot; Mat6x6 temp = m_nodesTangExpVectors[index]; temp.transpose(); @@ -377,10 +392,10 @@ void DiscreteCosseratMapping::applyJT( template void DiscreteCosseratMapping::applyJT( - const sofa::core::ConstraintParams * /*cparams*/, - const vector &dataMatOut1Const, - const vector &dataMatOut2Const, - const vector &dataMatInConst) { + const sofa::core::ConstraintParams * /*cparams*/, + const vector &dataMatOut1Const, + const vector &dataMatOut2Const, + const vector &dataMatInConst) { if (dataMatOut1Const.empty() || dataMatOut2Const.empty() || dataMatInConst.empty()) return; @@ -390,19 +405,19 @@ void DiscreteCosseratMapping::applyJT( << std::endl; // We need only one input In model and input Root model (if present) In1MatrixDeriv &out1 = - *dataMatOut1Const[0]->beginEdit(); // constraints on the strain space - // (reduced coordinate) + *dataMatOut1Const[0]->beginEdit(); // constraints on the strain space + // (reduced coordinate) In2MatrixDeriv &out2 = - *dataMatOut2Const[0] - ->beginEdit(); // constraints on the reference frame (base frame) + *dataMatOut2Const[0] + ->beginEdit(); // constraints on the reference frame (base frame) const OutMatrixDeriv &in = - dataMatInConst[0] + dataMatInConst[0] ->getValue(); // input constraints defined on the mapped frames const OutVecCoord &frame = - m_toModel->read(sofa::core::ConstVecCoordId::position())->getValue(); + m_toModel->read(sofa::core::ConstVecCoordId::position())->getValue(); const In1DataVecCoord *x1fromData = - m_fromModel1->read(sofa::core::ConstVecCoordId::position()); + m_fromModel1->read(sofa::core::ConstVecCoordId::position()); const In1VecCoord x1from = x1fromData->getValue(); Mat3x6 matB_trans; @@ -434,7 +449,7 @@ void DiscreteCosseratMapping::applyJT( continue; } typename In1MatrixDeriv::RowIterator o1 = - out1.writeLine(rowIt.index()); // we store the constraint number + out1.writeLine(rowIt.index()); // we store the constraint number typename In2MatrixDeriv::RowIterator o2 = out2.writeLine(rowIt.index()); NodesInvolved.clear(); @@ -455,16 +470,16 @@ void DiscreteCosseratMapping::applyJT( Mat6x6 coAdjoint; this->computeCoAdjoint( - m_framesExponentialSE3Vectors[childIndex], - coAdjoint); // m_framesExponentialSE3Vectors[s] computed in apply + m_framesExponentialSE3Vectors[childIndex], + coAdjoint); // m_framesExponentialSE3Vectors[s] computed in apply Mat6x6 temp = m_framesTangExpVectors[childIndex]; // m_framesTangExpVectors[s] - // computed in applyJ - // (here we transpose) + // computed in applyJ + // (here we transpose) temp.transpose(); Vec6 local_F = - coAdjoint * P_trans * - valueConst; // constraint direction in local frame of the beam. + coAdjoint * P_trans * + valueConst; // constraint direction in local frame of the beam. Vec3 f = matB_trans * temp * local_F; // constraint direction in the strain space. @@ -484,10 +499,10 @@ void DiscreteCosseratMapping::applyJT( // sort the Nodes Invoved by decreasing order std::sort( - begin(NodesInvolved), end(NodesInvolved), - [](std::tuple const &t1, std::tuple const &t2) { - return std::get<0>(t1) > std::get<0>(t2); // custom compare function - }); + begin(NodesInvolved), end(NodesInvolved), + [](std::tuple const &t1, std::tuple const &t2) { + return std::get<0>(t1) > std::get<0>(t2); // custom compare function + }); NodesInvolvedCompressed.clear(); @@ -515,7 +530,7 @@ void DiscreteCosseratMapping::applyJT( } } NodesInvolvedCompressed.push_back( - std::make_tuple(numNode_i, cumulativeF)); + std::make_tuple(numNode_i, cumulativeF)); } if (d_debug.getValue()) { @@ -535,8 +550,8 @@ void DiscreteCosseratMapping::applyJT( // cumulate on beam frame Mat6x6 coAdjoint; this->computeCoAdjoint( - m_nodesExponentialSE3Vectors[i - 1], - coAdjoint); // m_nodesExponentialSE3Vectors computed in apply + m_nodesExponentialSE3Vectors[i - 1], + coAdjoint); // m_nodesExponentialSE3Vectors computed in apply CumulativeF = coAdjoint * CumulativeF; // transfer to strain space (local coordinates) Mat6x6 temp = m_nodesTangExpVectors[i - 1]; @@ -549,7 +564,7 @@ void DiscreteCosseratMapping::applyJT( } Transform frame0 = - Transform(frame[0].getCenter(), frame[0].getOrientation()); + Transform(frame[0].getCenter(), frame[0].getOrientation()); Mat6x6 M = this->buildProjector(frame0); Vec6 base_force = M * CumulativeF; @@ -564,10 +579,10 @@ void DiscreteCosseratMapping::applyJT( template void DiscreteCosseratMapping::computeBBox( - const sofa::core::ExecParams *, bool) + const sofa::core::ExecParams *, bool) { const OutVecCoord &x = - m_toModel->read(sofa::core::ConstVecCoordId::position())->getValue(); + m_toModel->read(sofa::core::ConstVecCoordId::position())->getValue(); SReal minBBox[3] = {std::numeric_limits::max(), std::numeric_limits::max(), @@ -589,7 +604,7 @@ void DiscreteCosseratMapping::computeBBox( template void DiscreteCosseratMapping::draw( - const sofa::core::visual::VisualParams *vparams) { + const sofa::core::visual::VisualParams *vparams) { if (!vparams->displayFlags().getShowMechanicalMappings()) return; @@ -599,7 +614,7 @@ void DiscreteCosseratMapping::draw( const auto stateLifeCycle = vparams->drawTool()->makeStateLifeCycle(); const OutDataVecCoord *xfromData = - m_toModel->read(sofa::core::ConstVecCoordId::position()); + m_toModel->read(sofa::core::ConstVecCoordId::position()); const OutVecCoord xData = xfromData->getValue(); vector positions; vector> Orientation; @@ -613,14 +628,14 @@ void DiscreteCosseratMapping::draw( // Get access articulated const In1DataVecCoord *artiData = - m_fromModel1->read(sofa::core::ConstVecCoordId::position()); + m_fromModel1->read(sofa::core::ConstVecCoordId::position()); const In1VecCoord xPos = artiData->getValue(); RGBAColor drawColor = d_color.getValue(); // draw each segment of the beam as a cylinder. for (unsigned int i = 0; i < sz - 1; i++) vparams->drawTool()->drawCylinder(positions[i], positions[i + 1], - d_radius.getValue(), drawColor); + d_radius.getValue(), drawColor); // Define color map SReal min = d_min.getValue(); diff --git a/src/Cosserat/mapping/DiscreteDynamicCosseratMapping.h b/src/Cosserat/mapping/DiscreteDynamicCosseratMapping.h index 428a8b3..c7a318b 100644 --- a/src/Cosserat/mapping/DiscreteDynamicCosseratMapping.h +++ b/src/Cosserat/mapping/DiscreteDynamicCosseratMapping.h @@ -137,7 +137,7 @@ class DiscreteDynamicCosseratMapping : public BaseCosseratMapping::d_debug; using BaseCosseratMapping::m_vecTransform ; using BaseCosseratMapping::m_nodeAdjointVectors; - using BaseCosseratMapping::m_index_input; + using BaseCosseratMapping::m_indexInput; using BaseCosseratMapping::m_toModel; using BaseCosseratMapping::m_fromModel1; using BaseCosseratMapping::m_fromModel2; diff --git a/src/Cosserat/mapping/DiscreteDynamicCosseratMapping.inl b/src/Cosserat/mapping/DiscreteDynamicCosseratMapping.inl index 44b9204..3b6a8f4 100644 --- a/src/Cosserat/mapping/DiscreteDynamicCosseratMapping.inl +++ b/src/Cosserat/mapping/DiscreteDynamicCosseratMapping.inl @@ -70,16 +70,18 @@ void DiscreteDynamicCosseratMapping::apply( const In2VecCoord& in2 = dataVecIn2Pos[0]->getValue(); size_t sz = d_curv_abs_frames.getValue().size(); - OutVecCoord& out = *dataVecOutPos[0]->beginEdit(); + OutVecCoord out = sofa::helper::getWriteAccessor(*dataVecOutPos[0]); out.resize(sz); //update the Exponential Matrices according to new deformation this->updateExponentialSE3(in1); Transform frame0 = Transform(In2::getCPos(in2[0]),In2::getCRot(in2[0])); - for(unsigned int i=0; i::apply( out[i] = OutCoord(v,q); } - m_index_input = 0; - dataVecOutPos[0]->endEdit(); + m_indexInput = 0; } @@ -142,7 +143,7 @@ void DiscreteDynamicCosseratMapping:: applyJ( for (size_t i = 1 ; i < curv_abs_input.size(); i++) { Transform t= m_nodesExponentialSE3Vectors[i].inversed(); - Tangent Adjoint; Adjoint.clear(); + TangentTransform Adjoint; Adjoint.clear(); this->computeAdjoint(t,Adjoint); //Add this line because need for the jacobian computation m_nodeAdjointVectors.push_back(Adjoint); @@ -159,19 +160,21 @@ void DiscreteDynamicCosseratMapping:: applyJ( const OutVecCoord& out = m_toModel->read(sofa::core::ConstVecCoordId::position())->getValue(); size_t sz =curv_abs_output.size(); outVel.resize(sz); - for (size_t i = 0 ; i < sz; i++) { - Transform t= m_framesExponentialSE3Vectors[i].inversed(); - Mat6x6 Adjoint; Adjoint.clear(); - this->computeAdjoint(t,Adjoint); + for (size_t i = 0 ; i < sz; i++) + { + Transform transform= m_framesExponentialSE3Vectors[i].inversed(); + TangentTransform tangentTransform; + tangentTransform.clear(); + this->computeAdjoint(transform, tangentTransform); Vec6 Xi_dot = Vec6(in1[m_indicesVectors[i]-1], Vec3(0.0,0.0,0.0)); // eta - Vec6 etaFrame = Adjoint * (m_nodesVelocityVectors[m_indicesVectors[i]-1] + Vec6 etaFrame = tangentTransform * (m_nodesVelocityVectors[m_indicesVectors[i]-1] + m_framesTangExpVectors[i] * Xi_dot ); //Compute here jacobien and jacobien_dot vector J_i, J_dot_i; - computeJ_Jdot_i(Adjoint, i, J_i, etaFrame, J_dot_i); + computeJ_Jdot_i(tangentTransform, i, J_i, etaFrame, J_dot_i); m_frameJacobienVector.push_back(J_i); m_frameJacobienVector.push_back(J_dot_i); @@ -186,7 +189,7 @@ void DiscreteDynamicCosseratMapping:: applyJ( } // std::cout << "Inside the apply J, outVel after computation : "<< outVel << std::endl; dataVecOutVel[0]->endEdit(); - m_index_input = 0; + m_indexInput = 0; } template diff --git a/src/Cosserat/types.h b/src/Cosserat/types.h index 282c78c..0c016cd 100644 --- a/src/Cosserat/types.h +++ b/src/Cosserat/types.h @@ -47,8 +47,12 @@ namespace Cosserat typedef typename Eigen::Matrix3d RotMat; typedef typename Eigen::Matrix Vector6d; - using Tangent = sofa::type::Mat6x6; - //class Tangent : public sofa::type::Mat<6, 6, SReal>{}; + //using TangentTransform = sofa::type::Mat6x6; + class TangentTransform : public sofa::type::Mat<6, 6, SReal> + { + public: + //TangentTransform(SReal v=0) : sofa::type::Mat6x6(v){} + }; } } From 35dada72bef6c6f41a1376165713f39c0c52f170 Mon Sep 17 00:00:00 2001 From: Yinoussa Adagolodjo Date: Tue, 25 Jun 2024 15:46:57 +0200 Subject: [PATCH 25/26] add callback function in case we are doing dynamic discretisation of cosserat beam --- src/Cosserat/mapping/BaseCosseratMapping.h | 2 +- src/Cosserat/mapping/BaseCosseratMapping.inl | 3 +- .../mapping/DiscreteCosseratMapping.inl | 1156 +++++++++-------- 3 files changed, 590 insertions(+), 571 deletions(-) diff --git a/src/Cosserat/mapping/BaseCosseratMapping.h b/src/Cosserat/mapping/BaseCosseratMapping.h index c2371c3..d32d71f 100644 --- a/src/Cosserat/mapping/BaseCosseratMapping.h +++ b/src/Cosserat/mapping/BaseCosseratMapping.h @@ -28,7 +28,7 @@ namespace Cosserat::mapping { -// Use a private namespace so we are not polluating the Cosserat::mapping. +// Use a private namespace so we are not polluting the Cosserat::mapping. namespace { using namespace std; diff --git a/src/Cosserat/mapping/BaseCosseratMapping.inl b/src/Cosserat/mapping/BaseCosseratMapping.inl index 4f2cd43..da6deaa 100644 --- a/src/Cosserat/mapping/BaseCosseratMapping.inl +++ b/src/Cosserat/mapping/BaseCosseratMapping.inl @@ -90,9 +90,8 @@ void BaseCosseratMapping::init() const vector xfrom = xfromData->getValue(); m_vecTransform.clear(); - for (unsigned int i = 0; i < xfrom.size(); i++) { + for (unsigned int i = 0; i < xfrom.size(); i++) m_vecTransform.push_back(xfrom[i]); - } initializeFrames(); doBaseCosseratInit(); diff --git a/src/Cosserat/mapping/DiscreteCosseratMapping.inl b/src/Cosserat/mapping/DiscreteCosseratMapping.inl index 9d48634..d671f0f 100644 --- a/src/Cosserat/mapping/DiscreteCosseratMapping.inl +++ b/src/Cosserat/mapping/DiscreteCosseratMapping.inl @@ -43,629 +43,649 @@ using sofa::helper::WriteAccessor; using sofa::type::RGBAColor; template -DiscreteCosseratMapping::DiscreteCosseratMapping() : - d_deformationAxis( - initData(&d_deformationAxis, (int)1, "deformationAxis", - "the axis in which we want to show the deformation.\n")), - d_max(initData(&d_max, (SReal)1.0e-2, "max", - "the maximum of the deformation.\n")), - d_min(initData(&d_min, (SReal)0.0, "min", - "the minimum of the deformation.\n")), - d_radius( - initData(&d_radius, (SReal)0.05, "radius", - "the axis in which we want to show the deformation.\n")), - d_drawMapBeam( - initData(&d_drawMapBeam, true, "nonColored", - "if this parameter is false, you draw the beam with " - "color according to the force apply to each beam")), - d_color(initData( - &d_color, - sofa::type::RGBAColor(40 / 255.0, 104 / 255.0, 137 / 255.0, 0.8), - "color", "The default beam color")), - d_index( - initData(&d_index, "index", - "if this parameter is false, you draw the beam with color " - "according to the force apply to each beam")), - d_baseIndex(initData(&d_baseIndex, (unsigned int)0, "baseIndex", - "This parameter defines the index of the rigid " - "base of Cosserat models, 0 by default this can" - "take another value if the rigid base is given " - "by another body.")) {} +DiscreteCosseratMapping::DiscreteCosseratMapping() + : d_deformationAxis( + initData(&d_deformationAxis, (int)1, "deformationAxis", + "the axis in which we want to show the deformation.\n")), + d_max(initData(&d_max, (SReal)1.0e-2, "max", + "the maximum of the deformation.\n")), + d_min(initData(&d_min, (SReal)0.0, "min", + "the minimum of the deformation.\n")), + d_radius( + initData(&d_radius, (SReal)0.05, "radius", + "the axis in which we want to show the deformation.\n")), + d_drawMapBeam( + initData(&d_drawMapBeam, true, "nonColored", + "if this parameter is false, you draw the beam with " + "color according to the force apply to each beam")), + d_color(initData( + &d_color, + sofa::type::RGBAColor(40 / 255.0, 104 / 255.0, 137 / 255.0, 0.8), + "color", "The default beam color")), + d_index( + initData(&d_index, "index", + "if this parameter is false, you draw the beam with color " + "according to the force apply to each beam")), + d_baseIndex(initData(&d_baseIndex, (unsigned int)0, "baseIndex", + "This parameter defines the index of the rigid " + "base of Cosserat models, 0 by default this can" + "take another value if the rigid base is given " + "by another body.")) { + this->addUpdateCallback( + "updateFrames", {&d_curv_abs_section, &d_curv_abs_frames, &d_debug}, + [this](const sofa::core::DataTracker &t) { + SOFA_UNUSED(t); + this->initializeFrames(); + const In1VecCoord &inDeform = + m_fromModel1->read(sofa::core::ConstVecCoordId::position())->getValue(); + this->updateExponentialSE3(inDeform); + return sofa::core::objectmodel::ComponentState::Valid; + }, + {}); +} template -void DiscreteCosseratMapping::doBaseCosseratInit() -{ - m_colorMap.setColorScheme("Blue to Red"); - m_colorMap.reinit(); +void DiscreteCosseratMapping::doBaseCosseratInit() { + m_colorMap.setColorScheme("Blue to Red"); + m_colorMap.reinit(); } template void DiscreteCosseratMapping::apply( - const sofa::core::MechanicalParams * /* mparams */, - const vector &dataVecOutPos, - const vector &dataVecIn1Pos, - const vector &dataVecIn2Pos) { - - if (dataVecOutPos.empty() || dataVecIn1Pos.empty() || dataVecIn2Pos.empty()) - return; - - /// Do Apply - // We need only one input In model and input Root model (if present) - const In1VecCoord &in1 = dataVecIn1Pos[0]->getValue(); - const In2VecCoord &in2 = dataVecIn2Pos[0]->getValue(); - - const auto sz = d_curv_abs_frames.getValue().size(); - OutVecCoord &out = *dataVecOutPos[0]->beginEdit(); // frames states - out.resize(sz); - const auto baseIndex = d_baseIndex.getValue(); - - // update the Exponential matrices according to new deformation - // Here we update m_framesExponentialSE3Vectors & m_nodesExponentialSE3Vectors - // Which are the homogeneous matrices of the frames and the nodes in local - // coordinate. - this->updateExponentialSE3(in1); - - /* Apply the transformation to go from cossserat to SOFA frame*/ - Transform frame0 = - Transform(In2::getCPos(in2[baseIndex]), In2::getCRot(in2[baseIndex])); - - // Cache the printLog value out of the loop, otherwise it will trigger a graph update at - // every iteration. - bool doPrintLog = this->f_printLog.getValue(); - for (unsigned int i = 0; i < sz; i++) - { - Transform frame = frame0; - for (unsigned int u = 0; u < m_indicesVectors[i]; u++) { - frame *= - m_nodesExponentialSE3Vectors[u]; // frame = gX(L_0)*...*gX(L_{n-1}) - } - frame *= m_framesExponentialSE3Vectors[i]; // frame*gX(x) - - // This is a lazy printing approach, so there is no time consuming action in the core - // of the loop. - msg_info_when(doPrintLog) - << "Frame : " << i << " = " << frame; - - Vec3 origin = frame.getOrigin(); - Quat orientation = frame.getOrientation(); - out[i] = OutCoord(origin, orientation); + const sofa::core::MechanicalParams * /* mparams */, + const vector &dataVecOutPos, + const vector &dataVecIn1Pos, + const vector &dataVecIn2Pos) { + + if (dataVecOutPos.empty() || dataVecIn1Pos.empty() || dataVecIn2Pos.empty()) + return; + + // Checking the componentState, to trigger a callback if other data fields (specifically + // d_curv_abs_section and d_curv_abs_frames) were changed dynamically + if (this->d_componentState.getValue() != sofa::core::objectmodel::ComponentState::Valid) + return; + /// Do Apply + // We need only one input In model and input Root model (if present) + const In1VecCoord &in1 = dataVecIn1Pos[0]->getValue(); + const In2VecCoord &in2 = dataVecIn2Pos[0]->getValue(); + + const auto sz = d_curv_abs_frames.getValue().size(); + OutVecCoord &out = *dataVecOutPos[0]->beginEdit(); // frames states + out.resize(sz); + const auto baseIndex = d_baseIndex.getValue(); + + // update the Exponential matrices according to new deformation + // Here we update m_framesExponentialSE3Vectors & m_nodesExponentialSE3Vectors + // Which are the homogeneous matrices of the frames and the nodes in local + // coordinate. + this->updateExponentialSE3(in1); + + /* Apply the transformation to go from cossserat to SOFA frame*/ + Transform frame0 = + Transform(In2::getCPos(in2[baseIndex]), In2::getCRot(in2[baseIndex])); + + // Cache the printLog value out of the loop, otherwise it will trigger a graph + // update at every iteration. + bool doPrintLog = this->f_printLog.getValue(); + for (unsigned int i = 0; i < sz; i++) { + Transform frame = frame0; + for (unsigned int u = 0; u < m_indicesVectors[i]; u++) { + frame *= + m_nodesExponentialSE3Vectors[u]; // frame = gX(L_0)*...*gX(L_{n-1}) } - - // If the printLog attribute is checked then print distance between out frames. - if (doPrintLog) - { - std::stringstream tmp; - for (unsigned int i = 0; i < out.size() - 1; i++) { - Vec3 diff = out[i + 1].getCenter() - out[i].getCenter(); - tmp << "dist " << i << " : " << diff.norm() << msgendl; - } - msg_info() << tmp.str(); + frame *= m_framesExponentialSE3Vectors[i]; // frame*gX(x) + + // This is a lazy printing approach, so there is no time consuming action in + // the core of the loop. + msg_info_when(doPrintLog) << "Frame : " << i << " = " << frame; + + Vec3 origin = frame.getOrigin(); + Quat orientation = frame.getOrientation(); + out[i] = OutCoord(origin, orientation); + } + + // If the printLog attribute is checked then print distance between out + // frames. + if (doPrintLog) { + std::stringstream tmp; + for (unsigned int i = 0; i < out.size() - 1; i++) { + Vec3 diff = out[i + 1].getCenter() - out[i].getCenter(); + tmp << "dist " << i << " : " << diff.norm() << msgendl; } + msg_info() << tmp.str(); + } - // TODO(dmarchal:2024/06/13): This looks a suspicious design pattern, elaborate more on the purpose - // of m_indexInput and how to use it. - m_indexInput = 0; + // TODO(dmarchal:2024/06/13): This looks a suspicious design pattern, + // elaborate more on the purpose of m_indexInput and how to use it. + m_indexInput = 0; } template void DiscreteCosseratMapping::computeLogarithm( - const double &x, const Mat4x4 &gX, Mat4x4 &log_gX) { - - // computes theta - double theta = computeTheta(x, gX); - - // if theta is very small we retun log_gX as the identity. - if (theta <= std::numeric_limits::epsilon()) - { - log_gX = Mat4x4::Identity(); - return; - } - - // otherwise we compute it - double csc_theta = 1.0 / (sin(x * theta / 2.0)); - double sec_theta = 1.0 / (cos(x * theta / 2.0)); - double cst = (1.0 / 8) * (csc_theta * csc_theta * csc_theta) * sec_theta; - double x_theta = x * theta; - double cos_2Xtheta = cos(2.0 * x_theta); - double cos_Xtheta = cos(x_theta); - double sin_2Xtheta = sin(2.0 * x_theta); - double sin_Xtheta = sin(x_theta); - - // TODO(dmarchal:2024/06/13): Why is there a clear here. Does not the line - // just after enough ? - log_gX.clear(); - log_gX = cst * ((x_theta * cos_2Xtheta - sin_Xtheta) * Mat4x4::Identity() - - (x_theta * cos_Xtheta + 2.0 * x_theta * cos_2Xtheta - - sin_Xtheta - sin_2Xtheta) * - gX + - (2.0 * x_theta * cos_Xtheta + x_theta * cos_2Xtheta - - sin_Xtheta - sin_2Xtheta) * - (gX * gX) - - (x_theta * cos_Xtheta - sin_Xtheta) * (gX * gX * gX)); + const double &x, const Mat4x4 &gX, Mat4x4 &log_gX) { + + // computes theta + double theta = computeTheta(x, gX); + + // if theta is very small we retun log_gX as the identity. + if (theta <= std::numeric_limits::epsilon()) { + log_gX = Mat4x4::Identity(); + return; + } + + // otherwise we compute it + double csc_theta = 1.0 / (sin(x * theta / 2.0)); + double sec_theta = 1.0 / (cos(x * theta / 2.0)); + double cst = (1.0 / 8) * (csc_theta * csc_theta * csc_theta) * sec_theta; + double x_theta = x * theta; + double cos_2Xtheta = cos(2.0 * x_theta); + double cos_Xtheta = cos(x_theta); + double sin_2Xtheta = sin(2.0 * x_theta); + double sin_Xtheta = sin(x_theta); + + log_gX.clear(); + log_gX = cst * ((x_theta * cos_2Xtheta - sin_Xtheta) * Mat4x4::Identity() - + (x_theta * cos_Xtheta + 2.0 * x_theta * cos_2Xtheta - + sin_Xtheta - sin_2Xtheta) * + gX + + (2.0 * x_theta * cos_Xtheta + x_theta * cos_2Xtheta - + sin_Xtheta - sin_2Xtheta) * + (gX * gX) - + (x_theta * cos_Xtheta - sin_Xtheta) * (gX * gX * gX)); } template void DiscreteCosseratMapping::applyJ( - const sofa::core::MechanicalParams * /* mparams */, - const vector &dataVecOutVel, - const vector &dataVecIn1Vel, - const vector &dataVecIn2Vel) { - - if (dataVecOutVel.empty() || dataVecIn1Vel.empty() || dataVecIn2Vel.empty()) - return; + const sofa::core::MechanicalParams * /* mparams */, + const vector &dataVecOutVel, + const vector &dataVecIn1Vel, + const vector &dataVecIn2Vel) { + + if (dataVecOutVel.empty() || dataVecIn1Vel.empty() || dataVecIn2Vel.empty()) + return; + + if (this->d_componentState.getValue() != sofa::core::objectmodel::ComponentState::Valid) + return; + if (d_debug.getValue()) + std::cout << " ########## ApplyJ Function ########" << std::endl; + const In1VecDeriv &in1_vel = dataVecIn1Vel[0]->getValue(); + const In2VecDeriv &in2_vel = dataVecIn2Vel[0]->getValue(); + OutVecDeriv &out_vel = *dataVecOutVel[0]->beginEdit(); + const auto baseIndex = d_baseIndex.getValue(); + + // Curv abscissa of nodes and frames + sofa::helper::ReadAccessor>> curv_abs_section = + d_curv_abs_section; + sofa::helper::ReadAccessor>> curv_abs_frames = + d_curv_abs_frames; + + const In1VecCoord &inDeform = + m_fromModel1->read(sofa::core::ConstVecCoordId::position()) + ->getValue(); // Strains + // Compute the tangent Exponential SE3 vectors + this->updateTangExpSE3(inDeform); + + // Get base velocity as input this is also called eta + m_nodesVelocityVectors.clear(); + + // Get base velocity and convert to Vec6, for the facility of computation + Vec6 baseVelocity; // + for (auto u = 0; u < 6; u++) + baseVelocity[u] = in2_vel[baseIndex][u]; + + // Apply the local transform i.e. from SOFA's frame to Cosserat's frame + const In2VecCoord &xfrom2Data = + m_fromModel2->read(sofa::core::ConstVecCoordId::position())->getValue(); + Transform TInverse = Transform(xfrom2Data[baseIndex].getCenter(), + xfrom2Data[baseIndex].getOrientation()) + .inversed(); + Mat6x6 P = this->buildProjector(TInverse); + Vec6 baseLocalVelocity = + P * baseVelocity; // This is the base velocity in Locale frame + m_nodesVelocityVectors.push_back(baseLocalVelocity); + if (d_debug.getValue()) + std::cout << "Base local Velocity :" << baseLocalVelocity << std::endl; + + // Compute velocity at nodes + for (unsigned int i = 1; i < curv_abs_section.size(); i++) { + Transform Trans = m_nodesExponentialSE3Vectors[i].inversed(); + TangentTransform Adjoint; + Adjoint.clear(); + this->computeAdjoint(Trans, Adjoint); + + /// The null vector is replace by the linear velocity in Vec6Type + Vec6 Xi_dot = Vec6(in1_vel[i - 1], Vec3(0.0, 0.0, 0.0)); + + Vec6 eta_node_i = Adjoint * (m_nodesVelocityVectors[i - 1] + + m_nodesTangExpVectors[i] * Xi_dot); + m_nodesVelocityVectors.push_back(eta_node_i); if (d_debug.getValue()) - std::cout << " ########## ApplyJ Function ########" << std::endl; - const In1VecDeriv &in1_vel = dataVecIn1Vel[0]->getValue(); - const In2VecDeriv &in2_vel = dataVecIn2Vel[0]->getValue(); - OutVecDeriv &out_vel = *dataVecOutVel[0]->beginEdit(); - const auto baseIndex = d_baseIndex.getValue(); - - // Curv abscissa of nodes and frames - sofa::helper::ReadAccessor>> curv_abs_section = - d_curv_abs_section; - sofa::helper::ReadAccessor>> curv_abs_frames = - d_curv_abs_frames; - - const In1VecCoord &inDeform = - m_fromModel1->read(sofa::core::ConstVecCoordId::position()) - ->getValue(); // Strains - // Compute the tangent Exponential SE3 vectors - this->updateTangExpSE3(inDeform); - - // Get base velocity as input this is also called eta - m_nodesVelocityVectors.clear(); - - // Get base velocity and convert to Vec6, for the facility of computation - Vec6 baseVelocity; // - for (auto u = 0; u < 6; u++) - baseVelocity[u] = in2_vel[baseIndex][u]; - - // Apply the local transform i.e. from SOFA's frame to Cosserat's frame - const In2VecCoord &xfrom2Data = - m_fromModel2->read(sofa::core::ConstVecCoordId::position())->getValue(); - Transform TInverse = Transform(xfrom2Data[baseIndex].getCenter(), - xfrom2Data[baseIndex].getOrientation()) - .inversed(); - Mat6x6 P = this->buildProjector(TInverse); - Vec6 baseLocalVelocity = - P * baseVelocity; // This is the base velocity in Locale frame - m_nodesVelocityVectors.push_back(baseLocalVelocity); - if (d_debug.getValue()) - std::cout << "Base local Velocity :" << baseLocalVelocity << std::endl; - - // Compute velocity at nodes - for (unsigned int i = 1; i < curv_abs_section.size(); i++) { - Transform Trans = m_nodesExponentialSE3Vectors[i].inversed(); - TangentTransform Adjoint; - Adjoint.clear(); - this->computeAdjoint(Trans, Adjoint); - - /// The null vector is replace by the linear velocity in Vec6Type - Vec6 Xi_dot = Vec6(in1_vel[i - 1], Vec3(0.0, 0.0, 0.0)); - - Vec6 eta_node_i = Adjoint * (m_nodesVelocityVectors[i - 1] + - m_nodesTangExpVectors[i] * Xi_dot); - m_nodesVelocityVectors.push_back(eta_node_i); - if (d_debug.getValue()) - std::cout << "Node velocity : " << i << " = " << eta_node_i << std::endl; + std::cout << "Node velocity : " << i << " = " << eta_node_i << std::endl; + } + + const OutVecCoord &out = + m_toModel->read(sofa::core::ConstVecCoordId::position())->getValue(); + auto sz = curv_abs_frames.size(); + out_vel.resize(sz); + for (unsigned int i = 0; i < sz; i++) { + Transform Trans = m_framesExponentialSE3Vectors[i].inversed(); + TangentTransform + Adjoint; ///< the class insure that the constructed adjoint is zeroed. + Adjoint.clear(); + this->computeAdjoint(Trans, Adjoint); + Vec6 frame_Xi_dot; + + for (auto u = 0; u < 3; u++) { + frame_Xi_dot(u) = in1_vel[m_indicesVectors[i] - 1][u]; + frame_Xi_dot(u + 3) = 0.; } + Vec6 eta_frame_i = + Adjoint * (m_nodesVelocityVectors[m_indicesVectors[i] - 1] + + m_framesTangExpVectors[i] * frame_Xi_dot); // eta - const OutVecCoord &out = - m_toModel->read(sofa::core::ConstVecCoordId::position())->getValue(); - auto sz = curv_abs_frames.size(); - out_vel.resize(sz); - for (unsigned int i = 0; i < sz; i++) - { - Transform Trans = m_framesExponentialSE3Vectors[i].inversed(); - TangentTransform Adjoint; ///< the class insure that the constructed adjoint is zeroed. - Adjoint.clear(); - this->computeAdjoint(Trans, Adjoint); - Vec6 frame_Xi_dot; - - for (auto u = 0; u < 3; u++) { - frame_Xi_dot(u) = in1_vel[m_indicesVectors[i] - 1][u]; - frame_Xi_dot(u + 3) = 0.; - } - Vec6 eta_frame_i = - Adjoint * (m_nodesVelocityVectors[m_indicesVectors[i] - 1] + - m_framesTangExpVectors[i] * frame_Xi_dot); // eta - - auto T = Transform(out[i].getCenter(), out[i].getOrientation()); - Mat6x6 Proj = this->buildProjector(T); + auto T = Transform(out[i].getCenter(), out[i].getOrientation()); + Mat6x6 Proj = this->buildProjector(T); - out_vel[i] = Proj * eta_frame_i; + out_vel[i] = Proj * eta_frame_i; - if (d_debug.getValue()) - std::cout << "Frame velocity : " << i << " = " << eta_frame_i - << std::endl; - } - dataVecOutVel[0]->endEdit(); - m_indexInput = 0; + if (d_debug.getValue()) + std::cout << "Frame velocity : " << i << " = " << eta_frame_i + << std::endl; + } + dataVecOutVel[0]->endEdit(); + m_indexInput = 0; } template void DiscreteCosseratMapping::applyJT( - const sofa::core::MechanicalParams * /*mparams*/, - const vector &dataVecOut1Force, - const vector &dataVecOut2Force, - const vector &dataVecInForce) { + const sofa::core::MechanicalParams * /*mparams*/, + const vector &dataVecOut1Force, + const vector &dataVecOut2Force, + const vector &dataVecInForce) { + + if (dataVecOut1Force.empty() || dataVecInForce.empty() || + dataVecOut2Force.empty()) + return; + + if (this->d_componentState.getValue() != sofa::core::objectmodel::ComponentState::Valid) + return; + + if (d_debug.getValue()) + std::cout << " ########## ApplyJT force Function ########" << std::endl; + const OutVecDeriv &in = dataVecInForce[0]->getValue(); + + In1VecDeriv &out1 = *dataVecOut1Force[0]->beginEdit(); + In2VecDeriv &out2 = *dataVecOut2Force[0]->beginEdit(); + const auto baseIndex = d_baseIndex.getValue(); + + const OutVecCoord &frame = + m_toModel->read(sofa::core::ConstVecCoordId::position())->getValue(); + const In1DataVecCoord *x1fromData = + m_fromModel1->read(sofa::core::ConstVecCoordId::position()); + const In1VecCoord x1from = x1fromData->getValue(); + vector local_F_Vec; + local_F_Vec.clear(); + + out1.resize(x1from.size()); + + // convert the input from Deriv type to vec6 type, for the purpose of the + // matrix vector multiplication + for (unsigned int var = 0; var < in.size(); ++var) { + Vec6 vec; + for (unsigned j = 0; j < 6; j++) + vec[j] = in[var][j]; + // Convert input from global frame(SOFA) to local frame + Transform _T = + Transform(frame[var].getCenter(), frame[var].getOrientation()); + Mat6x6 P_trans = (this->buildProjector(_T)); + P_trans.transpose(); + Vec6 local_F = P_trans * vec; + local_F_Vec.push_back(local_F); + } + + // Compute output forces + auto sz = m_indicesVectors.size(); + auto index = m_indicesVectors[sz - 1]; + m_totalBeamForceVectors.clear(); + m_totalBeamForceVectors.resize(sz); + + Vec6 F_tot; + F_tot.clear(); + m_totalBeamForceVectors.push_back(F_tot); + + Mat3x6 matB_trans; + matB_trans.clear(); + for (unsigned int k = 0; k < 3; k++) + matB_trans[k][k] = 1.0; + + for (auto s = sz; s--;) { + Mat6x6 coAdjoint; + + this->computeCoAdjoint( + m_framesExponentialSE3Vectors[s], + coAdjoint); // m_framesExponentialSE3Vectors[s] computed in apply + Vec6 node_F_Vec = coAdjoint * local_F_Vec[s]; + Mat6x6 temp = + m_framesTangExpVectors[s]; // m_framesTangExpVectors[s] computed in + // applyJ (here we transpose) + temp.transpose(); + Vec3 f = matB_trans * temp * node_F_Vec; + + if (index != m_indicesVectors[s]) { + index--; + // bring F_tot to the reference of the new beam + this->computeCoAdjoint( + m_nodesExponentialSE3Vectors[index], + coAdjoint); // m_nodesExponentialSE3Vectors computed in apply + F_tot = coAdjoint * F_tot; + Mat6x6 temp = m_nodesTangExpVectors[index]; + temp.transpose(); + // apply F_tot to the new beam + Vec3 temp_f = matB_trans * temp * F_tot; + out1[index - 1] += temp_f; + } + if (d_debug.getValue()) + std::cout << "f at s =" << s << " and index" << index << " is : " << f + << std::endl; - if (dataVecOut1Force.empty() || dataVecInForce.empty() || - dataVecOut2Force.empty()) - return; + // compute F_tot + F_tot += node_F_Vec; + out1[m_indicesVectors[s] - 1] += f; + } - if (d_debug.getValue()) - std::cout << " ########## ApplyJT force Function ########" << std::endl; - const OutVecDeriv &in = dataVecInForce[0]->getValue(); - - In1VecDeriv &out1 = *dataVecOut1Force[0]->beginEdit(); - In2VecDeriv &out2 = *dataVecOut2Force[0]->beginEdit(); - const auto baseIndex = d_baseIndex.getValue(); - - const OutVecCoord &frame = - m_toModel->read(sofa::core::ConstVecCoordId::position())->getValue(); - const In1DataVecCoord *x1fromData = - m_fromModel1->read(sofa::core::ConstVecCoordId::position()); - const In1VecCoord x1from = x1fromData->getValue(); - vector local_F_Vec; - local_F_Vec.clear(); - - out1.resize(x1from.size()); - - // convert the input from Deriv type to vec6 type, for the purpose of the - // matrix vector multiplication - for (unsigned int var = 0; var < in.size(); ++var) { - Vec6 vec; - for (unsigned j = 0; j < 6; j++) - vec[j] = in[var][j]; - // Convert input from global frame(SOFA) to local frame - Transform _T = - Transform(frame[var].getCenter(), frame[var].getOrientation()); - Mat6x6 P_trans = (this->buildProjector(_T)); - P_trans.transpose(); - Vec6 local_F = P_trans * vec; - local_F_Vec.push_back(local_F); - } + Transform frame0 = Transform(frame[0].getCenter(), frame[0].getOrientation()); + Mat6x6 M = this->buildProjector(frame0); + out2[baseIndex] += M * F_tot; - // Compute output forces - auto sz = m_indicesVectors.size(); - auto index = m_indicesVectors[sz - 1]; - m_totalBeamForceVectors.clear(); - m_totalBeamForceVectors.resize(sz); + if (d_debug.getValue()) { + std::cout << "Node forces " << out1 << std::endl; + std::cout << "base Force: " << out2[baseIndex] << std::endl; + } - Vec6 F_tot; - F_tot.clear(); - m_totalBeamForceVectors.push_back(F_tot); + dataVecOut1Force[0]->endEdit(); + dataVecOut2Force[0]->endEdit(); +} - Mat3x6 matB_trans; - matB_trans.clear(); - for (unsigned int k = 0; k < 3; k++) - matB_trans[k][k] = 1.0; +template +void DiscreteCosseratMapping::applyJT( + const sofa::core::ConstraintParams * /*cparams*/, + const vector &dataMatOut1Const, + const vector &dataMatOut2Const, + const vector &dataMatInConst) { + if (dataMatOut1Const.empty() || dataMatOut2Const.empty() || + dataMatInConst.empty()) + return; + + if (this->d_componentState.getValue() != sofa::core::objectmodel::ComponentState::Valid) + return; + + if (d_debug.getValue()) + std::cout << " ########## ApplyJT Constraint Function ########" + << std::endl; + // We need only one input In model and input Root model (if present) + In1MatrixDeriv &out1 = + *dataMatOut1Const[0]->beginEdit(); // constraints on the strain space + // (reduced coordinate) + In2MatrixDeriv &out2 = + *dataMatOut2Const[0] + ->beginEdit(); // constraints on the reference frame (base frame) + const OutMatrixDeriv &in = + dataMatInConst[0] + ->getValue(); // input constraints defined on the mapped frames + + const OutVecCoord &frame = + m_toModel->read(sofa::core::ConstVecCoordId::position())->getValue(); + const In1DataVecCoord *x1fromData = + m_fromModel1->read(sofa::core::ConstVecCoordId::position()); + const In1VecCoord x1from = x1fromData->getValue(); + + Mat3x6 matB_trans; + matB_trans.clear(); + for (unsigned int k = 0; k < 3; k++) + matB_trans[k][k] = 1.0; + + vector> NodesInvolved; + vector> NodesInvolvedCompressed; + // helper::vector NodesConstraintDirection; + + typename OutMatrixDeriv::RowConstIterator rowItEnd = in.end(); + + for (typename OutMatrixDeriv::RowConstIterator rowIt = in.begin(); + rowIt != rowItEnd; ++rowIt) { + if (d_debug.getValue()) { + std::cout << "************* Apply JT (MatrixDeriv) iteration on line "; + std::cout << rowIt.index(); + std::cout << "************* " << std::endl; + } + typename OutMatrixDeriv::ColConstIterator colIt = rowIt.begin(); + typename OutMatrixDeriv::ColConstIterator colItEnd = rowIt.end(); + + // Creates a constraints if the input constraint is not empty. + if (colIt == colItEnd) { + if (d_debug.getValue()) { + std::cout << "no column for this constraint" << std::endl; + } + continue; + } + typename In1MatrixDeriv::RowIterator o1 = + out1.writeLine(rowIt.index()); // we store the constraint number + typename In2MatrixDeriv::RowIterator o2 = out2.writeLine(rowIt.index()); + + NodesInvolved.clear(); + while (colIt != colItEnd) { + int childIndex = colIt.index(); + + const OutDeriv valueConst_ = colIt.val(); + Vec6 valueConst; + for (unsigned j = 0; j < 6; j++) + valueConst[j] = valueConst_[j]; + + int indexBeam = m_indicesVectors[childIndex]; + + Transform _T = Transform(frame[childIndex].getCenter(), + frame[childIndex].getOrientation()); + Mat6x6 P_trans = (this->buildProjector(_T)); + P_trans.transpose(); + + Mat6x6 coAdjoint; + this->computeCoAdjoint( + m_framesExponentialSE3Vectors[childIndex], + coAdjoint); // m_framesExponentialSE3Vectors[s] computed in apply + Mat6x6 temp = + m_framesTangExpVectors[childIndex]; // m_framesTangExpVectors[s] + // computed in applyJ + // (here we transpose) + temp.transpose(); + + Vec6 local_F = + coAdjoint * P_trans * + valueConst; // constraint direction in local frame of the beam. + + Vec3 f = matB_trans * temp * + local_F; // constraint direction in the strain space. + + o1.addCol(indexBeam - 1, f); + std::tuple test = std::make_tuple(indexBeam, local_F); + + NodesInvolved.push_back(test); + colIt++; + } + if (d_debug.getValue()) { + std::cout << "==> NodesInvolved : " << std::endl; + for (size_t i = 0; i < NodesInvolved.size(); i++) + std::cout << "index :" << get<0>(NodesInvolved[i]) + << " force :" << get<1>(NodesInvolved[i]) << "\n "; + } - for (auto s = sz; s--;) { - Mat6x6 coAdjoint; + // sort the Nodes Invoved by decreasing order + std::sort( + begin(NodesInvolved), end(NodesInvolved), + [](std::tuple const &t1, std::tuple const &t2) { + return std::get<0>(t1) > std::get<0>(t2); // custom compare function + }); - this->computeCoAdjoint( - m_framesExponentialSE3Vectors[s], - coAdjoint); // m_framesExponentialSE3Vectors[s] computed in apply - Vec6 node_F_Vec = coAdjoint * local_F_Vec[s]; - Mat6x6 temp = - m_framesTangExpVectors[s]; // m_framesTangExpVectors[s] computed in - // applyJ (here we transpose) - temp.transpose(); - Vec3 f = matB_trans * temp * node_F_Vec; - - if (index != m_indicesVectors[s]) { - index--; - // bring F_tot to the reference of the new beam - this->computeCoAdjoint( - m_nodesExponentialSE3Vectors[index], - coAdjoint); // m_nodesExponentialSE3Vectors computed in apply - F_tot = coAdjoint * F_tot; - Mat6x6 temp = m_nodesTangExpVectors[index]; - temp.transpose(); - // apply F_tot to the new beam - Vec3 temp_f = matB_trans * temp * F_tot; - out1[index - 1] += temp_f; + NodesInvolvedCompressed.clear(); + + for (unsigned n = 0; n < NodesInvolved.size(); n++) { + std::tuple test_i = NodesInvolved[n]; + int numNode_i = std::get<0>(test_i); + Vec6 cumulativeF = std::get<1>(test_i); + + if (n < NodesInvolved.size() - 1) { + std::tuple test_i1 = NodesInvolved[n + 1]; + int numNode_i1 = std::get<0>(test_i1); + + while (numNode_i == numNode_i1) { + cumulativeF += std::get<1>(test_i1); + //// This was if ((n!=NodesInvolved.size()-2)||(n==0)) before and I + /// change it to + /// if ((n!=NodesInvolved.size()-1)||(n==0)) since the code can't + /// leave the will loop + if ((n != NodesInvolved.size() - 1) || (n == 0)) { + n++; + break; + } + test_i1 = NodesInvolved[n + 1]; + numNode_i1 = std::get<0>(test_i1); } - if (d_debug.getValue()) - std::cout << "f at s =" << s << " and index" << index << " is : " << f - << std::endl; - - // compute F_tot - F_tot += node_F_Vec; - out1[m_indicesVectors[s] - 1] += f; + } + NodesInvolvedCompressed.push_back( + std::make_tuple(numNode_i, cumulativeF)); } - Transform frame0 = Transform(frame[0].getCenter(), frame[0].getOrientation()); - Mat6x6 M = this->buildProjector(frame0); - out2[baseIndex] += M * F_tot; - if (d_debug.getValue()) { - std::cout << "Node forces " << out1 << std::endl; - std::cout << "base Force: " << out2[baseIndex] << std::endl; + std::cout << " NodesInvolved after sort and compress" << std::endl; + for (size_t i = 0; i < NodesInvolvedCompressed.size(); i++) + std::cout << "index :" << get<0>(NodesInvolvedCompressed[i]) + << " force :" << get<1>(NodesInvolvedCompressed[i]) << "\n "; } - dataVecOut1Force[0]->endEdit(); - dataVecOut2Force[0]->endEdit(); -} - -template -void DiscreteCosseratMapping::applyJT( - const sofa::core::ConstraintParams * /*cparams*/, - const vector &dataMatOut1Const, - const vector &dataMatOut2Const, - const vector &dataMatInConst) { - if (dataMatOut1Const.empty() || dataMatOut2Const.empty() || - dataMatInConst.empty()) - return; - - if (d_debug.getValue()) - std::cout << " ########## ApplyJT Constraint Function ########" - << std::endl; - // We need only one input In model and input Root model (if present) - In1MatrixDeriv &out1 = - *dataMatOut1Const[0]->beginEdit(); // constraints on the strain space - // (reduced coordinate) - In2MatrixDeriv &out2 = - *dataMatOut2Const[0] - ->beginEdit(); // constraints on the reference frame (base frame) - const OutMatrixDeriv &in = - dataMatInConst[0] - ->getValue(); // input constraints defined on the mapped frames - - const OutVecCoord &frame = - m_toModel->read(sofa::core::ConstVecCoordId::position())->getValue(); - const In1DataVecCoord *x1fromData = - m_fromModel1->read(sofa::core::ConstVecCoordId::position()); - const In1VecCoord x1from = x1fromData->getValue(); - - Mat3x6 matB_trans; - matB_trans.clear(); - for (unsigned int k = 0; k < 3; k++) - matB_trans[k][k] = 1.0; - - vector> NodesInvolved; - vector> NodesInvolvedCompressed; - // helper::vector NodesConstraintDirection; - - typename OutMatrixDeriv::RowConstIterator rowItEnd = in.end(); - - for (typename OutMatrixDeriv::RowConstIterator rowIt = in.begin(); - rowIt != rowItEnd; ++rowIt) { - if (d_debug.getValue()) { - std::cout << "************* Apply JT (MatrixDeriv) iteration on line "; - std::cout << rowIt.index(); - std::cout << "************* " << std::endl; - } - typename OutMatrixDeriv::ColConstIterator colIt = rowIt.begin(); - typename OutMatrixDeriv::ColConstIterator colItEnd = rowIt.end(); - - // Creates a constraints if the input constraint is not empty. - if (colIt == colItEnd) { - if (d_debug.getValue()) { - std::cout << "no column for this constraint" << std::endl; - } - continue; - } - typename In1MatrixDeriv::RowIterator o1 = - out1.writeLine(rowIt.index()); // we store the constraint number - typename In2MatrixDeriv::RowIterator o2 = out2.writeLine(rowIt.index()); - - NodesInvolved.clear(); - while (colIt != colItEnd) { - int childIndex = colIt.index(); - - const OutDeriv valueConst_ = colIt.val(); - Vec6 valueConst; - for (unsigned j = 0; j < 6; j++) - valueConst[j] = valueConst_[j]; - - int indexBeam = m_indicesVectors[childIndex]; - - Transform _T = Transform(frame[childIndex].getCenter(), - frame[childIndex].getOrientation()); - Mat6x6 P_trans = (this->buildProjector(_T)); - P_trans.transpose(); - - Mat6x6 coAdjoint; - this->computeCoAdjoint( - m_framesExponentialSE3Vectors[childIndex], - coAdjoint); // m_framesExponentialSE3Vectors[s] computed in apply - Mat6x6 temp = m_framesTangExpVectors[childIndex]; // m_framesTangExpVectors[s] - // computed in applyJ - // (here we transpose) - temp.transpose(); - - Vec6 local_F = - coAdjoint * P_trans * - valueConst; // constraint direction in local frame of the beam. - - Vec3 f = matB_trans * temp * - local_F; // constraint direction in the strain space. - - o1.addCol(indexBeam - 1, f); - std::tuple test = std::make_tuple(indexBeam, local_F); - - NodesInvolved.push_back(test); - colIt++; - } - if (d_debug.getValue()) { - std::cout << "==> NodesInvolved : " << std::endl; - for (size_t i = 0; i < NodesInvolved.size(); i++) - std::cout << "index :" << get<0>(NodesInvolved[i]) - << " force :" << get<1>(NodesInvolved[i]) << "\n "; - } + for (unsigned n = 0; n < NodesInvolvedCompressed.size(); n++) { + std::tuple test = NodesInvolvedCompressed[n]; + int numNode = std::get<0>(test); + int i = numNode; + Vec6 CumulativeF = std::get<1>(test); - // sort the Nodes Invoved by decreasing order - std::sort( - begin(NodesInvolved), end(NodesInvolved), - [](std::tuple const &t1, std::tuple const &t2) { - return std::get<0>(t1) > std::get<0>(t2); // custom compare function - }); + while (i > 0) { + // cumulate on beam frame + Mat6x6 coAdjoint; + this->computeCoAdjoint( + m_nodesExponentialSE3Vectors[i - 1], + coAdjoint); // m_nodesExponentialSE3Vectors computed in apply + CumulativeF = coAdjoint * CumulativeF; + // transfer to strain space (local coordinates) + Mat6x6 temp = m_nodesTangExpVectors[i - 1]; + temp.transpose(); + Vec3 temp_f = matB_trans * temp * CumulativeF; - NodesInvolvedCompressed.clear(); - - for (unsigned n = 0; n < NodesInvolved.size(); n++) { - std::tuple test_i = NodesInvolved[n]; - int numNode_i = std::get<0>(test_i); - Vec6 cumulativeF = std::get<1>(test_i); - - if (n < NodesInvolved.size() - 1) { - std::tuple test_i1 = NodesInvolved[n + 1]; - int numNode_i1 = std::get<0>(test_i1); - - while (numNode_i == numNode_i1) { - cumulativeF += std::get<1>(test_i1); - //// This was if ((n!=NodesInvolved.size()-2)||(n==0)) before and I - ///change it to - /// if ((n!=NodesInvolved.size()-1)||(n==0)) since the code can't - /// leave the will loop - if ((n != NodesInvolved.size() - 1) || (n == 0)) { - n++; - break; - } - test_i1 = NodesInvolved[n + 1]; - numNode_i1 = std::get<0>(test_i1); - } - } - NodesInvolvedCompressed.push_back( - std::make_tuple(numNode_i, cumulativeF)); - } + if (i > 1) + o1.addCol(i - 2, temp_f); + i--; + } - if (d_debug.getValue()) { - std::cout << " NodesInvolved after sort and compress" << std::endl; - for (size_t i = 0; i < NodesInvolvedCompressed.size(); i++) - std::cout << "index :" << get<0>(NodesInvolvedCompressed[i]) - << " force :" << get<1>(NodesInvolvedCompressed[i]) << "\n "; - } + Transform frame0 = + Transform(frame[0].getCenter(), frame[0].getOrientation()); + Mat6x6 M = this->buildProjector(frame0); - for (unsigned n = 0; n < NodesInvolvedCompressed.size(); n++) { - std::tuple test = NodesInvolvedCompressed[n]; - int numNode = std::get<0>(test); - int i = numNode; - Vec6 CumulativeF = std::get<1>(test); - - while (i > 0) { - // cumulate on beam frame - Mat6x6 coAdjoint; - this->computeCoAdjoint( - m_nodesExponentialSE3Vectors[i - 1], - coAdjoint); // m_nodesExponentialSE3Vectors computed in apply - CumulativeF = coAdjoint * CumulativeF; - // transfer to strain space (local coordinates) - Mat6x6 temp = m_nodesTangExpVectors[i - 1]; - temp.transpose(); - Vec3 temp_f = matB_trans * temp * CumulativeF; - - if (i > 1) - o1.addCol(i - 2, temp_f); - i--; - } - - Transform frame0 = - Transform(frame[0].getCenter(), frame[0].getOrientation()); - Mat6x6 M = this->buildProjector(frame0); - - Vec6 base_force = M * CumulativeF; - o2.addCol(d_baseIndex.getValue(), base_force); - } + Vec6 base_force = M * CumulativeF; + o2.addCol(d_baseIndex.getValue(), base_force); } + } - //"""END ARTICULATION SYSTEM MAPPING""" - dataMatOut1Const[0]->endEdit(); - dataMatOut2Const[0]->endEdit(); + //"""END ARTICULATION SYSTEM MAPPING""" + dataMatOut1Const[0]->endEdit(); + dataMatOut2Const[0]->endEdit(); } template void DiscreteCosseratMapping::computeBBox( - const sofa::core::ExecParams *, bool) -{ - const OutVecCoord &x = - m_toModel->read(sofa::core::ConstVecCoordId::position())->getValue(); - - SReal minBBox[3] = {std::numeric_limits::max(), - std::numeric_limits::max(), - std::numeric_limits::max()}; - SReal maxBBox[3] = {-std::numeric_limits::max(), - -std::numeric_limits::max(), - -std::numeric_limits::max()}; - for (std::size_t i = 0; i < x.size(); i++) { - const OutCoord &p = x[i]; - for (int c = 0; c < 3; c++) { - if (p[c] > maxBBox[c]) - maxBBox[c] = p[c]; - if (p[c] < minBBox[c]) - minBBox[c] = p[c]; - } + const sofa::core::ExecParams *, bool) { + const OutVecCoord &x = + m_toModel->read(sofa::core::ConstVecCoordId::position())->getValue(); + + SReal minBBox[3] = {std::numeric_limits::max(), + std::numeric_limits::max(), + std::numeric_limits::max()}; + SReal maxBBox[3] = {-std::numeric_limits::max(), + -std::numeric_limits::max(), + -std::numeric_limits::max()}; + for (std::size_t i = 0; i < x.size(); i++) { + const OutCoord &p = x[i]; + for (int c = 0; c < 3; c++) { + if (p[c] > maxBBox[c]) + maxBBox[c] = p[c]; + if (p[c] < minBBox[c]) + minBBox[c] = p[c]; } - this->f_bbox.setValue(sofa::type::TBoundingBox(minBBox, maxBBox)); + } + this->f_bbox.setValue(sofa::type::TBoundingBox(minBBox, maxBBox)); } template void DiscreteCosseratMapping::draw( - const sofa::core::visual::VisualParams *vparams) { - if (!vparams->displayFlags().getShowMechanicalMappings()) - return; - - // draw cable - typedef RGBAColor RGBAColor; - - const auto stateLifeCycle = vparams->drawTool()->makeStateLifeCycle(); - - const OutDataVecCoord *xfromData = - m_toModel->read(sofa::core::ConstVecCoordId::position()); - const OutVecCoord xData = xfromData->getValue(); - vector positions; - vector> Orientation; - positions.clear(); - Orientation.clear(); - unsigned int sz = xData.size(); - for (unsigned int i = 0; i < sz; i++) { - positions.push_back(xData[i].getCenter()); - Orientation.push_back(xData[i].getOrientation()); + const sofa::core::visual::VisualParams *vparams) { + if (!vparams->displayFlags().getShowMechanicalMappings()) + return; + + // draw cable + typedef RGBAColor RGBAColor; + + const auto stateLifeCycle = vparams->drawTool()->makeStateLifeCycle(); + + const OutDataVecCoord *xfromData = + m_toModel->read(sofa::core::ConstVecCoordId::position()); + const OutVecCoord xData = xfromData->getValue(); + vector positions; + vector> Orientation; + positions.clear(); + Orientation.clear(); + unsigned int sz = xData.size(); + for (unsigned int i = 0; i < sz; i++) { + positions.push_back(xData[i].getCenter()); + Orientation.push_back(xData[i].getOrientation()); + } + + // Get access articulated + const In1DataVecCoord *artiData = + m_fromModel1->read(sofa::core::ConstVecCoordId::position()); + const In1VecCoord xPos = artiData->getValue(); + + RGBAColor drawColor = d_color.getValue(); + // draw each segment of the beam as a cylinder. + for (unsigned int i = 0; i < sz - 1; i++) + vparams->drawTool()->drawCylinder(positions[i], positions[i + 1], + d_radius.getValue(), drawColor); + + // Define color map + SReal min = d_min.getValue(); + SReal max = d_max.getValue(); + sofa::helper::ColorMap::evaluator _eval = + m_colorMap.getEvaluator(min, max); + + glLineWidth(d_radius.getValue()); + glBegin(GL_LINES); + if (d_drawMapBeam.getValue()) { + sofa::type::RGBAColor _color = d_color.getValue(); + RGBAColor colorL = RGBAColor(_color[0], _color[1], _color[2], _color[3]); + glColor4f(colorL[0], colorL[1], colorL[2], colorL[3]); + for (unsigned int i = 0; i < sz - 1; i++) { + vparams->drawTool()->drawLine(positions[i], positions[i + 1], colorL); } - - // Get access articulated - const In1DataVecCoord *artiData = - m_fromModel1->read(sofa::core::ConstVecCoordId::position()); - const In1VecCoord xPos = artiData->getValue(); - - RGBAColor drawColor = d_color.getValue(); - // draw each segment of the beam as a cylinder. - for (unsigned int i = 0; i < sz - 1; i++) - vparams->drawTool()->drawCylinder(positions[i], positions[i + 1], - d_radius.getValue(), drawColor); - - // Define color map - SReal min = d_min.getValue(); - SReal max = d_max.getValue(); - sofa::helper::ColorMap::evaluator _eval = m_colorMap.getEvaluator(min, max); - - glLineWidth(d_radius.getValue()); - glBegin(GL_LINES); - if (d_drawMapBeam.getValue()) { - sofa::type::RGBAColor _color = d_color.getValue(); - RGBAColor colorL = RGBAColor(_color[0], _color[1], _color[2], _color[3]); - glColor4f(colorL[0], colorL[1], colorL[2], colorL[3]); - for (unsigned int i = 0; i < sz - 1; i++) { - vparams->drawTool()->drawLine(positions[i], positions[i + 1], colorL); - } - } else { - int j = 0; - vector index = d_index.getValue(); - for (unsigned int i = 0; i < sz - 1; i++) { - j = m_indicesVectorsDraw[i] - - 1; // to get the articulation on which the frame is related to - RGBAColor color = _eval(xPos[j][d_deformationAxis.getValue()]); - vparams->drawTool()->drawLine(positions[i], positions[i + 1], color); - } + } else { + int j = 0; + vector index = d_index.getValue(); + for (unsigned int i = 0; i < sz - 1; i++) { + j = m_indicesVectorsDraw[i] - + 1; // to get the articulation on which the frame is related to + RGBAColor color = _eval(xPos[j][d_deformationAxis.getValue()]); + vparams->drawTool()->drawLine(positions[i], positions[i + 1], color); } - glLineWidth(1); - if (!vparams->displayFlags().getShowMappings()) - if (!d_debug.getValue()) - return; - glEnd(); + } + glLineWidth(1); + if (!vparams->displayFlags().getShowMappings()) + if (!d_debug.getValue()) + return; + glEnd(); } -} // namespace sofa::component::mapping +} // namespace Cosserat::mapping From 6eee0d9827020e1c05fa84add54859fb1233ca17 Mon Sep 17 00:00:00 2001 From: Yinoussa Adagolodjo Date: Wed, 26 Jun 2024 11:41:03 +0200 Subject: [PATCH 26/26] mv the initCosserat.h.in file into config.h.in --- CMakeLists.txt | 2 +- Tests/CMakeLists.txt | 2 +- src/Cosserat/CMakeLists.txt | 2 +- src/Cosserat/{initCosserat.h.in => config.h.in} | 0 src/Cosserat/engine/ProjectionEngine.h | 2 +- src/Cosserat/forcefield/BeamHookeLawForceField.h | 2 +- src/Cosserat/forcefield/BeamHookeLawForceFieldRigid.h | 2 +- src/Cosserat/forcefield/CosseratInternalActuation.h | 2 +- src/Cosserat/forcefield/CosseratInternalActuation.inl | 2 +- src/Cosserat/forcefield/MyUniformVelocityDampingForceField.cpp | 2 +- src/Cosserat/forcefield/MyUniformVelocityDampingForceField.h | 2 +- src/Cosserat/forcefield/MyUniformVelocityDampingForceField.inl | 2 +- src/Cosserat/fwd.h | 2 +- src/Cosserat/initCosserat.cpp | 2 +- src/Cosserat/types.h | 2 +- 15 files changed, 14 insertions(+), 14 deletions(-) rename src/Cosserat/{initCosserat.h.in => config.h.in} (100%) diff --git a/CMakeLists.txt b/CMakeLists.txt index 162ee19..9a698d4 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -20,7 +20,7 @@ endif() set(SRC_ROOT_DIR src/${PROJECT_NAME}) set(HEADER_FILES - ${SRC_ROOT_DIR}/initCosserat.h.in + ${SRC_ROOT_DIR}/config.h.in ${SRC_ROOT_DIR}/fwd.h ${SRC_ROOT_DIR}/types.h ${SRC_ROOT_DIR}/mapping/BaseCosseratMapping.h diff --git a/Tests/CMakeLists.txt b/Tests/CMakeLists.txt index dd12b2f..d15db72 100644 --- a/Tests/CMakeLists.txt +++ b/Tests/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 3.1) +cmake_minimum_required(VERSION 3.12) set(This Cosserat_test) diff --git a/src/Cosserat/CMakeLists.txt b/src/Cosserat/CMakeLists.txt index 425aadd..fb8ef70 100644 --- a/src/Cosserat/CMakeLists.txt +++ b/src/Cosserat/CMakeLists.txt @@ -1,7 +1,7 @@ project(Cosserat.src VERSION 21.06.99) set(HEADER_FILES - initCosserat.h + config.h mapping/BaseCosserat.h mapping/BaseCosserat.inl diff --git a/src/Cosserat/initCosserat.h.in b/src/Cosserat/config.h.in similarity index 100% rename from src/Cosserat/initCosserat.h.in rename to src/Cosserat/config.h.in diff --git a/src/Cosserat/engine/ProjectionEngine.h b/src/Cosserat/engine/ProjectionEngine.h index 86e2ef3..a566242 100644 --- a/src/Cosserat/engine/ProjectionEngine.h +++ b/src/Cosserat/engine/ProjectionEngine.h @@ -22,7 +22,7 @@ #ifndef COSSERAT_ProjectionEngine_H #define COSSERAT_ProjectionEngine_H -#include +#include #include #include #include diff --git a/src/Cosserat/forcefield/BeamHookeLawForceField.h b/src/Cosserat/forcefield/BeamHookeLawForceField.h index 2c2f4e2..98163b2 100644 --- a/src/Cosserat/forcefield/BeamHookeLawForceField.h +++ b/src/Cosserat/forcefield/BeamHookeLawForceField.h @@ -28,7 +28,7 @@ * * ******************************************************************************/ #pragma once -#include +#include #include #include diff --git a/src/Cosserat/forcefield/BeamHookeLawForceFieldRigid.h b/src/Cosserat/forcefield/BeamHookeLawForceFieldRigid.h index 35b7b39..009abc4 100644 --- a/src/Cosserat/forcefield/BeamHookeLawForceFieldRigid.h +++ b/src/Cosserat/forcefield/BeamHookeLawForceFieldRigid.h @@ -28,7 +28,7 @@ * * ******************************************************************************/ #pragma once -#include +#include #include #include diff --git a/src/Cosserat/forcefield/CosseratInternalActuation.h b/src/Cosserat/forcefield/CosseratInternalActuation.h index bc64d3c..0e8e21b 100644 --- a/src/Cosserat/forcefield/CosseratInternalActuation.h +++ b/src/Cosserat/forcefield/CosseratInternalActuation.h @@ -28,7 +28,7 @@ * * ******************************************************************************/ #pragma once -#include +#include #include #include diff --git a/src/Cosserat/forcefield/CosseratInternalActuation.inl b/src/Cosserat/forcefield/CosseratInternalActuation.inl index a3fe434..eaffb92 100644 --- a/src/Cosserat/forcefield/CosseratInternalActuation.inl +++ b/src/Cosserat/forcefield/CosseratInternalActuation.inl @@ -28,7 +28,7 @@ * * ******************************************************************************/ #pragma once -#include +#include #include #include diff --git a/src/Cosserat/forcefield/MyUniformVelocityDampingForceField.cpp b/src/Cosserat/forcefield/MyUniformVelocityDampingForceField.cpp index 9ffb874..2f1d6d5 100644 --- a/src/Cosserat/forcefield/MyUniformVelocityDampingForceField.cpp +++ b/src/Cosserat/forcefield/MyUniformVelocityDampingForceField.cpp @@ -21,7 +21,7 @@ ******************************************************************************/ #define SOFA_COMPONENT_FORCEFIELD_UNIFORMVELOCITYDAMPINGFORCEFIELD_CPP -#include +#include #include #include #include diff --git a/src/Cosserat/forcefield/MyUniformVelocityDampingForceField.h b/src/Cosserat/forcefield/MyUniformVelocityDampingForceField.h index 48be188..f72318d 100644 --- a/src/Cosserat/forcefield/MyUniformVelocityDampingForceField.h +++ b/src/Cosserat/forcefield/MyUniformVelocityDampingForceField.h @@ -20,7 +20,7 @@ * Contact information: contact@sofa-framework.org * ******************************************************************************/ #pragma once -#include +#include #include diff --git a/src/Cosserat/forcefield/MyUniformVelocityDampingForceField.inl b/src/Cosserat/forcefield/MyUniformVelocityDampingForceField.inl index c5b4b89..ddadc22 100644 --- a/src/Cosserat/forcefield/MyUniformVelocityDampingForceField.inl +++ b/src/Cosserat/forcefield/MyUniformVelocityDampingForceField.inl @@ -20,7 +20,7 @@ * Contact information: contact@sofa-framework.org * ******************************************************************************/ #pragma once -#include +#include #include #include "MyUniformVelocityDampingForceField.h" #include diff --git a/src/Cosserat/fwd.h b/src/Cosserat/fwd.h index b800535..d50cb39 100644 --- a/src/Cosserat/fwd.h +++ b/src/Cosserat/fwd.h @@ -29,7 +29,7 @@ ******************************************************************************/ #pragma once -#include +#include #include namespace Cosserat { diff --git a/src/Cosserat/initCosserat.cpp b/src/Cosserat/initCosserat.cpp index d127d9f..63f8b6f 100644 --- a/src/Cosserat/initCosserat.cpp +++ b/src/Cosserat/initCosserat.cpp @@ -24,7 +24,7 @@ * * * Contact information: contact@sofa-framework.org * ******************************************************************************/ -#include +#include #include #include diff --git a/src/Cosserat/types.h b/src/Cosserat/types.h index 0c016cd..ce6d278 100644 --- a/src/Cosserat/types.h +++ b/src/Cosserat/types.h @@ -29,7 +29,7 @@ ******************************************************************************/ #pragma once -#include +#include #include #include #include