vrmaths.h

#ifndef __VRMATHS_H__
#define __VRMATHS_H__

#include <string.h>
#include <math.h>

class Matrix3;
class Quaternion;

class Vector3
{
public:
  float x, y, z;

public:
  static const Vector3 ZERO;

  inline Vector3()
  {
  }

  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;
  }
};

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 Matrix3& rot)
  {
    this->FromRotationMatrix(rot);
  }

  inline float operator [] (const size_t i) const
  {
    return *(&w + i);
  }

  inline float& operator [] (const size_t i)
  {
    return *(&w + i);
  }

  void FromRotationMatrix(const Matrix3& kRot);

  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);
  }
  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(const Matrix3& m3x3)
  {
    operator=(IDENTITY);
    operator=(m3x3);
  }

  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 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];
  }
};

#endif //__VRMATHS_H__