Biorbd Release_1.13.0
Loading...
Searching...
No Matches
Matrix3d.cpp
1#define BIORBD_API_EXPORTS
2#include "Utils/Matrix3d.h"
3
4#include "Utils/Error.h"
5#include "Utils/Vector.h"
6
7using namespace BIORBD_NAMESPACE;
8
9utils::Matrix3d::Matrix3d() : RigidBodyDynamics::Math::Matrix3d() {}
10
12 : RigidBodyDynamics::Math::Matrix3d(other) {}
13
15 const utils::Scalar &v00,
16 const utils::Scalar &v01,
17 const utils::Scalar &v02,
18 const utils::Scalar &v10,
19 const utils::Scalar &v11,
20 const utils::Scalar &v12,
21 const utils::Scalar &v20,
22 const utils::Scalar &v21,
23 const utils::Scalar &v22)
24 : RigidBodyDynamics::Math::Matrix3d(
25 v00,
26 v01,
27 v02,
28 v10,
29 v11,
30 v12,
31 v20,
32 v21,
33 v22) {}
34
35#ifndef BIORBD_USE_CASADI_MATH
36utils::Scalar utils::Matrix3d::normOne() const {
37 utils::Scalar value = 0.0;
38 for (size_t i = 0; i < 3; ++i) {
39 utils::Scalar col_sum =
40 fabs((*this)(0, i)) + fabs((*this)(1, i)) + fabs((*this)(2, i));
41 if (col_sum > value) value = col_sum;
42 }
43 return value;
44}
45
46utils::Scalar utils::Matrix3d::normInf() const {
47 utils::Scalar value = 0.0;
48 for (size_t i = 0; i < 3; ++i) {
49 utils::Scalar row_sum =
50 fabs((*this)(i, 0)) + fabs((*this)(i, 1)) + fabs((*this)(i, 2));
51 if (row_sum > value) value = row_sum;
52 }
53 return value;
54}
55
57#ifdef BIORBD_USE_EIGEN3_MATH
58 Eigen::JacobiSVD<Eigen::MatrixXd> svd(
59 *this, Eigen::ComputeFullU | Eigen::ComputeFullV);
60 return svd.matrixU() * svd.matrixV().transpose();
61#else
62#error "SVD decomposition not implemented for non-eigen3 backend"
63#endif
64}
65#endif
66
69 seq.length() == 3, "The angle sequence should be exactly 3.");
70
71 utils::Matrix3d baseUnitMatrix(utils::Matrix3d::Zero());
72 for (int i = 0; i < seq.length(); i++) {
73 int indexSeq;
74 if (seq[i] == 'x') {
75 indexSeq = 0;
76 } else if (seq[i] == 'y') {
77 indexSeq = 1;
78 } else if (seq[i] == 'z') {
79 indexSeq = 2;
80 } else {
81 utils::Error::raise("Angle sequence must be composed of x, y, and/or z.");
82 }
83 baseUnitMatrix(i, indexSeq) = 1;
84 }
85 return baseUnitMatrix;
86}
87
88#ifdef BIORBD_USE_CASADI_MATH
89
90utils::Matrix3d::Matrix3d(const RBDLCasadiMath::MX_Xd_static<3, 3> &v)
91 : RigidBodyDynamics::Math::Matrix3d(v) {}
92
93utils::Matrix3d::Matrix3d(const RBDLCasadiMath::MX_Xd_SubMatrix &m)
94 : RigidBodyDynamics::Math::Matrix3d(m) {}
95
96void utils::Matrix3d::operator=(
97 const RBDLCasadiMath::MX_Xd_static<3, 3> &other) {
98 this->MX_Xd_static<3, 3>::operator=(other);
99}
100
101void utils::Matrix3d::operator=(const RBDLCasadiMath::MX_Xd_SubMatrix &other) {
102 this->MX_Xd_static<3, 3>::operator=(other);
103}
104
105#endif
static void raise(const String &message)
Throw an error message.
Definition Error.cpp:6
static void check(bool cond, const String &message)
Assert that raises the error message if false.
Definition Error.cpp:10
A wrapper for the Eigen::MatrixXd.
Definition Matrix3d.h:21
utils::Scalar normOne() const
Return the 1-norm of the matrix.
Definition Matrix3d.cpp:36
utils::Matrix3d orthoNormalize() const
Produce an orthonormal matrix from the current matrix.
Definition Matrix3d.cpp:56
utils::Scalar normInf() const
Return the inf-norm of the matrix.
Definition Matrix3d.cpp:46
Matrix3d()
Construct matrix.
Definition Matrix3d.cpp:9
static utils::Matrix3d fromEulerSequence(const utils::String &seq)
Creates a base 3d matrix from a euler sequence of exactly 3 elements.
Definition Matrix3d.cpp:67
Wrapper around the std::string class with augmented functionality.
Definition String.h:18