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 2005/11/06: Etienne Lachance
00049     - No need to provide a copy constructor and the assignment operator 
00050       (operator=) for Quaternion class. Instead we use the one provide by the
00051       compiler.
00052 
00053 2005/11/13: Etienne Lachance
00054     - operator* and operator/ are now non-member functions when one of the
00055       operand is a real. With these modifications we support q2 = c * q1 and
00056       q2 = q1 * c
00057 -------------------------------------------------------------------------------
00058 */
00059 
00060 
00066 
00067 static const char rcsid[] = "$Id: quaternion.cpp,v 1.18 2005/11/15 19:25:58 gourdeau Exp $";
00068 
00069 #include "quaternion.h"
00070 
00071 #ifdef use_namespace
00072 namespace ROBOOP {
00073   using namespace NEWMAT;
00074 #endif
00075 
00076 
00077 Quaternion::Quaternion()
00079 {
00080    s_ = 1.0;
00081    v_ = ColumnVector(3);
00082    v_ = 0.0;
00083 }
00084 
00085 Quaternion::Quaternion(const Real angle, const ColumnVector & axis)
00087 {
00088    if(axis.Nrows() != 3)
00089    {
00090       cerr << "Quaternion::Quaternion, size of axis != 3" << endl;
00091       exit(1);
00092    }
00093 
00094    // make sure axis is a unit vector
00095    Real norm_axis = sqrt(DotProduct(axis, axis));
00096 
00097    if(norm_axis != 1)
00098    {
00099       cerr << "Quaternion::Quaternion(angle, axis), axis is not unit" << endl;
00100       cerr << "Make the axis unit." << endl;
00101       v_ = sin(angle/2) * axis/norm_axis;
00102    }
00103    else
00104       v_ = sin(angle/2) * axis;
00105 
00106    s_ = cos(angle/2);
00107 }
00108 
00109 Quaternion::Quaternion(const Real s_in, const Real v1, const Real v2,
00110                        const Real v3)
00112 {
00113    s_ = s_in;
00114    v_ = ColumnVector(3);
00115    v_(1) = v1;
00116    v_(2) = v2;
00117    v_(3) = v3;
00118 }
00119 
00120 Quaternion::Quaternion(const Matrix & R)
00161 {
00162    if( (R.Nrows() == 3) && (R.Ncols() == 3) ||
00163          (R.Nrows() == 4) && (R.Ncols() == 4) )
00164    {
00165       Real tmp = fabs(R(1,1) + R(2,2) + R(3,3) + 1);
00166       s_ = 0.5*sqrt(tmp);
00167       if(v_.Nrows() != 3)
00168          v_ = ColumnVector(3);
00169 
00170       if(s_ > EPSILON)
00171       {
00172          v_(1) = (R(3,2)-R(2,3))/(4*s_);
00173          v_(2) = (R(1,3)-R(3,1))/(4*s_);
00174          v_(3) = (R(2,1)-R(1,2))/(4*s_);
00175       }
00176       else
00177       {
00178          // |w| <= 1/2
00179          static int s_iNext[3] = { 2, 3, 1 };
00180          int i = 1;
00181          if ( R(2,2) > R(1,1) )
00182             i = 2;
00183          if ( R(3,3) > R(2,2) )
00184             i = 3;
00185          int j = s_iNext[i-1];
00186          int k = s_iNext[j-1];
00187 
00188          Real fRoot = sqrt(R(i,i)-R(j,j)-R(k,k) + 1.0);
00189 
00190          Real *tmp[3] = { &v_(1), &v_(2), &v_(3) };
00191          *tmp[i-1] = 0.5*fRoot;
00192          fRoot = 0.5/fRoot;
00193          s_ = (R(k,j)-R(j,k))*fRoot;
00194          *tmp[j-1] = (R(j,i)+R(i,j))*fRoot;
00195          *tmp[k-1] = (R(k,i)+R(i,k))*fRoot;
00196       }
00197 
00198    }
00199    else
00200       cerr << "Quaternion::Quaternion: matrix input is not 3x3 or 4x4" << endl;
00201 }
00202 
00203 Quaternion Quaternion::operator+(const Quaternion & rhs)const
00215 {
00216    Quaternion q;
00217    q.s_ = s_ + rhs.s_;
00218    q.v_ = v_ + rhs.v_;
00219 
00220    return q;
00221 }
00222 
00223 Quaternion Quaternion::operator-(const Quaternion & rhs)const
00235 {
00236    Quaternion q;
00237    q.s_ = s_ - rhs.s_;
00238    q.v_ = v_ - rhs.v_;
00239 
00240    return q;
00241 }
00242 
00243 Quaternion Quaternion::operator*(const Quaternion & rhs)const
00258 {
00259    Quaternion q;
00260    q.s_ = s_ * rhs.s_ - DotProduct(v_, rhs.v_);
00261    q.v_ = s_ * rhs.v_ + rhs.s_ * v_ + CrossProduct(v_, rhs.v_);
00262 
00263    return q;
00264 }
00265 
00266 
00267 Quaternion Quaternion::operator/(const Quaternion & rhs)const
00269 {
00270     return *this*rhs.i();
00271 }
00272 
00273 
00274 void Quaternion::set_v(const ColumnVector & v)
00276 {
00277    if(v.Nrows() == 3)
00278       v_ = v;
00279    else
00280        cerr << "Quaternion::set_v: input has a wrong size." << endl;
00281 }
00282 
00283 Quaternion Quaternion::conjugate()const
00290 {
00291    Quaternion q;
00292    q.s_ = s_;
00293    q.v_ = -1*v_;
00294 
00295    return q;
00296 }
00297 
00298 Real Quaternion::norm()const 
00307 { 
00308   return( sqrt(s_*s_ + DotProduct(v_, v_)) );
00309 }
00310 
00311 Quaternion & Quaternion::unit()
00313 {
00314    Real tmp = norm();
00315    if(tmp > EPSILON)
00316    {
00317       s_ = s_/tmp;
00318       v_ = v_/tmp;
00319    }
00320    return *this;
00321 }
00322 
00323 Quaternion Quaternion::i()const 
00332 { 
00333     return conjugate()/norm();
00334 }
00335 
00336 Quaternion Quaternion::exp() const
00344 {
00345    Quaternion q;
00346    Real theta = sqrt(DotProduct(v_,v_)),
00347                 sin_theta = sin(theta);
00348 
00349    q.s_ = cos(theta);
00350    if ( fabs(sin_theta) > EPSILON)
00351       q.v_ = v_*sin_theta/theta;
00352    else
00353       q.v_ = v_;
00354 
00355    return q;
00356 }
00357 
00358 Quaternion Quaternion::power(const Real t) const
00359 {
00360    Quaternion q = (Log()*t).exp();
00361 
00362    return q;
00363 }
00364 
00365 Quaternion Quaternion::Log()const
00374 {
00375    Quaternion q;
00376    q.s_ = 0;
00377    Real theta = acos(s_),
00378                 sin_theta = sin(theta);
00379 
00380    if ( fabs(sin_theta) > EPSILON)
00381       q.v_ = v_/sin_theta*theta;
00382    else
00383       q.v_ = v_;
00384 
00385    return q;
00386 }
00387 
00388 Quaternion Quaternion::dot(const ColumnVector & w, const short sign)const
00415 {
00416    Quaternion q;
00417    Matrix tmp;
00418 
00419    tmp = -0.5*v_.t()*w;
00420    q.s_ = tmp(1,1);
00421    q.v_ = 0.5*E(sign)*w;
00422 
00423    return q;
00424 }
00425 
00426 ReturnMatrix Quaternion::E(const short sign)const
00432 {
00433    Matrix E(3,3), I(3,3);
00434    I << threebythreeident;
00435 
00436    if(sign == BODY_FRAME)
00437       E = s_*I + x_prod_matrix(v_);
00438    else
00439       E = s_*I - x_prod_matrix(v_);
00440 
00441    E.Release();
00442    return E;
00443 }
00444 
00445 Real Quaternion::dot_prod(const Quaternion & q)const
00454 {
00455    return (s_*q.s_ + v_(1)*q.v_(1) + v_(2)*q.v_(2) + v_(3)*q.v_(3));
00456 }
00457 
00458 ReturnMatrix Quaternion::R()const
00490 {
00491    Matrix R(3,3);
00492    R << threebythreeident;
00493    R = (1 - 2*DotProduct(v_, v_))*R + 2*v_*v_.t() + 2*s_*x_prod_matrix(v_);
00494 
00495    R.Release();
00496    return R;
00497 }
00498 
00499 ReturnMatrix Quaternion::T()const
00505 {
00506    Matrix T(4,4);
00507    T << fourbyfourident;
00508    T.SubMatrix(1,3,1,3) = (1 - 2*DotProduct(v_, v_))*T.SubMatrix(1,3,1,3)
00509                           + 2*v_*v_.t() + 2*s_*x_prod_matrix(v_);
00510    T.Release();
00511    return T;
00512 }
00513 
00514 // -------------------------------------------------------------------------------------
00515 
00516 Quaternion operator*(const Real c, const Quaternion & q)
00526 {
00527     Quaternion out;
00528     out.set_s(q.s() * c);
00529     out.set_v(q.v() * c);
00530    return out;
00531 }
00532 
00533 Quaternion operator*(const Quaternion & q, const Real c)
00537 {
00538    return operator*(c, q);
00539 }
00540 
00541 
00542 Quaternion operator/(const Real c, const Quaternion & q)
00548 {
00549     Quaternion out;
00550     out.set_s(q.s() / c);
00551     out.set_v(q.v() / c);
00552    return out;
00553 }
00554 
00555 Quaternion operator/(const Quaternion & q, const Real c)
00556 {
00557     return operator/(c, q);
00558 }
00559 
00560 ReturnMatrix Omega(const Quaternion & q, const Quaternion & q_dot)
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)
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)
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)
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)
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)
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)
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)
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 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Properties Friends Defines


kni
Author(s): Neuronics AG (see AUTHORS.txt); ROS wrapper by Martin Günther
autogenerated on Tue May 28 2013 14:52:54