Public API Reference

csgeom/dualquaternion.h

Go to the documentation of this file.

/*
    Copyright (C) 2008 by Marten Svanfeldt 

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

    This library 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
    Library General Public License for more details.

    You should have received a copy of the GNU Library General Public
    License along with this library; if not, write to the Free
    Software Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
*/

#ifndef __CS_CSGEOM_DUALQUATERNION_H__
#define __CS_CSGEOM_DUALQUATERNION_H__

#include "csextern.h"
#include "csqsqrt.h"

#include "csgeom/vector3.h"
#include "csgeom/quaternion.h"
#include "csutil/tuple.h"

class csMatrix3;

typedef csTuple2<float, float> csDualNumber;

class csDualQuaternion
{
public:
  // Constructors

  inline csDualQuaternion ()
    : real (0,0,0,1), dual (0,0,0,0)
  {}

  inline csDualQuaternion (const csQuaternion& real, const csQuaternion& dual)
    : real (real), dual (dual)
  {}

  inline csDualQuaternion (const csQuaternion& real)
    : real (real)
  {}

  inline csDualQuaternion (const csQuaternion& real, const csVector3& t)
    : real (real), dual (0.5 * (csQuaternion (t, 0)*real))
  {}

  inline csDualQuaternion (const csDualQuaternion& q)
    : real (q.real), dual (q.dual)
  {}


  inline void SetIdentity () 
  {
    real.SetIdentity ();
    dual.Set (0, 0, 0, 0);
  }

  inline friend csDualQuaternion operator+ (const csDualQuaternion& q1, 
    const csDualQuaternion& q2)
  {
    return csDualQuaternion (q1.real + q2.real, q1.dual + q2.dual);
  }

  inline csDualQuaternion& operator+= (const csDualQuaternion& q)
  {
    real += q.real;
    dual += q.dual;
    return *this;
  }

  inline friend csDualQuaternion operator- (const csDualQuaternion& q1, 
    const csDualQuaternion& q2)
  {
    return csDualQuaternion (q1.real - q2.real, q1.dual - q2.dual);
  }

  inline csDualQuaternion& operator-= (const csDualQuaternion& q)
  {
    real -= q.real;
    dual -= q.dual;
    return *this;
  }
  
  inline friend csDualQuaternion operator- (const csDualQuaternion& q)
  {
    return csDualQuaternion (-q.real, -q.dual);
  }

  inline friend csDualQuaternion operator* (const csDualQuaternion& q1,
    const csDualQuaternion& q2)
  {
    return csDualQuaternion (q1.real * q2.real,
      q1.real * q2.dual + q2.real*q1.dual);
  }

  inline csDualQuaternion& operator*= (const csDualQuaternion& q)
  {
    const csQuaternion oldReal (real);
    
    real *= q.real;
    dual = (oldReal*q.dual + q.real*dual);

    return *this;
  }

  inline friend csDualQuaternion operator* (const csDualQuaternion& q, float f)
  {
    return csDualQuaternion (q.real*f, q.dual*f);
  }

  inline friend csDualQuaternion operator* (float f, const csDualQuaternion& q)
  {
    return csDualQuaternion (q.real*f, q.dual*f);
  }

  inline csDualQuaternion& operator*= (float f)
  {
    real *= f;
    dual *= f;
    return *this;
  }

  inline friend csDualQuaternion operator/ (const csDualQuaternion& q, float f)
  {
    float invF = 1.0f/f;
    return csDualQuaternion (q.real*invF, q.dual*invF);
  }

  inline friend csDualQuaternion operator/ (float f, const csDualQuaternion& q)
  {
    float invF = 1.0f/f;
    return csDualQuaternion (q.real*invF, q.dual*invF);
  }

  inline csDualQuaternion& operator/= (float f)
  {
    real /= f;
    dual /= f;
    return *this;
  }

  inline csDualQuaternion GetConjugate () const
  {
    return csDualQuaternion (real.GetConjugate (), dual.GetConjugate ());
  }

  inline void Conjugate () 
  {
    real.Conjugate ();
    dual.Conjugate ();
  }

  inline csDualNumber Dot (const csDualQuaternion& q) const
  {
    return csDualNumber (real.Dot (q.real), dual.Dot (q.dual));
  }  

  inline csDualNumber Norm () const
  {
    return csDualNumber (real.Norm (), real.Dot (dual) / real.Norm ());
  }

  inline csDualQuaternion Unit () const
  {    
    const float lenReal = real.Norm ();

    if (lenReal == 0)
    {
      return *this;
    }

    const float lenRealInv = 1.0f / lenReal;
    
    csQuaternion _real = real * lenRealInv;
    csQuaternion _dual = dual * lenRealInv;

    csQuaternion r (_real);
    r *= _real.Dot (_dual);
    r *= -1.0f;

    _dual += r;

    return csDualQuaternion (_real, _dual);
  }

  inline csDualQuaternion GetInverse () const
  {
    const float realNorm = real.Norm ();
    const float rdd = real.Dot (dual);

    return csDualQuaternion (
      real.GetConjugate ()*realNorm,
      dual.GetConjugate ()*(realNorm - 2*rdd));
  }

  inline csVector3 TransformPoint (const csVector3& v) const
  {
    csVector3 position = v + 2.0f * (real.v % ((real.v % v) + real.w * v));
    csVector3 trans = 2.0f * (real.w * dual.v - dual.w * real.v + (real.v % dual.v));

    return position + trans;
  }

  inline csVector3 Transform (const csVector3& v) const
  {
    csVector3 position = v + 2.0f * (real.v % ((real.v % v) + real.w * v));
    //csVector3 trans = 2.0f * (real.w * dual.v - dual.w * real.v + (real.v % dual.v));

    return position /*+ trans*/;
  }

  // Data
  csQuaternion real;

  csQuaternion dual;
};

#endif // __CS_QUATERNION_H__

---