Main MRPT website > C++ reference for MRPT 1.9.9
slerp.h
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file |
8  +------------------------------------------------------------------------+ */
9 #pragma once
10
11 #include <mrpt/math/CQuaternion.h>
13
14 namespace mrpt
15 {
16 namespace math
17 {
19  * @{ */
20
21 /** @name SLERP (Spherical Linear Interpolation) functions
22  @{ */
23
24 /** SLERP interpolation between two quaternions
25  * \param[in] q0 The quaternion for t=0
26  * \param[in] q1 The quaternion for t=1
27  * \param[in] t A "time" parameter, in the range [0,1].
28  * \param[out] q The output, interpolated quaternion.
29  * \tparam T The type of the quaternion (e.g. float, double).
30  * \exception std::exception Only in Debug, if t is not in the valid range.
31  * \sa http://en.wikipedia.org/wiki/Slerp
32  */
33 template <typename T>
34 void slerp(
35  const CQuaternion<T>& q0, const CQuaternion<T>& q1, const double t,
37 {
38  ASSERTDEB_(t >= 0 && t <= 1);
39  // See:
40  // http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/slerp/index.htm
41  // Angle between q0-q1:
42  double cosHalfTheta =
43  q0[0] * q1[0] + q0[1] * q1[1] + q0[2] * q1[2] + q0[3] * q1[3];
44  // if qa=qb or qa=-qb then theta = 0 and we can return qa
45  if (std::abs(cosHalfTheta) >= 1.0)
46  {
47  q = q0;
48  return;
49  }
50  bool reverse_q1 = false;
51  if (cosHalfTheta < 0) // Always follow the shortest path
52  {
53  reverse_q1 = true;
54  cosHalfTheta = -cosHalfTheta;
55  }
56  // Calculate temporary values.
57  const double halfTheta = acos(cosHalfTheta);
58  const double sinHalfTheta = std::sqrt(1.0 - mrpt::square(cosHalfTheta));
59  // if theta = 180 degrees then result is not fully defined
60  // we could rotate around any axis normal to qa or qb
61  if (std::abs(sinHalfTheta) < 0.001)
62  {
63  if (!reverse_q1)
64  for (int i = 0; i < 4; i++) q[i] = (1 - t) * q0[i] + t * q1[i];
65  else
66  for (int i = 0; i < 4; i++) q[i] = (1 - t) * q0[i] - t * q1[i];
67  return;
68  }
69  const double A = sin((1 - t) * halfTheta) / sinHalfTheta;
70  const double B = sin(t * halfTheta) / sinHalfTheta;
71  if (!reverse_q1)
72  for (int i = 0; i < 4; i++) q[i] = A * q0[i] + B * q1[i];
73  else
74  for (int i = 0; i < 4; i++) q[i] = A * q0[i] - B * q1[i];
75 }
76
77 /** SLERP interpolation between two 6D poses - like mrpt::math::slerp for
78  * quaternions, but interpolates the [X,Y,Z] coordinates as well.
79  * \param[in] p0 The pose for t=0
80  * \param[in] p1 The pose for t=1
81  * \param[in] t A "time" parameter, in the range [0,1].
82  * \param[out] p The output, interpolated pose.
83  * \exception std::exception Only in Debug, if t is not in the valid range.
84  */
85 void slerp(const TPose3D& q0, const TPose3D& q1, const double t, TPose3D& p);
86
87 /** \overload Interpolates two SO(3) elements (the rotational part only), given
88  * as mrpt::math::TPose3D
89  * form as yaw,pitch,roll angles. XYZ are ignored.
90  */
91 void slerp_ypr(
92  const mrpt::math::TPose3D& q0, const mrpt::math::TPose3D& q1,
93  const double t, mrpt::math::TPose3D& p);
94
95 /** @} */
96
97 /** @} */ // grouping
98 }
99 }
GLdouble GLdouble t
Definition: glext.h:3689
void slerp_ypr(const mrpt::math::TPose3D &q0, const mrpt::math::TPose3D &q1, const double t, mrpt::math::TPose3D &p)
Definition: slerp.cpp:32
void slerp(const CQuaternion< T > &q0, const CQuaternion< T > &q1, const double t, CQuaternion< T > &q)
SLERP interpolation between two quaternions.
Definition: slerp.h:34
GLdouble GLdouble GLdouble GLdouble q
Definition: glext.h:3721
T square(const T x)
Inline function for the square of a number.
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
#define ASSERTDEB_(f)
Defines an assertion mechanism - only when compiled in debug.
Definition: exceptions.h:205
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).
A quaternion, which can represent a 3D rotation as pair , with a real part "r" and a 3D vector ...
Definition: CQuaternion.h:46
GLfloat GLfloat p
Definition: glext.h:6305

 Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019