Quaternion.cpp

/* 
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
*/


#include "Quaternion.hh"


using namespace BIAS;
using namespace std;


template<class QUAT_TYPE> 
Quaternion<QUAT_TYPE>::~Quaternion() 
{}

template<class QUAT_TYPE> 
Quaternion<QUAT_TYPE>::Quaternion() 
: Vector4<QUAT_TYPE>() 
{}

template<class QUAT_TYPE> 
Quaternion<QUAT_TYPE>::Quaternion(const Vector4<QUAT_TYPE>& q)
  : Vector4<QUAT_TYPE>(q)
{}


template<class QUAT_TYPE> 
Quaternion<QUAT_TYPE>::Quaternion(QUAT_TYPE i, QUAT_TYPE j,
                                  QUAT_TYPE k, QUAT_TYPE r) 
                                  : Vector4<QUAT_TYPE>(i, j, k, r)
{}

template<class QUAT_TYPE> 
Matrix4x4<QUAT_TYPE> Quaternion<QUAT_TYPE>::GetQuaternionMultMatrixLeft() const
{
  Matrix4x4<QUAT_TYPE> res;
  
  res[0][0] = (*this)[3];
  res[0][1] = -(*this)[2];
  res[0][2] = (*this)[1];
  res[0][3] = (*this)[0];

  res[1][0] = (*this)[2];
  res[1][1] = (*this)[3];
  res[1][2] = -(*this)[0];
  res[1][3] = (*this)[1];

  res[2][0] = -(*this)[1];
  res[2][1] = (*this)[0];
  res[2][2] = (*this)[3];
  res[2][3] = (*this)[2];
  
  res[3][0] = -(*this)[0];
  res[3][1] = -(*this)[1];
  res[3][2] = -(*this)[2];
  res[3][3] = (*this)[3];
  
  return res;
}

template<class QUAT_TYPE> 
Matrix4x4<QUAT_TYPE> Quaternion<QUAT_TYPE>::GetQuaternionMultMatrixRight() const
{
  Matrix4x4<QUAT_TYPE> res;
  
  res[0][0] = (*this)[3];
  res[0][1] = (*this)[2];
  res[0][2] = -(*this)[1];
  res[0][3] = (*this)[0];

  res[1][0] = -(*this)[2];
  res[1][1] = (*this)[3];
  res[1][2] = (*this)[0];
  res[1][3] = (*this)[1];

  res[2][0] = (*this)[1];
  res[2][1] = -(*this)[0];
  res[2][2] = (*this)[3];
  res[2][3] = (*this)[2];
  
  res[3][0] = -(*this)[0];
  res[3][1] = -(*this)[1];
  res[3][2] = -(*this)[2];
  res[3][3] = (*this)[3];

  return res;
}

template<class QUAT_TYPE> 
int Quaternion<QUAT_TYPE>::GetAxisAngle(Vector3<QUAT_TYPE>& axis, 
                                        QUAT_TYPE& angle) const
{
  // nicked from OpenSG
  int res=0;
  Vector3<QUAT_TYPE> q((*this)[0], (*this)[1], (*this)[2]);
  QUAT_TYPE len = QUAT_TYPE(q.NormL2());
  
  if(len > QUATERNION_EPSILON) {
    q /= len;
    axis[0]  = q[0];
    axis[1]  = q[1];
    axis[2]  = q[2];
    angle = QUAT_TYPE(2.0 * acos((*this)[3]));
  } else {
    axis[0] = 0.0;
    axis[1] = 0.0;
    axis[2] = 1.0;
    angle = 0.0;
  }
  return res;
}

template<class QUAT_TYPE> 
Vector3<QUAT_TYPE> Quaternion<QUAT_TYPE>::GetRotationAxis() const
{
  Vector3<QUAT_TYPE> r((*this)[0], (*this)[1], (*this)[2]);
  QUAT_TYPE len = QUAT_TYPE(r.NormL2());
  if (len > QUATERNION_EPSILON)
    return r / len;
  else
    return Vector3<QUAT_TYPE>(0,0,1);
}

template<class QUAT_TYPE> 
QUAT_TYPE Quaternion<QUAT_TYPE>::GetRotationAngle() const
{
  const QUAT_TYPE angle = QUAT_TYPE(2.0 * acos((*this)[3]));
  if (isnan(angle) && fabs(fabs((*this)[3]) - 1.0) < QUATERNION_EPSILON)
    return QUAT_TYPE(0);
  else
    return angle;
}

template<class QUAT_TYPE> 
int Quaternion<QUAT_TYPE>::GetRotationMatrix(BIAS::RMatrixBase& /*R*/) const
{
  BIASERR("Quaternion<QUAT_TYPE>::GetRotationMatrix() is deprecated, because"
          <<" it returned a TRANSPOSED matrix. To prevent code duplication "
          <<" this function will be removed in the future. Check your code."
          <<"Use RMatrixBase::SetFromQuaternion instead.");
  BIASABORT;
  return 1;
}

template<class QUAT_TYPE> Quaternion<QUAT_TYPE>
Quaternion<QUAT_TYPE>::Power(const QUAT_TYPE & scale) const
{
  Quaternion<QUAT_TYPE> res = *this;
  if ((scale < 1e-7) && (scale > -1e-7)) {
    //scale near zero -> return identity
    res.SetIdentity();
  } else if ((res[3] > 0.999999) || (res[3] < -0.999999)) {
    //this is near identity (numerical unstable) -> return identity
    //res = *this
  } else {
    //else use slerp interpolation between identity and this with scale
    QUAT_TYPE temp1 = sin(scale * acos(res[3]))/sin(acos(res[3]));
    QUAT_TYPE res3 = res[3]; 
    res *= temp1;
    res[3] += QUAT_TYPE(sin((1.0-scale) * acos(res3))/sin(acos(res3)));
  }
  return res;
}

template<class QUAT_TYPE>
Quaternion<QUAT_TYPE> Quaternion<QUAT_TYPE>::
InterpolateLinear(const Quaternion<QUAT_TYPE> &to, const QUAT_TYPE & t) const
{
  BIASASSERT(t >= 0.0 && t <= 1.0);
  Quaternion<QUAT_TYPE> p = *this;
  Quaternion<QUAT_TYPE> q = to;
  //p.Normalize();
  //q.Normalize();
  if (t < 1e-7) {
    //t near zero -> return p
    return p;
  } else if (1.0-t < 1e-7) {
    //t near one -> return q
    return q;
  }
  //else use linear interpolation between this and q
  Quaternion<QUAT_TYPE> res;
  const QUAT_TYPE cosPhi = (QUAT_TYPE)p.ScalarProduct(q);
  const QUAT_TYPE a = (QUAT_TYPE)1.0 - t;
  const QUAT_TYPE b = (cosPhi < 0 ? -t : t);
  res[0] = a*p[0] + b*q[0];
  res[1] = a*p[1] + b*q[1];
  res[2] = a*p[2] + b*q[2];
  res[3] = a*p[3] + b*q[3];
  res.Normalize();
  return res;
}

template<class QUAT_TYPE>
Quaternion<QUAT_TYPE> Quaternion<QUAT_TYPE>::
Interpolate(const Quaternion<QUAT_TYPE> &to, const QUAT_TYPE & t) const
{
  BIASASSERT(t >= 0.0 && t <= 1.0);
  Quaternion<QUAT_TYPE> p = *this;
  Quaternion<QUAT_TYPE> q = to;
  //p.Normalize();
  //q.Normalize();
  if (t < 1e-7) {
    //t near zero -> return p
    return p;
  } else if (1.0-t < 1e-7) {
    //t near one -> return q
    return q;
  }
  Quaternion<QUAT_TYPE> res;
  QUAT_TYPE a, b;
  QUAT_TYPE cosPhi = (QUAT_TYPE)p.ScalarProduct(q);
  //adjust angle if neccessary
  if (cosPhi < 0) {
    q.MultiplyIP(-1.0);
    cosPhi = -cosPhi;
  }
  if (cosPhi > 0.9999 || cosPhi < -0.9999) {
    // p is very close to q, do linear interpolation instead of slerp
    a = (QUAT_TYPE)1.0 - t;
    b = (cosPhi < 0 ? -t : t);
  } else {
    //else use slerp interpolation between p and q
    const QUAT_TYPE phi = acos(cosPhi < 0 ? -cosPhi : cosPhi);
    const QUAT_TYPE sinPhi = sin(phi);
    a = sin(((QUAT_TYPE)1.0 - t) * phi) / sinPhi;
    b = sin(t * phi) / sinPhi;
    if (cosPhi < 0) b = -b;
  }
  res[0] = a*p[0] + b*q[0];
  res[1] = a*p[1] + b*q[1];
  res[2] = a*p[2] + b*q[2];
  res[3] = a*p[3] + b*q[3];
  res.Normalize();
  return res;
}

template<class QUAT_TYPE>
int Quaternion<QUAT_TYPE>::
SetXYZ(QUAT_TYPE radX, QUAT_TYPE radY, QUAT_TYPE radZ)
{
  Quaternion<QUAT_TYPE> q_x;
  Quaternion<QUAT_TYPE> q_y;
  Quaternion<QUAT_TYPE> q_z;

  q_x.SetValueAsAxisRad( 1.,0.,0.,  radX );
  q_y.SetValueAsAxisRad( 0.,1.,0.,  radY );
  q_z.SetValueAsAxisRad( 0.,0.,1.,  radZ );

  q_x.Mult(q_y);
  q_x.Mult(q_z);

  (*this) = q_x;

  return 0;
}

template<class QUAT_TYPE>
int Quaternion<QUAT_TYPE>::
SetZYX(QUAT_TYPE radX, QUAT_TYPE radY, QUAT_TYPE radZ)
{
  Quaternion<QUAT_TYPE> q_x;
  Quaternion<QUAT_TYPE> q_y;
  Quaternion<QUAT_TYPE> q_z;

  q_x.SetValueAsAxisRad( 1.,0.,0.,  radX );
  q_y.SetValueAsAxisRad( 0.,1.,0.,  radY );
  q_z.SetValueAsAxisRad( 0.,0.,1.,  radZ );

  q_z.Mult(q_y);
  q_z.Mult(q_x);

  (*this) = q_z;

  return 0;
}

template<class QUAT_TYPE>
Quaternion<QUAT_TYPE>& Quaternion<QUAT_TYPE>::
operator=(const Quaternion<QUAT_TYPE>& vec)
{
  memcpy((void *)this->data_, (void *)vec.GetData(), 
    VECTOR4SIZE*sizeof(QUAT_TYPE)); 
  return *this; 
}

template<class QUAT_TYPE> void
Quaternion<QUAT_TYPE>::
EnforceRigidCouplingConstraint(Quaternion<QUAT_TYPE> &other,
                               bool useOtherAngle)
{
  // @todo check unit norm constraint of this and other quaternion!!

  // make this and other quaternion unique
  MakeUnique();
  other.MakeUnique();

  // get aliases for both quaternions
  Quaternion<QUAT_TYPE> &q0 = *this, &q1 = other;

  // interpolate equal angle
  if (useOtherAngle)
    q0[3] = q1[3];
  else
    q0[3] = q1[3] = QUAT_TYPE(0.5) * (q0[3] + q1[3]);

  // re-enforce unit length constraint for q0 and q1
  QUAT_TYPE q0norm = q0[0]*q0[0] + q0[1]*q0[1] + q0[2]*q0[2];
  QUAT_TYPE q1norm = q1[0]*q1[0] + q1[1]*q1[1] + q1[2]*q1[2];
  if (q0norm > QUAT_TYPE(1e-8) && q1norm > QUAT_TYPE(1e-8)) {
    QUAT_TYPE qscale0 = (QUAT_TYPE)sqrt(double((1-q0[3]*q0[3])/q0norm));
    QUAT_TYPE qscale1 = (QUAT_TYPE)sqrt(double((1-q1[3]*q1[3])/q1norm));
    for (register int l = 0; l < 3; l++) {
      q0[l] *= qscale0;
      q1[l] *= qscale1;
    }
  }

  // @todo check unit norm constraint of resulting quaternions!!
}

template<class QUAT_TYPE> void
Quaternion<QUAT_TYPE>::
EnforceRigidCouplingConstraint(std::vector< Quaternion<QUAT_TYPE> > &quats,
                               bool useFirstAngle)
{
  // @todo check unit norm constraint of all quaternions!!
  if (quats.empty()) return;

  // make all quaternions unique
  const int n = quats.size();
  for (int i = 0; i < n; i++)
    quats[i].MakeUnique();

  // interpolate equal angle
  QUAT_TYPE a = quats[0][3];
  if (!useFirstAngle) {
    for (int i = 1; i < n; i++)
      a += quats[i][3];
    a /= QUAT_TYPE(n);
  }
  for (int i = 0; i < n; i++)
    quats[i][3] = a;

  // re-enforce unit length constraint for all quaternions
  for (int i = 0; i < n; i++) {
    Quaternion<QUAT_TYPE> &q = quats[i];
    QUAT_TYPE qnorm = q[0]*q[0] + q[1]*q[1] + q[2]*q[2];
    if (qnorm > QUAT_TYPE(1e-8)) {
      QUAT_TYPE qscale = (QUAT_TYPE)sqrt(double((1-q[3]*q[3])/qnorm));
      for (register int l = 0; l < 3; l++)
        q[l] *= qscale;
    }
  }

  // @todo check unit norm constraint of resulting quaternions!!
}

// instance definitions
template class BIASGeometryBase_EXPORT BIAS::Quaternion<float>;
template class BIASGeometryBase_EXPORT BIAS::Quaternion<double>;