Main MRPT website > C++ reference
MRPT logo
slerp.h
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | The Mobile Robot Programming Toolkit (MRPT) |
3  | |
4  | http://www.mrpt.org/ |
5  | |
6  | Copyright (c) 2005-2013, Individual contributors, see AUTHORS file |
7  | Copyright (c) 2005-2013, MAPIR group, University of Malaga |
8  | Copyright (c) 2012-2013, University of Almeria |
9  | All rights reserved. |
10  | |
11  | Redistribution and use in source and binary forms, with or without |
12  | modification, are permitted provided that the following conditions are |
13  | met: |
14  | * Redistributions of source code must retain the above copyright |
15  | notice, this list of conditions and the following disclaimer. |
16  | * Redistributions in binary form must reproduce the above copyright |
17  | notice, this list of conditions and the following disclaimer in the |
18  | documentation and/or other materials provided with the distribution. |
19  | * Neither the name of the copyright holders nor the |
20  | names of its contributors may be used to endorse or promote products |
21  | derived from this software without specific prior written permission.|
22  | |
23  | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS |
24  | 'AS IS' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED |
25  | TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR|
26  | PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS BE LIABLE |
27  | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL|
28  | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR|
29  | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) |
30  | HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, |
31  | STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN |
32  | ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
33  | POSSIBILITY OF SUCH DAMAGE. |
34  +---------------------------------------------------------------------------+ */
35 #ifndef mrpt_math_slerp_H
36 #define mrpt_math_slerp_H
37 
38 #include <mrpt/math/CQuaternion.h>
39 
40 namespace mrpt
41 {
42  namespace poses { class CPose3D; class CPose3DQuat; }
43 
44  namespace math
45  {
46  using namespace mrpt::poses;
47 
48  /** \addtogroup geometry_grp
49  * @{ */
50 
51  /** @name SLERP (Spherical Linear Interpolation) functions
52  @{ */
53 
54  /** SLERP interpolation between two quaternions
55  * \param[in] q0 The quaternion for t=0
56  * \param[in] q1 The quaternion for t=1
57  * \param[in] t A "time" parameter, in the range [0,1].
58  * \param[out] q The output, interpolated quaternion.
59  * \tparam T The type of the quaternion (e.g. float, double).
60  * \exception std::exception Only in Debug, if t is not in the valid range.
61  * \sa http://en.wikipedia.org/wiki/Slerp
62  */
63  template <typename T>
64  void slerp(
65  const CQuaternion<T> & q0,
66  const CQuaternion<T> & q1,
67  const double t,
68  CQuaternion<T> & q)
69  {
70  ASSERTDEB_(t>=0 && t<=1)
71  // See: http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/slerp/index.htm
72  // Angle between q0-q1:
73  double cosHalfTheta = q0[0]*q1[0]+q0[1]*q1[1]+q0[2]*q1[2]+q0[3]*q1[3];
74  // if qa=qb or qa=-qb then theta = 0 and we can return qa
75  if (std::abs(cosHalfTheta) >= 1.0)
76  {
77  q = q0;
78  return;
79  }
80  bool reverse_q1 = false;
81  if (cosHalfTheta < 0) // Always follow the shortest path
82  {
83  reverse_q1 = true;
84  cosHalfTheta = -cosHalfTheta;
85  }
86  // Calculate temporary values.
87  const double halfTheta = acos(cosHalfTheta);
88  const double sinHalfTheta = std::sqrt(1.0 - square(cosHalfTheta));
89  // if theta = 180 degrees then result is not fully defined
90  // we could rotate around any axis normal to qa or qb
91  if (std::abs(sinHalfTheta) < 0.001)
92  {
93  if (!reverse_q1)
94  for (int i=0;i<4;i++) q[i] = (1-t)*q0[i] + t*q1[i];
95  else for (int i=0;i<4;i++) q[i] = (1-t)*q0[i] - t*q1[i];
96  return;
97  }
98  const double A = sin((1-t) * halfTheta)/sinHalfTheta;
99  const double B = sin(t*halfTheta)/sinHalfTheta;
100  if (!reverse_q1)
101  for (int i=0;i<4;i++) q[i] = A*q0[i] + B*q1[i];
102  else for (int i=0;i<4;i++) q[i] = A*q0[i] - B*q1[i];
103  }
104 
105  /** SLERP interpolation between two 6D poses - like mrpt::math::slerp for quaternions, but interpolates the [X,Y,Z] coordinates as well.
106  * \param[in] p0 The pose for t=0
107  * \param[in] p1 The pose for t=1
108  * \param[in] t A "time" parameter, in the range [0,1].
109  * \param[out] p The output, interpolated pose.
110  * \exception std::exception Only in Debug, if t is not in the valid range.
111  */
112  void BASE_IMPEXP slerp(
113  const CPose3D & q0,
114  const CPose3D & q1,
115  const double t,
116  CPose3D & p);
117 
118  //! \overload
119  void BASE_IMPEXP slerp(
120  const CPose3DQuat & q0,
121  const CPose3DQuat & q1,
122  const double t,
123  CPose3DQuat & p);
124 
125  /** @} */
126 
127  /** @} */ // grouping
128  }
129 }
130 #endif
void slerp(const CQuaternion< T > &q0, const CQuaternion< T > &q1, const double t, CQuaternion< T > &q)
SLERP interpolation between two quaternions.
Definition: slerp.h:64
EIGEN_STRONG_INLINE const AdjointReturnType t() const
Transpose.
A class used to store a 3D pose as a translation (x,y,z) and a quaternion (qr,qx,qy,qz).
Definition: CPose3DQuat.h:70
class BASE_IMPEXP CPose3DQuat
Definition: CPose3D.h:48
T abs(T x)
Definition: nanoflann.hpp:237
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
T square(const T x)
Inline function for the square of a number.
Definition: bits.h:184
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:97
A quaternion, which can represent a 3D rotation as pair , with a real part "r" and a 3D vector ...
Definition: CQuaternion.h:68



Page generated by Doxygen 1.8.14 for MRPT 1.0.2 SVN: at lun oct 28 00:52:41 CET 2019 Hosted on:
SourceForge.net Logo