Biorbd Release_1.13.0
Loading...
Searching...
No Matches
Vector3d.cpp
1#define BIORBD_API_EXPORTS
2#include "Utils/Vector3d.h"
3
4#include "Utils/RotoTrans.h"
5#include "Utils/Vector.h"
6
7using namespace BIORBD_NAMESPACE;
8
10 : RigidBodyDynamics::Math::Vector3d(
11 RigidBodyDynamics::Math::Vector3d::Zero()),
12 utils::Node() {
13 setType();
14}
15
17 const utils::Scalar &x,
18 const utils::Scalar &y,
19 const utils::Scalar &z)
20 : RigidBodyDynamics::Math::Vector3d(x, y, z), utils::Node() {
21 setType();
22}
23
25 const utils::Scalar &x,
26 const utils::Scalar &y,
27 const utils::Scalar &z,
28 const utils::String &name,
29 const utils::String &parentName)
30 : RigidBodyDynamics::Math::Vector3d(x, y, z),
31 utils::Node(name, parentName) {
32 setType();
33}
34
36 const utils::Vector3d vec,
37 const utils::String &name,
38 const utils::String &parentName)
39 : RigidBodyDynamics::Math::Vector3d(vec), Node(name, parentName) {}
40
41utils::Vector3d::Vector3d(const RigidBodyDynamics::Math::Vector3d &other)
42 : RigidBodyDynamics::Math::Vector3d(other[0], other[1], other[2]),
43 utils::Node() {
44 setType();
45}
46
47utils::Vector3d::Vector3d(const RigidBodyDynamics::Math::VectorNd &other)
48 : RigidBodyDynamics::Math::Vector3d(other[0], other[1], other[2]),
49 utils::Node() {
50 setType();
51}
52
53utils::Vector3d::Vector3d(const RigidBodyDynamics::Math::Vector4d &other)
54 : RigidBodyDynamics::Math::Vector3d(other[0], other[1], other[2]),
55 utils::Node() {
56 setType();
57}
58
59#ifdef BIORBD_USE_CASADI_MATH
60
61utils::Vector3d::Vector3d(const RBDLCasadiMath::MX_Xd_SubMatrix &other)
62 : RigidBodyDynamics::Math::Vector3d(other), utils::Node() {
63 setType();
64}
65
66#endif
67
69 utils::Vector3d copy;
70 copy.DeepCopy(*this);
71 return copy;
72}
73
75 this->RigidBodyDynamics::Math::Vector3d::operator=(other);
77}
78
80 RigidBodyDynamics::Math::Vector4d v;
81 v.block(0, 0, 3, 1) = *this;
82 v[3] = 1;
83 return (rt * v).block(0, 0, 3, 1);
84}
85
87 RigidBodyDynamics::Math::Vector4d v;
88 v.block(0, 0, 3, 1) = *this;
89 v[3] = 1;
90 setPosition((rt * v).block(0, 0, 3, 1));
91}
92
94#ifdef BIORBD_USE_CASADI_MATH
95 (*this)(0) = v(0);
96 (*this)(1) = v(1);
97 (*this)(2) = v(2);
98#else
99 *this = v;
100#endif
101}
102
103void utils::Vector3d::setType() { *m_typeOfNode = utils::VECTOR3D; }
104
105#ifdef BIORBD_USE_CASADI_MATH
106utils::Scalar utils::Vector3d::x() const { return (*this)[0]; }
107
108utils::Scalar utils::Vector3d::y() const { return (*this)[1]; }
109
110utils::Scalar utils::Vector3d::z() const { return (*this)[2]; }
111
112void utils::Vector3d::operator=(const RBDLCasadiMath::MX_Xd_SubMatrix &other) {
113 *this = utils::Vector3d(other);
114}
115
116void utils::Vector3d::operator=(
117 const RigidBodyDynamics::Math::Vector4d &other) {
118 *this = utils::Vector3d(other);
119}
120#endif
A node is an abstract element which is assigned to a parent.
Definition Node.h:17
Node()
Construct Node.
Definition Node.cpp:8
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
void DeepCopy(const Node &other)
Deep copy of the node in another node.
Definition Node.cpp:35
Homogenous matrix to describe translations and rotations simultaneously.
Definition RotoTrans.h:33
Wrapper around the std::string class with augmented functionality.
Definition String.h:18
Wrapper around Eigen Vector3d and attach it to a parent.
Definition Vector3d.h:27
Vector3d()
Construct 3D vector.
Definition Vector3d.cpp:9
void setPosition(const Vector3d &v)
Set a new position.
Definition Vector3d.cpp:93
Vector3d applyRT(const RotoTrans &rt) const
Apply a RotoTrans to the 3D vector.
Definition Vector3d.cpp:79
void setType()
Set the type Vector3d.
Definition Vector3d.cpp:103
Vector3d DeepCopy() const
Deep copy of a 3D vector.
Definition Vector3d.cpp:68