Biorbd Release_1.13.0
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
8using namespace BIORBD_NAMESPACE;
9
10utils::Vector::Vector() : RigidBodyDynamics::Math::VectorNd() {}
11
13 : RigidBodyDynamics::Math::VectorNd(static_cast<unsigned int>(size)) {}
14
16 : RigidBodyDynamics::Math::VectorNd(other) {}
17
18utils::Vector::Vector(const RigidBodyDynamics::Math::VectorNd &other)
19 : RigidBodyDynamics::Math::VectorNd(other) {}
20
22 : RigidBodyDynamics::Math::VectorNd(other) {}
23
24#ifdef BIORBD_USE_CASADI_MATH
25
26utils::Vector::Vector(const casadi::MX &other)
27 : RigidBodyDynamics::Math::VectorNd(other) {}
28
29utils::Vector::Vector(const RBDLCasadiMath::MX_Xd_SubMatrix &other)
30 : RigidBodyDynamics::Math::VectorNd(other) {}
31
32#endif
33
34utils::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
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
84 this->RigidBodyDynamics::Math::VectorNd::operator=(other);
85}
86
87#ifdef BIORBD_USE_CASADI_MATH
88
89void utils::Vector::operator=(const RBDLCasadiMath::MX_Xd_SubMatrix &other) {
90 this->MX_Xd_dynamic::operator=(other);
91}
92
93void utils::Vector::operator=(const casadi::MX &other) {
94 this->MX_Xd_dynamic::operator=(other);
95}
96
97#endif
static void check(bool cond, const String &message)
Assert that raises the error message if false.
Definition Error.cpp:10
Wrapper around Eigen Vector3d and attach it to a parent.
Definition Vector3d.h:27
Wrapper of the Eigen VectorXd.
Definition Vector.h:21
Vector normGradient(size_t p=2, bool skipRoot=false)
Return the gradient of the p-norm.
Definition Vector.cpp:57
Scalar norm(size_t p=2, bool skipRoot=false) const
Return the Euclidian p-norm of the vector.
Definition Vector.cpp:34
Vector()
Construct vector.
Definition Vector.cpp:10
void operator=(const Vector &other)
operator= For submatrices
Definition Vector.cpp:83