QuaternionInl.hh

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


template <class QUAT_TYPE> inline
Quaternion<QUAT_TYPE>::Quaternion(const Quaternion<QUAT_TYPE> &q) 
{ 
  *this = q; 
} 

template <class QUAT_TYPE> inline
void Quaternion<QUAT_TYPE>::Invert()
{
  (*this)[0] *=-1;
  (*this)[1] *=-1;
  (*this)[2] *=-1;
}
 
template <class QUAT_TYPE> inline
Quaternion<QUAT_TYPE> Quaternion<QUAT_TYPE>::Inverse() const
{
  Quaternion<QUAT_TYPE> res(*this);
  res.Invert();
  return res;
}

template <class QUAT_TYPE> inline
void Quaternion<QUAT_TYPE>::MakeUnique()
{
  //project quaternion to upper hemisphere (first ima.part >= 0)
  QUAT_TYPE *qThis = this->GetData();
  if ((qThis[3] <= QUATERNION_EPSILON)
      &&(qThis[3] >= -QUATERNION_EPSILON)) {
    if ((qThis[1] <= QUATERNION_EPSILON)
        &&(qThis[1] >= -QUATERNION_EPSILON)) {
      if ((qThis[2] <= QUATERNION_EPSILON)
          &&(qThis[2] >= -QUATERNION_EPSILON)) {
        if (qThis[0] < 0.0) (*this) *= -1.0;
      } else if (qThis[2] < 0.0) {
        (*this) *= -1.0;
      }
    } else if (qThis[1] < 0.0) {
      (*this) *= -1.0;
    }
  } else if (qThis[3] < 0.0) {
    (*this) *= -1.0;
  }
}

template <class QUAT_TYPE> inline
void Quaternion<QUAT_TYPE>::Mult(const Quaternion<QUAT_TYPE> &quat)
{
  QUAT_TYPE s1, s2, s3, s4, s5, s6, s7, s8, s9, t;
  QUAT_TYPE *qThis = this->GetData();

  s1 = (qThis[2] - qThis[1]) * (quat[1] - quat[2]);
  s2 = (qThis[3] + qThis[0]) * (quat[3] + quat[0]);
  s3 = (qThis[3] - qThis[0]) * (quat[1] + quat[2]);
  s4 = (qThis[2] + qThis[1]) * (quat[3] - quat[0]);
  s5 = (qThis[2] - qThis[0]) * (quat[0] - quat[1]);
  s6 = (qThis[2] + qThis[0]) * (quat[0] + quat[1]);
  s7 = (qThis[3] + qThis[1]) * (quat[3] - quat[2]);
  s8 = (qThis[3] - qThis[1]) * (quat[3] + quat[2]);

  s9 = s6 + s7 + s8;

  t  = (s5 + s9) / 2.0f;

  qThis[3] = s1 + t - s6;
  qThis[0] = s2 + t - s9;
  qThis[1] = s3 + t - s8;
  qThis[2] = s4 + t - s7;
}

template <class QUAT_TYPE> inline
void Quaternion<QUAT_TYPE>::Mult(const Quaternion<QUAT_TYPE> &arg, 
                                 Quaternion<QUAT_TYPE> &res) const 
{
  res = *this;
  res.Mult(arg);
}

template <class QUAT_TYPE> inline 
void Quaternion<QUAT_TYPE>::MultLeft(const Quaternion<QUAT_TYPE> &quatLeft)
{
  QUAT_TYPE s1, s2, s3, s4, s5, s6, s7, s8, s9, t;
  QUAT_TYPE *qThis = this->GetData();

  s1 = (quatLeft[2] - quatLeft[1]) * (qThis[1] - qThis[2]);
  s2 = (quatLeft[3] + quatLeft[0]) * (qThis[3] + qThis[0]);
  s3 = (quatLeft[3] - quatLeft[0]) * (qThis[1] + qThis[2]);
  s4 = (quatLeft[2] + quatLeft[1]) * (qThis[3] - qThis[0]);
  s5 = (quatLeft[2] - quatLeft[0]) * (qThis[0] - qThis[1]);
  s6 = (quatLeft[2] + quatLeft[0]) * (qThis[0] + qThis[1]);
  s7 = (quatLeft[3] + quatLeft[1]) * (qThis[3] - qThis[2]);
  s8 = (quatLeft[3] - quatLeft[1]) * (qThis[3] + qThis[2]);

  s9 = s6 + s7 + s8;

  t  = (s5 + s9) / 2.0f;

  qThis[3] = s1 + t - s6;
  qThis[0] = s2 + t - s9;
  qThis[1] = s3 + t - s8;
  qThis[2] = s4 + t - s7;
}

/** rotates the given Vector with the quaternion ( q v q* )
    the resulting vector is given in res
    @returns 0 in case of no error
    @author Daniel Grest, June 2003
*/
template <class QUAT_TYPE> inline
int Quaternion<QUAT_TYPE>::MultVec(const Vector3<QUAT_TYPE> &vec, 
                                   Vector3<QUAT_TYPE> &res) const
{
  register QUAT_TYPE rx,ry,rz;
  register QUAT_TYPE Vx= vec[0], Vy=vec[1], Vz=vec[2];
  const QUAT_TYPE *q = this->GetData();
  {
    register QUAT_TYPE QwQx, QwQy, QwQz, QxQy, QxQz, QyQz;  
    QwQx = q[3] * q[0]; QwQy = q[3] * q[1]; QwQz = q[3] * q[2];
    QxQy = q[0] * q[1]; QxQz = q[0] * q[2]; QyQz = q[1] * q[2];
    rx = 2* (Vy * (-QwQz + QxQy) + Vz *( QwQy + QxQz));
    ry = 2* (Vx * ( QwQz + QxQy) + Vz *(-QwQx + QyQz));
    rz = 2* (Vx * (-QwQy + QxQz) + Vy *( QwQx + QyQz));
  }
  register QUAT_TYPE QwQw, QxQx, QyQy, QzQz;
  QwQw= q[3]*q[3]; QxQx = q[0]*q[0]; QyQy = q[1]*q[1]; QzQz= q[2]*q[2];
  rx+= Vx * (QwQw + QxQx - QyQy - QzQz);
  ry+= Vy * (QwQw - QxQx + QyQy - QzQz);
  rz+= Vz * (QwQw - QxQx - QyQy + QzQz);
  res[0]=rx; res[1]=ry; res[2]=rz;
  return 0;
}

template <class QUAT_TYPE> inline
Vector3<QUAT_TYPE> Quaternion<QUAT_TYPE>::
MultVec(const Vector3<QUAT_TYPE> &vec) const 
{ 
  Vector3<QUAT_TYPE> r; MultVec(vec,r); 
  return r; 
}
    
template <class QUAT_TYPE> inline
void Quaternion<QUAT_TYPE>::SetIdentity()
{
  (*this)[0] = (*this)[1] = (*this)[2] = 0;  
  (*this)[3] = 1;
}

template <class QUAT_TYPE> inline
void Quaternion<QUAT_TYPE>::SetQuaternion(QUAT_TYPE real, QUAT_TYPE i,
                                          QUAT_TYPE j, QUAT_TYPE k)
{
  //Components are stored in Vec4 in order imaginary part 1,2,3 and real part
  this->Set(i,j,k,real);
}

template <class QUAT_TYPE> inline
void Quaternion<QUAT_TYPE>::SetValueAsAxisRad(const Vector3<QUAT_TYPE> &axis,
                                              QUAT_TYPE angle)
{  
  SetValueAsAxisRad(axis[0], axis[1], axis[2], angle);
}

template <class QUAT_TYPE> inline
void Quaternion<QUAT_TYPE>::SetValueAsAxisRad(QUAT_TYPE x, QUAT_TYPE y,
                                              QUAT_TYPE z, QUAT_TYPE w)
{
  QUAT_TYPE rTmp = QUAT_TYPE(sqrt(x * x + y * y + z * z));

  if(rTmp > QUATERNION_EPSILON)
  {
    rTmp = QUAT_TYPE(sin(w / 2.0f)) / rTmp;

    (*this)[0] = x * rTmp;
    (*this)[1] = y * rTmp;
    (*this)[2] = z * rTmp;
    (*this)[3] = QUAT_TYPE(cos(w / 2.0f));
  }
  else
  {
    SetIdentity();
  }
}

template <class QUAT_TYPE> inline
void Quaternion<QUAT_TYPE>::
GetEquatorialPoint3D(Vector3<QUAT_TYPE> &p3D) const
{
  // @todo Check if quaternion has unit length!
  const QUAT_TYPE s = 1 / ((*this)[3] + ((*this)[3] < 0 ? -1 : 1));
  p3D[0] = s * (*this)[0];
  p3D[1] = s * (*this)[1];
  p3D[2] = s * (*this)[2];
}

template <class QUAT_TYPE> inline
void Quaternion<QUAT_TYPE>::
SetFromEquatorialPoint3D(const Vector3<QUAT_TYPE> &p3D)
{
  const QUAT_TYPE s = 2 / (p3D[0]*p3D[0] + p3D[1]*p3D[1] + p3D[2]*p3D[2] + 1);
  (*this)[0] = s * p3D[0];
  (*this)[1] = s * p3D[1];
  (*this)[2] = s * p3D[2];
  (*this)[3] = s - 1;
}