Main MRPT website > C++ reference for MRPT 1.5.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-2017, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +---------------------------------------------------------------------------+ */
9 #ifndef mrpt_math_slerp_H
10 #define mrpt_math_slerp_H
11 
12 #include <mrpt/math/CQuaternion.h>
13 #include <mrpt/poses/poses_frwds.h>
14 
15 namespace mrpt
16 {
17  namespace math
18  {
19  /** \addtogroup geometry_grp
20  * @{ */
21 
22  /** @name SLERP (Spherical Linear Interpolation) functions
23  @{ */
24 
25  /** SLERP interpolation between two quaternions
26  * \param[in] q0 The quaternion for t=0
27  * \param[in] q1 The quaternion for t=1
28  * \param[in] t A "time" parameter, in the range [0,1].
29  * \param[out] q The output, interpolated quaternion.
30  * \tparam T The type of the quaternion (e.g. float, double).
31  * \exception std::exception Only in Debug, if t is not in the valid range.
32  * \sa http://en.wikipedia.org/wiki/Slerp
33  */
34  template <typename T>
35  void slerp(
36  const CQuaternion<T> & q0,
37  const CQuaternion<T> & q1,
38  const double t,
39  CQuaternion<T> & q)
40  {
41  ASSERTDEB_(t>=0 && t<=1)
42  // See: http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/slerp/index.htm
43  // Angle between q0-q1:
44  double cosHalfTheta = q0[0]*q1[0]+q0[1]*q1[1]+q0[2]*q1[2]+q0[3]*q1[3];
45  // if qa=qb or qa=-qb then theta = 0 and we can return qa
46  if (std::abs(cosHalfTheta) >= 1.0)
47  {
48  q = q0;
49  return;
50  }
51  bool reverse_q1 = false;
52  if (cosHalfTheta < 0) // Always follow the shortest path
53  {
54  reverse_q1 = true;
55  cosHalfTheta = -cosHalfTheta;
56  }
57  // Calculate temporary values.
58  const double halfTheta = acos(cosHalfTheta);
59  const double sinHalfTheta = std::sqrt(1.0 - mrpt::math::square(cosHalfTheta));
60  // if theta = 180 degrees then result is not fully defined
61  // we could rotate around any axis normal to qa or qb
62  if (std::abs(sinHalfTheta) < 0.001)
63  {
64  if (!reverse_q1)
65  for (int i=0;i<4;i++) q[i] = (1-t)*q0[i] + t*q1[i];
66  else 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 for (int i=0;i<4;i++) q[i] = A*q0[i] - B*q1[i];
74  }
75 
76  /** SLERP interpolation between two 6D poses - like mrpt::math::slerp for quaternions, but interpolates the [X,Y,Z] coordinates as well.
77  * \param[in] p0 The pose for t=0
78  * \param[in] p1 The pose for t=1
79  * \param[in] t A "time" parameter, in the range [0,1].
80  * \param[out] p The output, interpolated pose.
81  * \exception std::exception Only in Debug, if t is not in the valid range.
82  */
83  void BASE_IMPEXP slerp(
84  const mrpt::poses::CPose3D & q0,
85  const mrpt::poses::CPose3D & q1,
86  const double t,
88 
89  //! \overload
90  void BASE_IMPEXP slerp(
91  const mrpt::poses::CPose3DQuat & q0,
92  const mrpt::poses::CPose3DQuat & q1,
93  const double t,
95 
96  /** \overload Interpolates two SO(3) elements (the rotational part only), given as mrpt::math::TPose3D
97  * form as yaw,pitch,roll angles. XYZ are ignored.
98  */
100  const mrpt::math::TPose3D& q0,
101  const mrpt::math::TPose3D & q1,
102  const double t,
104 
105  /** @} */
106 
107  /** @} */ // grouping
108  }
109 }
110 #endif
GLdouble GLdouble t
Definition: glext.h:3610
void slerp(const CQuaternion< T > &q0, const CQuaternion< T > &q1, const double t, CQuaternion< T > &q)
SLERP interpolation between two quaternions.
Definition: slerp.h:35
GLdouble GLdouble GLdouble GLdouble q
Definition: glext.h:3626
void BASE_IMPEXP slerp_ypr(const mrpt::math::TPose3D &q0, const mrpt::math::TPose3D &q1, const double t, mrpt::math::TPose3D &p)
Definition: slerp.cpp:54
T square(const T x)
Inline function for the square of a number.
Definition: bits.h:52
A class used to store a 3D pose as a translation (x,y,z) and a quaternion (qr,qx,qy,qz).
Definition: CPose3DQuat.h:41
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.
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:72
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:43
GLfloat GLfloat p
Definition: glext.h:5587



Page generated by Doxygen 1.8.14 for MRPT 1.5.9 Git: 690a4699f Wed Apr 15 19:29:53 2020 +0200 at miƩ abr 15 19:30:12 CEST 2020