vrmaths.h

 
#ifndef __VRMATHS_H__
#define __VRMATHS_H__
 
#include "xrmath.h"
 
#define VR_PI 3.14159265358979323846f
 
static inline float vrRadiansFromDegrees(float deg)
{
  return deg * VR_PI / 180.0f;
}
 
static inline float vrDegreesFromRadians(float rad)
{
  return rad * 180.f / VR_PI;
}
 
class Matrix3;
class Quaternion;
 
class Vector2i
{
public:
  int x, y;
 
public:
  static const Vector2i ZERO;
 
  inline Vector2i()
  {
  }
 
  inline Vector2i(const XrVector2f vec)
  {
    x = (int)vec.x;
    y = (int)vec.y;
  }
 
  inline Vector2i(const int fX, const int fY)
    : x(fX), y(fY) {}
 
  inline explicit Vector2i(int* const r)
    : x(r[0]), y(r[1]) {}
 
  inline explicit Vector2i(const int scaler)
    : x(scaler)
    , y(scaler) {}
 
  inline int operator [] (const size_t i) const
  {
    return *(&x + i);
  }
 
  inline int& operator [] (const size_t i)
  {
    return *(&x + i);
  }
  inline int* ptr()
  {
    return &x;
  }
  inline const int* ptr() const
  {
    return &x;
  }
 
  inline Vector2i& operator = (const Vector2i& rkVector)
  {
    x = rkVector.x;
    y = rkVector.y;
 
    return *this;
  }
 
  inline Vector2i& operator = (const int fScaler)
  {
    x = fScaler;
    y = fScaler;
 
    return *this;
  }
 
  inline bool operator == (const Vector2i& rkVector) const
  {
    return (x == rkVector.x && y == rkVector.y);
  }
 
  inline bool operator != (const Vector2i& rkVector) const
  {
    return (x != rkVector.x || y != rkVector.y);
  }
 
  // arithmetic operations
  inline Vector2i operator + (const Vector2i& rkVector) const
  {
    return Vector2i(
      x + rkVector.x,
      y + rkVector.y);
  }
 
  inline Vector2i operator - (const Vector2i& rkVector) const
  {
    return Vector2i(
      x - rkVector.x,
      y - rkVector.y);
  }
 
  inline Vector2i operator * (const int fScalar) const
  {
    return Vector2i(
      x * fScalar,
      y * fScalar);
  }
 
  inline Vector2i operator * (const Vector2i& rhs) const
  {
    return Vector2i(
      x * rhs.x,
      y * rhs.y);
  }
 
  inline Vector2i operator / (const int fScalar) const
  {
    float fInv = 1.0f / fScalar;
 
    return Vector2i(
      (int)((float)x * fInv),
      (int)((float)y * fInv));
  }
 
  inline Vector2i operator / (const Vector2i& rhs) const
  {
    return Vector2i(
      x / rhs.x,
      y / rhs.y);
  }
 
  inline const Vector2i& operator + () const
  {
    return *this;
  }
 
  inline Vector2i operator - () const
  {
    return Vector2i(-x, -y);
  }
 
  // overloaded operators to help Vector2
  inline friend Vector2i operator * (const int fScalar, const Vector2i& rkVector)
  {
    return Vector2i(
      fScalar * rkVector.x,
      fScalar * rkVector.y);
  }
 
  inline friend Vector2i operator / (const int fScalar, const Vector2i& rkVector)
  {
    return Vector2i(
      fScalar / rkVector.x,
      fScalar / rkVector.y);
  }
 
  inline friend Vector2i operator + (const Vector2i& lhs, const int rhs)
  {
    return Vector2i(
      lhs.x + rhs,
      lhs.y + rhs);
  }
 
  inline friend Vector2i operator + (const int lhs, const Vector2i& rhs)
  {
    return Vector2i(
      lhs + rhs.x,
      lhs + rhs.y);
  }
 
  inline friend Vector2i operator - (const Vector2i& lhs, const int rhs)
  {
    return Vector2i(
      lhs.x - rhs,
      lhs.y - rhs);
  }
 
  inline friend Vector2i operator - (const int lhs, const Vector2i& rhs)
  {
    return Vector2i(
      lhs - rhs.x,
      lhs - rhs.y);
  }
 
  // arithmetic updates
  inline Vector2i& operator += (const Vector2i& rkVector)
  {
    x += rkVector.x;
    y += rkVector.y;
 
    return *this;
  }
 
  inline Vector2i& operator += (const int fScalar)
  {
    x += fScalar;
    y += fScalar;
    return *this;
  }
 
  inline Vector2i& operator -= (const Vector2i& rkVector)
  {
    x -= rkVector.x;
    y -= rkVector.y;
 
    return *this;
  }
 
  inline Vector2i& operator -= (const int fScalar)
  {
    x -= fScalar;
    y -= fScalar;
    return *this;
  }
 
  inline Vector2i& operator *= (const int fScalar)
  {
    x *= fScalar;
    y *= fScalar;
    return *this;
  }
 
  inline Vector2i& operator *= (const Vector2i& rkVector)
  {
    x *= rkVector.x;
    y *= rkVector.y;
 
    return *this;
  }
 
  inline Vector2i& operator /= (const Vector2i& rkVector)
  {
    x /= rkVector.x;
    y /= rkVector.y;
 
    return *this;
  }
};
 
class Vector2
{
public:
  float x, y;
 
public:
  static const Vector2 ZERO;
 
  inline Vector2()
  {
  }
 
  inline Vector2(const XrVector2f vec)
  {
    x = (float)vec.x;
    y = (float)vec.y;
  }
 
  inline Vector2(const float fX, const float fY)
    : x(fX), y(fY) {}
 
  inline explicit Vector2(float* const r)
    : x(r[0]), y(r[1]) {}
 
  inline explicit Vector2(const float scaler)
    : x(scaler)
    , y(scaler) {}
 
  inline float operator [] (const size_t i) const
  {
    return *(&x + i);
  }
 
  inline float& operator [] (const size_t i)
  {
    return *(&x + i);
  }
  inline float* ptr()
  {
    return &x;
  }
  inline const float* ptr() const
  {
    return &x;
  }
 
  inline Vector2& operator = (const Vector2& rkVector)
  {
    x = rkVector.x;
    y = rkVector.y;
 
    return *this;
  }
 
  inline Vector2& operator = (const float fScaler)
  {
    x = fScaler;
    y = fScaler;
 
    return *this;
  }
 
  inline bool operator == (const Vector2& rkVector) const
  {
    return (x == rkVector.x && y == rkVector.y);
  }
 
  inline bool operator != (const Vector2& rkVector) const
  {
    return (x != rkVector.x || y != rkVector.y);
  }
 
  // arithmetic operations
  inline Vector2 operator + (const Vector2& rkVector) const
  {
    return Vector2(
      x + rkVector.x,
      y + rkVector.y);
  }
 
  inline Vector2 operator - (const Vector2& rkVector) const
  {
    return Vector2(
      x - rkVector.x,
      y - rkVector.y);
  }
 
  inline Vector2 operator * (const float fScalar) const
  {
    return Vector2(
      x * fScalar,
      y * fScalar);
  }
 
  inline Vector2 operator * (const Vector2& rhs) const
  {
    return Vector2(
      x * rhs.x,
      y * rhs.y);
  }
 
  inline Vector2 operator / (const float fScalar) const
  {
    float fInv = 1.0f / fScalar;
 
    return Vector2(
      x * fInv,
      y * fInv);
  }
 
  inline Vector2 operator / (const Vector2& rhs) const
  {
    return Vector2(
      x / rhs.x,
      y / rhs.y);
  }
 
  inline const Vector2& operator + () const
  {
    return *this;
  }
 
  inline Vector2 operator - () const
  {
    return Vector2(-x, -y);
  }
 
  // overloaded operators to help Vector2
  inline friend Vector2 operator * (const float fScalar, const Vector2& rkVector)
  {
    return Vector2(
      fScalar * rkVector.x,
      fScalar * rkVector.y);
  }
 
  inline friend Vector2 operator / (const float fScalar, const Vector2& rkVector)
  {
    return Vector2(
      fScalar / rkVector.x,
      fScalar / rkVector.y);
  }
 
  inline friend Vector2 operator + (const Vector2& lhs, const float rhs)
  {
    return Vector2(
      lhs.x + rhs,
      lhs.y + rhs);
  }
 
  inline friend Vector2 operator + (const float lhs, const Vector2& rhs)
  {
    return Vector2(
      lhs + rhs.x,
      lhs + rhs.y);
  }
 
  inline friend Vector2 operator - (const Vector2& lhs, const float rhs)
  {
    return Vector2(
      lhs.x - rhs,
      lhs.y - rhs);
  }
 
  inline friend Vector2 operator - (const float lhs, const Vector2& rhs)
  {
    return Vector2(
      lhs - rhs.x,
      lhs - rhs.y);
  }
 
  // arithmetic updates
  inline Vector2& operator += (const Vector2& rkVector)
  {
    x += rkVector.x;
    y += rkVector.y;
 
    return *this;
  }
 
  inline Vector2& operator += (const float fScalar)
  {
    x += fScalar;
    y += fScalar;
    return *this;
  }
 
  inline Vector2& operator -= (const Vector2& rkVector)
  {
    x -= rkVector.x;
    y -= rkVector.y;
 
    return *this;
  }
 
  inline Vector2& operator -= (const float fScalar)
  {
    x -= fScalar;
    y -= fScalar;
    return *this;
  }
 
  inline Vector2& operator *= (const float fScalar)
  {
    x *= fScalar;
    y *= fScalar;
    return *this;
  }
 
  inline Vector2& operator *= (const Vector2& rkVector)
  {
    x *= rkVector.x;
    y *= rkVector.y;
 
    return *this;
  }
 
  inline Vector2& operator /= (const float fScalar)
  {
    float fInv = 1.0f / fScalar;
 
    x *= fInv;
    y *= fInv;
 
    return *this;
  }
 
  inline Vector2& operator /= (const Vector2& rkVector)
  {
    x /= rkVector.x;
    y /= rkVector.y;
 
    return *this;
  }
};
 
class Vector3
{
public:
  float x, y, z;
 
public:
  static const Vector3 ZERO;
 
  inline Vector3()
  {
  }
 
  inline Vector3(const XrVector3f vec)
  {
    x = (float)vec.x;
    y = (float)vec.y;
    z = (float)vec.z;
  }
 
  inline Vector3(const float fX, const float fY, const float fZ)
    : x(fX), y(fY), z(fZ) {}
 
  inline explicit Vector3(const float afCoordinate[3])
    : x(afCoordinate[0]),
    y(afCoordinate[1]),
    z(afCoordinate[2]) {}
 
  inline explicit Vector3(const int afCoordinate[3])
  {
    x = (float)afCoordinate[0];
    y = (float)afCoordinate[1];
    z = (float)afCoordinate[2];
  }
 
  inline explicit Vector3(float* const r)
    : x(r[0]), y(r[1]), z(r[2])  {}
 
  inline explicit Vector3(const float scaler)
    : x(scaler)
    , y(scaler)
    , z(scaler) {}
 
  inline float operator [] (const size_t i) const
  {
    return *(&x + i);
  }
 
  inline float& operator [] (const size_t i)
  {
    return *(&x + i);
  }
  inline float* ptr()
  {
    return &x;
  }
  inline const float* ptr() const
  {
    return &x;
  }
 
  inline Vector3& operator = (const Vector3& rkVector)
  {
    x = rkVector.x;
    y = rkVector.y;
    z = rkVector.z;
 
    return *this;
  }
 
  inline Vector3& operator = (const float fScaler)
  {
    x = fScaler;
    y = fScaler;
    z = fScaler;
 
    return *this;
  }
 
  inline bool operator == (const Vector3& rkVector) const
  {
    return (x == rkVector.x && y == rkVector.y && z == rkVector.z);
  }
 
  inline bool operator != (const Vector3& rkVector) const
  {
    return (x != rkVector.x || y != rkVector.y || z != rkVector.z);
  }
 
  // arithmetic operations
  inline Vector3 operator + (const Vector3& rkVector) const
  {
    return Vector3(
      x + rkVector.x,
      y + rkVector.y,
      z + rkVector.z);
  }
 
  inline Vector3 operator - (const Vector3& rkVector) const
  {
    return Vector3(
      x - rkVector.x,
      y - rkVector.y,
      z - rkVector.z);
  }
 
  inline Vector3 operator * (const float fScalar) const
  {
    return Vector3(
      x * fScalar,
      y * fScalar,
      z * fScalar);
  }
 
  inline Vector3 operator * (const Vector3& rhs) const
  {
    return Vector3(
      x * rhs.x,
      y * rhs.y,
      z * rhs.z);
  }
 
  inline Vector3 operator / (const float fScalar) const
  {
    float fInv = 1.0f / fScalar;
 
    return Vector3(
      x * fInv,
      y * fInv,
      z * fInv);
  }
 
  inline Vector3 operator / (const Vector3& rhs) const
  {
    return Vector3(
      x / rhs.x,
      y / rhs.y,
      z / rhs.z);
  }
 
  inline const Vector3& operator + () const
  {
    return *this;
  }
 
  inline Vector3 operator - () const
  {
    return Vector3(-x, -y, -z);
  }
 
  // overloaded operators to help Vector3
  inline friend Vector3 operator * (const float fScalar, const Vector3& rkVector)
  {
    return Vector3(
      fScalar * rkVector.x,
      fScalar * rkVector.y,
      fScalar * rkVector.z);
  }
 
  inline friend Vector3 operator / (const float fScalar, const Vector3& rkVector)
  {
    return Vector3(
      fScalar / rkVector.x,
      fScalar / rkVector.y,
      fScalar / rkVector.z);
  }
 
  inline friend Vector3 operator + (const Vector3& lhs, const float rhs)
  {
    return Vector3(
      lhs.x + rhs,
      lhs.y + rhs,
      lhs.z + rhs);
  }
 
  inline friend Vector3 operator + (const float lhs, const Vector3& rhs)
  {
    return Vector3(
      lhs + rhs.x,
      lhs + rhs.y,
      lhs + rhs.z);
  }
 
  inline friend Vector3 operator - (const Vector3& lhs, const float rhs)
  {
    return Vector3(
      lhs.x - rhs,
      lhs.y - rhs,
      lhs.z - rhs);
  }
 
  inline friend Vector3 operator - (const float lhs, const Vector3& rhs)
  {
    return Vector3(
      lhs - rhs.x,
      lhs - rhs.y,
      lhs - rhs.z);
  }
 
  // arithmetic updates
  inline Vector3& operator += (const Vector3& rkVector)
  {
    x += rkVector.x;
    y += rkVector.y;
    z += rkVector.z;
 
    return *this;
  }
 
  inline Vector3& operator += (const float fScalar)
  {
    x += fScalar;
    y += fScalar;
    z += fScalar;
    return *this;
  }
 
  inline Vector3& operator -= (const Vector3& rkVector)
  {
    x -= rkVector.x;
    y -= rkVector.y;
    z -= rkVector.z;
 
    return *this;
  }
 
  inline Vector3& operator -= (const float fScalar)
  {
    x -= fScalar;
    y -= fScalar;
    z -= fScalar;
    return *this;
  }
 
  inline Vector3& operator *= (const float fScalar)
  {
    x *= fScalar;
    y *= fScalar;
    z *= fScalar;
    return *this;
  }
 
  inline Vector3& operator *= (const Vector3& rkVector)
  {
    x *= rkVector.x;
    y *= rkVector.y;
    z *= rkVector.z;
 
    return *this;
  }
 
  inline Vector3& operator /= (const float fScalar)
  {
    float fInv = 1.0f / fScalar;
 
    x *= fInv;
    y *= fInv;
    z *= fInv;
 
    return *this;
  }
 
  inline Vector3& operator /= (const Vector3& rkVector)
  {
    x /= rkVector.x;
    y /= rkVector.y;
    z /= rkVector.z;
 
    return *this;
  }
 
  inline Vector3 crossProduct(const Vector3& rkVector) const
  {
    return Vector3(
      y * rkVector.z - z * rkVector.y,
      z * rkVector.x - x * rkVector.z,
      x * rkVector.y - y * rkVector.x);
  }
 
  inline float dotProduct(const Vector3& vec) const
  {
    float ret = 0.0f;
    for (int i = 0; i < 3; i++)
      ret += ptr()[i] * vec.ptr()[i];
    return ret;
  }
 
  inline float squaredLength() const { return dotProduct(*this); }
 
  inline float length() const { return std::sqrt(squaredLength()); }
 
  float distance(const Vector3& rhs) const
  {
    return (*this - rhs).length();
  }
};
 
class Vector4
{
public:
  float x, y, z, w;
 
public:
  static const Vector4 ZERO;
 
  inline Vector4()
  {
  }
 
  inline Vector4(const float fX, const float fY, const float fZ, const float fW)
    : x(fX), y(fY), z(fZ), w(fW) {}
 
  inline explicit Vector4(const float afCoordinate[4])
    : x(afCoordinate[0]),
      y(afCoordinate[1]),
      z(afCoordinate[2]),
      w(afCoordinate[3]) {}
 
  inline explicit Vector4(const int afCoordinate[4])
  {
    x = (float)afCoordinate[0];
    y = (float)afCoordinate[1];
    z = (float)afCoordinate[2];
    w = (float)afCoordinate[3];
  }
 
  inline explicit Vector4(float* const r)
    : x(r[0]), y(r[1]), z(r[2]), w(r[3])  {}
 
  inline explicit Vector4(const float scaler)
    : x(scaler)
    , y(scaler)
    , z(scaler)
    , w(scaler) {}
 
  inline float operator [] (const size_t i) const
  {
    return *(&x + i);
  }
 
  inline float& operator [] (const size_t i)
  {
    return *(&x + i);
  }
  inline float* ptr()
  {
    return &x;
  }
  inline const float* ptr() const
  {
    return &x;
  }
 
  inline Vector4& operator = (const Vector4& rkVector)
  {
    x = rkVector.x;
    y = rkVector.y;
    z = rkVector.z;
    w = rkVector.w;
 
    return *this;
  }
 
  inline Vector4& operator = (const float fScaler)
  {
    x = fScaler;
    y = fScaler;
    z = fScaler;
    w = fScaler;
 
    return *this;
  }
 
  inline bool operator == (const Vector4& rkVector) const
  {
    return (x == rkVector.x && y == rkVector.y && z == rkVector.z);
  }
 
  inline bool operator != (const Vector4& rkVector) const
  {
    return (x != rkVector.x || y != rkVector.y || z != rkVector.z || w != rkVector.w);
  }
 
  // arithmetic operations
  inline Vector4 operator + (const Vector4& rkVector) const
  {
    return Vector4(
      x + rkVector.x,
      y + rkVector.y,
      z + rkVector.z,
      w + rkVector.w);
  }
 
  inline Vector4 operator - (const Vector4& rkVector) const
  {
    return Vector4(
      x - rkVector.x,
      y - rkVector.y,
      z - rkVector.z,
      w - rkVector.w);
  }
 
  inline Vector4 operator * (const float fScalar) const
  {
    return Vector4(
      x * fScalar,
      y * fScalar,
      z * fScalar,
      w * fScalar);
  }
 
  inline Vector4 operator * (const Vector4& rhs) const
  {
    return Vector4(
      x * rhs.x,
      y * rhs.y,
      z * rhs.z,
      w * rhs.w);
  }
 
  inline Vector4 operator / (const float fScalar) const
  {
    float fInv = 1.0f / fScalar;
 
    return Vector4(
      x * fInv,
      y * fInv,
      z * fInv,
      w * fInv);
  }
 
  inline Vector4 operator / (const Vector4& rhs) const
  {
    return Vector4(
      x / rhs.x,
      y / rhs.y,
      z / rhs.z,
      z / rhs.w);
  }
 
  inline const Vector4& operator + () const
  {
    return *this;
  }
 
  inline Vector4 operator - () const
  {
    return Vector4(-x, -y, -z, -w);
  }
 
  // overloaded operators to help Vector3
  inline friend Vector4 operator * (const float fScalar, const Vector4& rkVector)
  {
    return Vector4(
      fScalar * rkVector.x,
      fScalar * rkVector.y,
      fScalar * rkVector.z,
      fScalar * rkVector.w);
  }
 
  inline friend Vector4 operator / (const float fScalar, const Vector4& rkVector)
  {
    return Vector4(
      fScalar / rkVector.x,
      fScalar / rkVector.y,
      fScalar / rkVector.z,
      fScalar / rkVector.w);
  }
 
  inline friend Vector4 operator + (const Vector4& lhs, const float rhs)
  {
    return Vector4(
      lhs.x + rhs,
      lhs.y + rhs,
      lhs.z + rhs,
      lhs.w + rhs);
  }
 
  inline friend Vector4 operator + (const float lhs, const Vector4& rhs)
  {
    return Vector4(
      lhs + rhs.x,
      lhs + rhs.y,
      lhs + rhs.z,
      lhs + rhs.w);
  }
 
  inline friend Vector4 operator - (const Vector4& lhs, const float rhs)
  {
    return Vector4(
      lhs.x - rhs,
      lhs.y - rhs,
      lhs.z - rhs,
      lhs.w - rhs);
  }
 
  inline friend Vector4 operator - (const float lhs, const Vector4& rhs)
  {
    return Vector4(
      lhs - rhs.x,
      lhs - rhs.y,
      lhs - rhs.z,
      lhs - rhs.w);
  }
 
  // arithmetic updates
  inline Vector4& operator += (const Vector4& rkVector)
  {
    x += rkVector.x;
    y += rkVector.y;
    z += rkVector.z;
    w += rkVector.w;
 
    return *this;
  }
 
  inline Vector4& operator += (const float fScalar)
  {
    x += fScalar;
    y += fScalar;
    z += fScalar;
    w += fScalar;
    return *this;
  }
 
  inline Vector4& operator -= (const Vector4& rkVector)
  {
    x -= rkVector.x;
    y -= rkVector.y;
    z -= rkVector.z;
    w -= rkVector.w;
 
    return *this;
  }
 
  inline Vector4& operator -= (const float fScalar)
  {
    x -= fScalar;
    y -= fScalar;
    z -= fScalar;
    w -= fScalar;
    return *this;
  }
 
  inline Vector4& operator *= (const float fScalar)
  {
    x *= fScalar;
    y *= fScalar;
    z *= fScalar;
    w *= fScalar;
    return *this;
  }
 
  inline Vector4& operator *= (const Vector4& rkVector)
  {
    x *= rkVector.x;
    y *= rkVector.y;
    z *= rkVector.z;
    w *= rkVector.w;
 
    return *this;
  }
 
  inline Vector4& operator /= (const float fScalar)
  {
    float fInv = 1.0f / fScalar;
 
    x *= fInv;
    y *= fInv;
    z *= fInv;
    w *= fInv;
 
    return *this;
  }
 
  inline Vector4& operator /= (const Vector4& rkVector)
  {
    x /= rkVector.x;
    y /= rkVector.y;
    z /= rkVector.z;
    w /= rkVector.w;
 
    return *this;
  }
};
 
class Matrix3
{
public:
  inline Matrix3() {}
  inline explicit Matrix3(const float arr[3][3])
  {
    memcpy(m, arr, 9 * sizeof(float));
  }
  inline Matrix3(const Matrix3& rkMatrix)
  {
    memcpy(m, rkMatrix.m, 9 * sizeof(float));
  }
  Matrix3(float fEntry00, float fEntry01, float fEntry02,
    float fEntry10, float fEntry11, float fEntry12,
    float fEntry20, float fEntry21, float fEntry22)
  {
    m[0][0] = fEntry00;
    m[0][1] = fEntry01;
    m[0][2] = fEntry02;
    m[1][0] = fEntry10;
    m[1][1] = fEntry11;
    m[1][2] = fEntry12;
    m[2][0] = fEntry20;
    m[2][1] = fEntry21;
    m[2][2] = fEntry22;
  }
 
  inline const float* operator[] (size_t iRow) const
  {
    return m[iRow];
  }
 
  inline float* operator[] (size_t iRow)
  {
    return m[iRow];
  }
 
  inline Matrix3& operator= (const Matrix3& rkMatrix)
  {
    memcpy(m, rkMatrix.m, 9 * sizeof(float));
    return *this;
  }
 
  bool operator== (const Matrix3& rkMatrix) const;
 
  inline bool operator!= (const Matrix3& rkMatrix) const
  {
    return !operator==(rkMatrix);
  }
 
protected:
  float m[3][3];
 
  // for faster access
  friend class Matrix4;
};
 
class Quaternion
{
public:
  float w, x, y, z;
 
  inline Quaternion() : w(1), x(0), y(0), z(0) {}
 
  inline Quaternion(float fW, float fX, float fY, float fZ) : w(fW), x(fX), y(fY), z(fZ) {}
 
  inline Quaternion(const XrQuaternionf q)
  {
    w = q.w;
    x = q.x;
    y = q.y;
    z = q.z;
  }
 
  inline Quaternion(const Matrix3& rot)
  {
    this->FromRotationMatrix(rot);
  }
 
  inline Quaternion(const float& rfAngle, const Vector3& rkAxis)
  {
    this->FromAngleAxis(rfAngle, rkAxis);
  }
 
  inline float operator [] (const size_t i) const
  {
    return *(&w + i);
  }
 
  inline float& operator [] (const size_t i)
  {
    return *(&w + i);
  }
 
  inline void FromAngleAxis(const float& rfAngle, const Vector3& rkAxis)
  {
    // assert:  axis[] is unit length
    //
    // The quaternion representing the rotation is
    //   q = cos(A/2)+sin(A/2)*(x*i+y*j+z*k)
 
    float fHalfAngle = 0.5f * rfAngle;
    float fSin = std::sin(fHalfAngle);
    w = std::cos(fHalfAngle);
    x = fSin * rkAxis.x;
    y = fSin * rkAxis.y;
    z = fSin * rkAxis.z;
  }
 
  void FromRotationMatrix(const Matrix3& kRot);
 
  void ToRotationMatrix(Matrix3& kRot) const;
 
  inline Quaternion& operator= (const Quaternion& rkQ)
  {
    w = rkQ.w;
    x = rkQ.x;
    y = rkQ.y;
    z = rkQ.z;
    return *this;
  }
  Quaternion operator+ (const Quaternion& rkQ) const
  {
    return Quaternion(w + rkQ.w, x + rkQ.x, y + rkQ.y, z + rkQ.z);
  }
  Quaternion operator- (const Quaternion& rkQ) const
  {
    return Quaternion(w - rkQ.w, x - rkQ.x, y - rkQ.y, z - rkQ.z);
  }
  Quaternion operator* (const Quaternion& rkQ) const
  {
    return Quaternion
      (
      w * rkQ.w - x * rkQ.x - y * rkQ.y - z * rkQ.z,
      w * rkQ.x + x * rkQ.w + y * rkQ.z - z * rkQ.y,
      w * rkQ.y + y * rkQ.w + z * rkQ.x - x * rkQ.z,
      w * rkQ.z + z * rkQ.w + x * rkQ.y - y * rkQ.x
      );
  }
  Quaternion operator* (float fScalar) const
  {
    return Quaternion(fScalar*w, fScalar*x, fScalar*y, fScalar*z);
  }
 
  Vector3 operator* (const Vector3& v) const
  {
    // nVidia SDK implementation
    Vector3 uv, uuv;
    Vector3 qvec(x, y, z);
    uv = qvec.crossProduct(v);
    uuv = qvec.crossProduct(uv);
    uv *= (2.0f * w);
    uuv *= 2.0f;
 
    return v + uv + uuv;
  }
 
  Quaternion operator- () const
  {
    return Quaternion(-w, -x, -y, -z);
  }
  inline bool operator== (const Quaternion& rhs) const
  {
    return (rhs.x == x) && (rhs.y == y) && (rhs.z == z) && (rhs.w == w);
  }
  inline bool operator!= (const Quaternion& rhs) const
  {
    return !operator==(rhs);
  }
  inline float* ptr()
  {
    return &w;
  }
  inline const float* ptr() const
  {
    return &w;
  }
  float Norm() const;
  float normalise(void);
  Quaternion Inverse() const;
};
 
class Matrix4
{
protected:
  union {
    float m[4][4];
    float _m[16];
  };
public:
  static const Matrix4 IDENTITY;
 
  inline Matrix4() {}
 
  inline Matrix4(
    float m00, float m01, float m02, float m03,
    float m10, float m11, float m12, float m13,
    float m20, float m21, float m22, float m23,
    float m30, float m31, float m32, float m33)
  {
    m[0][0] = m00;
    m[0][1] = m01;
    m[0][2] = m02;
    m[0][3] = m03;
    m[1][0] = m10;
    m[1][1] = m11;
    m[1][2] = m12;
    m[1][3] = m13;
    m[2][0] = m20;
    m[2][1] = m21;
    m[2][2] = m22;
    m[2][3] = m23;
    m[3][0] = m30;
    m[3][1] = m31;
    m[3][2] = m32;
    m[3][3] = m33;
  }
 
  inline Matrix4(XrMatrix4x4f mat)
  {
    m[0][0] = mat.m[0];
    m[0][1] = mat.m[4];
    m[0][2] = mat.m[8];
    m[0][3] = mat.m[12];
    m[1][0] = mat.m[1];
    m[1][1] = mat.m[5];
    m[1][2] = mat.m[9];
    m[1][3] = mat.m[13];
    m[2][0] = mat.m[2];
    m[2][1] = mat.m[6];
    m[2][2] = mat.m[10];
    m[2][3] = mat.m[14];
    m[3][0] = mat.m[3];
    m[3][1] = mat.m[7];
    m[3][2] = mat.m[11];
    m[3][3] = mat.m[15];
  }
 
  inline Matrix4(const Matrix3& m3x3)
  {
    operator=(IDENTITY);
    operator=(m3x3);
  }
 
  inline Matrix4(const Quaternion& rot)
  {
    Matrix3 m3x3;
    rot.ToRotationMatrix(m3x3);
    operator=(IDENTITY);
    operator=(m3x3);
  }
 
  inline void extract3x3MatrixNegZ(Matrix3& m3x3) const
  {
    m3x3.m[0][0] = m[0][0];
    m3x3.m[0][1] = m[0][1];
    m3x3.m[0][2] = -m[0][2];
    m3x3.m[1][0] = m[1][0];
    m3x3.m[1][1] = m[1][1];
    m3x3.m[1][2] = -m[1][2];
    m3x3.m[2][0] = -m[2][0];
    m3x3.m[2][1] = -m[2][1];
    m3x3.m[2][2] = m[2][2];
  }
 
  inline void extract3x3MatrixNegX(Matrix3& m3x3) const
  {
    m3x3.m[0][0] = -m[0][0];
    m3x3.m[0][1] = m[0][1];
    m3x3.m[0][2] = m[0][2];
    m3x3.m[1][0] = -m[1][0];
    m3x3.m[1][1] = m[1][1];
    m3x3.m[1][2] = m[1][2];
    m3x3.m[2][0] = m[2][0];
    m3x3.m[2][1] = -m[2][1];
    m3x3.m[2][2] = -m[2][2];
  }
 
  inline void extract3x3Matrix(Matrix3& m3x3) const
  {
    m3x3.m[0][0] = m[0][0];
    m3x3.m[0][1] = m[0][1];
    m3x3.m[0][2] = m[0][2];
    m3x3.m[1][0] = m[1][0];
    m3x3.m[1][1] = m[1][1];
    m3x3.m[1][2] = m[1][2];
    m3x3.m[2][0] = m[2][0];
    m3x3.m[2][1] = m[2][1];
    m3x3.m[2][2] = m[2][2];
  }
 
  inline Quaternion extractQuaternion() const
  {
    Matrix3 m3x3;
    extract3x3Matrix(m3x3);
    return Quaternion(m3x3);
  }
 
  inline Quaternion extractQuaternionNegZ() const
  {
    Matrix3 m3x3;
    extract3x3MatrixNegZ(m3x3);
    return Quaternion(m3x3);
  }
 
  inline Quaternion extractQuaternionNegX() const
  {
    Matrix3 m3x3;
    extract3x3MatrixNegX(m3x3);
    return Quaternion(m3x3);
  }
 
  inline Vector3 getTrans() const
  {
    return Vector3(m[0][3], m[1][3], m[2][3]);
  }
  inline void operator = (const Matrix3& mat3)
  {
    m[0][0] = mat3.m[0][0]; m[0][1] = mat3.m[0][1]; m[0][2] = mat3.m[0][2];
    m[1][0] = mat3.m[1][0]; m[1][1] = mat3.m[1][1]; m[1][2] = mat3.m[1][2];
    m[2][0] = mat3.m[2][0]; m[2][1] = mat3.m[2][1]; m[2][2] = mat3.m[2][2];
  }
  inline float* operator [] (size_t iRow)
  {
    return m[iRow];
  }
 
  inline const float *operator [] (size_t iRow) const
  {
    return m[iRow];
  }
 
  inline Matrix4 operator * (const Matrix4 &m2) const
  {
    return concatenate(m2);
  }
 
  inline Matrix4 concatenate(const Matrix4 &m2) const
  {
    Matrix4 r;
    r.m[0][0] = m[0][0] * m2.m[0][0] + m[0][1] * m2.m[1][0] + m[0][2] * m2.m[2][0] + m[0][3] * m2.m[3][0];
    r.m[0][1] = m[0][0] * m2.m[0][1] + m[0][1] * m2.m[1][1] + m[0][2] * m2.m[2][1] + m[0][3] * m2.m[3][1];
    r.m[0][2] = m[0][0] * m2.m[0][2] + m[0][1] * m2.m[1][2] + m[0][2] * m2.m[2][2] + m[0][3] * m2.m[3][2];
    r.m[0][3] = m[0][0] * m2.m[0][3] + m[0][1] * m2.m[1][3] + m[0][2] * m2.m[2][3] + m[0][3] * m2.m[3][3];
 
    r.m[1][0] = m[1][0] * m2.m[0][0] + m[1][1] * m2.m[1][0] + m[1][2] * m2.m[2][0] + m[1][3] * m2.m[3][0];
    r.m[1][1] = m[1][0] * m2.m[0][1] + m[1][1] * m2.m[1][1] + m[1][2] * m2.m[2][1] + m[1][3] * m2.m[3][1];
    r.m[1][2] = m[1][0] * m2.m[0][2] + m[1][1] * m2.m[1][2] + m[1][2] * m2.m[2][2] + m[1][3] * m2.m[3][2];
    r.m[1][3] = m[1][0] * m2.m[0][3] + m[1][1] * m2.m[1][3] + m[1][2] * m2.m[2][3] + m[1][3] * m2.m[3][3];
 
    r.m[2][0] = m[2][0] * m2.m[0][0] + m[2][1] * m2.m[1][0] + m[2][2] * m2.m[2][0] + m[2][3] * m2.m[3][0];
    r.m[2][1] = m[2][0] * m2.m[0][1] + m[2][1] * m2.m[1][1] + m[2][2] * m2.m[2][1] + m[2][3] * m2.m[3][1];
    r.m[2][2] = m[2][0] * m2.m[0][2] + m[2][1] * m2.m[1][2] + m[2][2] * m2.m[2][2] + m[2][3] * m2.m[3][2];
    r.m[2][3] = m[2][0] * m2.m[0][3] + m[2][1] * m2.m[1][3] + m[2][2] * m2.m[2][3] + m[2][3] * m2.m[3][3];
 
    r.m[3][0] = m[3][0] * m2.m[0][0] + m[3][1] * m2.m[1][0] + m[3][2] * m2.m[2][0] + m[3][3] * m2.m[3][0];
    r.m[3][1] = m[3][0] * m2.m[0][1] + m[3][1] * m2.m[1][1] + m[3][2] * m2.m[2][1] + m[3][3] * m2.m[3][1];
    r.m[3][2] = m[3][0] * m2.m[0][2] + m[3][1] * m2.m[1][2] + m[3][2] * m2.m[2][2] + m[3][3] * m2.m[3][2];
    r.m[3][3] = m[3][0] * m2.m[0][3] + m[3][1] * m2.m[1][3] + m[3][2] * m2.m[2][3] + m[3][3] * m2.m[3][3];
 
    return r;
  }
};
 
#endif //__VRMATHS_H__