ExampleQuaternion.cpp

Example for quaternion usage and relation to rotation matrix

Author

MIP

/*
This file is part of the BIAS library (Basic ImageAlgorithmS).

Copyright (C) 2003-2009    (see file CONTACT for details)
  Multimediale Systeme der Informationsverarbeitung
  Institut fuer Informatik
  Christian-Albrechts-Universitaet Kiel


BIAS is free software; you can redistribute it and/or modify
it under the terms of the GNU Lesser General Public License as published by
the Free Software Foundation; either version 2.1 of the License, or
(at your option) any later version.

BIAS is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU Lesser General Public License for more details.

You should have received a copy of the GNU Lesser General Public License
along with BIAS; if not, write to the Free Software
Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
*/

/**
   @example ExampleQuaternion.cpp
   @relates Quaternion
   @brief Example for quaternion usage and relation to rotation matrix
   @ingroup g_examples
   @author MIP  
*/

#include "../Quaternion.hh"
#include <Base/Common/CompareFloatingPoint.hh>

using namespace std;
using namespace BIAS;

int main() 
{
  Quaternion<double> q, p;
  q.SetValueAsAxisRad(-0.97856,0.049681,-0.14840,0.00102);
  p.SetValueAsAxisRad(-0.97856,0.049681,-0.14840,0.00102);
  cout << "q = " << q << ", p = " << p << endl;

  Quaternion<double> qp = q*p;
  Matrix4x4<double> L = q.GetQuaternionMultMatrixLeft();
  Matrix4x4<double> R = p.GetQuaternionMultMatrixRight();
  Quaternion<double>Lp = L*p;
  Quaternion<double>Rq = R*q;

  // compare quaternion multiplication results
  cout << "q*p = " << qp << endl;
  cout << "L(q)*p = " << Lp << endl;
  cout << "R(p)*q = " << Rq << endl;
  BIASASSERT(BIAS::Equal((Lp-qp).NormL2(), 0.0, 1e-12));
  BIASASSERT(BIAS::Equal((Rq-qp).NormL2(), 0.0, 1e-12));

  // compare result with rotation matrix
  RMatrixBase R1, R2;
  R1.SetFromQuaternion(q);
  R2.SetFromQuaternion(p);
  RMatrixBase R1R2 = R1*R2;
  Quaternion<double> r;
#ifdef BIASASSERT_ISACTIVE
  int res_GetQuat = R1R2.GetQuaternion(r);
  BIASASSERT(res_GetQuat == 0);
  BIASASSERT(BIAS::Equal(fabs(r.ScalarProduct(qp)), 1.0, 1e-12));
#endif

  // compare quaternion - vector multiplication with
  // rotation matrix - vector multiplication
  Vector3<double> v(0.5, -0.5, 0.1), qvq;
  q.MultVec(v, qvq);
  cout << "v = " << v << ", q*v*q' = " << qvq << endl;
  Vector3<double> Rv = R1*v;
  BIASASSERT(BIAS::Equal((qvq-Rv).NormL2(), 0.0, 1e-12));

  return 0;
}