Main MRPT website > C++ reference for MRPT 1.4.0
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-2016, 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
14
15namespace 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,
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::utils::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 */
84 const mrpt::poses::CPose3D & q0,
85 const mrpt::poses::CPose3D & q1,
86 const double t,
88
89 //! \overload
91 const mrpt::poses::CPose3DQuat & q0,
92 const mrpt::poses::CPose3DQuat & q1,
93 const double t,
95
96 /** @} */
97
98 /** @} */ // grouping
99 }
100}
101#endif
A quaternion, which can represent a 3D rotation as pair , with a real part "r" and a 3D vector ,...
Definition CQuaternion.h:43
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition CPose3D.h:73
A class used to store a 3D pose as a translation (x,y,z) and a quaternion (qr,qx,qy,...
Definition CPose3DQuat.h:42
EIGEN_STRONG_INLINE const AdjointReturnType t() const
Transpose.
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
#define ASSERTDEB_(f)
Defines an assertion mechanism - only when compiled in debug.
T square(const T x)
Inline function for the square of a number.
Definition bits.h:113
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.



Page generated by Doxygen 1.9.7 for MRPT 1.4.0 SVN: at Tue Jun 13 14:27:49 UTC 2023