Biorbd
Release_1.13.0
Toggle main menu visibility
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
7
using namespace
BIORBD_NAMESPACE;
8
9
utils::Matrix3d::Matrix3d
() : RigidBodyDynamics::Math::
Matrix3d
() {}
10
11
utils::Matrix3d::Matrix3d
(
const
utils::Matrix3d
&other)
12
: RigidBodyDynamics::Math::
Matrix3d
(other) {}
13
14
utils::Matrix3d::Matrix3d
(
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
36
utils::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
46
utils::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
56
utils::Matrix3d
utils::Matrix3d::orthoNormalize
()
const
{
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
67
utils::Matrix3d
utils::Matrix3d::fromEulerSequence
(
const
utils::String
&seq) {
68
utils::Error::check
(
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
90
utils::Matrix3d::Matrix3d
(
const
RBDLCasadiMath::MX_Xd_static<3, 3> &v)
91
: RigidBodyDynamics::Math::
Matrix3d
(v) {}
92
93
utils::Matrix3d::Matrix3d
(
const
RBDLCasadiMath::MX_Xd_SubMatrix &m)
94
: RigidBodyDynamics::Math::Matrix3d(m) {}
95
96
void
utils::Matrix3d::operator=(
97
const
RBDLCasadiMath::MX_Xd_static<3, 3> &other) {
98
this->MX_Xd_static<3, 3>::operator=(other);
99
}
100
101
void
utils::Matrix3d::operator=(
const
RBDLCasadiMath::MX_Xd_SubMatrix &other) {
102
this->MX_Xd_static<3, 3>::operator=(other);
103
}
104
105
#endif
BIORBD_NAMESPACE::utils::Error::raise
static void raise(const String &message)
Throw an error message.
Definition
Error.cpp:6
BIORBD_NAMESPACE::utils::Error::check
static void check(bool cond, const String &message)
Assert that raises the error message if false.
Definition
Error.cpp:10
BIORBD_NAMESPACE::utils::Matrix3d
A wrapper for the Eigen::MatrixXd.
Definition
Matrix3d.h:21
BIORBD_NAMESPACE::utils::Matrix3d::normOne
utils::Scalar normOne() const
Return the 1-norm of the matrix.
Definition
Matrix3d.cpp:36
BIORBD_NAMESPACE::utils::Matrix3d::orthoNormalize
utils::Matrix3d orthoNormalize() const
Produce an orthonormal matrix from the current matrix.
Definition
Matrix3d.cpp:56
BIORBD_NAMESPACE::utils::Matrix3d::normInf
utils::Scalar normInf() const
Return the inf-norm of the matrix.
Definition
Matrix3d.cpp:46
BIORBD_NAMESPACE::utils::Matrix3d::Matrix3d
Matrix3d()
Construct matrix.
Definition
Matrix3d.cpp:9
BIORBD_NAMESPACE::utils::Matrix3d::fromEulerSequence
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
BIORBD_NAMESPACE::utils::String
Wrapper around the std::string class with augmented functionality.
Definition
String.h:18
src
Utils
Matrix3d.cpp
Generated by
1.18.0