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

ROBOOP v1.21a
Generated Thu Nov 22 00:51:28 2007 by Doxygen 1.5.4