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:16:28 UTC 2011 |