nuclear@1: /************************************************************************************
nuclear@1: 
nuclear@1: PublicHeader:   OVR.h
nuclear@1: Filename    :   OVR_Math.h
nuclear@1: Content     :   Implementation of 3D primitives such as vectors, matrices.
nuclear@1: Created     :   September 4, 2012
nuclear@1: Authors     :   Andrew Reisse, Michael Antonov, Steve LaValle, Anna Yershova
nuclear@1: 
nuclear@1: Copyright   :   Copyright 2012 Oculus VR, Inc. All Rights reserved.
nuclear@1: 
nuclear@1: Use of this software is subject to the terms of the Oculus license
nuclear@1: agreement provided at the time of installation or download, or which
nuclear@1: otherwise accompanies this software in either electronic or hard copy form.
nuclear@1: 
nuclear@1: *************************************************************************************/
nuclear@1: 
nuclear@1: #ifndef OVR_Math_h
nuclear@1: #define OVR_Math_h
nuclear@1: 
nuclear@1: #include <assert.h>
nuclear@1: #include <stdlib.h>
nuclear@1: #include <math.h>
nuclear@1: 
nuclear@1: #include "OVR_Types.h"
nuclear@1: #include "OVR_RefCount.h"
nuclear@1: 
nuclear@1: namespace OVR {
nuclear@1: 
nuclear@1: //-------------------------------------------------------------------------------------
nuclear@1: // Constants for 3D world/axis definitions.
nuclear@1: 
nuclear@1: // Definitions of axes for coordinate and rotation conversions.
nuclear@1: enum Axis
nuclear@1: {
nuclear@1:     Axis_X = 0, Axis_Y = 1, Axis_Z = 2
nuclear@1: };
nuclear@1: 
nuclear@1: // RotateDirection describes the rotation direction around an axis, interpreted as follows:
nuclear@1: //  CW  - Clockwise while looking "down" from positive axis towards the origin.
nuclear@1: //  CCW - Counter-clockwise while looking from the positive axis towards the origin,
nuclear@1: //        which is in the negative axis direction.
nuclear@1: //  CCW is the default for the RHS coordinate system. Oculus standard RHS coordinate
nuclear@1: //  system defines Y up, X right, and Z back (pointing out from the screen). In this
nuclear@1: //  system Rotate_CCW around Z will specifies counter-clockwise rotation in XY plane.
nuclear@1: enum RotateDirection
nuclear@1: {
nuclear@1:     Rotate_CCW = 1,
nuclear@1:     Rotate_CW  = -1 
nuclear@1: };
nuclear@1: 
nuclear@1: enum HandedSystem
nuclear@1: {
nuclear@1:     Handed_R = 1, Handed_L = -1
nuclear@1: };
nuclear@1: 
nuclear@1: // AxisDirection describes which way the axis points. Used by WorldAxes.
nuclear@1: enum AxisDirection
nuclear@1: {
nuclear@1:     Axis_Up    =  2,
nuclear@1:     Axis_Down  = -2,
nuclear@1:     Axis_Right =  1,
nuclear@1:     Axis_Left  = -1,
nuclear@1:     Axis_In    =  3,
nuclear@1:     Axis_Out   = -3
nuclear@1: };
nuclear@1: 
nuclear@1: struct WorldAxes
nuclear@1: {
nuclear@1:     AxisDirection XAxis, YAxis, ZAxis;
nuclear@1: 
nuclear@1:     WorldAxes(AxisDirection x, AxisDirection y, AxisDirection z)
nuclear@1:         : XAxis(x), YAxis(y), ZAxis(z) 
nuclear@1:     { OVR_ASSERT(abs(x) != abs(y) && abs(y) != abs(z) && abs(z) != abs(x));}
nuclear@1: };
nuclear@1: 
nuclear@1: 
nuclear@1: //-------------------------------------------------------------------------------------
nuclear@1: // ***** Math
nuclear@1: 
nuclear@1: // Math class contains constants and functions. This class is a template specialized
nuclear@1: // per type, with Math<float> and Math<double> being distinct.
nuclear@1: template<class Type>
nuclear@1: class Math
nuclear@1: {  
nuclear@1: };
nuclear@1: 
nuclear@1: // Single-precision Math constants class.
nuclear@1: template<>
nuclear@1: class Math<float>
nuclear@1: {
nuclear@1: public:
nuclear@1:     static const float Pi;
nuclear@1:     static const float TwoPi;
nuclear@1:     static const float PiOver2;
nuclear@1:     static const float PiOver4;
nuclear@1:     static const float E;
nuclear@1: 
nuclear@1:     static const float MaxValue;          // Largest positive float Value
nuclear@1:     static const float MinPositiveValue;  // Smallest possible positive value
nuclear@1: 
nuclear@1:     static const float RadToDegreeFactor;
nuclear@1:     static const float DegreeToRadFactor;
nuclear@1: 
nuclear@1:     static const float Tolerance; //  0.00001f;
nuclear@1:     static const float SingularityRadius; //0.00000000001f for Gimbal lock numerical problems
nuclear@1: };
nuclear@1: 
nuclear@1: // Double-precision Math constants class.
nuclear@1: template<>
nuclear@1: class Math<double>
nuclear@1: {
nuclear@1: public:
nuclear@1:     static const double Pi;
nuclear@1:     static const double TwoPi;
nuclear@1:     static const double PiOver2;
nuclear@1:     static const double PiOver4;
nuclear@1:     static const double E;
nuclear@1: 
nuclear@1:     static const double MaxValue;          // Largest positive double Value
nuclear@1:     static const double MinPositiveValue;  // Smallest possible positive value
nuclear@1: 
nuclear@1:     static const double RadToDegreeFactor;
nuclear@1:     static const double DegreeToRadFactor;
nuclear@1: 
nuclear@1:     static const double Tolerance; //  0.00001f;
nuclear@1:     static const double SingularityRadius; //0.00000000001 for Gimbal lock numerical problems
nuclear@1: };
nuclear@1: 
nuclear@1: typedef Math<float>  Mathf;
nuclear@1: typedef Math<double> Mathd;
nuclear@1: 
nuclear@1: // Conversion functions between degrees and radians
nuclear@1: template<class FT>
nuclear@1: FT RadToDegree(FT rads) { return rads * Math<FT>::RadToDegreeFactor; }
nuclear@1: template<class FT>
nuclear@1: FT DegreeToRad(FT rads) { return rads * Math<FT>::DegreeToRadFactor; }
nuclear@1: 
nuclear@1: template<class T>
nuclear@1: class Quat;
nuclear@1: 
nuclear@1: //-------------------------------------------------------------------------------------
nuclear@1: // ***** Vector2f - 2D Vector2f
nuclear@1: 
nuclear@1: // Vector2f represents a 2-dimensional vector or point in space,
nuclear@1: // consisting of coordinates x and y,
nuclear@1: 
nuclear@1: template<class T>
nuclear@1: class Vector2
nuclear@1: {
nuclear@1: public:
nuclear@1:     T x, y;
nuclear@1: 
nuclear@1:     Vector2() : x(0), y(0) { }
nuclear@1:     Vector2(T x_, T y_) : x(x_), y(y_) { }
nuclear@1:     explicit Vector2(T s) : x(s), y(s) { }
nuclear@1: 
nuclear@1:     bool     operator== (const Vector2& b) const  { return x == b.x && y == b.y; }
nuclear@1:     bool     operator!= (const Vector2& b) const  { return x != b.x || y != b.y; }
nuclear@1:              
nuclear@1:     Vector2  operator+  (const Vector2& b) const  { return Vector2(x + b.x, y + b.y); }
nuclear@1:     Vector2& operator+= (const Vector2& b)        { x += b.x; y += b.y; return *this; }
nuclear@1:     Vector2  operator-  (const Vector2& b) const  { return Vector2(x - b.x, y - b.y); }
nuclear@1:     Vector2& operator-= (const Vector2& b)        { x -= b.x; y -= b.y; return *this; }
nuclear@1:     Vector2  operator- () const                   { return Vector2(-x, -y); }
nuclear@1: 
nuclear@1:     // Scalar multiplication/division scales vector.
nuclear@1:     Vector2  operator*  (T s) const               { return Vector2(x*s, y*s); }
nuclear@1:     Vector2& operator*= (T s)                     { x *= s; y *= s; return *this; }
nuclear@1: 
nuclear@1:     Vector2  operator/  (T s) const               { T rcp = T(1)/s;
nuclear@1:                                                     return Vector2(x*rcp, y*rcp); }
nuclear@1:     Vector2& operator/= (T s)                     { T rcp = T(1)/s;
nuclear@1:                                                     x *= rcp; y *= rcp;
nuclear@1:                                                     return *this; }
nuclear@1: 
nuclear@1:     // Compare two vectors for equality with tolerance. Returns true if vectors match withing tolerance.
nuclear@1:     bool      Compare(const Vector2&b, T tolerance = Mathf::Tolerance)
nuclear@1:     {
nuclear@1:         return (fabs(b.x-x) < tolerance) && (fabs(b.y-y) < tolerance);
nuclear@1:     }
nuclear@1:     
nuclear@1:     // Dot product overload.
nuclear@1:     // Used to calculate angle q between two vectors among other things,
nuclear@1:     // as (A dot B) = |a||b|cos(q).
nuclear@1:     T     operator*  (const Vector2& b) const    { return x*b.x + y*b.y; }
nuclear@1: 
nuclear@1:     // Returns the angle from this vector to b, in radians.
nuclear@1:     T       Angle(const Vector2& b) const        { return acos((*this * b)/(Length()*b.Length())); }
nuclear@1: 
nuclear@1:     // Return Length of the vector squared.
nuclear@1:     T       LengthSq() const                     { return (x * x + y * y); }
nuclear@1:     // Return vector length.
nuclear@1:     T       Length() const                       { return sqrt(LengthSq()); }
nuclear@1: 
nuclear@1:     // Returns distance between two points represented by vectors.
nuclear@1:     T       Distance(Vector2& b) const           { return (*this - b).Length(); }
nuclear@1:     
nuclear@1:     // Determine if this a unit vector.
nuclear@1:     bool    IsNormalized() const                 { return fabs(LengthSq() - T(1)) < Math<T>::Tolerance; }
nuclear@1:     // Normalize, convention vector length to 1.    
nuclear@1:     void    Normalize()                          { *this /= Length(); }
nuclear@1:     // Returns normalized (unit) version of the vector without modifying itself.
nuclear@1:     Vector2 Normalized() const                   { return *this / Length(); }
nuclear@1: 
nuclear@1:     // Linearly interpolates from this vector to another.
nuclear@1:     // Factor should be between 0.0 and 1.0, with 0 giving full value to this.
nuclear@1:     Vector2 Lerp(const Vector2& b, T f) const    { return *this*(T(1) - f) + b*f; }
nuclear@1: 
nuclear@1:     // Projects this vector onto the argument; in other words,
nuclear@1:     // A.Project(B) returns projection of vector A onto B.
nuclear@1:     Vector2 ProjectTo(const Vector2& b) const    { return b * ((*this * b) / b.LengthSq()); }
nuclear@1: };
nuclear@1: 
nuclear@1: 
nuclear@1: typedef Vector2<float>  Vector2f;
nuclear@1: typedef Vector2<double> Vector2d;
nuclear@1: 
nuclear@1: //-------------------------------------------------------------------------------------
nuclear@1: // ***** Vector3f - 3D Vector3f
nuclear@1: 
nuclear@1: // Vector3f represents a 3-dimensional vector or point in space,
nuclear@1: // consisting of coordinates x, y and z.
nuclear@1: 
nuclear@1: template<class T>
nuclear@1: class Vector3
nuclear@1: {
nuclear@1: public:
nuclear@1:     T x, y, z;
nuclear@1: 
nuclear@1:     Vector3() : x(0), y(0), z(0) { }
nuclear@1:     Vector3(T x_, T y_, T z_ = 0) : x(x_), y(y_), z(z_) { }
nuclear@1:     explicit Vector3(T s) : x(s), y(s), z(s) { }
nuclear@1: 
nuclear@1:     bool     operator== (const Vector3& b) const  { return x == b.x && y == b.y && z == b.z; }
nuclear@1:     bool     operator!= (const Vector3& b) const  { return x != b.x || y != b.y || z != b.z; }
nuclear@1:              
nuclear@1:     Vector3  operator+  (const Vector3& b) const  { return Vector3(x + b.x, y + b.y, z + b.z); }
nuclear@1:     Vector3& operator+= (const Vector3& b)        { x += b.x; y += b.y; z += b.z; return *this; }
nuclear@1:     Vector3  operator-  (const Vector3& b) const  { return Vector3(x - b.x, y - b.y, z - b.z); }
nuclear@1:     Vector3& operator-= (const Vector3& b)        { x -= b.x; y -= b.y; z -= b.z; return *this; }
nuclear@1:     Vector3  operator- () const                   { return Vector3(-x, -y, -z); }
nuclear@1: 
nuclear@1:     // Scalar multiplication/division scales vector.
nuclear@1:     Vector3  operator*  (T s) const               { return Vector3(x*s, y*s, z*s); }
nuclear@1:     Vector3& operator*= (T s)                     { x *= s; y *= s; z *= s; return *this; }
nuclear@1: 
nuclear@1:     Vector3  operator/  (T s) const               { T rcp = T(1)/s;
nuclear@1:                                                     return Vector3(x*rcp, y*rcp, z*rcp); }
nuclear@1:     Vector3& operator/= (T s)                     { T rcp = T(1)/s;
nuclear@1:                                                     x *= rcp; y *= rcp; z *= rcp;
nuclear@1:                                                     return *this; }
nuclear@1: 
nuclear@1:     // Compare two vectors for equality with tolerance. Returns true if vectors match withing tolerance.
nuclear@1:     bool      Compare(const Vector3&b, T tolerance = Mathf::Tolerance)
nuclear@1:     {
nuclear@1:         return (fabs(b.x-x) < tolerance) && (fabs(b.y-y) < tolerance) && (fabs(b.z-z) < tolerance);
nuclear@1:     }
nuclear@1:     
nuclear@1:     // Dot product overload.
nuclear@1:     // Used to calculate angle q between two vectors among other things,
nuclear@1:     // as (A dot B) = |a||b|cos(q).
nuclear@1:     T     operator*  (const Vector3& b) const    { return x*b.x + y*b.y + z*b.z; }
nuclear@1: 
nuclear@1:     // Compute cross product, which generates a normal vector.
nuclear@1:     // Direction vector can be determined by right-hand rule: Pointing index finder in
nuclear@1:     // direction a and middle finger in direction b, thumb will point in a.Cross(b).
nuclear@1:     Vector3 Cross(const Vector3& b) const        { return Vector3(y*b.z - z*b.y,
nuclear@1:                                                                   z*b.x - x*b.z,
nuclear@1:                                                                   x*b.y - y*b.x); }
nuclear@1: 
nuclear@1:     // Returns the angle from this vector to b, in radians.
nuclear@1:     T       Angle(const Vector3& b) const        { return acos((*this * b)/(Length()*b.Length())); }
nuclear@1: 
nuclear@1:     // Return Length of the vector squared.
nuclear@1:     T       LengthSq() const                     { return (x * x + y * y + z * z); }
nuclear@1:     // Return vector length.
nuclear@1:     T       Length() const                       { return sqrt(LengthSq()); }
nuclear@1: 
nuclear@1:     // Returns distance between two points represented by vectors.
nuclear@1:     T       Distance(Vector3& b) const           { return (*this - b).Length(); }
nuclear@1:     
nuclear@1:     // Determine if this a unit vector.
nuclear@1:     bool    IsNormalized() const                 { return fabs(LengthSq() - T(1)) < Math<T>::Tolerance; }
nuclear@1:     // Normalize, convention vector length to 1.    
nuclear@1:     void    Normalize()                          { *this /= Length(); }
nuclear@1:     // Returns normalized (unit) version of the vector without modifying itself.
nuclear@1:     Vector3 Normalized() const                   { return *this / Length(); }
nuclear@1: 
nuclear@1:     // Linearly interpolates from this vector to another.
nuclear@1:     // Factor should be between 0.0 and 1.0, with 0 giving full value to this.
nuclear@1:     Vector3 Lerp(const Vector3& b, T f) const    { return *this*(T(1) - f) + b*f; }
nuclear@1: 
nuclear@1:     // Projects this vector onto the argument; in other words,
nuclear@1:     // A.Project(B) returns projection of vector A onto B.
nuclear@1:     Vector3 ProjectTo(const Vector3& b) const    { return b * ((*this * b) / b.LengthSq()); }
nuclear@1: };
nuclear@1: 
nuclear@1: 
nuclear@1: typedef Vector3<float>  Vector3f;
nuclear@1: typedef Vector3<double> Vector3d;
nuclear@1: 
nuclear@1: 
nuclear@1: //-------------------------------------------------------------------------------------
nuclear@1: // ***** Matrix4f 
nuclear@1: 
nuclear@1: // Matrix4f is a 4x4 matrix used for 3d transformations and projections.
nuclear@1: // Translation stored in the last column.
nuclear@1: // The matrix is stored in row-major order in memory, meaning that values
nuclear@1: // of the first row are stored before the next one.
nuclear@1: //
nuclear@1: // The arrangement of the matrix is chosen to be in Right-Handed 
nuclear@1: // coordinate system and counterclockwise rotations when looking down
nuclear@1: // the axis
nuclear@1: //
nuclear@1: // Transformation Order:
nuclear@1: //   - Transformations are applied from right to left, so the expression
nuclear@1: //     M1 * M2 * M3 * V means that the vector V is transformed by M3 first,
nuclear@1: //     followed by M2 and M1. 
nuclear@1: //
nuclear@1: // Coordinate system: Right Handed
nuclear@1: //
nuclear@1: // Rotations: Counterclockwise when looking down the axis. All angles are in radians.
nuclear@1: //    
nuclear@1: //  | sx   01   02   tx |    // First column  (sx, 10, 20): Axis X basis vector.
nuclear@1: //  | 10   sy   12   ty |    // Second column (01, sy, 21): Axis Y basis vector.
nuclear@1: //  | 20   21   sz   tz |    // Third columnt (02, 12, sz): Axis Z basis vector.
nuclear@1: //  | 30   31   32   33 |
nuclear@1: //
nuclear@1: //  The basis vectors are first three columns.
nuclear@1: 
nuclear@1: class Matrix4f
nuclear@1: {
nuclear@1:     static Matrix4f IdentityValue;
nuclear@1: 
nuclear@1: public:
nuclear@1:     float M[4][4];    
nuclear@1: 
nuclear@1:     enum NoInitType { NoInit };
nuclear@1: 
nuclear@1:     // Construct with no memory initialization.
nuclear@1:     Matrix4f(NoInitType) { }
nuclear@1: 
nuclear@1:     // By default, we construct identity matrix.
nuclear@1:     Matrix4f()
nuclear@1:     {
nuclear@1:         SetIdentity();        
nuclear@1:     }
nuclear@1: 
nuclear@1:     Matrix4f(float m11, float m12, float m13, float m14,
nuclear@1:              float m21, float m22, float m23, float m24,
nuclear@1:              float m31, float m32, float m33, float m34,
nuclear@1:              float m41, float m42, float m43, float m44)
nuclear@1:     {
nuclear@1:         M[0][0] = m11; M[0][1] = m12; M[0][2] = m13; M[0][3] = m14;
nuclear@1:         M[1][0] = m21; M[1][1] = m22; M[1][2] = m23; M[1][3] = m24;
nuclear@1:         M[2][0] = m31; M[2][1] = m32; M[2][2] = m33; M[2][3] = m34;
nuclear@1:         M[3][0] = m41; M[3][1] = m42; M[3][2] = m43; M[3][3] = m44;
nuclear@1:     }
nuclear@1: 
nuclear@1:     Matrix4f(float m11, float m12, float m13,
nuclear@1:              float m21, float m22, float m23,
nuclear@1:              float m31, float m32, float m33)
nuclear@1:     {
nuclear@1:         M[0][0] = m11; M[0][1] = m12; M[0][2] = m13; M[0][3] = 0;
nuclear@1:         M[1][0] = m21; M[1][1] = m22; M[1][2] = m23; M[1][3] = 0;
nuclear@1:         M[2][0] = m31; M[2][1] = m32; M[2][2] = m33; M[2][3] = 0;
nuclear@1:         M[3][0] = 0;   M[3][1] = 0;   M[3][2] = 0;   M[3][3] = 1;
nuclear@1:     }
nuclear@1: 
nuclear@1:     static const Matrix4f& Identity()  { return IdentityValue; }
nuclear@1: 
nuclear@1:     void SetIdentity()
nuclear@1:     {
nuclear@1:         M[0][0] = M[1][1] = M[2][2] = M[3][3] = 1;
nuclear@1:         M[0][1] = M[1][0] = M[2][3] = M[3][1] = 0;
nuclear@1:         M[0][2] = M[1][2] = M[2][0] = M[3][2] = 0;
nuclear@1:         M[0][3] = M[1][3] = M[2][1] = M[3][0] = 0;
nuclear@1:     }
nuclear@1: 
nuclear@1:     // Multiplies two matrices into destination with minimum copying.
nuclear@1:     static Matrix4f& Multiply(Matrix4f* d, const Matrix4f& a, const Matrix4f& b)
nuclear@1:     {
nuclear@1:         OVR_ASSERT((d != &a) && (d != &b));
nuclear@1:         int i = 0;
nuclear@1:         do {
nuclear@1:             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];
nuclear@1:             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];
nuclear@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];
nuclear@1:             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];
nuclear@1:         } while((++i) < 4);
nuclear@1: 
nuclear@1:         return *d;
nuclear@1:     }
nuclear@1: 
nuclear@1:     Matrix4f operator* (const Matrix4f& b) const
nuclear@1:     {
nuclear@1:         Matrix4f result(Matrix4f::NoInit);
nuclear@1:         Multiply(&result, *this, b);
nuclear@1:         return result;
nuclear@1:     }
nuclear@1: 
nuclear@1:     Matrix4f& operator*= (const Matrix4f& b)
nuclear@1:     {
nuclear@1:         return Multiply(this, Matrix4f(*this), b);
nuclear@1:     }
nuclear@1: 
nuclear@1:     Matrix4f operator* (float s) const
nuclear@1:     {
nuclear@1:         return Matrix4f(M[0][0] * s, M[0][1] * s, M[0][2] * s, M[0][3] * s,
nuclear@1:                         M[1][0] * s, M[1][1] * s, M[1][2] * s, M[1][3] * s,
nuclear@1:                         M[2][0] * s, M[2][1] * s, M[2][2] * s, M[2][3] * s,
nuclear@1:                         M[3][0] * s, M[3][1] * s, M[3][2] * s, M[3][3] * s);
nuclear@1:     }
nuclear@1: 
nuclear@1:     Matrix4f& operator*= (float s)
nuclear@1:     {
nuclear@1:         M[0][0] *= s; M[0][1] *= s; M[0][2] *= s; M[0][3] *= s;
nuclear@1:         M[1][0] *= s; M[1][1] *= s; M[1][2] *= s; M[1][3] *= s;
nuclear@1:         M[2][0] *= s; M[2][1] *= s; M[2][2] *= s; M[2][3] *= s;
nuclear@1:         M[3][0] *= s; M[3][1] *= s; M[3][2] *= s; M[3][3] *= s;
nuclear@1:         return *this;
nuclear@1:     }
nuclear@1: 
nuclear@1:     Vector3f Transform(const Vector3f& v) const
nuclear@1:     {
nuclear@1:         return Vector3f(M[0][0] * v.x + M[0][1] * v.y + M[0][2] * v.z + M[0][3],
nuclear@1:                         M[1][0] * v.x + M[1][1] * v.y + M[1][2] * v.z + M[1][3],
nuclear@1:                         M[2][0] * v.x + M[2][1] * v.y + M[2][2] * v.z + M[2][3]);
nuclear@1:     }
nuclear@1: 
nuclear@1:     Matrix4f Transposed() const
nuclear@1:     {
nuclear@1:         return Matrix4f(M[0][0], M[1][0], M[2][0], M[3][0],
nuclear@1:                         M[0][1], M[1][1], M[2][1], M[3][1],
nuclear@1:                         M[0][2], M[1][2], M[2][2], M[3][2],
nuclear@1:                         M[0][3], M[1][3], M[2][3], M[3][3]);
nuclear@1:     }
nuclear@1: 
nuclear@1:     void     Transpose()
nuclear@1:     {
nuclear@1:         *this = Transposed();
nuclear@1:     }
nuclear@1: 
nuclear@1: 
nuclear@1:     float SubDet (const int* rows, const int* cols) const
nuclear@1:     {
nuclear@1:         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]])
nuclear@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]])
nuclear@1:              + 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]]);
nuclear@1:     }
nuclear@1: 
nuclear@1:     float Cofactor(int I, int J) const
nuclear@1:     {
nuclear@1:         const int indices[4][3] = {{1,2,3},{0,2,3},{0,1,3},{0,1,2}};
nuclear@1:         return ((I+J)&1) ? -SubDet(indices[I],indices[J]) : SubDet(indices[I],indices[J]);
nuclear@1:     }
nuclear@1: 
nuclear@1:     float    Determinant() const
nuclear@1:     {
nuclear@1:         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);
nuclear@1:     }
nuclear@1: 
nuclear@1:     Matrix4f Adjugated() const
nuclear@1:     {
nuclear@1:         return Matrix4f(Cofactor(0,0), Cofactor(1,0), Cofactor(2,0), Cofactor(3,0), 
nuclear@1:                         Cofactor(0,1), Cofactor(1,1), Cofactor(2,1), Cofactor(3,1), 
nuclear@1:                         Cofactor(0,2), Cofactor(1,2), Cofactor(2,2), Cofactor(3,2),
nuclear@1:                         Cofactor(0,3), Cofactor(1,3), Cofactor(2,3), Cofactor(3,3));
nuclear@1:     }
nuclear@1: 
nuclear@1:     Matrix4f Inverted() const
nuclear@1:     {
nuclear@1:         float det = Determinant();
nuclear@1:         assert(det != 0);
nuclear@1:         return Adjugated() * (1.0f/det);
nuclear@1:     }
nuclear@1: 
nuclear@1:     void Invert()
nuclear@1:     {
nuclear@1:         *this = Inverted();
nuclear@1:     }
nuclear@1: 
nuclear@1:     //AnnaSteve:
nuclear@1:     // a,b,c, are the YawPitchRoll angles to be returned
nuclear@1:     // rotation a around axis A1
nuclear@1:     // is followed by rotation b around axis A2
nuclear@1:     // is followed by rotation c around axis A3
nuclear@1:     // rotations are CCW or CW (D) in LH or RH coordinate system (S)
nuclear@1:     template <Axis A1, Axis A2, Axis A3, RotateDirection D, HandedSystem S>
nuclear@1:     void ToEulerAngles(float *a, float *b, float *c)
nuclear@1:     {
nuclear@1:         OVR_COMPILER_ASSERT((A1 != A2) && (A2 != A3) && (A1 != A3));
nuclear@1: 
nuclear@1:         float psign = -1.0f;
nuclear@1:         if (((A1 + 1) % 3 == A2) && ((A2 + 1) % 3 == A3)) // Determine whether even permutation
nuclear@1:         psign = 1.0f;
nuclear@1:         
nuclear@1:         float pm = psign*M[A1][A3];
nuclear@1:         if (pm < -1.0f + Math<float>::SingularityRadius)
nuclear@1:         { // South pole singularity
nuclear@1:             *a = 0.0f;
nuclear@1:             *b = -S*D*Math<float>::PiOver2;
nuclear@1:             *c = S*D*atan2( psign*M[A2][A1], M[A2][A2] );
nuclear@1:         }
nuclear@1:         else if (pm > 1.0 - Math<float>::SingularityRadius)
nuclear@1:         { // North pole singularity
nuclear@1:             *a = 0.0f;
nuclear@1:             *b = S*D*Math<float>::PiOver2;
nuclear@1:             *c = S*D*atan2( psign*M[A2][A1], M[A2][A2] );
nuclear@1:         }
nuclear@1:         else
nuclear@1:         { // Normal case (nonsingular)
nuclear@1:             *a = S*D*atan2( -psign*M[A2][A3], M[A3][A3] );
nuclear@1:             *b = S*D*asin(pm);
nuclear@1:             *c = S*D*atan2( -psign*M[A1][A2], M[A1][A1] );
nuclear@1:         }
nuclear@1: 
nuclear@1:         return;
nuclear@1:     }
nuclear@1: 
nuclear@1:     //AnnaSteve:
nuclear@1:     // a,b,c, are the YawPitchRoll angles to be returned
nuclear@1:     // rotation a around axis A1
nuclear@1:     // is followed by rotation b around axis A2
nuclear@1:     // is followed by rotation c around axis A1
nuclear@1:     // rotations are CCW or CW (D) in LH or RH coordinate system (S)
nuclear@1:     template <Axis A1, Axis A2, RotateDirection D, HandedSystem S>
nuclear@1:     void ToEulerAnglesABA(float *a, float *b, float *c)
nuclear@1:     {        
nuclear@1:          OVR_COMPILER_ASSERT(A1 != A2);
nuclear@1:   
nuclear@1:         // Determine the axis that was not supplied
nuclear@1:         int m = 3 - A1 - A2;
nuclear@1: 
nuclear@1:         float psign = -1.0f;
nuclear@1:         if ((A1 + 1) % 3 == A2) // Determine whether even permutation
nuclear@1:             psign = 1.0f;
nuclear@1: 
nuclear@1:         float c2 = M[A1][A1];
nuclear@1:         if (c2 < -1.0 + Math<float>::SingularityRadius)
nuclear@1:         { // South pole singularity
nuclear@1:             *a = 0.0f;
nuclear@1:             *b = S*D*Math<float>::Pi;
nuclear@1:             *c = S*D*atan2( -psign*M[A2][m],M[A2][A2]);
nuclear@1:         }
nuclear@1:         else if (c2 > 1.0 - Math<float>::SingularityRadius)
nuclear@1:         { // North pole singularity
nuclear@1:             *a = 0.0f;
nuclear@1:             *b = 0.0f;
nuclear@1:             *c = S*D*atan2( -psign*M[A2][m],M[A2][A2]);
nuclear@1:         }
nuclear@1:         else
nuclear@1:         { // Normal case (nonsingular)
nuclear@1:             *a = S*D*atan2( M[A2][A1],-psign*M[m][A1]);
nuclear@1:             *b = S*D*acos(c2);
nuclear@1:             *c = S*D*atan2( M[A1][A2],psign*M[A1][m]);
nuclear@1:         }
nuclear@1:         return;
nuclear@1:     }
nuclear@1:   
nuclear@1:     // Creates a matrix that converts the vertices from one coordinate system
nuclear@1:     // to another.
nuclear@1:     // 
nuclear@1:     static Matrix4f AxisConversion(const WorldAxes& to, const WorldAxes& from)
nuclear@1:     {        
nuclear@1:         // Holds axis values from the 'to' structure
nuclear@1:         int toArray[3] = { to.XAxis, to.YAxis, to.ZAxis };
nuclear@1: 
nuclear@1:         // The inverse of the toArray
nuclear@1:         int inv[4]; 
nuclear@1:         inv[0] = inv[abs(to.XAxis)] = 0;
nuclear@1:         inv[abs(to.YAxis)] = 1;
nuclear@1:         inv[abs(to.ZAxis)] = 2;
nuclear@1: 
nuclear@1:         Matrix4f m(0,  0,  0, 
nuclear@1:                    0,  0,  0,
nuclear@1:                    0,  0,  0);
nuclear@1: 
nuclear@1:         // Only three values in the matrix need to be changed to 1 or -1.
nuclear@1:         m.M[inv[abs(from.XAxis)]][0] = float(from.XAxis/toArray[inv[abs(from.XAxis)]]);
nuclear@1:         m.M[inv[abs(from.YAxis)]][1] = float(from.YAxis/toArray[inv[abs(from.YAxis)]]);
nuclear@1:         m.M[inv[abs(from.ZAxis)]][2] = float(from.ZAxis/toArray[inv[abs(from.ZAxis)]]);
nuclear@1:         return m;
nuclear@1:     } 
nuclear@1: 
nuclear@1: 
nuclear@1: 
nuclear@1:     static Matrix4f Translation(const Vector3f& v)
nuclear@1:     {
nuclear@1:         Matrix4f t;
nuclear@1:         t.M[0][3] = v.x;
nuclear@1:         t.M[1][3] = v.y;
nuclear@1:         t.M[2][3] = v.z;
nuclear@1:         return t;
nuclear@1:     }
nuclear@1: 
nuclear@1:     static Matrix4f Translation(float x, float y, float z = 0.0f)
nuclear@1:     {
nuclear@1:         Matrix4f t;
nuclear@1:         t.M[0][3] = x;
nuclear@1:         t.M[1][3] = y;
nuclear@1:         t.M[2][3] = z;
nuclear@1:         return t;
nuclear@1:     }
nuclear@1: 
nuclear@1:     static Matrix4f Scaling(const Vector3f& v)
nuclear@1:     {
nuclear@1:         Matrix4f t;
nuclear@1:         t.M[0][0] = v.x;
nuclear@1:         t.M[1][1] = v.y;
nuclear@1:         t.M[2][2] = v.z;
nuclear@1:         return t;
nuclear@1:     }
nuclear@1: 
nuclear@1:     static Matrix4f Scaling(float x, float y, float z)
nuclear@1:     {
nuclear@1:         Matrix4f t;
nuclear@1:         t.M[0][0] = x;
nuclear@1:         t.M[1][1] = y;
nuclear@1:         t.M[2][2] = z;
nuclear@1:         return t;
nuclear@1:     }
nuclear@1: 
nuclear@1:     static Matrix4f Scaling(float s)
nuclear@1:     {
nuclear@1:         Matrix4f t;
nuclear@1:         t.M[0][0] = s;
nuclear@1:         t.M[1][1] = s;
nuclear@1:         t.M[2][2] = s;
nuclear@1:         return t;
nuclear@1:     }
nuclear@1: 
nuclear@1:   
nuclear@1: 
nuclear@1:     //AnnaSteve : Just for quick testing.  Not for final API.  Need to remove case.
nuclear@1:     static Matrix4f RotationAxis(Axis A, float angle, RotateDirection d, HandedSystem s)
nuclear@1:     {
nuclear@1:         float sina = s * d *sin(angle);
nuclear@1:         float cosa = cos(angle);
nuclear@1:         
nuclear@1:         switch(A)
nuclear@1:         {
nuclear@1:         case Axis_X:
nuclear@1:             return Matrix4f(1,  0,     0, 
nuclear@1:                             0,  cosa,  -sina,
nuclear@1:                             0,  sina,  cosa);
nuclear@1:         case Axis_Y:
nuclear@1:             return Matrix4f(cosa,  0,   sina, 
nuclear@1:                             0,     1,   0,
nuclear@1:                             -sina, 0,   cosa);
nuclear@1:         case Axis_Z:
nuclear@1:             return Matrix4f(cosa,  -sina,  0, 
nuclear@1:                             sina,  cosa,   0,
nuclear@1:                             0,     0,      1);
nuclear@1:         }
nuclear@1:     }
nuclear@1: 
nuclear@1: 
nuclear@1:     // Creates a rotation matrix rotating around the X axis by 'angle' radians.
nuclear@1:     // Rotation direction is depends on the coordinate system:
nuclear@1:     //  RHS (Oculus default): Positive angle values rotate Counter-clockwise (CCW),
nuclear@1:     //                        while looking in the negative axis direction. This is the
nuclear@1:     //                        same as looking down from positive axis values towards origin.
nuclear@1:     //  LHS: Positive angle values rotate clock-wise (CW), while looking in the
nuclear@1:     //       negative axis direction.
nuclear@1:     static Matrix4f RotationX(float angle)
nuclear@1:     {
nuclear@1:         float sina = sin(angle);
nuclear@1:         float cosa = cos(angle);
nuclear@1:         return Matrix4f(1,  0,     0, 
nuclear@1:                         0,  cosa,  -sina,
nuclear@1:                         0,  sina,  cosa);
nuclear@1:     }
nuclear@1: 
nuclear@1:     // Creates a rotation matrix rotating around the Y axis by 'angle' radians.
nuclear@1:     // Rotation direction is depends on the coordinate system:
nuclear@1:     //  RHS (Oculus default): Positive angle values rotate Counter-clockwise (CCW),
nuclear@1:     //                        while looking in the negative axis direction. This is the
nuclear@1:     //                        same as looking down from positive axis values towards origin.
nuclear@1:     //  LHS: Positive angle values rotate clock-wise (CW), while looking in the
nuclear@1:     //       negative axis direction.
nuclear@1:     static Matrix4f RotationY(float angle)
nuclear@1:     {
nuclear@1:         float sina = sin(angle);
nuclear@1:         float cosa = cos(angle);
nuclear@1:         return Matrix4f(cosa,  0,   sina, 
nuclear@1:                         0,     1,   0,
nuclear@1:                         -sina, 0,   cosa);
nuclear@1:     }
nuclear@1: 
nuclear@1:     // Creates a rotation matrix rotating around the Z axis by 'angle' radians.
nuclear@1:     // Rotation direction is depends on the coordinate system:
nuclear@1:     //  RHS (Oculus default): Positive angle values rotate Counter-clockwise (CCW),
nuclear@1:     //                        while looking in the negative axis direction. This is the
nuclear@1:     //                        same as looking down from positive axis values towards origin.
nuclear@1:     //  LHS: Positive angle values rotate clock-wise (CW), while looking in the
nuclear@1:     //       negative axis direction.
nuclear@1:     static Matrix4f RotationZ(float angle)
nuclear@1:     {
nuclear@1:         float sina = sin(angle);
nuclear@1:         float cosa = cos(angle);
nuclear@1:         return Matrix4f(cosa,  -sina,  0, 
nuclear@1:                         sina,  cosa,   0,
nuclear@1:                         0,     0,      1);
nuclear@1:     }
nuclear@1: 
nuclear@1: 
nuclear@1:     // LookAtRH creates a View transformation matrix for right-handed coordinate system.
nuclear@1:     // The resulting matrix points camera from 'eye' towards 'at' direction, with 'up'
nuclear@1:     // specifying the up vector. The resulting matrix should be used with PerspectiveRH
nuclear@1:     // projection.
nuclear@1:     static Matrix4f LookAtRH(const Vector3f& eye, const Vector3f& at, const Vector3f& up);
nuclear@1: 
nuclear@1:     // LookAtLH creates a View transformation matrix for left-handed coordinate system.
nuclear@1:     // The resulting matrix points camera from 'eye' towards 'at' direction, with 'up'
nuclear@1:     // specifying the up vector. 
nuclear@1:     static Matrix4f LookAtLH(const Vector3f& eye, const Vector3f& at, const Vector3f& up);
nuclear@1:     
nuclear@1:     
nuclear@1:     // PerspectiveRH creates a right-handed perspective projection matrix that can be
nuclear@1:     // used with the Oculus sample renderer. 
nuclear@1:     //  yfov   - Specifies vertical field of view in radians.
nuclear@1:     //  aspect - Screen aspect ration, which is usually width/height for square pixels.
nuclear@1:     //           Note that xfov = yfov * aspect.
nuclear@1:     //  znear  - Absolute value of near Z clipping clipping range.
nuclear@1:     //  zfar   - Absolute value of far  Z clipping clipping range (larger then near).
nuclear@1:     // Even though RHS usually looks in the direction of negative Z, positive values
nuclear@1:     // are expected for znear and zfar.
nuclear@1:     static Matrix4f PerspectiveRH(float yfov, float aspect, float znear, float zfar);
nuclear@1:     
nuclear@1:     
nuclear@1:     // PerspectiveRH creates a left-handed perspective projection matrix that can be
nuclear@1:     // used with the Oculus sample renderer. 
nuclear@1:     //  yfov   - Specifies vertical field of view in radians.
nuclear@1:     //  aspect - Screen aspect ration, which is usually width/height for square pixels.
nuclear@1:     //           Note that xfov = yfov * aspect.
nuclear@1:     //  znear  - Absolute value of near Z clipping clipping range.
nuclear@1:     //  zfar   - Absolute value of far  Z clipping clipping range (larger then near).
nuclear@1:     static Matrix4f PerspectiveLH(float yfov, float aspect, float znear, float zfar);
nuclear@1: 
nuclear@1: 
nuclear@1:     static Matrix4f Ortho2D(float w, float h);
nuclear@1: };
nuclear@1: 
nuclear@1: 
nuclear@1: //-------------------------------------------------------------------------------------
nuclear@1: // ***** Quat
nuclear@1: 
nuclear@1: // Quatf represents a quaternion class used for rotations.
nuclear@1: // 
nuclear@1: // Quaternion multiplications are done in right-to-left order, to match the
nuclear@1: // behavior of matrices.
nuclear@1: 
nuclear@1: 
nuclear@1: template<class T>
nuclear@1: class Quat
nuclear@1: {
nuclear@1: public:
nuclear@1:     // w + Xi + Yj + Zk
nuclear@1:     T x, y, z, w;    
nuclear@1: 
nuclear@1:     Quat() : x(0), y(0), z(0), w(1) {}
nuclear@1:     Quat(T x_, T y_, T z_, T w_) : x(x_), y(y_), z(z_), w(w_) {}
nuclear@1: 
nuclear@1: 
nuclear@1:     // Constructs rotation quaternion around the axis.
nuclear@1:     Quat(const Vector3<T>& axis, T angle)
nuclear@1:     {
nuclear@1:         Vector3<T> unitAxis = axis.Normalized();
nuclear@1:         T          sinHalfAngle = sin(angle * T(0.5));
nuclear@1: 
nuclear@1:         w = cos(angle * T(0.5));
nuclear@1:         x = unitAxis.x * sinHalfAngle;
nuclear@1:         y = unitAxis.y * sinHalfAngle;
nuclear@1:         z = unitAxis.z * sinHalfAngle;
nuclear@1:     }
nuclear@1: 
nuclear@1:     //AnnaSteve:
nuclear@1:     void AxisAngle(Axis A, T angle, RotateDirection d, HandedSystem s)
nuclear@1:     {
nuclear@1:         T sinHalfAngle = s * d *sin(angle * (T)0.5);
nuclear@1:         T v[3];
nuclear@1:         v[0] = v[1] = v[2] = (T)0;
nuclear@1:         v[A] = sinHalfAngle;
nuclear@1:         //return Quat(v[0], v[1], v[2], cos(angle * (T)0.5));
nuclear@1:         w = cos(angle * (T)0.5);
nuclear@1:         x = v[0];
nuclear@1:         y = v[1];
nuclear@1:         z = v[2];
nuclear@1:     }
nuclear@1: 
nuclear@1: 
nuclear@1:     void GetAxisAngle(Vector3<T>* axis, T* angle) const
nuclear@1:     {
nuclear@1:         if (LengthSq() > Math<T>::Tolerance * Math<T>::Tolerance)
nuclear@1:         {
nuclear@1:             *axis  = Vector3<T>(x, y, z).Normalized();
nuclear@1:             *angle = 2 * acos(w);
nuclear@1:         }
nuclear@1:         else
nuclear@1:         {
nuclear@1:             *axis = Vector3<T>(1, 0, 0);
nuclear@1:             *angle= 0;
nuclear@1:         }
nuclear@1:     }
nuclear@1: 
nuclear@1:     bool operator== (const Quat& b) const   { return x == b.x && y == b.y && z == b.z && w == b.w; }
nuclear@1:     bool operator!= (const Quat& b) const   { return x != b.x || y != b.y || z != b.z || w != b.w; }
nuclear@1: 
nuclear@1:     Quat  operator+  (const Quat& b) const  { return Quat(x + b.x, y + b.y, z + b.z, w + b.w); }
nuclear@1:     Quat& operator+= (const Quat& b)        { w += b.w; x += b.x; y += b.y; z += b.z; return *this; }
nuclear@1:     Quat  operator-  (const Quat& b) const  { return Quat(x - b.x, y - b.y, z - b.z, w - b.w); }
nuclear@1:     Quat& operator-= (const Quat& b)        { w -= b.w; x -= b.x; y -= b.y; z -= b.z; return *this; }
nuclear@1: 
nuclear@1:     Quat  operator*  (T s) const            { return Quat(x * s, y * s, z * s, w * s); }
nuclear@1:     Quat& operator*= (T s)                  { w *= s; x *= s; y *= s; z *= s; return *this; }
nuclear@1:     Quat  operator/  (T s) const            { T rcp = T(1)/s; return Quat(x * rcp, y * rcp, z * rcp, w *rcp); }
nuclear@1:     Quat& operator/= (T s)                  { T rcp = T(1)/s; w *= rcp; x *= rcp; y *= rcp; z *= rcp; return *this; }
nuclear@1: 
nuclear@1:     // Get Imaginary part vector
nuclear@1:     Vector3<T> Imag() const                 { return Vector3<T>(x,y,z); }
nuclear@1: 
nuclear@1:     // Get quaternion length.
nuclear@1:     T       Length() const                  { return sqrt(x * x + y * y + z * z + w * w); }
nuclear@1:     // Get quaternion length squared.
nuclear@1:     T       LengthSq() const                { return (x * x + y * y + z * z + w * w); }
nuclear@1:     // Simple Eulidean distance in R^4 (not SLERP distance, but at least respects Haar measure)
nuclear@1:     T       Distance(const Quat& q) const
nuclear@1:     {
nuclear@1:         T d1 = (*this - q).Length();
nuclear@1:         T d2 = (*this + q).Length(); // Antipoldal point check
nuclear@1:         return (d1 < d2) ? d1 : d2;
nuclear@1:     }
nuclear@1:     T       DistanceSq(const Quat& q) const
nuclear@1:     {
nuclear@1:         T d1 = (*this - q).LengthSq();
nuclear@1:         T d2 = (*this + q).LengthSq(); // Antipoldal point check
nuclear@1:         return (d1 < d2) ? d1 : d2;
nuclear@1:     }
nuclear@1: 
nuclear@1:     // Normalize
nuclear@1:     bool    IsNormalized() const            { return fabs(LengthSq() - 1) < Math<T>::Tolerance; }
nuclear@1:     void    Normalize()                     { *this /= Length(); }
nuclear@1:     Quat    Normalized() const              { return *this / Length(); }
nuclear@1: 
nuclear@1:     // Returns conjugate of the quaternion. Produces inverse rotation if quaternion is normalized.
nuclear@1:     Quat    Conj() const                    { return Quat(-x, -y, -z, w); }
nuclear@1: 
nuclear@1:     // AnnaSteve fixed: order of quaternion multiplication
nuclear@1:     // Quaternion multiplication. Combines quaternion rotations, performing the one on the 
nuclear@1:     // right hand side first.
nuclear@1:     Quat  operator* (const Quat& b) const   { return Quat(w * b.x + x * b.w + y * b.z - z * b.y,
nuclear@1:                                                           w * b.y - x * b.z + y * b.w + z * b.x,
nuclear@1:                                                           w * b.z + x * b.y - y * b.x + z * b.w,
nuclear@1:                                                           w * b.w - x * b.x - y * b.y - z * b.z); }
nuclear@1: 
nuclear@1:     // 
nuclear@1:     // this^p normalized; same as rotating by this p times.
nuclear@1:     Quat PowNormalized(T p) const
nuclear@1:     {
nuclear@1:         Vector3<T> v;
nuclear@1:         T          a;
nuclear@1:         GetAxisAngle(&v, &a);
nuclear@1:         return Quat(v, a * p);
nuclear@1:     }
nuclear@1:     
nuclear@1:     // Rotate transforms vector in a manner that matches Matrix rotations (counter-clockwise,
nuclear@1:     // assuming negative direction of the axis). Standard formula: q(t) * V * q(t)^-1. 
nuclear@1:     Vector3<T> Rotate(const Vector3<T>& v) const
nuclear@1:     {
nuclear@1:         return ((*this * Quat<T>(v.x, v.y, v.z, 0)) * Inverted()).Imag();
nuclear@1:     }
nuclear@1: 
nuclear@1:     
nuclear@1:     // Inversed quaternion rotates in the opposite direction.
nuclear@1:     Quat        Inverted() const
nuclear@1:     {
nuclear@1:         return Quat(-x, -y, -z, w);
nuclear@1:     }
nuclear@1: 
nuclear@1:     // Sets this quaternion to the one rotates in the opposite direction.
nuclear@1:     void        Invert()
nuclear@1:     {
nuclear@1:         *this = Quat(-x, -y, -z, w);
nuclear@1:     }
nuclear@1:     
nuclear@1:     // Converting quaternion to matrix.
nuclear@1:     operator Matrix4f() const
nuclear@1:     {
nuclear@1:         T ww = w*w;
nuclear@1:         T xx = x*x;
nuclear@1:         T yy = y*y;
nuclear@1:         T zz = z*z;
nuclear@1: 
nuclear@1:         return Matrix4f(float(ww + xx - yy - zz),  float(T(2) * (x*y - w*z)), float(T(2) * (x*z + w*y)),
nuclear@1:                         float(T(2) * (x*y + w*z)), float(ww - xx + yy - zz),  float(T(2) * (y*z - w*x)),
nuclear@1:                         float(T(2) * (x*z - w*y)), float(T(2) * (y*z + w*x)), float(ww - xx - yy + zz) );
nuclear@1:     }
nuclear@1: 
nuclear@1:     
nuclear@1:     // GetEulerAngles extracts Euler angles from the quaternion, in the specified order of
nuclear@1:     // axis rotations and the specified coordinate system. Right-handed coordinate system
nuclear@1:     // is the default, with CCW rotations while looking in the negative axis direction.
nuclear@1:     // Here a,b,c, are the Yaw/Pitch/Roll angles to be returned.
nuclear@1:     // rotation a around axis A1
nuclear@1:     // is followed by rotation b around axis A2
nuclear@1:     // is followed by rotation c around axis A3
nuclear@1:     // rotations are CCW or CW (D) in LH or RH coordinate system (S)
nuclear@1:     template <Axis A1, Axis A2, Axis A3, RotateDirection D, HandedSystem S>
nuclear@1:     void GetEulerAngles(T *a, T *b, T *c)
nuclear@1:     {
nuclear@1:         OVR_COMPILER_ASSERT((A1 != A2) && (A2 != A3) && (A1 != A3));
nuclear@1: 
nuclear@1:         T Q[3] = { x, y, z };  //Quaternion components x,y,z
nuclear@1: 
nuclear@1:         T ww  = w*w;
nuclear@1:         T Q11 = Q[A1]*Q[A1];
nuclear@1:         T Q22 = Q[A2]*Q[A2];
nuclear@1:         T Q33 = Q[A3]*Q[A3];
nuclear@1: 
nuclear@1:         T psign = T(-1.0);
nuclear@1:         // Determine whether even permutation
nuclear@1:         if (((A1 + 1) % 3 == A2) && ((A2 + 1) % 3 == A3))
nuclear@1:             psign = T(1.0);
nuclear@1:         
nuclear@1:         T s2 = psign * T(2.0) * (psign*w*Q[A2] + Q[A1]*Q[A3]);
nuclear@1: 
nuclear@1:         if (s2 < (T)-1.0 + Math<T>::SingularityRadius)
nuclear@1:         { // South pole singularity
nuclear@1:             *a = T(0.0);
nuclear@1:             *b = -S*D*Math<T>::PiOver2;
nuclear@1:             *c = S*D*atan2((T)2.0*(psign*Q[A1]*Q[A2] + w*Q[A3]),
nuclear@1: 		                   ww + Q22 - Q11 - Q33 );
nuclear@1:         }
nuclear@1:         else if (s2 > (T)1.0 - Math<T>::SingularityRadius)
nuclear@1:         {  // North pole singularity
nuclear@1:             *a = (T)0.0;
nuclear@1:             *b = S*D*Math<T>::PiOver2;
nuclear@1:             *c = S*D*atan2((T)2.0*(psign*Q[A1]*Q[A2] + w*Q[A3]),
nuclear@1: 		                   ww + Q22 - Q11 - Q33);
nuclear@1:         }
nuclear@1:         else
nuclear@1:         {
nuclear@1:             *a = -S*D*atan2((T)-2.0*(w*Q[A1] - psign*Q[A2]*Q[A3]),
nuclear@1: 		                    ww + Q33 - Q11 - Q22);
nuclear@1:             *b = S*D*asin(s2);
nuclear@1:             *c = S*D*atan2((T)2.0*(w*Q[A3] - psign*Q[A1]*Q[A2]),
nuclear@1: 		                   ww + Q11 - Q22 - Q33);
nuclear@1:         }      
nuclear@1:         return;
nuclear@1:     }
nuclear@1: 
nuclear@1:     template <Axis A1, Axis A2, Axis A3, RotateDirection D>
nuclear@1:     void GetEulerAngles(T *a, T *b, T *c)
nuclear@1:     { GetEulerAngles<A1, A2, A3, D, Handed_R>(a, b, c); }
nuclear@1: 
nuclear@1:     template <Axis A1, Axis A2, Axis A3>
nuclear@1:     void GetEulerAngles(T *a, T *b, T *c)
nuclear@1:     { GetEulerAngles<A1, A2, A3, Rotate_CCW, Handed_R>(a, b, c); }
nuclear@1: 
nuclear@1: 
nuclear@1:     // GetEulerAnglesABA extracts Euler angles from the quaternion, in the specified order of
nuclear@1:     // axis rotations and the specified coordinate system. Right-handed coordinate system
nuclear@1:     // is the default, with CCW rotations while looking in the negative axis direction.
nuclear@1:     // Here a,b,c, are the Yaw/Pitch/Roll angles to be returned.
nuclear@1:     // rotation a around axis A1
nuclear@1:     // is followed by rotation b around axis A2
nuclear@1:     // is followed by rotation c around axis A1
nuclear@1:     // Rotations are CCW or CW (D) in LH or RH coordinate system (S)
nuclear@1:     template <Axis A1, Axis A2, RotateDirection D, HandedSystem S>
nuclear@1:     void GetEulerAnglesABA(T *a, T *b, T *c)
nuclear@1:     {
nuclear@1:         OVR_COMPILER_ASSERT(A1 != A2);
nuclear@1: 
nuclear@1:         T Q[3] = {x, y, z}; // Quaternion components
nuclear@1: 
nuclear@1:         // Determine the missing axis that was not supplied
nuclear@1:         int m = 3 - A1 - A2;
nuclear@1: 
nuclear@1:         T ww = w*w;
nuclear@1:         T Q11 = Q[A1]*Q[A1];
nuclear@1:         T Q22 = Q[A2]*Q[A2];
nuclear@1:         T Qmm = Q[m]*Q[m];
nuclear@1: 
nuclear@1:         T psign = T(-1.0);
nuclear@1:         if ((A1 + 1) % 3 == A2) // Determine whether even permutation
nuclear@1:         {
nuclear@1:             psign = (T)1.0;
nuclear@1:         }
nuclear@1: 
nuclear@1:         T c2 = ww + Q11 - Q22 - Qmm;
nuclear@1:         if (c2 < (T)-1.0 + Math<T>::SingularityRadius)
nuclear@1:         { // South pole singularity
nuclear@1:             *a = (T)0.0;
nuclear@1:             *b = S*D*Math<T>::Pi;
nuclear@1:             *c = S*D*atan2( (T)2.0*(w*Q[A1] - psign*Q[A2]*Q[m]),
nuclear@1: 		                    ww + Q22 - Q11 - Qmm);
nuclear@1:         }
nuclear@1:         else if (c2 > (T)1.0 - Math<T>::SingularityRadius)
nuclear@1:         {  // North pole singularity
nuclear@1:             *a = (T)0.0;
nuclear@1:             *b = (T)0.0;
nuclear@1:             *c = S*D*atan2( (T)2.0*(w*Q[A1] - psign*Q[A2]*Q[m]),
nuclear@1: 		                   ww + Q22 - Q11 - Qmm);
nuclear@1:         }
nuclear@1:         else
nuclear@1:         {
nuclear@1:             *a = S*D*atan2( psign*w*Q[m] + Q[A1]*Q[A2],
nuclear@1: 		                   w*Q[A2] -psign*Q[A1]*Q[m]);
nuclear@1:             *b = S*D*acos(c2);
nuclear@1:             *c = S*D*atan2( -psign*w*Q[m] + Q[A1]*Q[A2],
nuclear@1: 		                   w*Q[A2] + psign*Q[A1]*Q[m]);
nuclear@1:         }
nuclear@1:         return;
nuclear@1:     }
nuclear@1: };
nuclear@1: 
nuclear@1: 
nuclear@1: typedef Quat<float>  Quatf;
nuclear@1: typedef Quat<double> Quatd;
nuclear@1: 
nuclear@1: 
nuclear@1: 
nuclear@1: //-------------------------------------------------------------------------------------
nuclear@1: // ***** Angle
nuclear@1: 
nuclear@1: // Cleanly representing the algebra of 2D rotations.
nuclear@1: // The operations maintain the angle between -Pi and Pi, the same range as atan2.
nuclear@1: // 
nuclear@1: 
nuclear@1: template<class T>
nuclear@1: class Angle
nuclear@1: {
nuclear@1: public:
nuclear@1: 	enum AngularUnits
nuclear@1: 	{
nuclear@1: 		Radians = 0,
nuclear@1: 		Degrees = 1
nuclear@1: 	};
nuclear@1: 
nuclear@1:     Angle() : a(0) {}
nuclear@1:     
nuclear@1: 	// Fix the range to be between -Pi and Pi
nuclear@1: 	Angle(T a_, AngularUnits u = Radians) : a((u == Radians) ? a_ : a_*Math<T>::DegreeToRadFactor) { FixRange(); }
nuclear@1: 
nuclear@1: 	T    Get(AngularUnits u = Radians) const       { return (u == Radians) ? a : a*Math<T>::RadToDegreeFactor; }
nuclear@1: 	void Set(const T& x, AngularUnits u = Radians) { a = (u == Radians) ? x : x*Math<T>::DegreeToRadFactor; FixRange(); }
nuclear@1: 	int Sign() const                               { if (a == 0) return 0; else return (a > 0) ? 1 : -1; }
nuclear@1: 	T   Abs() const                                { return (a > 0) ? a : -a; }
nuclear@1: 
nuclear@1:     bool operator== (const Angle& b) const    { return a == b.a; }
nuclear@1:     bool operator!= (const Angle& b) const    { return a != b.a; }
nuclear@1: //	bool operator<  (const Angle& b) const    { return a < a.b; } 
nuclear@1: //	bool operator>  (const Angle& b) const    { return a > a.b; } 
nuclear@1: //	bool operator<= (const Angle& b) const    { return a <= a.b; } 
nuclear@1: //	bool operator>= (const Angle& b) const    { return a >= a.b; } 
nuclear@1: //	bool operator= (const T& x)               { a = x; FixRange(); }
nuclear@1: 
nuclear@1: 	// These operations assume a is already between -Pi and Pi.
nuclear@1:     Angle  operator+  (const Angle& b) const  { return Angle(a + b.a); }
nuclear@1: 	Angle  operator+  (const T& x) const      { return Angle(a + x); }
nuclear@1: 	Angle& operator+= (const Angle& b)        { a = a + b.a; FastFixRange(); return *this; }
nuclear@1: 	Angle& operator+= (const T& x)            { a = a + x; FixRange(); return *this; }
nuclear@1: 	Angle  operator-  (const Angle& b) const  { return Angle(a - b.a); }
nuclear@1: 	Angle  operator-  (const T& x) const      { return Angle(a - x); }
nuclear@1: 	Angle& operator-= (const Angle& b)        { a = a - b.a; FastFixRange(); return *this; }
nuclear@1: 	Angle& operator-= (const T& x)            { a = a - x; FixRange(); return *this; }
nuclear@1: 	
nuclear@1: 	T   Distance(const Angle& b)              { T c = fabs(a - b.a); return (c <= Math<T>::Pi) ? c : Math<T>::TwoPi - c; }
nuclear@1: 
nuclear@1: private:
nuclear@1: 
nuclear@1: 	// The stored angle, which should be maintained between -Pi and Pi
nuclear@1: 	T a;
nuclear@1: 
nuclear@1: 	// Fixes the angle range to [-Pi,Pi], but assumes no more than 2Pi away on either side 
nuclear@1: 	inline void FastFixRange()
nuclear@1: 	{
nuclear@1: 		if (a < -Math<T>::Pi)
nuclear@1: 			a += Math<T>::TwoPi;
nuclear@1: 		else if (a > Math<T>::Pi)
nuclear@1: 			a -= Math<T>::TwoPi;
nuclear@1: 	}
nuclear@1: 
nuclear@1: 	// Fixes the angle range to [-Pi,Pi] for any given range, but slower then the fast method
nuclear@1: 	inline void FixRange()
nuclear@1: 	{
nuclear@1: 		a = fmod(a,Math<T>::TwoPi);
nuclear@1: 		if (a < -Math<T>::Pi)
nuclear@1: 			a += Math<T>::TwoPi;
nuclear@1: 		else if (a > Math<T>::Pi)
nuclear@1: 			a -= Math<T>::TwoPi;
nuclear@1: 	}
nuclear@1: };
nuclear@1: 
nuclear@1: 
nuclear@1: typedef Angle<float>  Anglef;
nuclear@1: typedef Angle<double> Angled;
nuclear@1: 
nuclear@1: 
nuclear@1: //-------------------------------------------------------------------------------------
nuclear@1: // ***** Plane
nuclear@1: 
nuclear@1: // Consists of a normal vector and distance from the origin where the plane is located.
nuclear@1: 
nuclear@1: template<class T>
nuclear@1: class Plane : public RefCountBase<Plane<T> >
nuclear@1: {
nuclear@1: public:
nuclear@1:     Vector3<T> N;
nuclear@1:     T          D;
nuclear@1: 
nuclear@1:     Plane() : D(0) {}
nuclear@1: 
nuclear@1:     // Normals must already be normalized
nuclear@1:     Plane(const Vector3<T>& n, T d) : N(n), D(d) {}
nuclear@1:     Plane(T x, T y, T z, T d) : N(x,y,z), D(d) {}
nuclear@1: 
nuclear@1:     // construct from a point on the plane and the normal
nuclear@1:     Plane(const Vector3<T>& p, const Vector3<T>& n) : N(n), D(-(p * n)) {}
nuclear@1: 
nuclear@1:     // Find the point to plane distance. The sign indicates what side of the plane the point is on (0 = point on plane).
nuclear@1:     T TestSide(const Vector3<T>& p) const
nuclear@1:     {
nuclear@1:         return (N * p) + D;
nuclear@1:     }
nuclear@1: 
nuclear@1:     Plane<T> Flipped() const
nuclear@1:     {
nuclear@1:         return Plane(-N, -D);
nuclear@1:     }
nuclear@1: 
nuclear@1:     void Flip()
nuclear@1:     {
nuclear@1:         N = -N;
nuclear@1:         D = -D;
nuclear@1:     }
nuclear@1: 
nuclear@1: 	bool operator==(const Plane<T>& rhs) const
nuclear@1: 	{
nuclear@1: 		return (this->D == rhs.D && this->N == rhs.N);
nuclear@1: 	}
nuclear@1: };
nuclear@1: 
nuclear@1: typedef Plane<float> Planef;
nuclear@1: 
nuclear@1: }
nuclear@1: 
nuclear@1: #endif