Gazebo Math

API Reference

8.0.0
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>
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
29namespace 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(
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 {
119 vec.X() = _v[0];
120 vec.Y() = _v[1];
121 vec.Z() = _v[2];
122
123 return vec;
124 }
125
130 const Eigen::AlignedBox3d &_b)
131 {
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 {
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_