Biorbd
Release_1.13.0
Toggle main menu visibility
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
7
using namespace
BIORBD_NAMESPACE;
8
9
utils::Vector3d::Vector3d
()
10
: RigidBodyDynamics::Math::
Vector3d
(
11
RigidBodyDynamics::Math::
Vector3d
::Zero()),
12
utils::
Node
() {
13
setType
();
14
}
15
16
utils::Vector3d::Vector3d
(
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
24
utils::Vector3d::Vector3d
(
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
35
utils::Vector3d::Vector3d
(
36
const
utils::Vector3d
vec,
37
const
utils::String
&
name
,
38
const
utils::String
&parentName)
39
: RigidBodyDynamics::Math::
Vector3d
(vec),
Node
(
name
, parentName) {}
40
41
utils::Vector3d::Vector3d
(
const
RigidBodyDynamics::Math::Vector3d &other)
42
: RigidBodyDynamics::Math::
Vector3d
(other[0], other[1], other[2]),
43
utils::
Node
() {
44
setType
();
45
}
46
47
utils::Vector3d::Vector3d
(
const
RigidBodyDynamics::Math::VectorNd &other)
48
: RigidBodyDynamics::Math::
Vector3d
(other[0], other[1], other[2]),
49
utils::
Node
() {
50
setType
();
51
}
52
53
utils::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
61
utils::Vector3d::Vector3d
(
const
RBDLCasadiMath::MX_Xd_SubMatrix &other)
62
: RigidBodyDynamics::Math::
Vector3d
(other), utils::
Node
() {
63
setType
();
64
}
65
66
#endif
67
68
utils::Vector3d
utils::Vector3d::DeepCopy
()
const
{
69
utils::Vector3d
copy;
70
copy.
DeepCopy
(*
this
);
71
return
copy;
72
}
73
74
void
utils::Vector3d::DeepCopy
(
const
Vector3d
&other) {
75
this->RigidBodyDynamics::Math::Vector3d::operator=(other);
76
utils::Node::DeepCopy
(other);
77
}
78
79
utils::Vector3d
utils::Vector3d::applyRT
(
const
utils::RotoTrans
&rt)
const
{
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
86
void
utils::Vector3d::applyRT
(
const
utils::RotoTrans
&rt) {
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
93
void
utils::Vector3d::setPosition
(
const
utils::Vector3d
&v) {
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
103
void
utils::Vector3d::setType
() { *
m_typeOfNode
= utils::VECTOR3D; }
104
105
#ifdef BIORBD_USE_CASADI_MATH
106
utils::Scalar utils::Vector3d::x()
const
{
return
(*
this
)[0]; }
107
108
utils::Scalar utils::Vector3d::y()
const
{
return
(*
this
)[1]; }
109
110
utils::Scalar utils::Vector3d::z()
const
{
return
(*
this
)[2]; }
111
112
void
utils::Vector3d::operator=(
const
RBDLCasadiMath::MX_Xd_SubMatrix &other) {
113
*
this
= utils::Vector3d(other);
114
}
115
116
void
utils::Vector3d::operator=(
117
const
RigidBodyDynamics::Math::Vector4d &other) {
118
*
this
= utils::Vector3d(other);
119
}
120
#endif
BIORBD_NAMESPACE::utils::Node
A node is an abstract element which is assigned to a parent.
Definition
Node.h:17
BIORBD_NAMESPACE::utils::Node::Node
Node()
Construct Node.
Definition
Node.cpp:8
BIORBD_NAMESPACE::utils::Node::name
const String & name() const
Return the name of the node.
Definition
Node.cpp:43
BIORBD_NAMESPACE::utils::Node::m_typeOfNode
std::shared_ptr< NODE_TYPE > m_typeOfNode
The type of the node.
Definition
Node.h:92
BIORBD_NAMESPACE::utils::Node::DeepCopy
void DeepCopy(const Node &other)
Deep copy of the node in another node.
Definition
Node.cpp:35
BIORBD_NAMESPACE::utils::RotoTrans
Homogenous matrix to describe translations and rotations simultaneously.
Definition
RotoTrans.h:33
BIORBD_NAMESPACE::utils::String
Wrapper around the std::string class with augmented functionality.
Definition
String.h:18
BIORBD_NAMESPACE::utils::Vector3d
Wrapper around Eigen Vector3d and attach it to a parent.
Definition
Vector3d.h:27
BIORBD_NAMESPACE::utils::Vector3d::Vector3d
Vector3d()
Construct 3D vector.
Definition
Vector3d.cpp:9
BIORBD_NAMESPACE::utils::Vector3d::setPosition
void setPosition(const Vector3d &v)
Set a new position.
Definition
Vector3d.cpp:93
BIORBD_NAMESPACE::utils::Vector3d::applyRT
Vector3d applyRT(const RotoTrans &rt) const
Apply a RotoTrans to the 3D vector.
Definition
Vector3d.cpp:79
BIORBD_NAMESPACE::utils::Vector3d::setType
void setType()
Set the type Vector3d.
Definition
Vector3d.cpp:103
BIORBD_NAMESPACE::utils::Vector3d::DeepCopy
Vector3d DeepCopy() const
Deep copy of a 3D vector.
Definition
Vector3d.cpp:68
src
Utils
Vector3d.cpp
Generated by
1.18.0