Biorbd
Release_1.13.0
Toggle main menu visibility
Loading...
Searching...
No Matches
Vector.cpp
1
#define BIORBD_API_EXPORTS
2
#include "Utils/Vector.h"
3
4
#include "Utils/Error.h"
5
#include "Utils/String.h"
6
#include "Utils/Vector3d.h"
7
8
using namespace
BIORBD_NAMESPACE;
9
10
utils::Vector::Vector
() : RigidBodyDynamics::Math::VectorNd() {}
11
12
utils::Vector::Vector
(
size_t
size)
13
: RigidBodyDynamics::Math::VectorNd(static_cast<unsigned int>(size)) {}
14
15
utils::Vector::Vector
(
const
utils::Vector
&other)
16
: RigidBodyDynamics::Math::VectorNd(other) {}
17
18
utils::Vector::Vector
(
const
RigidBodyDynamics::Math::VectorNd &other)
19
: RigidBodyDynamics::Math::VectorNd(other) {}
20
21
utils::Vector::Vector
(
const
utils::Vector3d
&other)
22
: RigidBodyDynamics::Math::VectorNd(other) {}
23
24
#ifdef BIORBD_USE_CASADI_MATH
25
26
utils::Vector::Vector
(
const
casadi::MX &other)
27
: RigidBodyDynamics::Math::VectorNd(other) {}
28
29
utils::Vector::Vector
(
const
RBDLCasadiMath::MX_Xd_SubMatrix &other)
30
: RigidBodyDynamics::Math::VectorNd(other) {}
31
32
#endif
33
34
utils::Scalar
utils::Vector::norm
(
size_t
p,
bool
skipRoot)
const
{
35
utils::Error::check
(p >= 2,
"p must be superior or equal to 2"
);
36
37
if
(p == 2) {
38
utils::Scalar n = dot(*
this
);
39
if
(skipRoot) {
40
return
n;
41
}
else
{
42
return
std::sqrt(n);
43
}
44
}
else
{
45
utils::Scalar res(0);
46
for
(
unsigned
int
i = 0; i < static_cast<unsigned int>(size()); ++i) {
47
res += std::pow(fabs((*
this
)[i]),
static_cast<
unsigned
int
>
(p));
48
}
49
if
(skipRoot) {
50
return
res;
51
}
else
{
52
return
std::pow(res, 1.0 / p);
53
}
54
}
55
}
56
57
utils::Vector
utils::Vector::normGradient
(
size_t
p,
bool
skipRoot) {
58
utils::Error::check
(p >= 2,
"p must be superior or equal to 2"
);
59
60
if
(p == 2) {
61
if
(skipRoot) {
62
return
utils::Vector
(*
this
* 2.);
63
}
else
{
64
return
utils::Vector
(*
this
* 1.0 /
norm
(2));
65
}
66
}
else
{
67
utils::Vector
res(
static_cast<
size_t
>
(size()));
68
for
(
unsigned
int
i = 0; i < static_cast<unsigned int>(size()); ++i) {
69
res[i] = (*this)[i] *
70
std::pow(fabs((*
this
)[i]),
static_cast<
unsigned
int
>
(p - 2));
71
}
72
if
(skipRoot) {
73
// d(sum |x_i|^p)/dx_i = p * x_i * |x_i|^(p-2)
74
res *=
static_cast<
double
>
(p);
75
}
else
{
76
// d(sum |x_i|^p)^(1/p)/dx_i = x_i * |x_i|^(p-2) / norm_p^(p-1)
77
res /= std::pow(
norm
(p),
static_cast<
unsigned
int
>
(p - 1));
78
}
79
return
res;
80
}
81
}
82
83
void
utils::Vector::operator=
(
const
utils::Vector
&other) {
84
this->RigidBodyDynamics::Math::VectorNd::operator=(other);
85
}
86
87
#ifdef BIORBD_USE_CASADI_MATH
88
89
void
utils::Vector::operator=
(
const
RBDLCasadiMath::MX_Xd_SubMatrix &other) {
90
this->MX_Xd_dynamic::operator=(other);
91
}
92
93
void
utils::Vector::operator=
(
const
casadi::MX &other) {
94
this->MX_Xd_dynamic::operator=(other);
95
}
96
97
#endif
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::Vector3d
Wrapper around Eigen Vector3d and attach it to a parent.
Definition
Vector3d.h:27
BIORBD_NAMESPACE::utils::Vector
Wrapper of the Eigen VectorXd.
Definition
Vector.h:21
BIORBD_NAMESPACE::utils::Vector::normGradient
Vector normGradient(size_t p=2, bool skipRoot=false)
Return the gradient of the p-norm.
Definition
Vector.cpp:57
BIORBD_NAMESPACE::utils::Vector::norm
Scalar norm(size_t p=2, bool skipRoot=false) const
Return the Euclidian p-norm of the vector.
Definition
Vector.cpp:34
BIORBD_NAMESPACE::utils::Vector::Vector
Vector()
Construct vector.
Definition
Vector.cpp:10
BIORBD_NAMESPACE::utils::Vector::operator=
void operator=(const Vector &other)
operator= For submatrices
Definition
Vector.cpp:83
src
Utils
Vector.cpp
Generated by
1.18.0