Gazebo Math
API Reference
8.1.0
insert_drive_file
Tutorials
library_books
Classes
toc
Namespaces
insert_drive_file
Files
launch
Gazebo Website
Index
List
Hierarchy
Members: All
Members: Functions
Members: Variables
Members: Typedefs
Members: Enumerations
Members: Enumerator
List
Members
Functions
Typedefs
Variables
Enumerations
Enumerator
src
gz-math
eigen3
include
gz
math
eigen3
Conversions.hh
Go to the documentation of this file.
1
/*
2
* Copyright (C) 2018 Open Source Robotics Foundation
3
*
4
* Licensed under the Apache License, Version 2.0 (the "License");
5
* you may not use this file except in compliance with the License.
6
* You may obtain a copy of the License at
7
*
8
* http://www.apache.org/licenses/LICENSE-2.0
9
*
10
* Unless required by applicable law or agreed to in writing, software
11
* distributed under the License is distributed on an "AS IS" BASIS,
12
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13
* See the License for the specific language governing permissions and
14
* limitations under the License.
15
*
16
*/
17
18
#ifndef GZ_MATH_EIGEN3_CONVERSIONS_HH_
19
#define GZ_MATH_EIGEN3_CONVERSIONS_HH_
20
21
#include <Eigen/Geometry>
22
#include <
gz/math/AxisAlignedBox.hh
>
23
#include <
gz/math/Matrix3.hh
>
24
#include <
gz/math/Matrix6.hh
>
25
#include <
gz/math/Pose3.hh
>
26
#include <
gz/math/Quaternion.hh
>
27
#include <
gz/math/Vector3.hh
>
28
29
namespace
gz::math::eigen3
30
{
34
inline
Eigen::Vector3d
convert
(
const
gz::math::Vector3d
&_v)
35
{
36
return
Eigen::Vector3d(_v[0], _v[1], _v[2]);
37
}
38
43
inline
Eigen::AlignedBox3d
convert
(
44
const
gz::math::AxisAlignedBox
&_b)
45
{
46
return
Eigen::AlignedBox3d(
convert
(_b.
Min
()),
convert
(_b.
Max
()));
47
}
48
52
inline
Eigen::Matrix3d
convert
(
const
gz::math::Matrix3d
&_m)
53
{
54
Eigen::Matrix3d matrix;
55
for
(
std::size_t
i=0; i < 3; ++i)
56
{
57
for
(
std::size_t
j=0; j < 3; ++j)
58
{
59
matrix(i, j) = _m(i, j);
60
}
61
}
62
63
return
matrix;
64
}
65
71
template
<
typename
Precision>
72
inline
73
Eigen::Matrix<Precision, 6, 6>
convert
(
const
Matrix6<Precision> &_m)
74
{
75
Eigen::Matrix<Precision, 6, 6> matrix;
76
for
(
std::size_t
i = 0; i < 6; ++i)
77
{
78
for
(
std::size_t
j = 0; j < 6; ++j)
79
{
80
matrix(i, j) = _m(i, j);
81
}
82
}
83
84
return
matrix;
85
}
86
90
inline
Eigen::Quaterniond
convert
(
const
gz::math::Quaterniond
&_q)
91
{
92
Eigen::Quaterniond quat;
93
quat.w() = _q.
W
();
94
quat.x() = _q.
X
();
95
quat.y() = _q.
Y
();
96
quat.z() = _q.
Z
();
97
98
return
quat;
99
}
100
104
inline
Eigen::Isometry3d
convert
(
const
gz::math::Pose3d
&_pose)
105
{
106
Eigen::Isometry3d tf = Eigen::Isometry3d::Identity();
107
tf.translation() =
convert
(_pose.Pos());
108
tf.linear() = Eigen::Matrix3d(
convert
(_pose.Rot()));
109
110
return
tf;
111
}
112
116
inline
gz::math::Vector3d
convert
(
const
Eigen::Vector3d &_v)
117
{
118
gz::math::Vector3d
vec;
119
vec.
X
() = _v[0];
120
vec.
Y
() = _v[1];
121
vec.
Z
() = _v[2];
122
123
return
vec;
124
}
125
129
inline
gz::math::AxisAlignedBox
convert
(
130
const
Eigen::AlignedBox3d &_b)
131
{
132
gz::math::AxisAlignedBox
box;
133
box.
Min
() =
convert
(_b.min());
134
box.
Max
() =
convert
(_b.max());
135
136
return
box;
137
}
138
142
inline
gz::math::Matrix3d
convert
(
const
Eigen::Matrix3d &_m)
143
{
144
gz::math::Matrix3d
matrix;
145
for
(
std::size_t
i=0; i < 3; ++i)
146
{
147
for
(
std::size_t
j=0; j < 3; ++j)
148
{
149
matrix(i, j) = _m(i, j);
150
}
151
}
152
153
return
matrix;
154
}
155
161
template
<
typename
Precision>
162
inline
163
Matrix6<Precision>
convert
(
const
Eigen::Matrix<Precision, 6, 6> &_m)
164
{
165
Matrix6<Precision> matrix;
166
for
(
std::size_t
i = 0; i < 6; ++i)
167
{
168
for
(
std::size_t
j = 0; j < 6; ++j)
169
{
170
matrix(i, j) = _m(i, j);
171
}
172
}
173
174
return
matrix;
175
}
176
180
inline
gz::math::Quaterniond
convert
(
const
Eigen::Quaterniond &_q)
181
{
182
gz::math::Quaterniond
quat;
183
quat.
W
() = _q.w();
184
quat.
X
() = _q.x();
185
quat.
Y
() = _q.y();
186
quat.
Z
() = _q.z();
187
188
return
quat;
189
}
190
194
inline
gz::math::Pose3d
convert
(
const
Eigen::Isometry3d &_tf)
195
{
196
gz::math::Pose3d
pose;
197
pose.Pos() =
convert
(Eigen::Vector3d(_tf.translation()));
198
pose.Rot() =
convert
(Eigen::Quaterniond(_tf.linear()));
199
200
return
pose;
201
}
202
}
// namespace gz::math::eigen3
203
#endif
// GZ_MATH_EIGEN3_CONVERSIONS_HH_