00001 /* +---------------------------------------------------------------------------+ 00002 | The Mobile Robot Programming Toolkit (MRPT) C++ library | 00003 | | 00004 | http://www.mrpt.org/ | 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 using namespace mrpt::poses; 00040 00041 /** \addtogroup geometry_grp 00042 * @{ */ 00043 00044 /** @name SLERP (Spherical Linear Interpolation) functions 00045 @{ */ 00046 00047 /** SLERP interpolation between two quaternions 00048 * \param[in] q0 The quaternion for t=0 00049 * \param[in] q1 The quaternion for t=1 00050 * \param[in] t A "time" parameter, in the range [0,1]. 00051 * \param[out] q The output, interpolated quaternion. 00052 * \tparam T The type of the quaternion (e.g. float, double). 00053 * \exception std::exception Only in Debug, if t is not in the valid range. 00054 * \sa http://en.wikipedia.org/wiki/Slerp 00055 */ 00056 template <typename T> 00057 void slerp( 00058 const CQuaternion<T> & q0, 00059 const CQuaternion<T> & q1, 00060 const double t, 00061 CQuaternion<T> & q) 00062 { 00063 ASSERTDEB_(t>=0 && t<=1) 00064 // See: http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/slerp/index.htm 00065 // Angle between q0-q1: 00066 double cosHalfTheta = q0[0]*q1[0]+q0[1]*q1[1]+q0[2]*q1[2]+q0[3]*q1[3]; 00067 // if qa=qb or qa=-qb then theta = 0 and we can return qa 00068 if (std::abs(cosHalfTheta) >= 1.0) 00069 { 00070 q = q0; 00071 return; 00072 } 00073 bool reverse_q1 = false; 00074 if (cosHalfTheta < 0) // Always follow the shortest path 00075 { 00076 reverse_q1 = true; 00077 cosHalfTheta = -cosHalfTheta; 00078 } 00079 // Calculate temporary values. 00080 const double halfTheta = acos(cosHalfTheta); 00081 const double sinHalfTheta = std::sqrt(1.0 - square(cosHalfTheta)); 00082 // if theta = 180 degrees then result is not fully defined 00083 // we could rotate around any axis normal to qa or qb 00084 if (std::abs(sinHalfTheta) < 0.001) 00085 { 00086 if (!reverse_q1) 00087 for (int i=0;i<4;i++) q[i] = (1-t)*q0[i] + t*q1[i]; 00088 else for (int i=0;i<4;i++) q[i] = (1-t)*q0[i] - t*q1[i]; 00089 return; 00090 } 00091 const double A = sin((1-t) * halfTheta)/sinHalfTheta; 00092 const double B = sin(t*halfTheta)/sinHalfTheta; 00093 if (!reverse_q1) 00094 for (int i=0;i<4;i++) q[i] = A*q0[i] + B*q1[i]; 00095 else for (int i=0;i<4;i++) q[i] = A*q0[i] - B*q1[i]; 00096 } 00097 00098 /** SLERP interpolation between two 6D poses - like mrpt::math::slerp for quaternions, but interpolates the [X,Y,Z] coordinates as well. 00099 * \param[in] p0 The pose for t=0 00100 * \param[in] p1 The pose for t=1 00101 * \param[in] t A "time" parameter, in the range [0,1]. 00102 * \param[out] p The output, interpolated pose. 00103 * \exception std::exception Only in Debug, if t is not in the valid range. 00104 */ 00105 void BASE_IMPEXP slerp( 00106 const CPose3D & q0, 00107 const CPose3D & q1, 00108 const double t, 00109 CPose3D & p); 00110 00111 //! \overload 00112 void BASE_IMPEXP slerp( 00113 const CPose3DQuat & q0, 00114 const CPose3DQuat & q1, 00115 const double t, 00116 CPose3DQuat & p); 00117 00118 /** @} */ 00119 00120 /** @} */ // grouping 00121 } 00122 } 00123 #endif
| Page generated by Doxygen 1.7.5 for MRPT 0.9.5 SVN: at Thu Oct 13 21:25:36 UTC 2011 |