/************************************************************************************

PublicHeader:   OVR.h
Filename    :   OVR_Math.h
Content     :   Implementation of 3D primitives such as vectors, matrices.
Created     :   September 4, 2012
Authors     :   Andrew Reisse, Michael Antonov, Steve LaValle, Anna Yershova, Max Katsev

Copyright   :   Copyright 2012 Oculus VR, Inc. All Rights reserved.

Use of this software is subject to the terms of the Oculus license
agreement provided at the time of installation or download, or which
otherwise accompanies this software in either electronic or hard copy form.

*************************************************************************************/

#ifndef OVR_Math_h
#define OVR_Math_h

#include <assert.h>
#include <stdlib.h>
#include <math.h>

#include "OVR_Types.h"
#include "OVR_RefCount.h"
#include "OVR_Std.h"

namespace OVR {

//-------------------------------------------------------------------------------------
// ***** Constants for 3D world/axis definitions.

// Definitions of axes for coordinate and rotation conversions.
enum Axis
{
    Axis_X = 0, Axis_Y = 1, Axis_Z = 2
};

// RotateDirection describes the rotation direction around an axis, interpreted as follows:
//  CW  - Clockwise while looking "down" from positive axis towards the origin.
//  CCW - Counter-clockwise while looking from the positive axis towards the origin,
//        which is in the negative axis direction.
//  CCW is the default for the RHS coordinate system. Oculus standard RHS coordinate
//  system defines Y up, X right, and Z back (pointing out from the screen). In this
//  system Rotate_CCW around Z will specifies counter-clockwise rotation in XY plane.
enum RotateDirection
{
    Rotate_CCW = 1,
    Rotate_CW  = -1 
};

// Constants for right handed and left handed coordinate systems
enum HandedSystem
{
    Handed_R = 1, Handed_L = -1
};

// AxisDirection describes which way the coordinate axis points. Used by WorldAxes.
enum AxisDirection
{
    Axis_Up    =  2,
    Axis_Down  = -2,
    Axis_Right =  1,
    Axis_Left  = -1,
    Axis_In    =  3,
    Axis_Out   = -3
};

struct WorldAxes
{
    AxisDirection XAxis, YAxis, ZAxis;

    WorldAxes(AxisDirection x, AxisDirection y, AxisDirection z)
        : XAxis(x), YAxis(y), ZAxis(z) 
    { OVR_ASSERT(abs(x) != abs(y) && abs(y) != abs(z) && abs(z) != abs(x));}
};


//------------------------------------------------------------------------------------//
// ************************************ Math *****************************************//
//
// Math class contains constants and functions. This class is a template specialized
// per type, with Math<float> and Math<double> being distinct.
template<class Type>
class Math
{  
};

// Single-precision Math constants class.
template<>
class Math<float>
{
public:
    static const float Pi;
    static const float TwoPi;
    static const float PiOver2;
    static const float PiOver4;
    static const float E;

    static const float MaxValue;			// Largest positive float Value
    static const float MinPositiveValue;	// Smallest possible positive value

    static const float RadToDegreeFactor;
    static const float DegreeToRadFactor;

    static const float Tolerance;			// 0.00001f;
    static const float SingularityRadius;	// 0.0000001f for Gimbal lock numerical problems
};

// Double-precision Math constants class.
template<>
class Math<double>
{
public:
    static const double Pi;
    static const double TwoPi;
    static const double PiOver2;
    static const double PiOver4;
    static const double E;

    static const double MaxValue;			// Largest positive double Value
    static const double MinPositiveValue;	// Smallest possible positive value

    static const double RadToDegreeFactor;
    static const double DegreeToRadFactor;

    static const double Tolerance;			// 0.00001;
    static const double SingularityRadius;	// 0.000000000001 for Gimbal lock numerical problems

};

typedef Math<float>  Mathf;
typedef Math<double> Mathd;

// Conversion functions between degrees and radians
template<class T>
T RadToDegree(T rads) { return rads * Math<T>::RadToDegreeFactor; }
template<class T>
T DegreeToRad(T rads) { return rads * Math<T>::DegreeToRadFactor; }

// Numerically stable acos function
template<class T>
T Acos(T val) { 
		if (val > T(1))				return T(0);
		else if (val < T(-1))		return Math<T>::Pi;
		else						return acos(val); 
};

// Numerically stable asin function
template<class T>
T Asin(T val) { 
	if (val > T(1))				return Math<T>::PiOver2;
	else if (val < T(-1))		return Math<T>::PiOver2 * T(3);
	else						return asin(val); 
};

#ifdef OVR_CC_MSVC
inline int isnan(double x) { return _isnan(x); };
#endif

template<class T>
class Quat;

//-------------------------------------------------------------------------------------
// ***** Vector2<>

// Vector2f (Vector2d) represents a 2-dimensional vector or point in space,
// consisting of coordinates x and y

template<class T>
class Vector2
{
public:
    T x, y;

    Vector2() : x(0), y(0) { }
    Vector2(T x_, T y_) : x(x_), y(y_) { }
    explicit Vector2(T s) : x(s), y(s) { }

    bool     operator== (const Vector2& b) const  { return x == b.x && y == b.y; }
    bool     operator!= (const Vector2& b) const  { return x != b.x || y != b.y; }
             
    Vector2  operator+  (const Vector2& b) const  { return Vector2(x + b.x, y + b.y); }
    Vector2& operator+= (const Vector2& b)        { x += b.x; y += b.y; return *this; }
    Vector2  operator-  (const Vector2& b) const  { return Vector2(x - b.x, y - b.y); }
    Vector2& operator-= (const Vector2& b)        { x -= b.x; y -= b.y; return *this; }
    Vector2  operator- () const                   { return Vector2(-x, -y); }

    // Scalar multiplication/division scales vector.
    Vector2  operator*  (T s) const               { return Vector2(x*s, y*s); }
    Vector2& operator*= (T s)                     { x *= s; y *= s; return *this; }

    Vector2  operator/  (T s) const               { T rcp = T(1)/s;
                                                    return Vector2(x*rcp, y*rcp); }
    Vector2& operator/= (T s)                     { T rcp = T(1)/s;
                                                    x *= rcp; y *= rcp;
                                                    return *this; }

    // Compare two vectors for equality with tolerance. Returns true if vectors match withing tolerance.
    bool	Compare(const Vector2&b, T tolerance = Mathf::Tolerance)
    {
        return (fabs(b.x-x) < tolerance) && (fabs(b.y-y) < tolerance);
    }
    
    // Entrywise product of two vectors
    Vector2	EntrywiseMultiply(const Vector2& b) const	{ return Vector2(x * b.x, y * b.y);}

	// Dot product
    // Used to calculate angle q between two vectors among other things,
    // as (A dot B) = |a||b|cos(q).
    T		Dot(const Vector2& b) const          { return x*b.x + y*b.y; }

    // Returns the angle from this vector to b, in radians.
    T       Angle(const Vector2& b) const        
	{ 
		T div = LengthSq()*b.LengthSq();
		OVR_ASSERT(div != T(0));
		T result = Acos((this->Dot(b))/sqrt(div));
		return result;
	}

    // Return Length of the vector squared.
    T       LengthSq() const                     { return (x * x + y * y); }
    // Return vector length.
    T       Length() const                       { return sqrt(LengthSq()); }

	// Returns distance between two points represented by vectors.
    T       Distance(Vector2& b) const           { return (*this - b).Length(); }

	// Determine if this a unit vector.
    bool    IsNormalized() const                 { return fabs(LengthSq() - T(1)) < Math<T>::Tolerance; }

    // Normalize, convention vector length to 1.    
    void    Normalize()                          
	{
		T l = Length();
		OVR_ASSERT(l != T(0));
		*this /= l; 
	}
    // Returns normalized (unit) version of the vector without modifying itself.
    Vector2 Normalized() const                   
	{ 
		T l = Length();
		OVR_ASSERT(l != T(0));
		return *this / l; 
	}

    // Linearly interpolates from this vector to another.
    // Factor should be between 0.0 and 1.0, with 0 giving full value to this.
    Vector2 Lerp(const Vector2& b, T f) const    { return *this*(T(1) - f) + b*f; }

    // Projects this vector onto the argument; in other words,
    // A.Project(B) returns projection of vector A onto B.
    Vector2 ProjectTo(const Vector2& b) const    
	{ 
		T l2 = b.LengthSq();
		OVR_ASSERT(l2 != T(0));
		return b * ( Dot(b) / l2 ); 
	}
};


typedef Vector2<float>  Vector2f;
typedef Vector2<double> Vector2d;


//-------------------------------------------------------------------------------------
// ***** Vector3<> - 3D vector of {x, y, z}

//
// Vector3f (Vector3d) represents a 3-dimensional vector or point in space,
// consisting of coordinates x, y and z.

template<class T>
class Vector3
{
public:
    T x, y, z;

    Vector3() : x(0), y(0), z(0) { }
    Vector3(T x_, T y_, T z_ = 0) : x(x_), y(y_), z(z_) { }
    explicit Vector3(T s) : x(s), y(s), z(s) { }

    bool     operator== (const Vector3& b) const  { return x == b.x && y == b.y && z == b.z; }
    bool     operator!= (const Vector3& b) const  { return x != b.x || y != b.y || z != b.z; }
             
    Vector3  operator+  (const Vector3& b) const  { return Vector3(x + b.x, y + b.y, z + b.z); }
    Vector3& operator+= (const Vector3& b)        { x += b.x; y += b.y; z += b.z; return *this; }
    Vector3  operator-  (const Vector3& b) const  { return Vector3(x - b.x, y - b.y, z - b.z); }
    Vector3& operator-= (const Vector3& b)        { x -= b.x; y -= b.y; z -= b.z; return *this; }
    Vector3  operator- () const                   { return Vector3(-x, -y, -z); }

    // Scalar multiplication/division scales vector.
    Vector3  operator*  (T s) const               { return Vector3(x*s, y*s, z*s); }
    Vector3& operator*= (T s)                     { x *= s; y *= s; z *= s; return *this; }

    Vector3  operator/  (T s) const               { T rcp = T(1)/s;
                                                    return Vector3(x*rcp, y*rcp, z*rcp); }
    Vector3& operator/= (T s)                     { T rcp = T(1)/s;
                                                    x *= rcp; y *= rcp; z *= rcp;
                                                    return *this; }

    // Compare two vectors for equality with tolerance. Returns true if vectors match withing tolerance.
    bool      Compare(const Vector3&b, T tolerance = Mathf::Tolerance)
    {
        return (fabs(b.x-x) < tolerance) && 
			   (fabs(b.y-y) < tolerance) && 
			   (fabs(b.z-z) < tolerance);
    }
    
    // Entrywise product of two vectors
    Vector3	EntrywiseMultiply(const Vector3& b) const	{ return Vector3(x * b.x, 
																		 y * b.y, 
																		 z * b.z);}

	// Dot product
    // Used to calculate angle q between two vectors among other things,
    // as (A dot B) = |a||b|cos(q).
     T      Dot(const Vector3& b) const          { return x*b.x + y*b.y + z*b.z; }

    // Compute cross product, which generates a normal vector.
    // Direction vector can be determined by right-hand rule: Pointing index finder in
    // direction a and middle finger in direction b, thumb will point in a.Cross(b).
    Vector3 Cross(const Vector3& b) const        { return Vector3(y*b.z - z*b.y,
                                                                  z*b.x - x*b.z,
                                                                  x*b.y - y*b.x); }

    // Returns the angle from this vector to b, in radians.
    T       Angle(const Vector3& b) const 
	{
		T div = LengthSq()*b.LengthSq();
		OVR_ASSERT(div != T(0));
		T result = Acos((this->Dot(b))/sqrt(div));
		return result;
	}

    // Return Length of the vector squared.
    T       LengthSq() const                     { return (x * x + y * y + z * z); }
    // Return vector length.
    T       Length() const                       { return sqrt(LengthSq()); }

    // Returns distance between two points represented by vectors.
    T       Distance(Vector3& b) const           { return (*this - b).Length(); }
    
    // Determine if this a unit vector.
    bool    IsNormalized() const                 { return fabs(LengthSq() - T(1)) < Math<T>::Tolerance; }

    // Normalize, convention vector length to 1.    
    void    Normalize()                          
	{
		T l = Length();
		OVR_ASSERT(l != T(0));
		*this /= l; 
	}

    // Returns normalized (unit) version of the vector without modifying itself.
    Vector3 Normalized() const                   
	{ 
		T l = Length();
		OVR_ASSERT(l != T(0));
		return *this / l; 
	}

    // Linearly interpolates from this vector to another.
    // Factor should be between 0.0 and 1.0, with 0 giving full value to this.
    Vector3 Lerp(const Vector3& b, T f) const    { return *this*(T(1) - f) + b*f; }

    // Projects this vector onto the argument; in other words,
    // A.Project(B) returns projection of vector A onto B.
    Vector3 ProjectTo(const Vector3& b) const    
	{ 
		T l2 = b.LengthSq();
		OVR_ASSERT(l2 != T(0));
		return b * ( Dot(b) / l2 ); 
	}

    // Projects this vector onto a plane defined by a normal vector
    Vector3 ProjectToPlane(const Vector3& normal) const { return *this - this->ProjectTo(normal); }
};


typedef Vector3<float>  Vector3f;
typedef Vector3<double> Vector3d;


//-------------------------------------------------------------------------------------
// ***** Size

// Size class represents 2D size with Width, Height components.
// Used to describe distentions of render targets, etc.

template<class T>
class Size
{
public:
    T   Width, Height;

    Size() : Width(0), Height(0) { }
    Size(T w_, T h_) : Width(w_), Height(h_) { }
    explicit Size(T s) : Width(s), Height(s) { }

    bool     operator== (const Size& b) const  { return Width == b.Width && Height == b.Height; }
    bool     operator!= (const Size& b) const  { return Width != b.Width || Height != b.Height; }
             
    Size  operator+  (const Size& b) const  { return Size(Width + b.Width, Height + b.Height); }
    Size& operator+= (const Size& b)        { Width += b.Width; Height += b.Height; return *this; }
    Size  operator-  (const Size& b) const  { return Size(Width - b.Width, Height - b.Height); }
    Size& operator-= (const Size& b)        { Width -= b.Width; Height -= b.Height; return *this; }
    Size  operator- () const                { return Size(-Width, -Height); }
    Size  operator*  (const Size& b) const  { return Size(Width * b.Width, Height * b.Height); }
    Size& operator*= (const Size& b)        { Width *= b.Width; Height *= b.Height; return *this; }
    Size  operator/  (const Size& b) const  { return Size(Width / b.Width, Height / b.Height); }
    Size& operator/= (const Size& b)        { Width /= b.Width; Height /= b.Height; return *this; }

    // Scalar multiplication/division scales both components.
    Size  operator*  (T s) const            { return Size(Width*s, Height*s); }
    Size& operator*= (T s)                  { Width *= s; Height *= s; return *this; }    
    Size  operator/  (T s) const            { return Size(Width/s, Height/s); }
    Size& operator/= (T s)                  { Width /= s; Height /= s; return *this; }

    T       Area() const                    { return Width * Height; }

    inline  Vector2<T> ToVector() const     { return Vector2<T>(Width, Height); }
};


typedef Size<int>       Sizei;
typedef Size<unsigned>  Sizeu;
typedef Size<float>     Sizef;
typedef Size<double>    Sized;


//-------------------------------------------------------------------------------------
// ***** Matrix4f
//
// Matrix4f is a 4x4 matrix used for 3d transformations and projections.
// Translation stored in the last column.
// The matrix is stored in row-major order in memory, meaning that values
// of the first row are stored before the next one.
//
// The arrangement of the matrix is chosen to be in Right-Handed 
// coordinate system and counterclockwise rotations when looking down
// the axis
//
// Transformation Order:
//   - Transformations are applied from right to left, so the expression
//     M1 * M2 * M3 * V means that the vector V is transformed by M3 first,
//     followed by M2 and M1. 
//
// Coordinate system: Right Handed
//
// Rotations: Counterclockwise when looking down the axis. All angles are in radians.
//    
//  | sx   01   02   tx |    // First column  (sx, 10, 20): Axis X basis vector.
//  | 10   sy   12   ty |    // Second column (01, sy, 21): Axis Y basis vector.
//  | 20   21   sz   tz |    // Third columnt (02, 12, sz): Axis Z basis vector.
//  | 30   31   32   33 |
//
//  The basis vectors are first three columns.

class Matrix4f
{
    static Matrix4f IdentityValue;

public:
    float M[4][4];    

    enum NoInitType { NoInit };

    // Construct with no memory initialization.
    Matrix4f(NoInitType) { }

    // By default, we construct identity matrix.
    Matrix4f()
    {
        SetIdentity();        
    }

    Matrix4f(float m11, float m12, float m13, float m14,
             float m21, float m22, float m23, float m24,
             float m31, float m32, float m33, float m34,
             float m41, float m42, float m43, float m44)
    {
        M[0][0] = m11; M[0][1] = m12; M[0][2] = m13; M[0][3] = m14;
        M[1][0] = m21; M[1][1] = m22; M[1][2] = m23; M[1][3] = m24;
        M[2][0] = m31; M[2][1] = m32; M[2][2] = m33; M[2][3] = m34;
        M[3][0] = m41; M[3][1] = m42; M[3][2] = m43; M[3][3] = m44;
    }

    Matrix4f(float m11, float m12, float m13,
             float m21, float m22, float m23,
             float m31, float m32, float m33)
    {
        M[0][0] = m11; M[0][1] = m12; M[0][2] = m13; M[0][3] = 0;
        M[1][0] = m21; M[1][1] = m22; M[1][2] = m23; M[1][3] = 0;
        M[2][0] = m31; M[2][1] = m32; M[2][2] = m33; M[2][3] = 0;
        M[3][0] = 0;   M[3][1] = 0;   M[3][2] = 0;   M[3][3] = 1;
    }

    void ToString(char* dest, UPInt destsize)
    {
        UPInt pos = 0;
        for (int r=0; r<4; r++)
            for (int c=0; c<4; c++)
                pos += OVR_sprintf(dest+pos, destsize-pos, "%g ", M[r][c]);
    }

    static Matrix4f FromString(const char* src)
    {
        Matrix4f result;
        for (int r=0; r<4; r++)
            for (int c=0; c<4; c++)
            {
                result.M[r][c] = (float)atof(src);
                while (src && *src != ' ')
                    src++;
                while (src && *src == ' ')
                    src++;
            }
        return result;
    }

    static const Matrix4f& Identity()  { return IdentityValue; }

    void SetIdentity()
    {
        M[0][0] = M[1][1] = M[2][2] = M[3][3] = 1;
        M[0][1] = M[1][0] = M[2][3] = M[3][1] = 0;
        M[0][2] = M[1][2] = M[2][0] = M[3][2] = 0;
        M[0][3] = M[1][3] = M[2][1] = M[3][0] = 0;
    }

    Matrix4f operator+ (const Matrix4f& b) const
    {
        Matrix4f result(*this);
        result += b;
        return result;
    }

    Matrix4f& operator+= (const Matrix4f& b)
    {
        for (int i = 0; i < 4; i++)
            for (int j = 0; j < 4; j++)
                M[i][j] += b.M[i][j];
        return *this;
    }

    Matrix4f operator- (const Matrix4f& b) const
    {
        Matrix4f result(*this);
        result -= b;
        return result;
    }

    Matrix4f& operator-= (const Matrix4f& b)
    {
        for (int i = 0; i < 4; i++)
            for (int j = 0; j < 4; j++)
                M[i][j] -= b.M[i][j];
        return *this;
    }

    // Multiplies two matrices into destination with minimum copying.
    static Matrix4f& Multiply(Matrix4f* d, const Matrix4f& a, const Matrix4f& b)
    {
        OVR_ASSERT((d != &a) && (d != &b));
        int i = 0;
        do {
            d->M[i][0] = a.M[i][0] * b.M[0][0] + a.M[i][1] * b.M[1][0] + a.M[i][2] * b.M[2][0] + a.M[i][3] * b.M[3][0];
            d->M[i][1] = a.M[i][0] * b.M[0][1] + a.M[i][1] * b.M[1][1] + a.M[i][2] * b.M[2][1] + a.M[i][3] * b.M[3][1];
            d->M[i][2] = a.M[i][0] * b.M[0][2] + a.M[i][1] * b.M[1][2] + a.M[i][2] * b.M[2][2] + a.M[i][3] * b.M[3][2];
            d->M[i][3] = a.M[i][0] * b.M[0][3] + a.M[i][1] * b.M[1][3] + a.M[i][2] * b.M[2][3] + a.M[i][3] * b.M[3][3];
        } while((++i) < 4);

        return *d;
    }

    Matrix4f operator* (const Matrix4f& b) const
    {
        Matrix4f result(Matrix4f::NoInit);
        Multiply(&result, *this, b);
        return result;
    }

    Matrix4f& operator*= (const Matrix4f& b)
    {
        return Multiply(this, Matrix4f(*this), b);
    }

    Matrix4f operator* (float s) const
    {
        Matrix4f result(*this);
        result *= s;
        return result;
    }

    Matrix4f& operator*= (float s)
    {
        for (int i = 0; i < 4; i++)
            for (int j = 0; j < 4; j++)
                M[i][j] *= s;
        return *this;
    }


    Matrix4f operator/ (float s) const
    {
        Matrix4f result(*this);
        result /= s;
        return result;
    }

    Matrix4f& operator/= (float s)
    {
        for (int i = 0; i < 4; i++)
            for (int j = 0; j < 4; j++)
                M[i][j] /= s;
        return *this;
    }

    Vector3f Transform(const Vector3f& v) const
    {
        return Vector3f(M[0][0] * v.x + M[0][1] * v.y + M[0][2] * v.z + M[0][3],
                        M[1][0] * v.x + M[1][1] * v.y + M[1][2] * v.z + M[1][3],
                        M[2][0] * v.x + M[2][1] * v.y + M[2][2] * v.z + M[2][3]);
    }

    Matrix4f Transposed() const
    {
        return Matrix4f(M[0][0], M[1][0], M[2][0], M[3][0],
                        M[0][1], M[1][1], M[2][1], M[3][1],
                        M[0][2], M[1][2], M[2][2], M[3][2],
                        M[0][3], M[1][3], M[2][3], M[3][3]);
    }

    void     Transpose()
    {
        *this = Transposed();
    }


    float SubDet (const UPInt* rows, const UPInt* cols) const
    {
        return M[rows[0]][cols[0]] * (M[rows[1]][cols[1]] * M[rows[2]][cols[2]] - M[rows[1]][cols[2]] * M[rows[2]][cols[1]])
             - M[rows[0]][cols[1]] * (M[rows[1]][cols[0]] * M[rows[2]][cols[2]] - M[rows[1]][cols[2]] * M[rows[2]][cols[0]])
             + M[rows[0]][cols[2]] * (M[rows[1]][cols[0]] * M[rows[2]][cols[1]] - M[rows[1]][cols[1]] * M[rows[2]][cols[0]]);
    }

    float Cofactor(UPInt I, UPInt J) const
    {
        const UPInt indices[4][3] = {{1,2,3},{0,2,3},{0,1,3},{0,1,2}};
        return ((I+J)&1) ? -SubDet(indices[I],indices[J]) : SubDet(indices[I],indices[J]);
    }

    float    Determinant() const
    {
        return M[0][0] * Cofactor(0,0) + M[0][1] * Cofactor(0,1) + M[0][2] * Cofactor(0,2) + M[0][3] * Cofactor(0,3);
    }

    Matrix4f Adjugated() const
    {
        return Matrix4f(Cofactor(0,0), Cofactor(1,0), Cofactor(2,0), Cofactor(3,0), 
                        Cofactor(0,1), Cofactor(1,1), Cofactor(2,1), Cofactor(3,1), 
                        Cofactor(0,2), Cofactor(1,2), Cofactor(2,2), Cofactor(3,2),
                        Cofactor(0,3), Cofactor(1,3), Cofactor(2,3), Cofactor(3,3));
    }

    Matrix4f Inverted() const
    {
        float det = Determinant();
        assert(det != 0);
        return Adjugated() * (1.0f/det);
    }

    void Invert()
    {
        *this = Inverted();
    }

	// This is more efficient than general inverse, but ONLY works
	// correctly if it is a homogeneous transform matrix (rot + trans)
	Matrix4f InvertedHomogeneousTransform() const
	{
		// Make the inverse rotation matrix
		Matrix4f rinv = this->Transposed();
		rinv.M[3][0] = rinv.M[3][1] = rinv.M[3][2] = 0.0f;
		// Make the inverse translation matrix
		Vector3f tvinv = Vector3f(-M[0][3],-M[1][3],-M[2][3]);
		Matrix4f tinv = Matrix4f::Translation(tvinv);
		return rinv * tinv;  // "untranslate", then "unrotate"
	}

	// This is more efficient than general inverse, but ONLY works
	// correctly if it is a homogeneous transform matrix (rot + trans)
	void InvertHomogeneousTransform()
	{
        *this = InvertedHomogeneousTransform();
	}

	// Matrix to Euler Angles conversion
    // a,b,c, are the YawPitchRoll angles to be returned
    // rotation a around axis A1
    // is followed by rotation b around axis A2
    // is followed by rotation c around axis A3
    // rotations are CCW or CW (D) in LH or RH coordinate system (S)
    template <Axis A1, Axis A2, Axis A3, RotateDirection D, HandedSystem S>
    void ToEulerAngles(float *a, float *b, float *c)
    {
        OVR_COMPILER_ASSERT((A1 != A2) && (A2 != A3) && (A1 != A3));

        float psign = -1.0f;
        if (((A1 + 1) % 3 == A2) && ((A2 + 1) % 3 == A3)) // Determine whether even permutation
        psign = 1.0f;
        
        float pm = psign*M[A1][A3];
        if (pm < -1.0f + Math<float>::SingularityRadius)
        { // South pole singularity
            *a = 0.0f;
            *b = -S*D*Math<float>::PiOver2;
            *c = S*D*atan2( psign*M[A2][A1], M[A2][A2] );
        }
        else if (pm > 1.0f - Math<float>::SingularityRadius)
        { // North pole singularity
            *a = 0.0f;
            *b = S*D*Math<float>::PiOver2;
            *c = S*D*atan2( psign*M[A2][A1], M[A2][A2] );
        }
        else
        { // Normal case (nonsingular)
            *a = S*D*atan2( -psign*M[A2][A3], M[A3][A3] );
            *b = S*D*asin(pm);
            *c = S*D*atan2( -psign*M[A1][A2], M[A1][A1] );
        }

        return;
    }

	// Matrix to Euler Angles conversion
    // a,b,c, are the YawPitchRoll angles to be returned
    // rotation a around axis A1
    // is followed by rotation b around axis A2
    // is followed by rotation c around axis A1
    // rotations are CCW or CW (D) in LH or RH coordinate system (S)
    template <Axis A1, Axis A2, RotateDirection D, HandedSystem S>
    void ToEulerAnglesABA(float *a, float *b, float *c)
    {        
         OVR_COMPILER_ASSERT(A1 != A2);
  
        // Determine the axis that was not supplied
        int m = 3 - A1 - A2;

        float psign = -1.0f;
        if ((A1 + 1) % 3 == A2) // Determine whether even permutation
            psign = 1.0f;

        float c2 = M[A1][A1];
        if (c2 < -1.0f + Math<float>::SingularityRadius)
        { // South pole singularity
            *a = 0.0f;
            *b = S*D*Math<float>::Pi;
            *c = S*D*atan2( -psign*M[A2][m],M[A2][A2]);
        }
        else if (c2 > 1.0f - Math<float>::SingularityRadius)
        { // North pole singularity
            *a = 0.0f;
            *b = 0.0f;
            *c = S*D*atan2( -psign*M[A2][m],M[A2][A2]);
        }
        else
        { // Normal case (nonsingular)
            *a = S*D*atan2( M[A2][A1],-psign*M[m][A1]);
            *b = S*D*acos(c2);
            *c = S*D*atan2( M[A1][A2],psign*M[A1][m]);
        }
        return;
    }
  
    // Creates a matrix that converts the vertices from one coordinate system
    // to another.
    static Matrix4f AxisConversion(const WorldAxes& to, const WorldAxes& from)
    {        
        // Holds axis values from the 'to' structure
        int toArray[3] = { to.XAxis, to.YAxis, to.ZAxis };

        // The inverse of the toArray
        int inv[4]; 
        inv[0] = inv[abs(to.XAxis)] = 0;
        inv[abs(to.YAxis)] = 1;
        inv[abs(to.ZAxis)] = 2;

        Matrix4f m(0,  0,  0, 
                   0,  0,  0,
                   0,  0,  0);

        // Only three values in the matrix need to be changed to 1 or -1.
        m.M[inv[abs(from.XAxis)]][0] = float(from.XAxis/toArray[inv[abs(from.XAxis)]]);
        m.M[inv[abs(from.YAxis)]][1] = float(from.YAxis/toArray[inv[abs(from.YAxis)]]);
        m.M[inv[abs(from.ZAxis)]][2] = float(from.ZAxis/toArray[inv[abs(from.ZAxis)]]);
        return m;
    } 


	// Creates a matrix for translation by vector
    static Matrix4f Translation(const Vector3f& v)
    {
        Matrix4f t;
        t.M[0][3] = v.x;
        t.M[1][3] = v.y;
        t.M[2][3] = v.z;
        return t;
    }

	// Creates a matrix for translation by vector
    static Matrix4f Translation(float x, float y, float z = 0.0f)
    {
        Matrix4f t;
        t.M[0][3] = x;
        t.M[1][3] = y;
        t.M[2][3] = z;
        return t;
    }

	// Creates a matrix for scaling by vector
    static Matrix4f Scaling(const Vector3f& v)
    {
        Matrix4f t;
        t.M[0][0] = v.x;
        t.M[1][1] = v.y;
        t.M[2][2] = v.z;
        return t;
    }

	// Creates a matrix for scaling by vector
    static Matrix4f Scaling(float x, float y, float z)
    {
        Matrix4f t;
        t.M[0][0] = x;
        t.M[1][1] = y;
        t.M[2][2] = z;
        return t;
    }

	// Creates a matrix for scaling by constant
    static Matrix4f Scaling(float s)
    {
        Matrix4f t;
        t.M[0][0] = s;
        t.M[1][1] = s;
        t.M[2][2] = s;
        return t;
    }

  

    // Creates a rotation matrix rotating around the X axis by 'angle' radians.
    // Just for quick testing.  Not for final API.  Need to remove case.
    static Matrix4f RotationAxis(Axis A, float angle, RotateDirection d, HandedSystem s)
    {
        float sina = s * d *sin(angle);
        float cosa = cos(angle);
        
        switch(A)
        {
        case Axis_X:
            return Matrix4f(1,  0,     0, 
                            0,  cosa,  -sina,
                            0,  sina,  cosa);
        case Axis_Y:
            return Matrix4f(cosa,  0,   sina, 
                            0,     1,   0,
                            -sina, 0,   cosa);
        case Axis_Z:
            return Matrix4f(cosa,  -sina,  0, 
                            sina,  cosa,   0,
                            0,     0,      1);
        }
    }


    // Creates a rotation matrix rotating around the X axis by 'angle' radians.
    // Rotation direction is depends on the coordinate system:
    // RHS (Oculus default): Positive angle values rotate Counter-clockwise (CCW),
    //                        while looking in the negative axis direction. This is the
    //                        same as looking down from positive axis values towards origin.
    // LHS: Positive angle values rotate clock-wise (CW), while looking in the
    //       negative axis direction.
    static Matrix4f RotationX(float angle)
    {
        float sina = sin(angle);
        float cosa = cos(angle);
        return Matrix4f(1,  0,     0, 
                        0,  cosa,  -sina,
                        0,  sina,  cosa);
    }

    // Creates a rotation matrix rotating around the Y axis by 'angle' radians.
    // Rotation direction is depends on the coordinate system:
    //  RHS (Oculus default): Positive angle values rotate Counter-clockwise (CCW),
    //                        while looking in the negative axis direction. This is the
    //                        same as looking down from positive axis values towards origin.
    //  LHS: Positive angle values rotate clock-wise (CW), while looking in the
    //       negative axis direction.
    static Matrix4f RotationY(float angle)
    {
        float sina = sin(angle);
        float cosa = cos(angle);
        return Matrix4f(cosa,  0,   sina, 
                        0,     1,   0,
                        -sina, 0,   cosa);
    }

    // Creates a rotation matrix rotating around the Z axis by 'angle' radians.
    // Rotation direction is depends on the coordinate system:
    //  RHS (Oculus default): Positive angle values rotate Counter-clockwise (CCW),
    //                        while looking in the negative axis direction. This is the
    //                        same as looking down from positive axis values towards origin.
    //  LHS: Positive angle values rotate clock-wise (CW), while looking in the
    //       negative axis direction.
    static Matrix4f RotationZ(float angle)
    {
        float sina = sin(angle);
        float cosa = cos(angle);
        return Matrix4f(cosa,  -sina,  0, 
                        sina,  cosa,   0,
                        0,     0,      1);
    }


    // LookAtRH creates a View transformation matrix for right-handed coordinate system.
    // The resulting matrix points camera from 'eye' towards 'at' direction, with 'up'
    // specifying the up vector. The resulting matrix should be used with PerspectiveRH
    // projection.
    static Matrix4f LookAtRH(const Vector3f& eye, const Vector3f& at, const Vector3f& up);

    // LookAtLH creates a View transformation matrix for left-handed coordinate system.
    // The resulting matrix points camera from 'eye' towards 'at' direction, with 'up'
    // specifying the up vector. 
    static Matrix4f LookAtLH(const Vector3f& eye, const Vector3f& at, const Vector3f& up);
    
    
    // PerspectiveRH creates a right-handed perspective projection matrix that can be
    // used with the Oculus sample renderer. 
    //  yfov   - Specifies vertical field of view in radians.
    //  aspect - Screen aspect ration, which is usually width/height for square pixels.
    //           Note that xfov = yfov * aspect.
    //  znear  - Absolute value of near Z clipping clipping range.
    //  zfar   - Absolute value of far  Z clipping clipping range (larger then near).
    // Even though RHS usually looks in the direction of negative Z, positive values
    // are expected for znear and zfar.
    static Matrix4f PerspectiveRH(float yfov, float aspect, float znear, float zfar);
    
    
    // PerspectiveRH creates a left-handed perspective projection matrix that can be
    // used with the Oculus sample renderer. 
    //  yfov   - Specifies vertical field of view in radians.
    //  aspect - Screen aspect ration, which is usually width/height for square pixels.
    //           Note that xfov = yfov * aspect.
    //  znear  - Absolute value of near Z clipping clipping range.
    //  zfar   - Absolute value of far  Z clipping clipping range (larger then near).
    static Matrix4f PerspectiveLH(float yfov, float aspect, float znear, float zfar);


    static Matrix4f Ortho2D(float w, float h);
};


//-------------------------------------------------------------------------------------//
// **************************************** Quat **************************************//
//
// Quatf represents a quaternion class used for rotations.
// 
// Quaternion multiplications are done in right-to-left order, to match the
// behavior of matrices.


template<class T>
class Quat
{
public:
    // w + Xi + Yj + Zk
    T x, y, z, w;    

    Quat() : x(0), y(0), z(0), w(1) {}
    Quat(T x_, T y_, T z_, T w_) : x(x_), y(y_), z(z_), w(w_) {}


    // Constructs quaternion for rotation around the axis by an angle.
    Quat(const Vector3<T>& axis, T angle)
    {
        Vector3<T> unitAxis = axis.Normalized();
        T          sinHalfAngle = sin(angle * T(0.5));

        w = cos(angle * T(0.5));
        x = unitAxis.x * sinHalfAngle;
        y = unitAxis.y * sinHalfAngle;
        z = unitAxis.z * sinHalfAngle;
    }

    // Constructs quaternion for rotation around one of the coordinate axis by an angle.
    void AxisAngle(Axis A, T angle, RotateDirection d, HandedSystem s)
    {
        T sinHalfAngle = s * d *sin(angle * T(0.5));
        T v[3];
        v[0] = v[1] = v[2] = T(0);
        v[A] = sinHalfAngle;

        w = cos(angle * T(0.5));
        x = v[0];
        y = v[1];
        z = v[2];
    }


   // Compute axis and angle from quaternion
   void GetAxisAngle(Vector3<T>* axis, T* angle) const
    {
		if ( x*x + y*y + z*z > Math<T>::Tolerance * Math<T>::Tolerance ) {
			*axis  = Vector3<T>(x, y, z).Normalized();
			*angle = T(2) * Acos(w);
		}
		else 
		{
			*axis = Vector3<T>(1, 0, 0);
			*angle= 0;
		}
    }

    bool operator== (const Quat& b) const   { return x == b.x && y == b.y && z == b.z && w == b.w; }
    bool operator!= (const Quat& b) const   { return x != b.x || y != b.y || z != b.z || w != b.w; }

    Quat  operator+  (const Quat& b) const  { return Quat(x + b.x, y + b.y, z + b.z, w + b.w); }
    Quat& operator+= (const Quat& b)        { w += b.w; x += b.x; y += b.y; z += b.z; return *this; }
    Quat  operator-  (const Quat& b) const  { return Quat(x - b.x, y - b.y, z - b.z, w - b.w); }
    Quat& operator-= (const Quat& b)        { w -= b.w; x -= b.x; y -= b.y; z -= b.z; return *this; }

    Quat  operator*  (T s) const            { return Quat(x * s, y * s, z * s, w * s); }
    Quat& operator*= (T s)                  { w *= s; x *= s; y *= s; z *= s; return *this; }
    Quat  operator/  (T s) const            { T rcp = T(1)/s; return Quat(x * rcp, y * rcp, z * rcp, w *rcp); }
    Quat& operator/= (T s)                  { T rcp = T(1)/s; w *= rcp; x *= rcp; y *= rcp; z *= rcp; return *this; }


    // Get Imaginary part vector
    Vector3<T> Imag() const                 { return Vector3<T>(x,y,z); }

    // Get quaternion length.
    T       Length() const                  { return sqrt(x * x + y * y + z * z + w * w); }
    // Get quaternion length squared.
    T       LengthSq() const                { return (x * x + y * y + z * z + w * w); }

    // Simple Eulidean distance in R^4 (not SLERP distance, but at least respects Haar measure)
    T       Distance(const Quat& q) const	
	{ 
        T d1 = (*this - q).Length();
        T d2 = (*this + q).Length(); // Antipodal point check
        return (d1 < d2) ? d1 : d2;
	}

    T       DistanceSq(const Quat& q) const
    {
        T d1 = (*this - q).LengthSq();
        T d2 = (*this + q).LengthSq(); // Antipodal point check
        return (d1 < d2) ? d1 : d2;
    }

    // Normalize
    bool    IsNormalized() const            { return fabs(LengthSq() - T(1)) < Math<T>::Tolerance; }

    void    Normalize()                     
	{
 		T l = Length();
		OVR_ASSERT(l != T(0));
		*this /= l; 
	}

	Quat    Normalized() const              
	{ 
		T l = Length();
		OVR_ASSERT(l != T(0));
		return *this / l; 
	}

    // Returns conjugate of the quaternion. Produces inverse rotation if quaternion is normalized.
    Quat    Conj() const                    { return Quat(-x, -y, -z, w); }

    // Quaternion multiplication. Combines quaternion rotations, performing the one on the 
    // right hand side first.
    Quat  operator* (const Quat& b) const   { return Quat(w * b.x + x * b.w + y * b.z - z * b.y,
                                                          w * b.y - x * b.z + y * b.w + z * b.x,
                                                          w * b.z + x * b.y - y * b.x + z * b.w,
                                                          w * b.w - x * b.x - y * b.y - z * b.z); }

    // 
    // this^p normalized; same as rotating by this p times.
    Quat PowNormalized(T p) const
    {
        Vector3<T> v;
        T          a;
        GetAxisAngle(&v, &a);
        return Quat(v, a * p);
    }
    
    // Rotate transforms vector in a manner that matches Matrix rotations (counter-clockwise,
    // assuming negative direction of the axis). Standard formula: q(t) * V * q(t)^-1. 
    Vector3<T> Rotate(const Vector3<T>& v) const
    {
        return ((*this * Quat<T>(v.x, v.y, v.z, T(0))) * Inverted()).Imag();
    }

    
    // Inversed quaternion rotates in the opposite direction.
    Quat        Inverted() const
    {
        return Quat(-x, -y, -z, w);
    }

    // Sets this quaternion to the one rotates in the opposite direction.
    void        Invert()
    {
        *this = Quat(-x, -y, -z, w);
    }
    
    // Converting quaternion to matrix.
    operator Matrix4f() const
    {
        T ww = w*w;
        T xx = x*x;
        T yy = y*y;
        T zz = z*z;

        return Matrix4f(float(ww + xx - yy - zz),  float(T(2) * (x*y - w*z)), float(T(2) * (x*z + w*y)),
                        float(T(2) * (x*y + w*z)), float(ww - xx + yy - zz),  float(T(2) * (y*z - w*x)),
                        float(T(2) * (x*z - w*y)), float(T(2) * (y*z + w*x)), float(ww - xx - yy + zz) );
    }


	// Converting matrix to quaternion
	static Quat<T> Matrix4fToQuat(const Matrix4f& m)
	{
		T trace = m.M[0][0] + m.M[1][1] + m.M[2][2];
		Quat<T> q;

		// In almost all cases, the first part is executed.
		// However, if the trace is not positive, the other
		// cases arise.
		if (trace > T(0)) 
		{
			T s = sqrt(trace + T(1)) * T(2); // s=4*qw
			q.w = T(0.25) * s;
			q.x = (m.M[2][1] - m.M[1][2]) / s;
			q.y = (m.M[0][2] - m.M[2][0]) / s;
			q.z = (m.M[1][0] - m.M[0][1]) / s; 
		} 
		else if ((m.M[0][0] > m.M[1][1])&&(m.M[0][0] > m.M[2][2])) 
		{
			T s = sqrt(T(1) + m.M[0][0] - m.M[1][1] - m.M[2][2]) * T(2);
			q.w = (m.M[2][1] - m.M[1][2]) / s;
			q.x = T(0.25) * s;
			q.y = (m.M[0][1] + m.M[1][0]) / s;
			q.z = (m.M[2][0] + m.M[0][2]) / s;
		} 
		else if (m.M[1][1] > m.M[2][2]) 
		{
			T s = sqrt(T(1) + m.M[1][1] - m.M[0][0] - m.M[2][2]) * T(2); // S=4*qy
			q.w = (m.M[0][2] - m.M[2][0]) / s;
			q.x = (m.M[0][1] + m.M[1][0]) / s;
			q.y = T(0.25) * s;
			q.z = (m.M[1][2] + m.M[2][1]) / s;
		} 
		else 
		{
			T s = sqrt(T(1) + m.M[2][2] - m.M[0][0] - m.M[1][1]) * T(2); // S=4*qz
			q.w = (m.M[1][0] - m.M[0][1]) / s;
			q.x = (m.M[0][2] + m.M[2][0]) / s;
			q.y = (m.M[1][2] + m.M[2][1]) / s;
			q.z = T(0.25) * s;
		}
		return q;
	}


    
    // GetEulerAngles extracts Euler angles from the quaternion, in the specified order of
    // axis rotations and the specified coordinate system. Right-handed coordinate system
    // is the default, with CCW rotations while looking in the negative axis direction.
    // Here a,b,c, are the Yaw/Pitch/Roll angles to be returned.
    // rotation a around axis A1
    // is followed by rotation b around axis A2
    // is followed by rotation c around axis A3
    // rotations are CCW or CW (D) in LH or RH coordinate system (S)
    template <Axis A1, Axis A2, Axis A3, RotateDirection D, HandedSystem S>
    void GetEulerAngles(T *a, T *b, T *c)
    {
        OVR_COMPILER_ASSERT((A1 != A2) && (A2 != A3) && (A1 != A3));

        T Q[3] = { x, y, z };  //Quaternion components x,y,z

        T ww  = w*w;
        T Q11 = Q[A1]*Q[A1];
        T Q22 = Q[A2]*Q[A2];
        T Q33 = Q[A3]*Q[A3];

        T psign = T(-1);
        // Determine whether even permutation
        if (((A1 + 1) % 3 == A2) && ((A2 + 1) % 3 == A3))
            psign = T(1);
        
        T s2 = psign * T(2) * (psign*w*Q[A2] + Q[A1]*Q[A3]);

        if (s2 < T(-1) + Math<T>::SingularityRadius)
        { // South pole singularity
            *a = T(0);
            *b = -S*D*Math<T>::PiOver2;
            *c = S*D*atan2(T(2)*(psign*Q[A1]*Q[A2] + w*Q[A3]),
		                   ww + Q22 - Q11 - Q33 );
        }
        else if (s2 > T(1) - Math<T>::SingularityRadius)
        {  // North pole singularity
            *a = T(0);
            *b = S*D*Math<T>::PiOver2;
            *c = S*D*atan2(T(2)*(psign*Q[A1]*Q[A2] + w*Q[A3]),
		                   ww + Q22 - Q11 - Q33);
        }
        else
        {
            *a = -S*D*atan2(T(-2)*(w*Q[A1] - psign*Q[A2]*Q[A3]),
		                    ww + Q33 - Q11 - Q22);
            *b = S*D*asin(s2);
            *c = S*D*atan2(T(2)*(w*Q[A3] - psign*Q[A1]*Q[A2]),
		                   ww + Q11 - Q22 - Q33);
        }      
        return;
    }

    template <Axis A1, Axis A2, Axis A3, RotateDirection D>
    void GetEulerAngles(T *a, T *b, T *c)
    { GetEulerAngles<A1, A2, A3, D, Handed_R>(a, b, c); }

    template <Axis A1, Axis A2, Axis A3>
    void GetEulerAngles(T *a, T *b, T *c)
    { GetEulerAngles<A1, A2, A3, Rotate_CCW, Handed_R>(a, b, c); }


    // GetEulerAnglesABA extracts Euler angles from the quaternion, in the specified order of
    // axis rotations and the specified coordinate system. Right-handed coordinate system
    // is the default, with CCW rotations while looking in the negative axis direction.
    // Here a,b,c, are the Yaw/Pitch/Roll angles to be returned.
    // rotation a around axis A1
    // is followed by rotation b around axis A2
    // is followed by rotation c around axis A1
    // Rotations are CCW or CW (D) in LH or RH coordinate system (S)
    template <Axis A1, Axis A2, RotateDirection D, HandedSystem S>
    void GetEulerAnglesABA(T *a, T *b, T *c)
    {
        OVR_COMPILER_ASSERT(A1 != A2);

        T Q[3] = {x, y, z}; // Quaternion components

        // Determine the missing axis that was not supplied
        int m = 3 - A1 - A2;

        T ww = w*w;
        T Q11 = Q[A1]*Q[A1];
        T Q22 = Q[A2]*Q[A2];
        T Qmm = Q[m]*Q[m];

        T psign = T(-1);
        if ((A1 + 1) % 3 == A2) // Determine whether even permutation
        {
            psign = T(1);
        }

        T c2 = ww + Q11 - Q22 - Qmm;
        if (c2 < T(-1) + Math<T>::SingularityRadius)
        { // South pole singularity
            *a = T(0);
            *b = S*D*Math<T>::Pi;
            *c = S*D*atan2( T(2)*(w*Q[A1] - psign*Q[A2]*Q[m]),
		                    ww + Q22 - Q11 - Qmm);
        }
        else if (c2 > T(1) - Math<T>::SingularityRadius)
        {  // North pole singularity
            *a = T(0);
            *b = T(0);
            *c = S*D*atan2( T(2)*(w*Q[A1] - psign*Q[A2]*Q[m]),
		                   ww + Q22 - Q11 - Qmm);
        }
        else
        {
            *a = S*D*atan2( psign*w*Q[m] + Q[A1]*Q[A2],
		                   w*Q[A2] -psign*Q[A1]*Q[m]);
            *b = S*D*acos(c2);
            *c = S*D*atan2( -psign*w*Q[m] + Q[A1]*Q[A2],
		                   w*Q[A2] + psign*Q[A1]*Q[m]);
        }
        return;
    }

};

typedef Quat<float>  Quatf;
typedef Quat<double> Quatd;



//-------------------------------------------------------------------------------------
// ***** Angle

// Cleanly representing the algebra of 2D rotations.
// The operations maintain the angle between -Pi and Pi, the same range as atan2.

template<class T>
class Angle
{
public:
	enum AngularUnits
	{
		Radians = 0,
		Degrees = 1
	};

    Angle() : a(0) {}
    
	// Fix the range to be between -Pi and Pi
	Angle(T a_, AngularUnits u = Radians) : a((u == Radians) ? a_ : a_*Math<T>::DegreeToRadFactor) { FixRange(); }

	T    Get(AngularUnits u = Radians) const       { return (u == Radians) ? a : a*Math<T>::RadToDegreeFactor; }
	void Set(const T& x, AngularUnits u = Radians) { a = (u == Radians) ? x : x*Math<T>::DegreeToRadFactor; FixRange(); }
	int Sign() const                               { if (a == 0) return 0; else return (a > 0) ? 1 : -1; }
	T   Abs() const                                { return (a > 0) ? a : -a; }

    bool operator== (const Angle& b) const    { return a == b.a; }
    bool operator!= (const Angle& b) const    { return a != b.a; }
//	bool operator<  (const Angle& b) const    { return a < a.b; } 
//	bool operator>  (const Angle& b) const    { return a > a.b; } 
//	bool operator<= (const Angle& b) const    { return a <= a.b; } 
//	bool operator>= (const Angle& b) const    { return a >= a.b; } 
//	bool operator= (const T& x)               { a = x; FixRange(); }

	// These operations assume a is already between -Pi and Pi.
	Angle& operator+= (const Angle& b)        { a = a + b.a; FastFixRange(); return *this; }
	Angle& operator+= (const T& x)            { a = a + x; FixRange(); return *this; }
    Angle  operator+  (const Angle& b) const  { Angle res = *this; res += b; return res; }
	Angle  operator+  (const T& x) const      { Angle res = *this; res += x; return res; }
	Angle& operator-= (const Angle& b)        { a = a - b.a; FastFixRange(); return *this; }
	Angle& operator-= (const T& x)            { a = a - x; FixRange(); return *this; }
	Angle  operator-  (const Angle& b) const  { Angle res = *this; res -= b; return res; }
	Angle  operator-  (const T& x) const      { Angle res = *this; res -= x; return res; }
	
	T   Distance(const Angle& b)              { T c = fabs(a - b.a); return (c <= Math<T>::Pi) ? c : Math<T>::TwoPi - c; }

private:

	// The stored angle, which should be maintained between -Pi and Pi
	T a;

	// Fixes the angle range to [-Pi,Pi], but assumes no more than 2Pi away on either side 
	inline void FastFixRange()
	{
		if (a < -Math<T>::Pi)
			a += Math<T>::TwoPi;
		else if (a > Math<T>::Pi)
			a -= Math<T>::TwoPi;
	}

	// Fixes the angle range to [-Pi,Pi] for any given range, but slower then the fast method
	inline void FixRange()
	{
        // do nothing if the value is already in the correct range, since fmod call is expensive
        if (a >= -Math<T>::Pi && a <= Math<T>::Pi)
            return;
		a = fmod(a,Math<T>::TwoPi);
		if (a < -Math<T>::Pi)
			a += Math<T>::TwoPi;
		else if (a > Math<T>::Pi)
			a -= Math<T>::TwoPi;
	}
};


typedef Angle<float>  Anglef;
typedef Angle<double> Angled;


//-------------------------------------------------------------------------------------
// ***** Plane

// Consists of a normal vector and distance from the origin where the plane is located.

template<class T>
class Plane : public RefCountBase<Plane<T> >
{
public:
    Vector3<T> N;
    T          D;

    Plane() : D(0) {}

    // Normals must already be normalized
    Plane(const Vector3<T>& n, T d) : N(n), D(d) {}
    Plane(T x, T y, T z, T d) : N(x,y,z), D(d) {}

    // construct from a point on the plane and the normal
    Plane(const Vector3<T>& p, const Vector3<T>& n) : N(n), D(-(p * n)) {}

    // Find the point to plane distance. The sign indicates what side of the plane the point is on (0 = point on plane).
    T TestSide(const Vector3<T>& p) const
    {
        return (N.Dot(p)) + D;
    }

    Plane<T> Flipped() const
    {
        return Plane(-N, -D);
    }

    void Flip()
    {
        N = -N;
        D = -D;
    }

	bool operator==(const Plane<T>& rhs) const
	{
		return (this->D == rhs.D && this->N == rhs.N);
	}
};

typedef Plane<float> Planef;

} // Namespace OVR

#endif