Homepage Demos Overview Downloads Tutorials Reference
Credits

quaternion.cpp

Go to the documentation of this file.
00001 /*
00002 Copyright (C) 2002-2004  Etienne Lachance
00003 
00004 This library is free software; you can redistribute it and/or modify
00005 it under the terms of the GNU Lesser General Public License as
00006 published by the Free Software Foundation; either version 2.1 of the
00007 License, or (at your option) any later version.
00008 
00009 This library is distributed in the hope that it will be useful,
00010 but WITHOUT ANY WARRANTY; without even the implied warranty of
00011 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00012 GNU Lesser General Public License for more details.
00013 
00014 You should have received a copy of the GNU Lesser General Public
00015 License along with this library; if not, write to the Free Software
00016 Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
00017 
00018 
00019 Report problems and direct all questions to:
00020 
00021 email: etienne.lachance@polymtl.ca or richard.gourdeau@polymtl.ca
00022 
00023 -------------------------------------------------------------------------------
00024 Revision_history:
00025 
00026 2004/01/19: Etienne Lachance
00027     -Removed function Exp and Ln.
00028     -Added function power and Log.
00029     -Fixed bugs in Slerp, Slerp_prime, Squad and Squad_prime.
00030 
00031 2003/05/23: Etienne Lachance
00032     -Added the following member function -=, +=, *=, /=, Exp, Ln, dot, d_dt, E
00033     -Added functions Integ_Trap_quat, Slerp, Slerp_prime, Squad, Squad_prime,
00034 
00035 2004/05/14: Etienne Lachance
00036     -Replaced vec_x_prod by CrossProduct.
00037 
00038 2004/05/21: Etienne Lachance
00039    -Added comments that can be used with Doxygen.
00040 
00041 2004/07/01: Etienne Lachance
00042    -Replaced vec_dot_prod by DotProdut of Newmat library.
00043 
00044 2004/07/01: Ethan Tira-Thompson
00045     -Added support for newmat's use_namespace #define, using ROBOOP namespace
00046     -Fixed problem in constructor using float as Real type
00047 -------------------------------------------------------------------------------
00048 */
00049 
00050 
00051 /*!
00052   @file quaternion.cpp
00053   @brief Quaternion functions.
00054 */
00055 
00056 //! @brief RCS/CVS version.
00057 static const char rcsid[] = "$Id: quaternion.cpp,v 1.4 2004/07/14 02:32:12 ejt Exp $";
00058 
00059 #include "quaternion.h"
00060 
00061 #ifdef use_namespace
00062 namespace ROBOOP {
00063   using namespace NEWMAT;
00064 #endif
00065 
00066 
00067 Quaternion::Quaternion()
00068 //! @brief Constructor.
00069 {
00070    s_ = 1.0;
00071    v_ = ColumnVector(3);
00072    v_ = 0.0;
00073 }
00074 
00075 Quaternion::Quaternion(const Quaternion & q)
00076 //! @brief Constructor.
00077 {
00078    s_ = q.s_;
00079    v_ = q.v_;
00080 }
00081 
00082 Quaternion::Quaternion(const Real angle, const ColumnVector & axis)
00083 //! @brief Constructor.
00084 {
00085    if(axis.Nrows() != 3)
00086    {
00087       cerr << "Quaternion::Quaternion, size of axis != 3" << endl;
00088       exit(1);
00089    }
00090 
00091    // make sure axis is a unit vector
00092    Real norm_axis = sqrt(DotProduct(axis, axis));
00093 
00094    if(norm_axis != 1)
00095    {
00096       cerr << "Quaternion::Quaternion(angle, axis), axis is not unit" << endl;
00097       cerr << "Make the axis unit." << endl;
00098       v_ = sin(angle/2) * axis/norm_axis;
00099    }
00100    else
00101       v_ = sin(angle/2) * axis;
00102 
00103    s_ = cos(angle/2);
00104 }
00105 
00106 Quaternion::Quaternion(const Real s_in, const Real v1, const Real v2,
00107                        const Real v3)
00108 //!  @brief Constructor.
00109 {
00110    s_ = s_in;
00111    v_ = ColumnVector(3);
00112    v_(1) = v1;
00113    v_(2) = v2;
00114    v_(3) = v3;
00115 }
00116 
00117 Quaternion::Quaternion(const Matrix & R)
00118 /*!
00119   @brief Constructor.
00120 
00121   Cite_: Dam.
00122   The unit quaternion obtained from a matrix (see Quaternion::R())
00123   \f[
00124   R(s,v) =
00125    \left[
00126     \begin{array}{ccc}
00127       s^2+v_1^2-v_2^2-v_3^2 & 2v_1v_2+2sv_3 & 2v_1v_3-2sv_2 \\
00128       2v_1v_2-2sv_3 & s^2-v_1^2+v_2^2-v_3^2 & 2v_2v_3+2sv_1 \\
00129       2v_1v_3+2sv_2 &2v_2v_3-2sv_1 & s^2-v_1^2-v_2^2+v_3^2  
00130     \end{array}
00131    \right]
00132   \f]
00133   
00134   First we find \f$s\f$:
00135   \f[
00136    R_{11} + R_{22} + R_{33} + R_{44} = 4s^2
00137   \f]
00138   Now the other values are:
00139   \f[
00140     s = \pm \frac{1}{2}\sqrt{R_{11} + R_{22} + R_{33} + R_{44}}
00141   \f]
00142   \f[
00143    v_1 = \frac{R_{32}-R_{23}}{4s}
00144   \f]
00145   \f[
00146    v_2 = \frac{R_{13}-R_{31}}{4s}
00147   \f]
00148   \f[
00149    v_3 = \frac{R_{21}-R_{12}}{4s}
00150   \f]
00151 
00152   The sign of \f$s\f$ cannot be determined. Depending on the choice of the sign 
00153   for s the sign of \f$v\f$ change as well. Thus the quaternions \f$q\f$ and 
00154   \f$-q\f$ represent the same rotation, but the interpolation curve changed with
00155   the choice of the sign. 
00156   A positive sign has been chosen.
00157 */
00158 {
00159    if( (R.Nrows() == 3) && (R.Ncols() == 3) ||
00160          (R.Nrows() == 4) && (R.Ncols() == 4) )
00161    {
00162       Real tmp = fabs(R(1,1) + R(2,2) + R(3,3) + 1);
00163       s_ = 0.5*sqrt(tmp);
00164       if(v_.Nrows() != 3)
00165          v_ = ColumnVector(3);
00166 
00167       if(s_ > EPSILON)
00168       {
00169          v_(1) = (R(3,2)-R(2,3))/(4*s_);
00170          v_(2) = (R(1,3)-R(3,1))/(4*s_);
00171          v_(3) = (R(2,1)-R(1,2))/(4*s_);
00172       }
00173       else
00174       {
00175          // |w| <= 1/2
00176          static int s_iNext[3] = { 2, 3, 1 };
00177          int i = 1;
00178          if ( R(2,2) > R(1,1) )
00179             i = 2;
00180          if ( R(3,3) > R(2,2) )
00181             i = 3;
00182          int j = s_iNext[i-1];
00183          int k = s_iNext[j-1];
00184 
00185          Real fRoot = sqrt(R(i,i)-R(j,j)-R(k,k) + 1.0);
00186 
00187          Real *tmp[3] = { &v_(1), &v_(2), &v_(3) };
00188          *tmp[i-1] = 0.5*fRoot;
00189          fRoot = 0.5/fRoot;
00190          s_ = (R(k,j)-R(j,k))*fRoot;
00191          *tmp[j-1] = (R(j,i)+R(i,j))*fRoot;
00192          *tmp[k-1] = (R(k,i)+R(i,k))*fRoot;
00193       }
00194 
00195    }
00196    else
00197       cerr << "Quaternion::Quaternion: matrix input is not 3x3 or 4x4" << endl;
00198 }
00199 
00200 Quaternion & Quaternion::operator=(const Quaternion & q)
00201 //! @brief Overload = operator.
00202 {
00203    s_ = q.s_;
00204    v_ = q.v_;
00205 
00206    return *this;
00207 }
00208 
00209 Quaternion Quaternion::operator+(const Quaternion & rhs)const
00210 /*!
00211   @brief Overload + operator.
00212 
00213   The quaternion addition is 
00214   \f[
00215   q_1 + q_2 = [s_1, v_1] + [s_2, v_2] = [s_1+s_2, v_1+v_2]
00216   \f]
00217 
00218   The result is not necessarily a unit quaternion even if \f$q_1\f$ and
00219   \f$q_2\f$ are unit quaternions.
00220 */
00221 {
00222    Quaternion q;
00223    q.s_ = s_ + rhs.s_;
00224    q.v_ = v_ + rhs.v_;
00225 
00226    return q;
00227 }
00228 
00229 Quaternion Quaternion::operator-(const Quaternion & rhs)const
00230 /*!
00231   @brief Overload - operator.
00232 
00233   The quaternion soustraction is 
00234   \f[
00235   q_1 - q_2 = [s_1, v_1] - [s_2, v_2] = [s_1-s_2, v_1-v_2]
00236   \f]
00237 
00238   The result is not necessarily a unit quaternion even if \f$q_1\f$ and
00239   \f$q_2\f$ are unit quaternions.
00240 */
00241 {
00242    Quaternion q;
00243    q.s_ = s_ - rhs.s_;
00244    q.v_ = v_ - rhs.v_;
00245 
00246    return q;
00247 }
00248 
00249 Quaternion Quaternion::operator*(const Quaternion & rhs)const
00250 /*!
00251   @brief Overload * operator.
00252 
00253   The multiplication of two quaternions is
00254 
00255   \f[
00256   q = q_1q_2 = [s_1s_2 - v_1\cdot v_2, v_1 \times v_2 + s_1v_2 + s_2v_1]
00257   \f]
00258   where \f$\cdot\f$ and \f$\times\f$ denote the scalar and vector product
00259   in \f$R^3\f$ respectively.
00260 
00261   If \f$q_1\f$ and \f$q_2\f$ are unit quaternions, then q will also be a 
00262   unit quaternion.
00263 */
00264 {
00265    Quaternion q;
00266    q.s_ = s_ * rhs.s_ - DotProduct(v_, rhs.v_);
00267    q.v_ = s_ * rhs.v_ + rhs.s_ * v_ + CrossProduct(v_, rhs.v_);
00268 
00269    return q;
00270 }
00271 
00272 Quaternion Quaternion::operator*(const Real c)const
00273 /*!
00274   @brief Overload * operator, multiplication by a scalar.
00275 
00276   \f$q = [s, v]\f$ and let \f$r \in R\f$. Then
00277   \f$rq = qr = [r, 0][s, v] = [rs, rv]\f$
00278 
00279   The result is not necessarily a unit quaternion even if \f$q\f$ 
00280   is a unit quaternions.
00281 */
00282 {
00283    Quaternion q;
00284    q.s_ = s_ * c;
00285    q.v_ = v_ * c;
00286 
00287    return q;
00288 }
00289 
00290 Quaternion Quaternion::operator/(const Quaternion & rhs)const
00291 //! @brief Overload / operator.
00292 {
00293 //   Quaternion q = rhs;
00294 //   q = *this * q.conjugate();
00295 //   q = *this * q.i();
00296 
00297 //   return q;
00298 
00299     return *this*rhs.i();
00300 }
00301 
00302 Quaternion Quaternion::operator/(const Real c)const
00303 /*!
00304   @brief Overload / operator, division by a scalar.
00305 
00306   Same explanation as multiplication by scaler.
00307 */
00308 {
00309    Quaternion q;
00310    q.s_ = s_ / c;
00311    q.v_ = v_ / c;
00312 
00313    return q;
00314 }
00315 
00316 void Quaternion::set_v(const ColumnVector & v)
00317 //! @brief Set quaternion vector part.
00318 {
00319    if(v.Nrows() == 3)
00320       v_ = v;
00321    else
00322        cerr << "Quaternion::set_v: input has a wrong size." << endl;
00323 }
00324 
00325 Quaternion Quaternion::conjugate()const
00326 /*!
00327   @brief Conjugate.
00328 
00329   The conjugate of a quaternion \f$q = [s, v]\f$ is
00330   \f$q^{*} = [s, -v]\f$
00331 */
00332 {
00333    Quaternion q;
00334    q.s_ = s_;
00335    q.v_ = -1*v_;
00336 
00337    return q;
00338 }
00339 
00340 Real Quaternion::norm()const 
00341 /*!
00342   @brief Return the quaternion norm.
00343 
00344   The norm of quaternion is defined by
00345   \f[
00346   N(q) = s^2 + v\cdot v
00347   \f]
00348 */
00349 { 
00350   return( sqrt(s_*s_ + DotProduct(v_, v_)) );
00351 }
00352 
00353 Quaternion & Quaternion::unit()
00354 //! @brief Normalize a quaternion.
00355 {
00356    Real tmp = norm();
00357    if(tmp > EPSILON)
00358    {
00359       s_ = s_/tmp;
00360       v_ = v_/tmp;
00361    }
00362    return *this;
00363 }
00364 
00365 Quaternion Quaternion::i()const 
00366 /*!
00367   @brief Quaternion inverse.
00368   \f[
00369     q^{-1} = \frac{q^{*}}{N(q)}
00370   \f]
00371   where \f$q^{*}\f$ and \f$N(q)\f$ are the quaternion
00372   conjugate and the quaternion norm respectively.
00373 */
00374 { 
00375     return conjugate()/norm();
00376 }
00377 
00378 Quaternion Quaternion::exp() const
00379 /*!
00380   @brief Exponential of a quaternion.
00381 
00382   Let a quaternion of the form \f$q = [0, \theta v]\f$, q is not
00383   necessarily a unit quaternion. Then the exponential function
00384   is defined by \f$q = [\cos(\theta),v \sin(\theta)]\f$.
00385 */
00386 {
00387    Quaternion q;
00388    Real theta = sqrt(DotProduct(v_,v_)),
00389                 sin_theta = sin(theta);
00390 
00391    q.s_ = cos(theta);
00392    if ( fabs(sin_theta) > EPSILON)
00393       q.v_ = v_*sin_theta/theta;
00394    else
00395       q.v_ = v_;
00396 
00397    return q;
00398 }
00399 
00400 Quaternion Quaternion::power(const Real t) const
00401 {
00402    Quaternion q = (Log()*t).exp();
00403 
00404    return q;
00405 }
00406 
00407 Quaternion Quaternion::Log()const
00408 /*!
00409   @brief Logarithm of a unit quaternion.
00410 
00411   The logarithm function of a unit quaternion 
00412   \f$q = [\cos(\theta), v \sin(\theta)]\f$ is defined as 
00413   \f$log(q) = [0, v\theta]\f$. The result is not necessary 
00414   a unit quaternion.
00415 */
00416 {
00417    Quaternion q;
00418    q.s_ = 0;
00419    Real theta = acos(s_),
00420                 sin_theta = sin(theta);
00421 
00422    if ( fabs(sin_theta) > EPSILON)
00423       q.v_ = v_/sin_theta*theta;
00424    else
00425       q.v_ = v_;
00426 
00427    return q;
00428 }
00429 
00430 Quaternion Quaternion::dot(const ColumnVector & w, const short sign)const
00431 /*!
00432   @brief Quaternion time derivative.
00433 
00434   The quaternion time derivative, quaternion propagation equation, is
00435   \f[
00436     \dot{s} = - \frac{1}{2}v^Tw_{_0}
00437   \f]
00438   \f[
00439     \dot{v} = \frac{1}{2}E(s,v)w_{_0}
00440   \f]
00441   \f[
00442     E = sI - S(v)
00443   \f]
00444   where \f$w_{_0}\f$ is the angular velocity vector expressed in the base
00445   frame. If the vector is expressed in the object frame, \f$w_{_b}\f$, the
00446   time derivative becomes
00447   \f[
00448    \dot{s} = - \frac{1}{2}v^Tw_{_b}
00449   \f]
00450   \f[
00451    \dot{v} = \frac{1}{2}E(s,v)w_{_b}
00452   \f]
00453   \f[
00454    E = sI + S(v)
00455   \f]
00456 */
00457 {
00458    Quaternion q;
00459    Matrix tmp;
00460 
00461    tmp = -0.5*v_.t()*w;
00462    q.s_ = tmp(1,1);
00463    q.v_ = 0.5*E(sign)*w;
00464 
00465    return q;
00466 }
00467 
00468 ReturnMatrix Quaternion::E(const short sign)const
00469 /*!
00470   @brief Matrix E.
00471 
00472   See Quaternion::dot for explanation.
00473 */
00474 {
00475    Matrix E(3,3), I(3,3);
00476    I << threebythreeident;
00477 
00478    if(sign == BODY_FRAME)
00479       E = s_*I + x_prod_matrix(v_);
00480    else
00481       E = s_*I - x_prod_matrix(v_);
00482 
00483    E.Release();
00484    return E;
00485 }
00486 
00487 Real Quaternion::dot_prod(const Quaternion & q)const
00488 /*!
00489   @brief Quaternion dot product.
00490 
00491   The dot product of quaternion is defined by  
00492   \f[
00493   q_1\cdot q_2 = s_1s_2 + v_1 \cdot v_2
00494   \f]
00495 */
00496 {
00497    return (s_*q.s_ + v_(1)*q.v_(1) + v_(2)*q.v_(2) + v_(3)*q.v_(3));
00498 }
00499 
00500 ReturnMatrix Quaternion::R()const
00501 /*!
00502   @brief Rotation matrix from a unit quaternion.
00503 
00504   \f$p'=qpq^{-1} = Rp\f$ where \f$p\f$ is a vector, \f$R\f$ a rotation
00505   matrix and \f$q\f$ q quaternion. The rotation matrix obtained from a 
00506   quaternion is then
00507   \f[
00508   R(s,v) = (s^2 - v^Tv)I + 2vv^T - 2s S(v)
00509   \f]
00510   \f[
00511   R(s,v) =
00512    \left[
00513     \begin{array}{ccc}
00514       s^2+v_1^2-v_2^2-v_3^2 & 2v_1v_2+2sv_3 & 2v_1v_3-2sv_2 \\
00515       2v_1v_2-2sv_3 & s^2-v_1^2+v_2^2-v_3^2 & 2v_2v_3+2sv_1 \\
00516       2v_1v_3+2sv_2 &2v_2v_3-2sv_1 & s^2-v_1^2-v_2^2+v_3^2  
00517     \end{array}
00518    \right]
00519   \f]
00520   where \f$S(\cdot)\f$ is the cross product matrix defined by
00521   \f[
00522     S(u) = 
00523      \left[
00524       \begin{array}{ccc}
00525         0 & -u_3 & u_2 \\
00526   u_3 &0 & -u_1  \\
00527   -u_2 & u_1 & 0 \\
00528       \end{array}
00529      \right]
00530   \f]
00531 */
00532 {
00533    Matrix R(3,3);
00534    R << threebythreeident;
00535    R = (1 - 2*DotProduct(v_, v_))*R + 2*v_*v_.t() + 2*s_*x_prod_matrix(v_);
00536 
00537    R.Release();
00538    return R;
00539 }
00540 
00541 ReturnMatrix Quaternion::T()const
00542 /*!
00543   @brief Transformation matrix from a quaternion.
00544 
00545   See Quaternion::R() for equations.
00546 */
00547 {
00548    Matrix T(4,4);
00549    T << fourbyfourident;
00550    T.SubMatrix(1,3,1,3) = (1 - 2*DotProduct(v_, v_))*T.SubMatrix(1,3,1,3)
00551                           + 2*v_*v_.t() + 2*s_*x_prod_matrix(v_);
00552    T.Release();
00553    return T;
00554 }
00555 
00556 // -------------------------------------------------------------------------------------
00557 
00558 ReturnMatrix Omega(const Quaternion & q, const Quaternion & q_dot)
00559 /*!
00560   @brief Return angular velocity from a quaternion and it's time derivative
00561 
00562   See Quaternion::dot for explanation.
00563 */
00564 {
00565    Matrix A, B, M;
00566    UpperTriangularMatrix U;
00567    ColumnVector w(3);
00568    A = 0.5*q.E(BASE_FRAME);
00569    B = q_dot.v();
00570    if(A.Determinant())
00571    {
00572       QRZ(A,U);             //QR decomposition
00573       QRZ(A,B,M);
00574       w = U.i()*M;
00575    }
00576    else
00577       w = 0;
00578 
00579    w.Release();
00580    return w;
00581 }
00582 
00583 short Integ_quat(Quaternion & dquat_present, Quaternion & dquat_past,
00584                  Quaternion & quat, const Real dt)
00585 //! @brief Trapezoidal quaternion integration.
00586 {
00587    if (dt < 0)
00588    {
00589       cerr << "Integ_Trap(quat1, quat2, dt): dt < 0. dt is set to 0." << endl;
00590       return -1;
00591    }
00592 
00593    // Quaternion algebraic constraint
00594    //  Real Klambda = 0.5*(1 - quat.norm_sqr());
00595 
00596    dquat_present.set_s(dquat_present.s() );//+ Klambda*quat.s());
00597    dquat_present.set_v(dquat_present.v() ); //+ Klambda*quat.v());
00598 
00599    quat.set_s(quat.s() + Integ_Trap_quat_s(dquat_present, dquat_past, dt));
00600    quat.set_v(quat.v() + Integ_Trap_quat_v(dquat_present, dquat_past, dt));
00601 
00602    dquat_past.set_s(dquat_present.s());
00603    dquat_past.set_v(dquat_present.v());
00604 
00605    quat.unit();
00606 
00607    return 0;
00608 }
00609 
00610 Real Integ_Trap_quat_s(const Quaternion & present, Quaternion & past,
00611                        const Real dt)
00612 //! @brief Trapezoidal quaternion scalar part integration.
00613 {
00614    Real integ = 0.5*(present.s()+past.s())*dt;
00615    past.set_s(present.s());
00616    return integ;
00617 }
00618 
00619 ReturnMatrix Integ_Trap_quat_v(const Quaternion & present, Quaternion & past,
00620                                const Real dt)
00621 //! @brief Trapezoidal quaternion vector part integration.
00622 {
00623    ColumnVector integ = 0.5*(present.v()+past.v())*dt;
00624    past.set_v(present.v());
00625    integ.Release();
00626    return integ;
00627 }
00628 
00629 Quaternion Slerp(const Quaternion & q0, const Quaternion & q1, const Real t)
00630 /*!
00631   @brief Spherical Linear Interpolation.
00632 
00633   Cite_:Dam
00634 
00635   The quaternion \f$q(t)\f$ interpolate the quaternions \f$q_0\f$ 
00636   and \f$q_1\f$ given the parameter \f$t\f$ along the quaternion sphere.
00637   \f[
00638    q(t) = c_0(t)q_0 + c_1(t)q_1
00639   \f]
00640   where \f$c_0\f$ and \f$c_1\f$ are real functions with \f$0\leq t \leq 1\f$.
00641   As \f$t\f$ varies between 0 and 1. the values \f$q(t)\f$ varies uniformly 
00642   along the circular arc from \f$q_0\f$ and \f$q_1\f$. The angle between 
00643   \f$q(t)\f$ and \f$q_0\f$ is \f$\cos(t\theta)\f$ and the angle between
00644   \f$q(t)\f$ and \f$q_1\f$ is \f$\cos((1-t)\theta)\f$. Taking the dot product
00645   of \f$q(t)\f$ and \f$q_0\f$ yields
00646   \f[
00647    \cos(t\theta) = c_0(t) + \cos(\theta)c_1(t)
00648   \f]
00649   and taking the dot product of \f$q(t)\f$ and \f$q_1\f$ yields
00650   \f[
00651    \cos((1-t)\theta) = \cos(\theta)c_0(t) + c_1(t)
00652   \f]
00653   These are two equations with \f$c_0\f$ and \f$c_1\f$. The solution is
00654   \f[
00655    c_0 = \frac{\sin((1-t)\theta)}{\sin(\theta)}
00656   \f]
00657   \f[
00658    c_1 = \frac{\sin(t\theta)}{sin(\theta)}
00659   \f]
00660   The interpolation is then
00661   \f[
00662    Slerp(q_0, q_1, t) = \frac{q_0\sin((1-t)\theta)+q_1\sin(t\theta)}{\sin(\theta)}
00663   \f]
00664   If \f$q_0\f$ and \f$q_1\f$ are unit quaternions the \f$q(t)\f$ is also a unit
00665   quaternions. For unit quaternions we have
00666   \f[
00667    Slerp(q_0, q_1, t) = q_0(q_0^{-1}q_1)^t
00668   \f]
00669   For t = 0 and t = 1 we have
00670   \f[
00671    q_0 = Slerp(q_0, q_1, 0)
00672   \f]
00673   \f[
00674    q_1 = Slerp(q_0, q_1, 1)
00675   \f]
00676   It is customary to choose the sign G on q1 so that q0.Gq1 >=0 (the angle
00677   between q0 ang Gq1 is acute). This choice avoids extra spinning caused
00678   by the interpolated rotations.
00679 */
00680 {
00681    if( (t < 0) || (t > 1) )
00682       cerr << "Slerp(q0, q1, t): t < 0 or t > 1. t is set to 0." << endl;
00683 
00684    if(q0.dot_prod(q1) >= 0)
00685       return q0*((q0.i()*q1).power(t));
00686    else
00687       return  q0*((q0.i()*-1*q1).power(t));
00688 }
00689 
00690 Quaternion Slerp_prime(const Quaternion & q0, const Quaternion & q1,
00691                        const Real t)
00692 /*!
00693   @brief Spherical Linear Interpolation derivative.
00694 
00695   Cite_: Dam
00696 
00697   The derivative of the function \f$q^t\f$ where \f$q\f$ is a constant
00698   unit quaternion is
00699   \f[
00700    \frac{d}{dt}q^t = q^t log(q)
00701   \f]
00702   Using the preceding equation the Slerp derivative is then
00703   \f[
00704    Slerp'(q_0, q_1, t) = q_0(q_0^{-1}q_1)^t log(q_0^{-1}q_1)
00705   \f]
00706 
00707   It is customary to choose the sign G on q1 so that q0.Gq1 >=0 (the angle
00708   between q0 ang Gq1 is acute). This choice avoids extra spinning caused
00709   by the interpolated rotations.
00710   The result is not necessary a unit quaternion.
00711 */
00712 
00713 {
00714    if( (t < 0) || (t > 1) )
00715       cerr << "Slerp_prime(q0, q1, t): t < 0 or t > 1. t is set to 0." << endl;
00716 
00717    if(q0.dot_prod(q1) >= 0)
00718       return Slerp(q0, q1, t)*(q0.i()*q1).Log();
00719    else
00720       return Slerp(q0, q1, t)*(q0.i()*-1*q1).Log();
00721 }
00722 
00723 Quaternion Squad(const Quaternion & p, const Quaternion & a, const Quaternion & b,
00724                  const Quaternion & q, const Real t)
00725 /*!
00726   @brief Spherical Cubic Interpolation.
00727 
00728   Cite_: Dam
00729 
00730   Let four quaternions be \f$q_i\f$ (p), \f$s_i\f$ (a), \f$s_{i+1}\f$ (b) and \f$q_{i+1}\f$ 
00731   (q) be the ordered vertices of a quadrilateral. Obtain c from \f$q_i\f$ to \f$q_{i+1}\f$ 
00732   interpolation. Obtain d from \f$s_i\f$ to \f$s_{i+1}\f$ interpolation. Obtain e,
00733   the final result, from c to d interpolation.
00734   \f[
00735    Squad(q_i, s_i, s_{i+1}, q_{i+1}, t) = Slerp(Slerp(q_i,q_{i+1},t),Slerp(s_i,s_{i+1},t), 2t(1-t))
00736   \f]
00737   The intermediate quaternion \f$s_i\f$ and \f$s_{i+1}\f$ are given by
00738   \f[
00739    s_i = q_i exp\Big ( - \frac{log(q_i^{-1}q_{i+1}) + log(q_i^{-1}q_{i-1})}{4}\Big )
00740   \f]
00741 */
00742 {
00743    if( (t < 0) || (t > 1) )
00744       cerr << "Squad(p,a,b,q, t): t < 0 or t > 1. t is set to 0." << endl;
00745 
00746    return Slerp(Slerp(p,q,t),Slerp(a,b,t),2*t*(1-t));
00747 }
00748 
00749 Quaternion Squad_prime(const Quaternion & p, const Quaternion & a, const Quaternion & b,
00750                        const Quaternion & q, const Real t)
00751 /*!
00752   @brief Spherical Cubic Interpolation derivative.
00753 
00754   Cite_: www.magic-software.com
00755 
00756 
00757   The derivative of the function \f$q^t\f$ where \f$q\f$ is a constant
00758   unit quaternion is
00759   \f[
00760    \frac{d}{dt}q^t = q^t log(q)
00761   \f]
00762   Recalling that \f$log(q) = [0, v\theta]\f$ (see Quaternion::Log()). If the power
00763   is a function we have
00764   \f[
00765    \frac{d}{dt}q^{f(t)} = f'(t)q^{f(t)}log(q)
00766   \f]
00767   If \f$q\f$ is a function of time and the power is differentiable function of time
00768   we have
00769   \f[
00770   \frac{d}{dt}(q(t))^{f(t)} = f'(t)(q(t))^{f(t)}log(q) + f(t)(q(t))^{f(t)-1}q'(t)
00771   \f]
00772   Using these last three equations Squad derivative can be define. Let 
00773   \f$U(t)=Slerp(p,q,t)\f$, \f$V(t)=Slerp(q,b,t)\f$, \f$W(t)=U(t)^{-1}V(t)\f$. We then
00774   have \f$Squad(p,a,b,q,t)=Slerp(U(t),V(t),2t(1-t))=U(t)W(t)^{2t(1-t)}\f$
00775 
00776   \f[
00777    Squad'(p,a,b,q,t) = \frac{d}{dt}\Big [ UW^{2t(1-t)}\Big ]
00778   \f]
00779   \f[
00780    Squad'(p,a,b,q,t) = U\frac{d}{dt}\Big [ W^{2t(1-t)}\Big ] + U'\Big [W^{2t(1-t)}\Big]
00781   \f]
00782   \f[
00783    Squad'(p,a,b,q,t) = U\Big[(2-4t)W^{2t(1-t)}log(W)+2t(1-t)W^{2t(1-t)-1}W'\Big]
00784     + U'\Big[W^{2t(1-t)} \Big]
00785   \f]
00786   where \f$U'=Ulog(p^{-1}q)\f$, \f$V'=Vlog(a^{-1},b)\f$, \f$W'=U^{-1}V'-U^{-2}U'V\f$
00787   
00788 
00789   The result is not necessarily a unit quaternion even if all the input quaternions are unit.
00790 */
00791 {
00792    if( (t < 0) || (t > 1) )
00793       cerr << "Squad_prime(p,a,b,q, t): t < 0 or t > 1. t is set to 0." << endl;
00794 
00795    Quaternion q_squad,
00796    U = Slerp(p, q, t),
00797        V = Slerp(a, b, t),
00798            W = U.i()*V,
00799                U_prime = U*(p.i()*q).Log(),
00800                          V_prime = V*(a.i()*b).Log(),
00801                                    W_prime = U.i()*V_prime - U.power(-2)*U_prime*V;
00802 
00803    q_squad = U*( W.power(2*t*(1-t))*W.Log()*(2-4*t) + W.power(2*t*(1-t)-1)*W_prime*2*t*(1-t) )
00804              + U_prime*( W.power(2*t*(1-t)) );
00805 
00806    return q_squad;
00807 }
00808 
00809 #ifdef use_namespace
00810 }
00811 #endif
00812 
00813 
00814 
00815 
00816 
00817 

ROBOOP v1.21a
Generated Tue Oct 19 14:18:25 2004 by Doxygen 1.3.9.1