17 #ifndef GZ_MATH_POSE_HH_
18 #define GZ_MATH_POSE_HH_
22 #include <gz/math/config.hh>
27 inline namespace GZ_MATH_VERSION_NAMESPACE {
97 public:
Pose3(T _x, T _y, T _z, T _roll, T _pitch, T _yaw)
98 : p(_x, _y, _z), q(_roll, _pitch, _yaw)
112 public:
Pose3(T _x, T _y, T _z, T _qw, T _qx, T _qy, T _qz)
113 : p(_x, _y, _z), q(_qw, _qx, _qy, _qz)
139 this->q.SetFromEuler(_rpy);
150 public:
void Set(T _x, T _y, T _z, T _roll, T _pitch, T _yaw)
152 this->p.Set(_x, _y, _z);
160 return this->p.IsFinite() && this->q.IsFinite();
175 return Pose3<T>(inv * (this->p*-1), inv);
184 public: GZ_DEPRECATED(7)
Pose3<T> operator+(const
Pose3<T> &_pose)
const
188 result.p = this->CoordPositionAdd(_pose);
189 result.q = this->CoordRotationAdd(_pose.q);
198 public: GZ_DEPRECATED(7) const
Pose3<T> &
201 this->p = this->CoordPositionAdd(_pose);
202 this->q = this->CoordRotationAdd(_pose.q);
211 public: GZ_DEPRECATED(7)
Pose3<T> operator-()
const
213 return this->Inverse();
222 public: GZ_DEPRECATED(7)
Pose3<T> operator-(const
Pose3<T> &_pose)
const
224 return Pose3<T>(this->CoordPositionSub(_pose),
225 this->CoordRotationSub(_pose.q));
232 public: GZ_DEPRECATED(7) const
Pose3<T> &
235 this->p = this->CoordPositionSub(_pose);
236 this->q = this->CoordRotationSub(_pose.q);
246 return this->p == _pose.p && this->q == _pose.q;
254 return this->p != _pose.p || this->q != _pose.q;
274 *
this = *
this * _pose;
290 tmp = this->q * (tmp * this->q.
Inverse());
293 this->p.Y() + tmp.
Y(),
294 this->p.Z() + tmp.
Z());
303 this->p.X(), this->p.Y(), this->p.Z());
306 tmp = _pose.q * (tmp * _pose.q.Inverse());
309 _pose.p.Y() + tmp.
Y(),
310 _pose.p.Z() + tmp.
Z());
319 this->p.X() - _pose.p.X(),
320 this->p.Y() - _pose.p.Y(),
321 this->p.Z() - _pose.p.Z());
323 tmp = _pose.q.Inverse() * (tmp * _pose.q);
355 a.q = this->q.Inverse() * _b.q;
356 qt = a.q *
Quaternion<T>(0, this->p.X(), this->p.Y(), this->p.Z());
357 qt = qt * a.q.Inverse();
378 a.p.X((1.0 - 2.0*_q.
Y()*_q.
Y() - 2.0*_q.
Z()*_q.
Z()) * this->p.X()
379 +(2.0*(_q.
X()*_q.
Y()+_q.
W()*_q.
Z())) * this->p.Y()
380 +(2.0*(_q.
X()*_q.
Z()-_q.
W()*_q.
Y())) * this->p.Z());
381 a.p.Y((2.0*(_q.
X()*_q.
Y()-_q.
W()*_q.
Z())) * this->p.X()
382 +(1.0 - 2.0*_q.
X()*_q.
X() - 2.0*_q.
Z()*_q.
Z()) * this->p.Y()
383 +(2.0*(_q.
Y()*_q.
Z()+_q.
W()*_q.
X())) * this->p.Z());
384 a.p.Z((2.0*(_q.
X()*_q.
Z()+_q.
W()*_q.
Y())) * this->p.X()
385 +(2.0*(_q.
Y()*_q.
Z()-_q.
W()*_q.
X())) * this->p.Y()
386 +(1.0 - 2.0*_q.
X()*_q.
X() - 2.0*_q.
Y()*_q.
Y()) * this->p.Z());
394 this->q.Round(_precision);
395 this->p.Round(_precision);
416 public:
inline const T
X()
const
431 public:
inline const T
Y()
const
446 public:
inline const T
Z()
const
475 public:
inline const T
Roll()
const
477 return this->q.Roll();
484 public:
inline const T
Pitch()
const
486 return this->q.Pitch();
493 public:
inline const T
Yaw()
const
495 return this->q.Yaw();
505 _out << _pose.
Pos() <<
" " << _pose.
Rot();
517 _in.
setf(std::ios_base::skipws);
533 return this->p.Equal(_p.p, _tol) && this->q.Equal(_p.q, _tol);
545 template<
typename T> constexpr Pose3<T> gPose3Zero{};
549 template<
typename T>
const Pose3<T> &
Pose3<T>::Zero = detail::gPose3Zero<T>;