Biorbd Release_1.13.0
Loading...
Searching...
No Matches
WrappingHalfCylinder.cpp
1#define BIORBD_API_EXPORTS
2#include "InternalForces/WrappingHalfCylinder.h"
3
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"
9
10#ifdef USE_SMOOTH_IF_ELSE
11#include "Utils/CasadiExpand.h"
12#endif
13
14using namespace BIORBD_NAMESPACE;
15
17 : internal_forces::WrappingObject(),
18 m_radius(std::make_shared<utils::Scalar>(0)),
19 m_length(std::make_shared<utils::Scalar>(0)),
20 m_RTtoParent(std::make_shared<utils::RotoTrans>()),
21 m_p1Wrap(std::make_shared<utils::Vector3d>()),
22 m_p2Wrap(std::make_shared<utils::Vector3d>()),
23 m_lengthAroundWrap(std::make_shared<utils::Scalar>(0)) {
24 *m_typeOfNode = utils::NODE_TYPE::WRAPPING_HALF_CYLINDER;
25}
26
28 const utils::Vector3d& other)
29 : internal_forces::WrappingObject(other) {
32 dynamic_cast<const internal_forces::WrappingHalfCylinder&>(other)));
33 m_radius = otherWrap.m_radius;
34 m_length = otherWrap.m_length;
35 m_RTtoParent = otherWrap.m_RTtoParent;
36 m_p1Wrap = otherWrap.m_p1Wrap;
37 m_p2Wrap = otherWrap.m_p2Wrap;
39}
40
42 const utils::Vector3d* other)
43 : internal_forces::WrappingObject(*other) {
46 dynamic_cast<const internal_forces::WrappingHalfCylinder*>(other)));
47 m_radius = otherWrap->m_radius;
48 m_length = otherWrap->m_length;
49 m_RTtoParent = otherWrap->m_RTtoParent;
50 m_p1Wrap = otherWrap->m_p1Wrap;
51 m_p2Wrap = otherWrap->m_p2Wrap;
53}
54
56 const utils::RotoTrans& rt,
57 const utils::Scalar& radius,
58 const utils::Scalar& length)
59 : internal_forces::WrappingObject(rt.trans()),
60 m_radius(std::make_shared<utils::Scalar>(radius)),
61 m_length(std::make_shared<utils::Scalar>(length)),
62 m_RTtoParent(std::make_shared<utils::RotoTrans>(rt)),
63 m_p1Wrap(std::make_shared<utils::Vector3d>()),
64 m_p2Wrap(std::make_shared<utils::Vector3d>()),
65 m_lengthAroundWrap(std::make_shared<utils::Scalar>(0)) {
66 *m_typeOfNode = utils::NODE_TYPE::WRAPPING_HALF_CYLINDER;
67}
68
70 const utils::RotoTrans& rt,
71 const utils::Scalar& radius,
72 const utils::Scalar& length,
73 const utils::String& name,
74 const utils::String& parentName)
75 : internal_forces::WrappingObject(rt.trans(), name, parentName),
76 m_radius(std::make_shared<utils::Scalar>(radius)),
77 m_length(std::make_shared<utils::Scalar>(length)),
78 m_RTtoParent(std::make_shared<utils::RotoTrans>(rt)),
79 m_p1Wrap(std::make_shared<utils::Vector3d>()),
80 m_p2Wrap(std::make_shared<utils::Vector3d>()),
81 m_lengthAroundWrap(std::make_shared<utils::Scalar>(0)) {
82 *m_typeOfNode = utils::NODE_TYPE::WRAPPING_HALF_CYLINDER;
83}
84
91
102
104 const utils::RotoTrans& rt,
105 const utils::Vector3d& p1_bone,
106 const utils::Vector3d& p2_bone,
107 utils::Vector3d& p1,
108 utils::Vector3d& p2,
109 utils::Scalar* length) {
110 // This function takes the position of the wrapping and finds the location
111 // where muscle 1 and 2 leave the wrapping object
112
113 // Find the nodes in the RT reference (of the cylinder)
114 NodeMusclePair p_glob(p1_bone, p2_bone);
115 p_glob.m_p1->applyRT(rt.transpose());
116 p_glob.m_p2->applyRT(rt.transpose());
117
118 // Find the tangents of these points to the circle (cylinder seen from above)
119 utils::Vector3d p1_tan(0, 0, 0);
120 utils::Vector3d p2_tan(0, 0, 0);
121 findTangentToCircle(*p_glob.m_p1, p1_tan);
122 findTangentToCircle(*p_glob.m_p2, p2_tan);
123
124 // Find the vertical component
125 NodeMusclePair tanPoints(p1_tan, p2_tan);
126
127 // if the wrap is not supposed to happen
128 // if there is a straight line in between two points not passing throught the
129 // cylinder
130 if (!findVerticalNode(p_glob, tanPoints)) {
131 // add the two wrapping points on that streight line
132 // each one at one third of length
133 Vector3d vec((*p_glob.m_p2 - *p_glob.m_p1) / 3);
134 p1_tan = *p_glob.m_p1 + vec;
135 p2_tan = p1_tan + vec;
136 tanPoints = NodeMusclePair(p1_tan, p2_tan);
137 }
138
139 // If asked, compute the distance distance traveled on the periphery of the
140 // cylinder Apply pythagorus to the cercle arc
141 if (length != nullptr) { // If it is not nullptr
142 *length = computeLength(tanPoints);
143 }
144
145 // Reset the points in global (space)
146 tanPoints.m_p1->applyRT(rt);
147 tanPoints.m_p2->applyRT(rt);
148
149 // Reset the desired values
150 p1 = *tanPoints.m_p1;
151 p2 = *tanPoints.m_p2;
152
153 // Store the values for a futur call
154 m_p1Wrap = tanPoints.m_p1;
155 m_p2Wrap = tanPoints.m_p2;
156 if (length != nullptr) { // If it is not nullptr
158 }
159}
160
162 rigidbody::Joints& model,
164 const utils::Vector3d& p1_bone,
165 const utils::Vector3d& p2_bone,
166 utils::Vector3d& p1,
167 utils::Vector3d& p2,
168 utils::Scalar* length) {
169 // This function takes a model and a position of the model and returns the
170 // location where muscle 1 et 2 leave the wrapping object
171
172 wrapPoints(RT(model, Q), p1_bone, p2_bone, p1, p2, length);
173}
174
176 utils::Vector3d& p1,
177 utils::Vector3d& p2,
178 utils::Scalar* length) {
179 p1 = *m_p1Wrap;
180 p2 = *m_p2Wrap;
181 if (length != nullptr) {
183 }
184}
185
187 rigidbody::Joints& model,
189 bool updateKin) {
190 // Get the RotoTrans matrix of the cylinder in space
191 *m_RT = model.globalJCS(Q, *m_parentName, updateKin) * *m_RTtoParent;
192 return *m_RT;
193}
194
196 const utils::Scalar& val) {
197 *m_radius = val;
198}
199
201 return *m_radius;
202}
203
205 return 2 * *m_radius;
206}
207
209 const utils::Scalar& val) {
210 *m_length = val;
211}
212
214 return *m_length;
215}
216
218 const utils::Vector3d& p,
219 utils::Vector3d& p_tan) const {
220 // This function ignores the Z axis of the vector p to create the circle
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)));
225#else
226 utils::Scalar p_dot = p.block(0, 0, 2, 1).dot(p.block(0, 0, 2, 1));
227#endif
228
229 const utils::Vector2d& Q0(radius() * radius() / p_dot * p.block(0, 0, 2, 1));
230 utils::Matrix2d tp(utils::Matrix2d::Zero());
231 tp(0, 1) = -1;
232 tp(1, 0) = 1;
233
234 const utils::Vector2d& T(
235 radius() / p_dot * std::sqrt(p_dot - radius() * radius()) * tp *
236 p.block(0, 0, 2, 1));
237
238 // GEt the tangent on both sides
239 NodeMusclePair m(p, p);
240 m.m_p1->block(0, 0, 2, 1) = Q0 + T;
241 m.m_p2->block(0, 0, 2, 1) = Q0 - T;
242
243 // Select on of the two tangents
244 selectTangents(m, p_tan);
245}
246
248 const NodeMusclePair& p1,
249 utils::Vector3d& p_tan) const {
250#ifdef BIORBD_USE_CASADI_MATH
251 p_tan = IF_ELSE_NAMESPACE::if_else(
252 IF_ELSE_NAMESPACE::ge((*p1.m_p2)(0), (*p1.m_p1)(0)), *p1.m_p2, *p1.m_p1);
253#else
254 if ((*p1.m_p2)(0) >= (*p1.m_p1)(0)) {
255 p_tan = *p1.m_p2;
256 } else {
257 p_tan = *p1.m_p1;
258 }
259#endif
260}
262 const NodeMusclePair& pointsInGlobal,
263 NodeMusclePair& pointsToWrap) const {
264 // Before everything, make sure the point wrap
265#ifdef BIORBD_USE_CASADI_MATH
266 // In CASADI, we have to assume it does...
267#else
268 if (!checkIfWraps(
269 pointsInGlobal,
270 pointsToWrap)) { // If it doesn't pass by the wrap, put NaN and stop
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));
276 }
277 return false;
278 } else {
279 // Make sure the z component won't cause any problem in the rotation
280 // computation
281 (*pointsToWrap.m_p1)(2) = 0;
282 (*pointsToWrap.m_p2)(2) = 0;
283 }
284#endif
285
286 // Strategy : Find the matrix of the passage between the aligned points in x
287 // and the cylinder. Find the location where the points cross the cylinder
288
289 // X is the straight line between the two points
290 utils::Vector3d X(*pointsInGlobal.m_p2 - *pointsInGlobal.m_p1);
291 // Z is the empty axis of the cylinder
292 utils::Vector3d Z(0, 0, 1);
293
294 utils::Vector3d Y(Z.cross(X));
295 // Re-compute X for it to be aligned with the cylinder
296 X = Y.cross(Z);
297 // Normalise everything
298 X = X / X.norm();
299 Y = Y / Y.norm();
300 Z = Z / Z.norm();
301 // Concatenate to get the rotation matrix
303 X(0),
304 X(1),
305 X(2),
306 0,
307 Y(0),
308 Y(1),
309 Y(2),
310 0,
311 Z(0),
312 Z(1),
313 Z(2),
314 0,
315 0,
316 0,
317 0,
318 1);
319
320 // Turn the points in the R reference
321 utils::Vector3d globA(*pointsInGlobal.m_p1);
322 utils::Vector3d globB(*pointsInGlobal.m_p2);
323 utils::Vector3d wrapA(*pointsToWrap.m_p1);
324 utils::Vector3d wrapB(*pointsToWrap.m_p2);
325 globA.applyRT(R);
326 globB.applyRT(R);
327 wrapA.applyRT(R);
328 wrapB.applyRT(R);
329
330 // The height depends on the relative distance
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);
339
340 return true;
341}
342
343#ifndef BIORBD_USE_CASADI_MATH
345 const NodeMusclePair& pointsInGlobal,
346 NodeMusclePair& pointsToWrap) const {
347 // It seems that all this function is ignored up to the last check... Once
348 // it is checked, validate the Casadi implementation
349
350 // First quick tests
351 // if both points are on the left and we have to go left
352 if ((*pointsInGlobal.m_p1)(0) > radius() &&
353 (*pointsInGlobal.m_p2)(0) > radius()) {
354 return false;
355 }
356
357 // If we are on top of the wrap, it is impossible to determine because the
358 // wrap is not a cylinder but a half-cylinder
359 if (((*pointsInGlobal.m_p1)(1) > 0 && (*pointsInGlobal.m_p2)(1) > 0) ||
360 ((*pointsInGlobal.m_p1)(1) < 0 && (*pointsInGlobal.m_p2)(1) < 0)) {
361 return false;
362 }
363
364 // If we have a height* smaller than the radius, there is a numerical
365 // aberation
366 if (fabs((*pointsInGlobal.m_p1)(1)) < radius() ||
367 fabs((*pointsInGlobal.m_p2)(1)) < radius()) {
368 return false;
369 }
370
371 // If we have reached this stage, one test is left
372 // If the straight line between the two points go through the cylinder,there
373 // is a wrap
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))) {
378 return false;
379 }
380
381 // Return the answer
382 return true;
383}
384#endif
385
387 const NodeMusclePair& p) const {
388 utils::Scalar arc =
389 std::acos(
390 ((*p.m_p1)(0) * (*p.m_p2)(0) + (*p.m_p1)(1) * (*p.m_p2)(1)) /
391 std::sqrt(
392 ((*p.m_p1)(0) * (*p.m_p1)(0) + (*p.m_p1)(1) * (*p.m_p1)(1)) *
393 ((*p.m_p2)(0) * (*p.m_p2)(0) + (*p.m_p2)(1) * (*p.m_p2)(1)))) *
394 radius();
395
396 return std::sqrt(
397 arc * arc +
398 ((*p.m_p1)(2) - (*p.m_p2)(2)) * ((*p.m_p1)(2) - (*p.m_p2)(2)));
399}
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.
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.
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.
This is the core of the musculoskeletal model in biorbd.
Definition Joints.h:48
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.
Definition Joints.cpp:360
A wrapper for the Eigen::MatrixXd.
Definition Matrix2d.h:21
const String & name() const
Return the name of the node.
Definition Node.cpp:43
std::shared_ptr< NODE_TYPE > m_typeOfNode
The type of the node.
Definition Node.h:92
std::shared_ptr< String > m_parentName
The parent name of the node.
Definition Node.h:91
Homogenous matrix to describe translations and rotations simultaneously.
Definition RotoTrans.h:33
RotoTrans transpose() const
Return the tranposed matrix.
Definition RotoTrans.cpp:95
Wrapper around the std::string class with augmented functionality.
Definition String.h:18
Wrapper around Eigen Vector2d and attach it to a parent.
Definition Vector2d.h:27
Wrapper around Eigen Vector3d and attach it to a parent.
Definition Vector3d.h:27
Vector3d()
Construct 3D vector.
Definition Vector3d.cpp:9
Vector3d applyRT(const RotoTrans &rt) const
Apply a RotoTrans to the 3D vector.
Definition Vector3d.cpp:79
Vector3d DeepCopy() const
Deep copy of a 3D vector.
Definition Vector3d.cpp:68