DualQuaternion.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 "DualQuaternion.hh"
#include <Base/Math/Matrix4x4.hh>

using namespace BIAS;
using namespace std;


template <class QUAT_TYPE>
void DualQuaternion<QUAT_TYPE>::
GetRigidMotion(Quaternion<QUAT_TYPE> &rotation,
               Vector3<QUAT_TYPE> &translation) const
{
  // @todo check unit norm constraint of this dual quaternion!!

  rotation = real;
  Quaternion<QUAT_TYPE> qt;
  dual.Mult(real.Inverse(), qt);
  translation.Set(QUAT_TYPE(2.0)*qt[0], QUAT_TYPE(2.0)*qt[1], QUAT_TYPE(2.0)*qt[2]);
}


template <class QUAT_TYPE>
void DualQuaternion<QUAT_TYPE>::
SetFromRigidMotion(const Quaternion<QUAT_TYPE> &rotation,
                   const Vector3<QUAT_TYPE> &translation)
{
  real = rotation;
  Vector3<QUAT_TYPE> r(real[0], real[1], real[2]);
  Vector3<QUAT_TYPE> d = QUAT_TYPE(0.5) * (real[3]*translation +
                                           translation.CrossProduct(r));
  dual[0] = d[0]; dual[1] = d[1]; dual[2] = d[2];
  dual[3] = QUAT_TYPE(-0.5) * translation.ScalarProduct(r);

  // @todo check unit norm constraint of this dual quaternion!!
}


template <class QUAT_TYPE>
PoseParametrization DualQuaternion<QUAT_TYPE>::
GetPoseParametrization() const
{
  PoseParametrization params;
  Quaternion<QUAT_TYPE> rotation;
  Vector3<QUAT_TYPE> translation;
  GetRigidMotion(rotation, translation);
  params.Set(Vector3<PP_TYPE>((PP_TYPE)translation[0],
                              (PP_TYPE)translation[1],
                              (PP_TYPE)translation[2]),
             Quaternion<PP_TYPE>((PP_TYPE)rotation[0],
                                 (PP_TYPE)rotation[1],
                                 (PP_TYPE)rotation[2],
                                 (PP_TYPE)rotation[3]));
  return params;
}


template <class QUAT_TYPE>
void DualQuaternion<QUAT_TYPE>::
SetFromPoseParametrization(const PoseParametrization &params)
{
  Quaternion<PP_TYPE> rotation = params.GetOrientation();
  Vector3<PP_TYPE> translation = params.GetPosition();
  SetFromRigidMotion(Quaternion<QUAT_TYPE>((QUAT_TYPE)rotation[0],
                                           (QUAT_TYPE)rotation[1],
                                           (QUAT_TYPE)rotation[2],
                                           (QUAT_TYPE)rotation[3]),
                     Vector3<QUAT_TYPE>((QUAT_TYPE)translation[0],
                                        (QUAT_TYPE)translation[1],
                                        (QUAT_TYPE)translation[2]));
}


template<class QUAT_TYPE>
Matrix<QUAT_TYPE> DualQuaternion<QUAT_TYPE>::GetLeftMultMatrix() const
{
  Matrix<QUAT_TYPE> res(8, 8, MatrixZero);
  Matrix4x4<QUAT_TYPE> Lq = real.GetQuaternionMultMatrixLeft();
  Matrix4x4<QUAT_TYPE> Lp = dual.GetQuaternionMultMatrixLeft();
  for (register int i = 0; i < 4; i++) {
    for (register int j = 0; j < 4; j++) {
      res[i][j] = res[i+4][j+4] = Lq[i][j];
      res[i+4][j] = Lp[i][j];
    }
  }
  return res;
}


template<class QUAT_TYPE>
Matrix<QUAT_TYPE> DualQuaternion<QUAT_TYPE>::GetRightMultMatrix() const
{
  Matrix<QUAT_TYPE> res(8, 8, MatrixZero);
  Matrix4x4<QUAT_TYPE> Rq = real.GetQuaternionMultMatrixRight();
  Matrix4x4<QUAT_TYPE> Rp = dual.GetQuaternionMultMatrixRight();
  for (register int i = 0; i < 4; i++) {
    for (register int j = 0; j < 4; j++) {
      res[i][j] = res[i+4][j+4] = Rq[i][j];
      res[i+4][j] = Rp[i][j];
    }
  }
  return res;
}


template<class QUAT_TYPE>
DualQuaternion<QUAT_TYPE> DualQuaternion<QUAT_TYPE>::
InterpolateLinear(const DualQuaternion<QUAT_TYPE> &to,
                  const QUAT_TYPE &t) const
{
  BIASASSERT(t >= QUAT_TYPE(0.0) && t <= QUAT_TYPE(1.0));

  // @todo check unit norm constraint of input!!

  DualQuaternion res;
  res.real = real.InterpolateLinear(to.real, t);
  res.dual = dual.InterpolateLinear(to.dual, t);

  res.Normalize();

  // @todo check unit norm constraint!!

  return res;
}


template<class QUAT_TYPE>
DualQuaternion<QUAT_TYPE> DualQuaternion<QUAT_TYPE>::
Interpolate(const DualQuaternion<QUAT_TYPE> &to,
            const QUAT_TYPE &t) const
{
  BIASASSERT(t >= QUAT_TYPE(0.0) && t <= QUAT_TYPE(1.0));

  // @todo check unit norm constraint of input!!

  DualQuaternion res;
  res.real = real.Interpolate(to.real, t);

  // @todo complete spherical interpolation of dual part!!

  // @todo check unit norm constraint!!

  return res;
}


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

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

  // get aliases for real and dual parts of both quaternions
  Quaternion<QUAT_TYPE> &q0 = real, &p0 = dual, &q1 = other.real, &p1 = other.dual;

  // interpolate equal angle and pitch
  if (useOtherAngle)
    q0[3] = q1[3];
  else
    q0[3] = q1[3] = QUAT_TYPE(0.5) * (q0[3] + q1[3]);
  if (useOtherPitch)
    p0[3] = p1[3];
  else
    p0[3] = p1[3] = QUAT_TYPE(0.5) * (p0[3] + p1[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;
    }
  }
  // re-enforce orthogonality constraint for q0,p0 and q1,p1
  QUAT_TYPE q0p0 = q0[0]*p0[0]+q0[1]*p0[1]+q0[2]*p0[2];
  QUAT_TYPE q1p1 = q1[0]*p1[0]+q1[1]*p1[1]+q1[2]*p1[2];
  if ((q0p0 > 0 ? q0p0 > QUAT_TYPE(1e-8) : q0p0 < -QUAT_TYPE(1e-8)) &&
      (q1p1 > 0 ? q1p1 > QUAT_TYPE(1e-8) : q1p1 < -QUAT_TYPE(1e-8))) {
    QUAT_TYPE pscale0 = -q0[3]*p0[3]/q0p0;
    QUAT_TYPE pscale1 = -q1[3]*p1[3]/q1p1;
    for (register int l = 0; l < 3; l++) {
      p0[l] *= pscale0;
      p1[l] *= pscale1;
    }
  }

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


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

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

  // interpolate equal angle and pitch
  QUAT_TYPE a = quats[0].real[3], p = quats[0].dual[3];
  if (!useFirstAngle) {
    for (int i = 1; i < n; i++)
      a += quats[i].real[3];
    a /= QUAT_TYPE(n);
  }
  if (!useFirstPitch) {
    for (int i = 1; i < n; i++)
      p += quats[i].dual[3];
    p /= QUAT_TYPE(n);
  }
  for (int i = 0; i < n; i++) {
    quats[i].real[3] = a;
    quats[i].dual[3] = p;
  }

  // re-enforce unit length constraint for all dual quaternions
  for (int i = 0; i < n; i++) {
    Quaternion<QUAT_TYPE> &q = quats[i].real;
    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;
    }
  }

  // re-enforce orthogonality constraint for all dual quaternions
  for (int i = 0; i < n; i++) {
    Quaternion<QUAT_TYPE> &q = quats[i].real;
    Quaternion<QUAT_TYPE> &p = quats[i].dual;
    QUAT_TYPE qp = q[0]*p[0]+q[1]*p[1]+q[2]*p[2];
    if (qp > 0 ? (qp > QUAT_TYPE(1e-8)) : (qp < -QUAT_TYPE(1e-8))) {
      QUAT_TYPE pscale = -q[3]*p[3]/qp;
      for (register int l = 0; l < 3; l++)
        p[l] *= pscale;
    }
  }

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


// instances for dual quaternions
template class BIASGeometryBase_EXPORT BIAS::DualQuaternion<float>;
template class BIASGeometryBase_EXPORT BIAS::DualQuaternion<double>;