stlquat.h

矩阵、向量以及四元数的封装类

//========================================================================================
// Copyright (C) 2009 by Ma Zhi-Hao, Key Lab.
//          		The Academy of Equipment Command & Technology, PLA
//          		HuaiRou, Beijing, 101416, China
// File 		：STLquat.h
// Version	：1.0
// Email  	：[e-mail]
// Address  : 164, P.O.Box 3380, Beijing
//========================================================================================

/** \file
 * Quaternion
 */

#ifndef __STLMATH_QUAT_H__
#define __STLMATH_QUAT_H__

#include "STLvec.h"

namespace shooter{ namespace math{

template <size_t R, size_t C>
struct Matrix;

/**
 * The quaternion.
 */
struct Quaternion
{
public:
  /// The quaternion components.
  union
  {
    struct
    {
      /// The scalar.
      double      s;
      /// The x component
      double      x;
      /// The y component
      double      y;
      /// The z component
      double      z;
    };
    /// The component array.
    double        quat[4];
  };

public:
  /**
   * Construct a new quaternion.
   */
  Quaternion() :
    s(1), x(0), y(0), z(0)
  {}

  /**
   * Construct a new quaternion with given values.
   */
  Quaternion(double s, double x, double y, double z) :
    s(s), x(x), y(y), z(z)
  {}

  /**
   * Construct a new quaternion with given values array.
   */
  Quaternion(const double *q) :
    s(q[0]), x(q[1]), y(q[2]), z(q[3])
  {}

  /**
   * Construct a new quaternion with given type compatible values array.
   */
  template <typename T>
  Quaternion(const T *q) :
    s(q[0]), x(q[1]), y(q[2]), z(q[3])
  {}

  /**
   * Construct a new quaternion with given vector and scalar.
   */
  Quaternion(double s, const Vector<3> &v) :
    s(s), x(v.x), y(v.y), z(v.z)
  {}

  /**
   * Construct a new quaternion with another one.
   */
  Quaternion(const Quaternion &q) :
    s(q.s), x(q.x), y(q.y), z(q.z)
  {}

  /**
   * Construct a new quaternion from a rotation matrix.
   */
  Quaternion(const Matrix<3, 3> &m);

  /**
   * Assign the quaternion with given values array.
   */
  inline Quaternion &operator =(const double *q)
  {
    s = q[0];
    x = q[1]; y = q[2]; z = q[3];
    return *this;
  }

  /**
   * Assign the quaternion with given type compatible values array.
   */
  template <typename T>
  inline Quaternion &operator =(const T *q)
  {
    s = q[0]; x = q[1]; y = q[2]; z = q[3];
    return *this;
  }

  /**
   * Assign this quaternion with given rotation matrix.
   */
  inline Quaternion &operator =(const Matrix<3, 3> &m);

  /**
   * Assign this quaternion with another one.
   */
  inline Quaternion &operator =(const Quaternion &q)
  {
    s = q.s;
    x = q.x;
    y = q.y;
    z = q.z;
    return *this;
  }

public:
  /**
   * Set the quaternion with given values.
   */
	inline Quaternion &set(double s, double x, double y, double z)
  {
    this->s = s;
    this->x = x;
    this->y = y;
    this->z = z;
    return *this;
  }

  /**
   * Set the quaternion with given values array.
   */
	inline Quaternion &set(const double *q)
  {
    s = q[0];
    x = q[1]; y = q[2]; z = q[3];
    return *this;
  }

  /**
   * Set the quaternion with given type compatible values array.
   */
  template <typename T>
  inline Quaternion &set(const T *q)
  {
    s = q[0]; x = q[1]; y = q[2]; z = q[3];
    return *this;
  }

  /**
   * Set the quaternion with given scalar and vector.
   */
  inline Quaternion &set(double s, const Vector<3> &v)
  {
    this->s = s;
    x = v.x; y = v.y; z = v.z;
    return *this;
  }

  /**
   * Set the quaternion with another one.
   */
  inline Quaternion &set(const Quaternion &q)
  {
    s = q.s;
    x = q.x; y = q.y; z = q.z;
    return *this;
  }

  /**
   * Set the quaternion with given matrix.
   */
  inline Quaternion &set(const Matrix<3, 3> &m);

  /**
   * Set a quaternion using axis-angle representation
   * \param axis
   *  Rotation axis. Should be normalized before calling this function.
   * \param angle
   *  Angle to rotate about axis (in radians)
   */
  inline Quaternion &setAxisAngle(const Vector<3> &axis, double angle)
  {
    double s1 = sin(angle * 0.5);
    x = axis.x * s1;
    y = axis.y * s1;
    z = axis.z * s1;
    s = cos(angle * 0.5);
    return *this;
  }

  /**
   * Get a quaternion as axis-angle representation
   * \param axis
   * Rotation axis.
   * \param angle
   * Angle to rotate about axis (in radians)
   */
  inline void getAxisAngle(Vector<3> &axis, double &angle) const
  {
    angle = 2.0 * acos(s);
    axis.set(x, y, z);
    if (axis.getSquaredNorm() != 0)
      axis.normalize();
    else
      axis.set(1.0, 0.0, 0.0);
  }

  /**
   * Set quaternion to identity rotation
   */
  inline Quaternion &identity()
  {
    s = 1;
    x = y = z = 0;
    return *this;
  }

  /**
   * Get the conjugate quaternion
   */
  inline Quaternion getConjugate() const
  {
  	return Quaternion(s, -x, -y, -z);
  }

  /**
   * Set this quaternion to its own conjugate
   */
  inline Quaternion &conjugate()
  {
  	x = -x; y = -y; z = -z;
  	return *this;
  }

  /**
   * Get the squared norm of this quaternion (equals dot with itself)
   */
  inline double getSquaredNorm() const
  {
  	return s * s + x * x + y * y + z * z;
  }

  /**
   * Get the norm of this quaternion
   */
  inline double getNorm() const
  {
  	return sqrt(s * s + x * x + y * y + z * z);
  }

  /**
   * Return a unit-lenght version of this quaternion (also called sgn)
   * Attempting to normalize a zero-length quaternion will result in a divide by
   * zero error.  This is as it should be... fix the calling code.
   */
  inline Quaternion getUnit() const
  {
  	return (*this) / getNorm();
  }

  /**
   * Rotate the given vector.
   */
  inline void rotate(Vector<3> &v)
  {
    Quaternion p(0, v);
    p = (*this % p) % getConjugate();
    v.set(p.x, p.y, p.z);
  }

  /**
   * Query the rotated vector of given one by this quaternion.
   */
  inline Vector<3> getRotVector(const Vector<3> &v)
  {
    Quaternion p(0, v);
    p = (*this % p) % getConjugate();
    return Vector<3>(p.x, p.y, p.z);
  }

  /**
   * Query the euler angle in radians with 312 order.
   */
  inline void getEuler312(double &rZ, double &rX, double &rY) const
  {
    double sx = 2 * (quat[0] * quat[1] + quat[2] * quat[3]);
    if (sx <= -1) rX = -STLC_HPI;
    else if (sx >= 1) rX = STLC_HPI;
    else rX = asin(sx);

    rY = atan2(2 * (quat[0] * quat[2] - quat[1] * quat[3]),
               quat[0] * quat[0] - quat[1] * quat[1] -
               quat[2] * quat[2] + quat[3] * quat[3]);
    rZ = atan2(2 * (quat[0] * quat[3] - quat[1] * quat[2]),
               quat[0] * quat[0] - quat[1] * quat[1] +
               quat[2] * quat[2] - quat[3] * quat[3]);
  }

public:
  /**
   * Unary + operator.
   */
	inline Quaternion operator +() const
  {
  	return *this;
  }

  /**
   * Unary - operator.
   */
	inline Quaternion operator -() const
  {
  	return Quaternion(-s, -x, -y, -z);
  }

  /**
   * Add quaternion to this one
   */
	inline Quaternion &operator +=(const Quaternion &q)
  {
    s += q.s;
    x += q.x; y += q.y; z += q.z;
    return *this;
  }

  /**
   * Subtract quaternion from this one
   */
	inline Quaternion &operator -=(const Quaternion &q)
  {
    s -= q.s;
    x -= q.x; y -= q.y; z -= q.z;
    return *this;
  }

  /**
   * Multiply by scalar
   */
	inline Quaternion &operator *=(double s)
  {
    s *= s;
    x *= s; y *= s; z *= s;
    return *this;
  }

  /**
   * Divide this by a scalar.
   */
	inline Quaternion &operator /=(double s)
  {
    s = 1.0 / s;
    s *= s;
    x *= s; y *= s; z *= s;
    return *this;
  }

  /**
   * Add two quaternions.
   */
  friend Quaternion operator +(const Quaternion &q1, const Quaternion &q2);

  /**
   * Subtract two quaternions.
   */
  friend Quaternion operator -(const Quaternion &q1, const Quaternion &q2);

  /**
   * Multiply a quaternion with a scalar.
   */
  friend Quaternion operator *(const Quaternion &q, double s);

  /**
   * Multiply a quaternion with a scalar.
   */
  friend Quaternion operator *(double s, const Quaternion &q);

  /**
   * Divide a quaternion by a scalar.
   */
  friend Quaternion operator /(const Quaternion &q, double s);

  /**
   * Return euclidian inner-product (dot)
   */
  friend double operator *(const Quaternion &q1, const Quaternion &q2);

  /**
   * Multiply two quaternions.
   */
  friend Quaternion operator %(const Quaternion &q1, const Quaternion &q2);

public:
  /**
   * Interpolate a quaternion @a q1 with another using normalized linear
   * interpolation (nlerp) using given interpolation factor.
   */
  static inline Quaternion interpolateNlerp(double t,
           const Quaternion &q1, const Quaternion &q2)
  {
  	return (q1 + t * (q2 - q1)).getUnit();
  }

  /**
   * Interpolate a quaternion @a q1 with another using spherical linear
   * interpolation (slerp) using given interpolation factor.
   */
  static inline Quaternion interpolateSlerp(double t,
           const Quaternion &q1, const Quaternion &q2)
  {
    double omega, cosom, invsinom, scale0, scale1;

    Quaternion quato(q2);

    // decide if one of the quaternions is backwards
    double a = (q1-q2).getSquaredNorm ();
    double b = (q2+q2).getSquaredNorm ();
    if (a > b)
    {
      quato = -q2;
    }

    // Calculate dot between quats
    cosom = q1 * quato;

    // Make sure the two quaternions are not exactly opposite? (within a little
    // slop).
    if (cosom > -0.9998)
    {
      // Are they more than a little bit different?  Avoid a divided by zero
      // and lerp if not.
      if (cosom < 0.9998)
      {
        // Yes, do a slerp
        omega = acos(cosom);
        invsinom = 1.0 / sin(omega);
        scale0 = sin((1.0 - t) * omega) * invsinom;
        scale1 = sin(t * omega) * invsinom;
      }
      else
      {
        // Not a very big difference, do a lerp
        scale0 = 1.0 - t;
        scale1 = t;
      }

      return Quaternion (
        scale0 * q1.x + scale1 * quato.x,
        scale0 * q1.y + scale1 * quato.y,
        scale0 * q1.z + scale1 * quato.z,
        scale0 * q1.s + scale1 * quato.s);
    }

    // The quaternions are nearly opposite so to avoid a divided by zero error
    // Calculate a perpendicular quaternion and slerp that direction
    scale0 = sin((1.0 - t) * STLC_PI);
    scale1 = sin(t * STLC_PI);
    return Quaternion (
      scale0 * q1.x + scale1 * -quato.y,
      scale0 * q1.y + scale1 * quato.x,
      scale0 * q1.z + scale1 * -quato.s,
      scale0 * q1.s + scale1 * quato.z);
  }
};

}}

#endif /* __STLMATH_QUAT_H__ */