Skip to content

Commit

Permalink
Update the Cosserat mapping names to follow a consistent naming
Browse files Browse the repository at this point in the history
  • Loading branch information
@adagolodjo
adagolodjo committed Jun 9, 2024
1 parent 3a9a50c commit 5042087
Show file tree
Hide file tree
Showing 18 changed files with 2,680 additions and 2,597 deletions.
2 changes: 1 addition & 1 deletion src/Cosserat/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -40,8 +40,8 @@ set(SOURCE_FILES
initCosserat.cpp

mapping/BaseCosserat.cpp
mapping/DiscreteCosseratMapping.cpp
mapping/ProjectionEngine.cpp
mapping/DiscreteCosseratMapping.cpp
mapping/DiscreteDynamicCosseratMapping.cpp
mapping/DifferenceMultiMapping.cpp
mapping/RigidDistanceMapping.cpp
Expand Down
15 changes: 4 additions & 11 deletions src/Cosserat/initCosserat.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -35,7 +35,7 @@ using sofa::helper::system::PluginManager;

// TODO(dmarchal: 2024/06/07) This is the wrong namespace, it should be
// "cosserat" or sofacosserat if you rename the plugin into SofaCosserat :)
namespace cosserat {
namespace Cosserat {

extern "C" {
SOFA_COSSERAT_API void initExternalModule();
Expand Down Expand Up @@ -65,19 +65,12 @@ void initExternalModule() {
}
const char *getModuleLicense() { return "LGPL"; }

const char *getModuleName() { return cosserat::MODULE_NAME; }
const char *getModuleName() { return Cosserat::MODULE_NAME; }

const char *getModuleVersion() { return cosserat::MODULE_VERSION; }
const char *getModuleVersion() { return Cosserat::MODULE_VERSION; }

const char *getModuleDescription() {
static std::string classes =
sofa::core::ObjectFactory::getInstance()->listClassesFromTarget(
cosserat::MODULE_NAME);
// TODO(dmarchal: 2024/06/07): I think this is not the correct module
// description... as we are planning to use these string to generate the
// documentation... please correct that and also be much more descriptive with
// multi line comment.
return classes.c_str();
return "This plugin is used to implement slender object";
}

const char *getModuleComponentList() {
Expand Down
6 changes: 3 additions & 3 deletions src/Cosserat/initCosserat.h.in
View file
Original file line number Diff line number Diff line change
Expand Up @@ -35,7 +35,7 @@
#define COSSERAT_VERSION @PROJECT_VERSION @

#ifdef SOFA_BUILD_COSSERAT
#define SOFA_TARGET @PROJECT_NAME @
#define SOFA_TARGET Cosserat
#define SOFA_COSSERAT_API SOFA_EXPORT_DYNAMIC_LIBRARY
#else
#define SOFA_COSSERAT_API SOFA_IMPORT_DYNAMIC_LIBRARY
Expand All @@ -45,10 +45,10 @@
This is the plugin for the Discret Cosserat Plugin
*/

namespace cosserat {
namespace Cosserat {
SOFA_COSSERAT_API void init();
constexpr const char *MODULE_NAME = "@PROJECT_NAME@";
constexpr const char *MODULE_VERSION = "@PROJECT_VERSION@";
} // namespace cosserat
} // namespace Cosserat

#endif /* SOFA_COSSERAT_INIT_H */
237 changes: 133 additions & 104 deletions src/Cosserat/mapping/BaseCosserat.cpp
View file
Original file line number Diff line number Diff line change
@@ -1,137 +1,166 @@
/******************************************************************************
* SOFA, Simulation Open-Framework Architecture, development version *
* (c) 2006-2019 INRIA, USTL, UJF, CNRS, MGH *
* *
* This program is free software; you can redistribute it and/or modify it *
* under the terms of the GNU Lesser General Public License as published by *
* the Free Software Foundation; either version 2.1 of the License, or (at *
* your option) any later version. *
* *
* This program is distributed in the hope that it will be useful, but WITHOUT *
* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or *
* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License *
* for more details. *
* *
* You should have received a copy of the GNU Lesser General Public License *
* along with this program. If not, see <http://www.gnu.org/licenses/>. *
*******************************************************************************
* Authors: The SOFA Team and external contributors (see Authors.txt) *
* *
* Contact information: contact@sofa-framework.org *
******************************************************************************/
* SOFA, Simulation Open-Framework Architecture, development version *
* (c) 2006-2019 INRIA, USTL, UJF, CNRS, MGH *
* *
* This program is free software; you can redistribute it and/or modify it *
* under the terms of the GNU Lesser General Public License as published by *
* the Free Software Foundation; either version 2.1 of the License, or (at *
* your option) any later version. *
* *
* This program is distributed in the hope that it will be useful, but WITHOUT *
* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or *
* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License *
* for more details. *
* *
* You should have received a copy of the GNU Lesser General Public License *
* along with this program. If not, see <http://www.gnu.org/licenses/>. *
*******************************************************************************
* Authors: The SOFA Team and external contributors (see Authors.txt) *
* *
* Contact information: contact@sofa-framework.org *
******************************************************************************/
#define SOFA_COSSERAT_CPP_BaseCosserat
#include "BaseCosserat.inl"
#include <Cosserat/mapping/BaseCosserat.inl>

#include <sofa/defaulttype/VecTypes.h>
#include <sofa/defaulttype/RigidTypes.h>
#include <sofa/core/ObjectFactory.h>
#include <sofa/defaulttype/RigidTypes.h>
#include <sofa/defaulttype/VecTypes.h>

namespace sofa::component::mapping
{
namespace Cosserat::mapping {
using namespace sofa::defaulttype;

template<>
BaseCosserat<Vec6Types, Rigid3Types, Rigid3Types>::se3 BaseCosserat<Vec6Types, Rigid3Types, Rigid3Types>::buildXiHat(const Coord1& strain_i){
se3 Xi;
template <>
BaseCosserat<Vec6Types, Rigid3Types, Rigid3Types>::se3
BaseCosserat<Vec6Types, Rigid3Types, Rigid3Types>::buildXiHat(
const Coord1 &strain_i) {
se3 Xi;

Xi[0][1] = -strain_i(2);
Xi[0][2] = strain_i[1];
Xi[1][2] = -strain_i[0];
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[1][0] = -Xi(0, 1);
Xi[2][0] = -Xi(0, 2);
Xi[2][1] = -Xi(1, 2);

Xi[0][3] = 1.0;
Xi[0][3] = 1.0;

for (unsigned int i=0; i<3; i++)
Xi[i][3] += strain_i(i+3);
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];
// 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;
return Xi;
}

template <>
void BaseCosserat<Vec6Types, Rigid3Types, Rigid3Types>::computeExponentialSE3(
const double &curv_abs_x_n, const Coord1 &strain_n, Transform &Trans) {
Matrix4 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<double>::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;

template<>
void BaseCosserat<Vec6Types, Rigid3Types, Rigid3Types>::computeExponentialSE3(const double & curv_abs_x_n, const Coord1& strain_n, Transform & Trans){
Matrix4 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<double>::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;

type::Mat3x3 M;
g_X_n.getsub(0,0,M); //get the rotation matrix
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<Real> R ; R.fromMatrix(M);
Trans = Transform(type::Vec3(g_X_n(0,3),g_X_n(1,3),g_X_n(2,3)),R);
// convert the rotation 3x3 matrix to a quaternion
sofa::type::Quat<Real> 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 BaseCosserat<Vec6Types, Rigid3Types, Rigid3Types>::computeTangExp(double & curv_abs_n, const Coord1 & strain_i, Mat6x6 & TgX){
template <>
void BaseCosserat<Vec6Types, Rigid3Types, Rigid3Types>::computeTangExp(
double &curv_abs_n, const Coord1 &strain_i, Mat6x6 &TgX) {

SReal theta = type::Vec3(strain_i(0), strain_i(1), strain_i(2)).norm(); //Sometimes this is computed over all strain
Matrix3 tilde_k = getTildeMatrix(type::Vec3(strain_i(0), strain_i(1), strain_i(2)));
SReal theta = Vec3(strain_i(0), strain_i(1), strain_i(2))
.norm(); // Sometimes this is computed over all strain
Matrix3 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
@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 ?
Matrix3 tilde_q = getTildeMatrix(type::Vec3(strain_i(3), strain_i(4), strain_i(5)));
// TODO(dmarchal: 2024/06/07) could the debt by solved ?
Matrix3 tilde_q =
getTildeMatrix(Vec3(strain_i(3), strain_i(4), strain_i(5)));

Mat6x6 ad_Xi ;
Mat6x6 ad_Xi;
buildAdjoint(tilde_k, tilde_q, ad_Xi);

Mat6x6 Id6 = Mat6x6::Identity() ;

if(theta <= std::numeric_limits<double>::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 ;
Mat6x6 Id6 = Mat6x6::Identity();

if (theta <= std::numeric_limits<double>::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;
}
}


// Register in the Factory
int BaseCosseratClass = core::RegisterObject("Set the positions and velocities of points attached to a rigid parent")
.add< BaseCosserat< sofa::defaulttype::Vec3Types, sofa::defaulttype::Rigid3Types, sofa::defaulttype::Rigid3Types > >()
.add< BaseCosserat< sofa::defaulttype::Vec6Types, sofa::defaulttype::Rigid3Types, sofa::defaulttype::Rigid3Types > >()
;


template class SOFA_COSSERAT_API BaseCosserat< sofa::defaulttype::Vec3Types, sofa::defaulttype::Rigid3Types, sofa::defaulttype::Rigid3Types >;
template class SOFA_COSSERAT_API BaseCosserat< sofa::defaulttype::Vec6Types, sofa::defaulttype::Rigid3Types, sofa::defaulttype::Rigid3Types >;

} // namespace sofa.
int BaseCosseratClass =
sofa::core::RegisterObject(
"Set the positions and velocities of points attached to a rigid parent")
.add<BaseCosserat<sofa::defaulttype::Vec3Types,
sofa::defaulttype::Rigid3Types,
sofa::defaulttype::Rigid3Types>>()
.add<BaseCosserat<sofa::defaulttype::Vec6Types,
sofa::defaulttype::Rigid3Types,
sofa::defaulttype::Rigid3Types>>();

template class SOFA_COSSERAT_API
BaseCosserat<sofa::defaulttype::Vec3Types, sofa::defaulttype::Rigid3Types,
sofa::defaulttype::Rigid3Types>;
template class SOFA_COSSERAT_API
BaseCosserat<sofa::defaulttype::Vec6Types, sofa::defaulttype::Rigid3Types,
sofa::defaulttype::Rigid3Types>;

} // namespace cosserat::mapping
Loading

0 comments on commit 5042087

Please sign in to comment.