Main MRPT website > C++ reference
MRPT logo

slerp.h

Go to the documentation of this file.
00001 /* +---------------------------------------------------------------------------+
00002    |          The Mobile Robot Programming Toolkit (MRPT) C++ library          |
00003    |                                                                           |
00004    |                   http://mrpt.sourceforge.net/                            |
00005    |                                                                           |
00006    |   Copyright (C) 2005-2011  University of Malaga                           |
00007    |                                                                           |
00008    |    This software was written by the Machine Perception and Intelligent    |
00009    |      Robotics Lab, University of Malaga (Spain).                          |
00010    |    Contact: Jose-Luis Blanco  <jlblanco@ctima.uma.es>                     |
00011    |                                                                           |
00012    |  This file is part of the MRPT project.                                   |
00013    |                                                                           |
00014    |     MRPT is free software: you can redistribute it and/or modify          |
00015    |     it under the terms of the GNU General Public License as published by  |
00016    |     the Free Software Foundation, either version 3 of the License, or     |
00017    |     (at your option) any later version.                                   |
00018    |                                                                           |
00019    |   MRPT is distributed in the hope that it will be useful,                 |
00020    |     but WITHOUT ANY WARRANTY; without even the implied warranty of        |
00021    |     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         |
00022    |     GNU General Public License for more details.                          |
00023    |                                                                           |
00024    |     You should have received a copy of the GNU General Public License     |
00025    |     along with MRPT.  If not, see <http://www.gnu.org/licenses/>.         |
00026    |                                                                           |
00027    +---------------------------------------------------------------------------+ */
00028 #ifndef  mrpt_math_slerp_H
00029 #define  mrpt_math_slerp_H
00030 
00031 #include <mrpt/math/CQuaternion.h>
00032 
00033 namespace mrpt
00034 {
00035         namespace poses  { class CPose3D; class CPose3DQuat; }
00036         
00037         namespace math
00038         {
00039                 /** @name SLERP (Spherical Linear Interpolation) functions
00040                     @{ */
00041 
00042                 /** SLERP interpolation between two quaternions
00043                   * \param[in] q0 The quaternion for t=0
00044                   * \param[in] q1 The quaternion for t=1
00045                   * \param[in] t  A "time" parameter, in the range [0,1].
00046                   * \param[out] q The output, interpolated quaternion.
00047                   * \tparam T  The type of the quaternion (e.g. float, double).
00048                   * \exception std::exception Only in Debug, if t is not in the valid range.
00049                   * \sa http://en.wikipedia.org/wiki/Slerp
00050                   */
00051                 template <typename T>
00052                 void slerp(
00053                         const CQuaternion<T>  & q0,
00054                         const CQuaternion<T>  & q1,
00055                         const double            t, 
00056                         CQuaternion<T>        & q)
00057                 {
00058                         ASSERTDEB_(t>=0 && t<=1)
00059                         // See: http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/slerp/index.htm
00060                         // Angle between q0-q1:
00061                         double cosHalfTheta = q0[0]*q1[0]+q0[1]*q1[1]+q0[2]*q1[2]+q0[3]*q1[3];
00062                         // if qa=qb or qa=-qb then theta = 0 and we can return qa
00063                         if (std::abs(cosHalfTheta) >= 1.0)
00064                         {
00065                                 q = q0; 
00066                                 return;
00067                         }
00068                         bool reverse_q1 = false;
00069                         if (cosHalfTheta < 0) // Always follow the shortest path
00070                         {
00071                                 reverse_q1 = true;
00072                                 cosHalfTheta = -cosHalfTheta;
00073                         }
00074                         // Calculate temporary values.
00075                         const double halfTheta = acos(cosHalfTheta);
00076                         const double sinHalfTheta = std::sqrt(1.0 - square(cosHalfTheta));
00077                         // if theta = 180 degrees then result is not fully defined
00078                         // we could rotate around any axis normal to qa or qb
00079                         if (std::abs(sinHalfTheta) < 0.001)
00080                         { 
00081                                 if (!reverse_q1)
00082                                      for (int i=0;i<4;i++) q[i] = (1-t)*q0[i] + t*q1[i];
00083                                 else for (int i=0;i<4;i++) q[i] = (1-t)*q0[i] - t*q1[i];
00084                                 return;
00085                         }
00086                         const double A = sin((1-t) * halfTheta)/sinHalfTheta;
00087                         const double B = sin(t*halfTheta)/sinHalfTheta;
00088                         if (!reverse_q1)
00089                              for (int i=0;i<4;i++) q[i] = A*q0[i] + B*q1[i];
00090                         else for (int i=0;i<4;i++) q[i] = A*q0[i] - B*q1[i];
00091                 }
00092 
00093                 /** SLERP interpolation between two 6D poses - like mrpt::math::slerp for quaternions, but interpolates the [X,Y,Z] coordinates as well.
00094                   * \param[in] p0 The pose for t=0
00095                   * \param[in] p1 The pose for t=1
00096                   * \param[in] t  A "time" parameter, in the range [0,1].
00097                   * \param[out] p The output, interpolated pose.
00098                   * \exception std::exception Only in Debug, if t is not in the valid range.
00099                   */
00100                 void BASE_IMPEXP slerp(
00101                         const CPose3D  & q0,
00102                         const CPose3D  & q1,
00103                         const double     t, 
00104                         CPose3D        & p);
00105                         
00106                 //! \overload
00107                 void BASE_IMPEXP slerp(
00108                         const CPose3DQuat & q0,
00109                         const CPose3DQuat & q1,
00110                         const double        t, 
00111                         CPose3DQuat       & p);
00112                                           
00113                 /** @} */
00114         }
00115 }
00116 #endif



Page generated by Doxygen 1.7.3 for MRPT 0.9.4 SVN: at Sat Mar 26 06:40:17 UTC 2011