1#define BIORBD_API_EXPORTS
2#include "InternalForces/WrappingHalfCylinder.h"
4#include "RigidBody/Joints.h"
5#include "Utils/Matrix2d.h"
6#include "Utils/RotoTrans.h"
7#include "Utils/String.h"
8#include "Utils/Vector2d.h"
10#ifdef USE_SMOOTH_IF_ELSE
11#include "Utils/CasadiExpand.h"
14using namespace BIORBD_NAMESPACE;
18 m_radius(std::make_shared<utils::Scalar>(0)),
19 m_length(std::make_shared<utils::Scalar>(0)),
24 *
m_typeOfNode = utils::NODE_TYPE::WRAPPING_HALF_CYLINDER;
57 const utils::Scalar&
radius,
58 const utils::Scalar&
length)
66 *
m_typeOfNode = utils::NODE_TYPE::WRAPPING_HALF_CYLINDER;
71 const utils::Scalar&
radius,
72 const utils::Scalar&
length,
82 *
m_typeOfNode = utils::NODE_TYPE::WRAPPING_HALF_CYLINDER;
134 p1_tan = *p_glob.
m_p1 + vec;
135 p2_tan = p1_tan + vec;
146 tanPoints.
m_p1->applyRT(rt);
147 tanPoints.
m_p2->applyRT(rt);
150 p1 = *tanPoints.
m_p1;
151 p2 = *tanPoints.
m_p2;
196 const utils::Scalar& val) {
209 const utils::Scalar& val) {
221#ifdef BIORBD_USE_EIGEN3_MATH
222 utils::Scalar p_dot =
223 static_cast<Eigen::Vector2d
>(p.block(0, 0, 2, 1))
224 .dot(
static_cast<Eigen::Vector2d
>(p.block(0, 0, 2, 1)));
226 utils::Scalar p_dot = p.block(0, 0, 2, 1).dot(p.block(0, 0, 2, 1));
236 p.block(0, 0, 2, 1));
240 m.
m_p1->block(0, 0, 2, 1) = Q0 + T;
241 m.
m_p2->block(0, 0, 2, 1) = Q0 - T;
250#ifdef BIORBD_USE_CASADI_MATH
251 p_tan = IF_ELSE_NAMESPACE::if_else(
254 if ((*p1.
m_p2)(0) >= (*p1.
m_p1)(0)) {
265#ifdef BIORBD_USE_CASADI_MATH
271 for (
size_t i = 0; i < 3; ++i) {
272 (*pointsToWrap.
m_p1)(i) =
273 static_cast<utils::Scalar
>(
static_cast<double>(NAN));
274 (*pointsToWrap.
m_p2)(i) =
275 static_cast<utils::Scalar
>(
static_cast<double>(NAN));
281 (*pointsToWrap.
m_p1)(2) = 0;
282 (*pointsToWrap.
m_p2)(2) = 0;
331 (*pointsToWrap.
m_p1)(2) =
332 utils::Scalar(wrapA(0) - globB(0)) / utils::Scalar(globA(0) - globB(0)) *
333 ((*pointsInGlobal.
m_p1)(2) - (*pointsInGlobal.
m_p2)(2)) +
334 (*pointsInGlobal.
m_p2)(2);
335 (*pointsToWrap.
m_p2)(2) =
336 utils::Scalar(wrapB(0) - globB(0)) / utils::Scalar(globA(0) - globB(0)) *
337 ((*pointsInGlobal.
m_p1)(2) - (*pointsInGlobal.
m_p2)(2)) +
338 (*pointsInGlobal.
m_p2)(2);
343#ifndef BIORBD_USE_CASADI_MATH
359 if (((*pointsInGlobal.
m_p1)(1) > 0 && (*pointsInGlobal.
m_p2)(1) > 0) ||
360 ((*pointsInGlobal.
m_p1)(1) < 0 && (*pointsInGlobal.
m_p2)(1) < 0)) {
366 if (fabs((*pointsInGlobal.
m_p1)(1)) <
radius() ||
374 if (((*pointsToWrap.
m_p1)(0) < (*pointsToWrap.
m_p2)(0) &&
375 (*pointsInGlobal.
m_p1)(0) > (*pointsInGlobal.
m_p2)(0)) ||
376 ((*pointsToWrap.
m_p1)(0) > (*pointsToWrap.
m_p2)(0) &&
377 (*pointsInGlobal.
m_p1)(0) < (*pointsInGlobal.
m_p2)(0))) {
Pair of 2 muscles points.
std::shared_ptr< utils::Vector3d > m_p1
Point 1.
std::shared_ptr< utils::Vector3d > m_p2
Point 2.
Half cylinder of infinite length (the length only affect the graphical representation) object that ma...
bool findVerticalNode(const NodeMusclePair &pointsInGlobal, NodeMusclePair &pointsToWrap) const
Find the height of both points.
std::shared_ptr< utils::Scalar > m_length
Length of the half cylinder.
void wrapPoints(const utils::RotoTrans &rt, const utils::Vector3d &p1_bone, const utils::Vector3d &p2_bone, utils::Vector3d &p1, utils::Vector3d &p2, utils::Scalar *length=nullptr)
From the position of the half cylinder, return the 2 locations where the muscle leaves the wrapping o...
virtual const utils::RotoTrans & RT(rigidbody::Joints &model, const rigidbody::GeneralizedCoordinates &Q, bool updateKin=true)
Return the RotoTrans matrix of the half cylinder.
void setRadius(const utils::Scalar &val)
Set the diameter of the wrapping half cylinder.
utils::Scalar radius() const
Return the radius of the half cylinder.
WrappingHalfCylinder()
Construct a wrapping half cylinder.
std::shared_ptr< utils::Vector3d > m_p2Wrap
Second point of contact with the wrap.
void setLength(const utils::Scalar &val)
Set the length of the half cylinder.
utils::Scalar diameter() const
Return the diamter of the half cylinder.
WrappingHalfCylinder DeepCopy() const
Deep copy of the wrapping half cylinder.
const utils::Scalar & length() const
Return the length of the half cylinder.
bool checkIfWraps(const NodeMusclePair &pointsInGlobal, NodeMusclePair &pointsToWrap) const
Check if a wrapper has to be done.
std::shared_ptr< utils::Scalar > m_lengthAroundWrap
Length between p1 and p2.
std::shared_ptr< utils::Vector3d > m_p1Wrap
First point of contact with the wrap.
std::shared_ptr< utils::RotoTrans > m_RTtoParent
RotoTrans matrix with the parent.
std::shared_ptr< utils::Scalar > m_radius
Diameter of the half cylinder diametre du cylindre.
void findTangentToCircle(const utils::Vector3d &p, utils::Vector3d &p_tan) const
Find the two tangents of a point with a circle.
void selectTangents(const NodeMusclePair &p, utils::Vector3d &p_tan) const
Select between a set of nodes which ones to keep.
utils::Scalar computeLength(const NodeMusclePair &p) const
Compute the muscle length on the half cylinder.
WrappingObject()
Construct a wrapping object.
std::shared_ptr< utils::RotoTrans > m_RT
RotoTrans matrix of the wrapping object.
const utils::RotoTrans & RT() const
Return the RotoTrans matrix of the wrapping object.
Class GeneralizedCoordinates.
This is the core of the musculoskeletal model in biorbd.
utils::RotoTrans globalJCS(const GeneralizedCoordinates &Q, const utils::String &name, bool updateKin=true)
Return the joint coordinate system (JCS) for the segment in global reference frame at a given Q.
A wrapper for the Eigen::MatrixXd.
const String & name() const
Return the name of the node.
std::shared_ptr< NODE_TYPE > m_typeOfNode
The type of the node.
std::shared_ptr< String > m_parentName
The parent name of the node.
Homogenous matrix to describe translations and rotations simultaneously.
RotoTrans transpose() const
Return the tranposed matrix.
Wrapper around the std::string class with augmented functionality.
Wrapper around Eigen Vector2d and attach it to a parent.
Wrapper around Eigen Vector3d and attach it to a parent.
Vector3d()
Construct 3D vector.
Vector3d applyRT(const RotoTrans &rt) const
Apply a RotoTrans to the 3D vector.
Vector3d DeepCopy() const
Deep copy of a 3D vector.