kquaternion.h from Spatial Knowledge Experiments at Krugle

Show kquaternion.h syntax highlighted

/*
    Copyright (C) 2003, 2004, 2005 by Luca Cappa
    Written by Luca Cappa groton@users.sourceforge.net
  
    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 __KQUATERNION_H__
#define __KQUATERNION_H__

#include "eulerangles.h"


class KQuaternion
{
private:
  int m_order; //order of applying the Eulers Angles.
public:
  
  /// Construct a 0,0,0,1 quaternion.
  KQuaternion (int p_order = EulOrdYXZs) : m_order (p_order)
  { 
    Init(0, 0, 0, 1);
  }

  /// Construct a quaternion with the given parameters.
  KQuaternion (float theR, float theX=0.0, float theY=0.0, float theZ=0.0,
    int p_order = EulOrdYXZs) : m_order (p_order)
  { 
    Init (theR, theX, theY, theZ ); 
  }

  /// Copy constructor.
  KQuaternion (const KQuaternion& q) :
    m_order (q.m_order)
  { 
    Init (q.r, q.x, q.y, q.z); 
  }
  /// Construct quaternion from a vector.
  KQuaternion (const csVector3& q, int p_order = EulOrdYXZs) :
    m_order (p_order)
  { 
    Init (0, q.x, q.y, q.z);
  }

  /// Construct quaternion from a matrix
  KQuaternion (const csMatrix3& smat, int p_order = EulOrdYXZs);

  ~KQuaternion ()
  {
    //Intentionally blank.
  }

  void SetWithEuler (const csVector3& p_angles);

  void SetWithEuler (const csVector3& p_angles, int p_order);

  void GetEulerAngles (csVector3& p_angles);
  
  void GetEulerAngles (csVector3& p_angles, int p_order);

  void Normalize ()
  {
    float l_dist, l_square;

    l_square = x * x + y * y + z * z + r * r;

    if (l_square > 0.0f)
      l_dist = (float)(1.0f / sqrt (l_square));
    else 
      l_dist = 1;

    x *= l_dist;
    y *= l_dist;
    z *= l_dist;
    r *= l_dist;
  }

  /// Initialize a quaternion with specific values.
  inline void Init (float theR, float theX, float theY, float theZ)
  { 
    r = theR; 
    x = theX; 
    y = theY; 
    z = theZ; 
  }

  ///
  inline friend KQuaternion operator+ (const KQuaternion& q1, const KQuaternion& q2)
  { 
    return KQuaternion (q1.r + q2.r, q1.x + q2.x, q1.y + q2.y, q1.z + q2.z,
      q1.m_order);
  }
  
  ///
  inline friend KQuaternion operator- (const KQuaternion& q1, const KQuaternion& q2)
  { 
    return KQuaternion (q1.r - q2.r, q1.x - q2.x, q1.y - q2.y, q1.z - q2.z, 
      q1.m_order);
  }
  
  ///
  inline friend KQuaternion operator* (const KQuaternion& q1, const KQuaternion& q2)
  { 
    return KQuaternion (q1.r*q2.r -  q1.x*q2.x - q1.y*q2.y - q1.z*q2.z,
             q1.y*q2.z -  q1.z*q2.y + q1.r*q2.x + q1.x*q2.r,
             q1.z*q2.x -  q1.x*q2.z + q1.r*q2.y + q1.y*q2.r,
             q1.x*q2.y -  q1.y*q2.x + q1.r*q2.z + q1.z*q2.r, q1.m_order); 
  }

  ///
  KQuaternion& operator*= (const KQuaternion& q2)
  {
    Init (r*q2.r -  x*q2.x - y*q2.y - z*q2.z,
      y*q2.z -  z*q2.y + r*q2.x + x*q2.r,
      z*q2.x -  x*q2.z + r*q2.y + y*q2.r,
      x*q2.y -  y*q2.x + r*q2.z + z*q2.r);
    return *this;
  }


  const csMatrix3 GetMatrix () const;

  ///
  void Conjugate () 
  { 
    Init (r, -x, -y, -z); 
  }

  /// Negate all parameters of the quaternion
  void Negate () 
  { 
    Init(-r, -x, -y, -z); 
  }

  /** Invert the orientation of this quaternion */
  void Invert();

  /// rotated = q * vec * qConj.
  csVector3 Rotate (csVector3 const&vec) const
  {
    KQuaternion p (vec);
    KQuaternion qConj (r, -x, -y, -z);

    p = *this * p;
    p *= qConj;
    return csVector3 (p.x, p.y, p.z);
  }


  /**
   * Spherical Linear Interpolation between two quaternions
   * Calculated between this class & the second quaternion by the slerp
   * factor and returned as a new quaternion
   */
  KQuaternion Slerp (const KQuaternion &quat2, float slerp) const;

  //Most important datas!
  float x;
  float y;
  float z;
  float r;
};

#endif
See more files for this project here