MRPT  2.0.3
slerp.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2020, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include "math-precomp.h" // Precompiled headers
11 
12 #include <mrpt/math/slerp.h>
13 
14 using namespace mrpt;
15 using namespace mrpt::math;
16 
18  const TPose3D& p0, const TPose3D& p1, const double t, TPose3D& p)
19 {
20  CQuaternionDouble q0, q1, q;
21  p0.getAsQuaternion(q0);
22  p1.getAsQuaternion(q1);
23  // The quaternion part (this will raise exception on t not in [0,1])
24  mrpt::math::slerp(q0, q1, t, q);
25  q.rpy(p.roll, p.pitch, p.yaw);
26  // XYZ:
27  p.x = (1 - t) * p0.x + t * p1.x;
28  p.y = (1 - t) * p0.y + t * p1.y;
29  p.z = (1 - t) * p0.z + t * p1.z;
30 }
31 
33  const mrpt::math::TPose3D& q0, const mrpt::math::TPose3D& q1,
34  const double t, mrpt::math::TPose3D& p)
35 {
38  q0.getAsQuaternion(quat0);
39  q1.getAsQuaternion(quat1);
40  mrpt::math::slerp(quat0, quat1, t, q);
41 
42  p.x = p.y = p.z = 0;
43  q.rpy(p.roll, p.pitch, p.yaw);
44 }
math-precomp.h
mrpt::math::CQuaternion::rpy
void rpy(T &roll, T &pitch, T &yaw) const
Return the yaw, pitch & roll angles associated to quaternion.
Definition: CQuaternion.h:434
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: BaseAppDataSource.h:15
mrpt::math::TPose3D::y
double y
Definition: TPose3D.h:32
mrpt::math::slerp
void slerp(const CQuaternion< T > &q0, const CQuaternion< T > &q1, const double t, CQuaternion< T > &q)
SLERP interpolation between two quaternions.
Definition: slerp.h:32
mrpt::math::TPose3D::z
double z
Definition: TPose3D.h:32
slerp.h
mrpt::math::TPose3D::x
double x
X,Y,Z, coords.
Definition: TPose3D.h:32
mrpt::math::TPose3D
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).
Definition: TPose3D.h:24
mrpt::math::TPose3D::pitch
double pitch
Pitch coordinate (rotation angle over Y axis).
Definition: TPose3D.h:36
mrpt::math::CQuaternion
A quaternion, which can represent a 3D rotation as pair , with a real part "r" and a 3D vector ,...
Definition: CQuaternion.h:44
mrpt::math::TPose3D::yaw
double yaw
Yaw coordinate (rotation angle over Z axis).
Definition: TPose3D.h:34
mrpt::math::TPose3D::roll
double roll
Roll coordinate (rotation angle over X coordinate).
Definition: TPose3D.h:38
mrpt::math::slerp_ypr
void slerp_ypr(const mrpt::math::TPose3D &q0, const mrpt::math::TPose3D &q1, const double t, mrpt::math::TPose3D &p)
Definition: slerp.cpp:32
mrpt::math
This base provides a set of functions for maths stuff.
Definition: math/include/mrpt/math/bits_math.h:11
mrpt::math::UNINITIALIZED_QUATERNION
@ UNINITIALIZED_QUATERNION
Definition: CQuaternion.h:22
mrpt::math::TPose3D::getAsQuaternion
void getAsQuaternion(mrpt::math::CQuaternion< double > &q, mrpt::optional_ref< mrpt::math::CMatrixFixed< double, 4, 3 >> out_dq_dr=std::nullopt) const
Returns the quaternion associated to the rotation of this object (NOTE: XYZ translation is ignored)
Definition: TPose3D.cpp:42



Page generated by Doxygen 1.8.17 for MRPT 2.0.3 at Fri May 15 15:49:54 UTC 2020