CovQuaternion2AxisAngle.cpp

#include <Geometry/CovQuaternion2AxisAngle.hh>
#include <Base/Debug/Exception.hh>
#include <Base/Geometry/Quaternion.hh>

using namespace BIAS;
using namespace std;

int CovQuaternion2AxisAngle::
Transform_(const Vector<double>& src, Vector<double>& dst) const
{ 
  Quaternion<double> q;
  for (int i=0; i<4; i++){ q[i] = src[i]; }
  Vector3<double> axis;
  double angle;
  int res = q.GetAxisAngle(axis, angle);
  dst.newsize(4);
  if (res!=0){
    BIASERR("failed to extract axis and angle from "<<q<<"   error code "
            <<res<<endl);
    return res;
  }
  for (int i=0; i<3; i++){ dst[i] = axis[i]; }
  dst[3] = angle;
  return 0;
}