1.1 --- /dev/null	Thu Jan 01 00:00:00 1970 +0000
     1.2 +++ b/libovr/Src/Kernel/OVR_Math.h	Sat Sep 14 16:14:59 2013 +0300
     1.3 @@ -0,0 +1,1 @@
     1.4 +/************************************************************************************
     1.5 
     1.6 PublicHeader:   OVR.h
     1.7 Filename    :   OVR_Math.h
     1.8 Content     :   Implementation of 3D primitives such as vectors, matrices.
     1.9 Created     :   September 4, 2012
    1.10 Authors     :   Andrew Reisse, Michael Antonov, Steve LaValle, Anna Yershova
    1.11 
    1.12 Copyright   :   Copyright 2012 Oculus VR, Inc. All Rights reserved.
    1.13 
    1.14 Use of this software is subject to the terms of the Oculus license
    1.15 agreement provided at the time of installation or download, or which
    1.16 otherwise accompanies this software in either electronic or hard copy form.
    1.17 
    1.18 *************************************************************************************/
    1.19 
    1.20 #ifndef OVR_Math_h
    1.21 #define OVR_Math_h
    1.22 
    1.23 #include <assert.h>
    1.24 #include <stdlib.h>
    1.25 #include <math.h>
    1.26 
    1.27 #include "OVR_Types.h"
    1.28 #include "OVR_RefCount.h"
    1.29 
    1.30 namespace OVR {
    1.31 
    1.32 //-------------------------------------------------------------------------------------
    1.33 // Constants for 3D world/axis definitions.
    1.34 
    1.35 // Definitions of axes for coordinate and rotation conversions.
    1.36 enum Axis
    1.37 {
    1.38     Axis_X = 0, Axis_Y = 1, Axis_Z = 2
    1.39 };
    1.40 
    1.41 // RotateDirection describes the rotation direction around an axis, interpreted as follows:
    1.42 //  CW  - Clockwise while looking "down" from positive axis towards the origin.
    1.43 //  CCW - Counter-clockwise while looking from the positive axis towards the origin,
    1.44 //        which is in the negative axis direction.
    1.45 //  CCW is the default for the RHS coordinate system. Oculus standard RHS coordinate
    1.46 //  system defines Y up, X right, and Z back (pointing out from the screen). In this
    1.47 //  system Rotate_CCW around Z will specifies counter-clockwise rotation in XY plane.
    1.48 enum RotateDirection
    1.49 {
    1.50     Rotate_CCW = 1,
    1.51     Rotate_CW  = -1 
    1.52 };
    1.53 
    1.54 enum HandedSystem
    1.55 {
    1.56     Handed_R = 1, Handed_L = -1
    1.57 };
    1.58 
    1.59 // AxisDirection describes which way the axis points. Used by WorldAxes.
    1.60 enum AxisDirection
    1.61 {
    1.62     Axis_Up    =  2,
    1.63     Axis_Down  = -2,
    1.64     Axis_Right =  1,
    1.65     Axis_Left  = -1,
    1.66     Axis_In    =  3,
    1.67     Axis_Out   = -3
    1.68 };
    1.69 
    1.70 struct WorldAxes
    1.71 {
    1.72     AxisDirection XAxis, YAxis, ZAxis;
    1.73 
    1.74     WorldAxes(AxisDirection x, AxisDirection y, AxisDirection z)
    1.75         : XAxis(x), YAxis(y), ZAxis(z) 
    1.76     { OVR_ASSERT(abs(x) != abs(y) && abs(y) != abs(z) && abs(z) != abs(x));}
    1.77 };
    1.78 
    1.79 
    1.80 //-------------------------------------------------------------------------------------
    1.81 // ***** Math
    1.82 
    1.83 // Math class contains constants and functions. This class is a template specialized
    1.84 // per type, with Math<float> and Math<double> being distinct.
    1.85 template<class Type>
    1.86 class Math
    1.87 {  
    1.88 };
    1.89 
    1.90 // Single-precision Math constants class.
    1.91 template<>
    1.92 class Math<float>
    1.93 {
    1.94 public:
    1.95     static const float Pi;
    1.96     static const float TwoPi;
    1.97     static const float PiOver2;
    1.98     static const float PiOver4;
    1.99     static const float E;
   1.100 
   1.101     static const float MaxValue;          // Largest positive float Value
   1.102     static const float MinPositiveValue;  // Smallest possible positive value
   1.103 
   1.104     static const float RadToDegreeFactor;
   1.105     static const float DegreeToRadFactor;
   1.106 
   1.107     static const float Tolerance; //  0.00001f;
   1.108     static const float SingularityRadius; //0.00000000001f for Gimbal lock numerical problems
   1.109 };
   1.110 
   1.111 // Double-precision Math constants class.
   1.112 template<>
   1.113 class Math<double>
   1.114 {
   1.115 public:
   1.116     static const double Pi;
   1.117     static const double TwoPi;
   1.118     static const double PiOver2;
   1.119     static const double PiOver4;
   1.120     static const double E;
   1.121 
   1.122     static const double MaxValue;          // Largest positive double Value
   1.123     static const double MinPositiveValue;  // Smallest possible positive value
   1.124 
   1.125     static const double RadToDegreeFactor;
   1.126     static const double DegreeToRadFactor;
   1.127 
   1.128     static const double Tolerance; //  0.00001f;
   1.129     static const double SingularityRadius; //0.00000000001 for Gimbal lock numerical problems
   1.130 };
   1.131 
   1.132 typedef Math<float>  Mathf;
   1.133 typedef Math<double> Mathd;
   1.134 
   1.135 // Conversion functions between degrees and radians
   1.136 template<class FT>
   1.137 FT RadToDegree(FT rads) { return rads * Math<FT>::RadToDegreeFactor; }
   1.138 template<class FT>
   1.139 FT DegreeToRad(FT rads) { return rads * Math<FT>::DegreeToRadFactor; }
   1.140 
   1.141 template<class T>
   1.142 class Quat;
   1.143 
   1.144 //-------------------------------------------------------------------------------------
   1.145 // ***** Vector2f - 2D Vector2f
   1.146 
   1.147 // Vector2f represents a 2-dimensional vector or point in space,
   1.148 // consisting of coordinates x and y,
   1.149 
   1.150 template<class T>
   1.151 class Vector2
   1.152 {
   1.153 public:
   1.154     T x, y;
   1.155 
   1.156     Vector2() : x(0), y(0) { }
   1.157     Vector2(T x_, T y_) : x(x_), y(y_) { }
   1.158     explicit Vector2(T s) : x(s), y(s) { }
   1.159 
   1.160     bool     operator== (const Vector2& b) const  { return x == b.x && y == b.y; }
   1.161     bool     operator!= (const Vector2& b) const  { return x != b.x || y != b.y; }
   1.162              
   1.163     Vector2  operator+  (const Vector2& b) const  { return Vector2(x + b.x, y + b.y); }
   1.164     Vector2& operator+= (const Vector2& b)        { x += b.x; y += b.y; return *this; }
   1.165     Vector2  operator-  (const Vector2& b) const  { return Vector2(x - b.x, y - b.y); }
   1.166     Vector2& operator-= (const Vector2& b)        { x -= b.x; y -= b.y; return *this; }
   1.167     Vector2  operator- () const                   { return Vector2(-x, -y); }
   1.168 
   1.169     // Scalar multiplication/division scales vector.
   1.170     Vector2  operator*  (T s) const               { return Vector2(x*s, y*s); }
   1.171     Vector2& operator*= (T s)                     { x *= s; y *= s; return *this; }
   1.172 
   1.173     Vector2  operator/  (T s) const               { T rcp = T(1)/s;
   1.174                                                     return Vector2(x*rcp, y*rcp); }
   1.175     Vector2& operator/= (T s)                     { T rcp = T(1)/s;
   1.176                                                     x *= rcp; y *= rcp;
   1.177                                                     return *this; }
   1.178 
   1.179     // Compare two vectors for equality with tolerance. Returns true if vectors match withing tolerance.
   1.180     bool      Compare(const Vector2&b, T tolerance = Mathf::Tolerance)
   1.181     {
   1.182         return (fabs(b.x-x) < tolerance) && (fabs(b.y-y) < tolerance);
   1.183     }
   1.184     
   1.185     // Dot product overload.
   1.186     // Used to calculate angle q between two vectors among other things,
   1.187     // as (A dot B) = |a||b|cos(q).
   1.188     T     operator*  (const Vector2& b) const    { return x*b.x + y*b.y; }
   1.189 
   1.190     // Returns the angle from this vector to b, in radians.
   1.191     T       Angle(const Vector2& b) const        { return acos((*this * b)/(Length()*b.Length())); }
   1.192 
   1.193     // Return Length of the vector squared.
   1.194     T       LengthSq() const                     { return (x * x + y * y); }
   1.195     // Return vector length.
   1.196     T       Length() const                       { return sqrt(LengthSq()); }
   1.197 
   1.198     // Returns distance between two points represented by vectors.
   1.199     T       Distance(Vector2& b) const           { return (*this - b).Length(); }
   1.200     
   1.201     // Determine if this a unit vector.
   1.202     bool    IsNormalized() const                 { return fabs(LengthSq() - T(1)) < Math<T>::Tolerance; }
   1.203     // Normalize, convention vector length to 1.    
   1.204     void    Normalize()                          { *this /= Length(); }
   1.205     // Returns normalized (unit) version of the vector without modifying itself.
   1.206     Vector2 Normalized() const                   { return *this / Length(); }
   1.207 
   1.208     // Linearly interpolates from this vector to another.
   1.209     // Factor should be between 0.0 and 1.0, with 0 giving full value to this.
   1.210     Vector2 Lerp(const Vector2& b, T f) const    { return *this*(T(1) - f) + b*f; }
   1.211 
   1.212     // Projects this vector onto the argument; in other words,
   1.213     // A.Project(B) returns projection of vector A onto B.
   1.214     Vector2 ProjectTo(const Vector2& b) const    { return b * ((*this * b) / b.LengthSq()); }
   1.215 };
   1.216 
   1.217 
   1.218 typedef Vector2<float>  Vector2f;
   1.219 typedef Vector2<double> Vector2d;
   1.220 
   1.221 //-------------------------------------------------------------------------------------
   1.222 // ***** Vector3f - 3D Vector3f
   1.223 
   1.224 // Vector3f represents a 3-dimensional vector or point in space,
   1.225 // consisting of coordinates x, y and z.
   1.226 
   1.227 template<class T>
   1.228 class Vector3
   1.229 {
   1.230 public:
   1.231     T x, y, z;
   1.232 
   1.233     Vector3() : x(0), y(0), z(0) { }
   1.234     Vector3(T x_, T y_, T z_ = 0) : x(x_), y(y_), z(z_) { }
   1.235     explicit Vector3(T s) : x(s), y(s), z(s) { }
   1.236 
   1.237     bool     operator== (const Vector3& b) const  { return x == b.x && y == b.y && z == b.z; }
   1.238     bool     operator!= (const Vector3& b) const  { return x != b.x || y != b.y || z != b.z; }
   1.239              
   1.240     Vector3  operator+  (const Vector3& b) const  { return Vector3(x + b.x, y + b.y, z + b.z); }
   1.241     Vector3& operator+= (const Vector3& b)        { x += b.x; y += b.y; z += b.z; return *this; }
   1.242     Vector3  operator-  (const Vector3& b) const  { return Vector3(x - b.x, y - b.y, z - b.z); }
   1.243     Vector3& operator-= (const Vector3& b)        { x -= b.x; y -= b.y; z -= b.z; return *this; }
   1.244     Vector3  operator- () const                   { return Vector3(-x, -y, -z); }
   1.245 
   1.246     // Scalar multiplication/division scales vector.
   1.247     Vector3  operator*  (T s) const               { return Vector3(x*s, y*s, z*s); }
   1.248     Vector3& operator*= (T s)                     { x *= s; y *= s; z *= s; return *this; }
   1.249 
   1.250     Vector3  operator/  (T s) const               { T rcp = T(1)/s;
   1.251                                                     return Vector3(x*rcp, y*rcp, z*rcp); }
   1.252     Vector3& operator/= (T s)                     { T rcp = T(1)/s;
   1.253                                                     x *= rcp; y *= rcp; z *= rcp;
   1.254                                                     return *this; }
   1.255 
   1.256     // Compare two vectors for equality with tolerance. Returns true if vectors match withing tolerance.
   1.257     bool      Compare(const Vector3&b, T tolerance = Mathf::Tolerance)
   1.258     {
   1.259         return (fabs(b.x-x) < tolerance) && (fabs(b.y-y) < tolerance) && (fabs(b.z-z) < tolerance);
   1.260     }
   1.261     
   1.262     // Dot product overload.
   1.263     // Used to calculate angle q between two vectors among other things,
   1.264     // as (A dot B) = |a||b|cos(q).
   1.265     T     operator*  (const Vector3& b) const    { return x*b.x + y*b.y + z*b.z; }
   1.266 
   1.267     // Compute cross product, which generates a normal vector.
   1.268     // Direction vector can be determined by right-hand rule: Pointing index finder in
   1.269     // direction a and middle finger in direction b, thumb will point in a.Cross(b).
   1.270     Vector3 Cross(const Vector3& b) const        { return Vector3(y*b.z - z*b.y,
   1.271                                                                   z*b.x - x*b.z,
   1.272                                                                   x*b.y - y*b.x); }
   1.273 
   1.274     // Returns the angle from this vector to b, in radians.
   1.275     T       Angle(const Vector3& b) const        { return acos((*this * b)/(Length()*b.Length())); }
   1.276 
   1.277     // Return Length of the vector squared.
   1.278     T       LengthSq() const                     { return (x * x + y * y + z * z); }
   1.279     // Return vector length.
   1.280     T       Length() const                       { return sqrt(LengthSq()); }
   1.281 
   1.282     // Returns distance between two points represented by vectors.
   1.283     T       Distance(Vector3& b) const           { return (*this - b).Length(); }
   1.284     
   1.285     // Determine if this a unit vector.
   1.286     bool    IsNormalized() const                 { return fabs(LengthSq() - T(1)) < Math<T>::Tolerance; }
   1.287     // Normalize, convention vector length to 1.    
   1.288     void    Normalize()                          { *this /= Length(); }
   1.289     // Returns normalized (unit) version of the vector without modifying itself.
   1.290     Vector3 Normalized() const                   { return *this / Length(); }
   1.291 
   1.292     // Linearly interpolates from this vector to another.
   1.293     // Factor should be between 0.0 and 1.0, with 0 giving full value to this.
   1.294     Vector3 Lerp(const Vector3& b, T f) const    { return *this*(T(1) - f) + b*f; }
   1.295 
   1.296     // Projects this vector onto the argument; in other words,
   1.297     // A.Project(B) returns projection of vector A onto B.
   1.298     Vector3 ProjectTo(const Vector3& b) const    { return b * ((*this * b) / b.LengthSq()); }
   1.299 };
   1.300 
   1.301 
   1.302 typedef Vector3<float>  Vector3f;
   1.303 typedef Vector3<double> Vector3d;
   1.304 
   1.305 
   1.306 //-------------------------------------------------------------------------------------
   1.307 // ***** Matrix4f 
   1.308 
   1.309 // Matrix4f is a 4x4 matrix used for 3d transformations and projections.
   1.310 // Translation stored in the last column.
   1.311 // The matrix is stored in row-major order in memory, meaning that values
   1.312 // of the first row are stored before the next one.
   1.313 //
   1.314 // The arrangement of the matrix is chosen to be in Right-Handed 
   1.315 // coordinate system and counterclockwise rotations when looking down
   1.316 // the axis
   1.317 //
   1.318 // Transformation Order:
   1.319 //   - Transformations are applied from right to left, so the expression
   1.320 //     M1 * M2 * M3 * V means that the vector V is transformed by M3 first,
   1.321 //     followed by M2 and M1. 
   1.322 //
   1.323 // Coordinate system: Right Handed
   1.324 //
   1.325 // Rotations: Counterclockwise when looking down the axis. All angles are in radians.
   1.326 //    
   1.327 //  | sx   01   02   tx |    // First column  (sx, 10, 20): Axis X basis vector.
   1.328 //  | 10   sy   12   ty |    // Second column (01, sy, 21): Axis Y basis vector.
   1.329 //  | 20   21   sz   tz |    // Third columnt (02, 12, sz): Axis Z basis vector.
   1.330 //  | 30   31   32   33 |
   1.331 //
   1.332 //  The basis vectors are first three columns.
   1.333 
   1.334 class Matrix4f
   1.335 {
   1.336     static Matrix4f IdentityValue;
   1.337 
   1.338 public:
   1.339     float M[4][4];    
   1.340 
   1.341     enum NoInitType { NoInit };
   1.342 
   1.343     // Construct with no memory initialization.
   1.344     Matrix4f(NoInitType) { }
   1.345 
   1.346     // By default, we construct identity matrix.
   1.347     Matrix4f()
   1.348     {
   1.349         SetIdentity();        
   1.350     }
   1.351 
   1.352     Matrix4f(float m11, float m12, float m13, float m14,
   1.353              float m21, float m22, float m23, float m24,
   1.354              float m31, float m32, float m33, float m34,
   1.355              float m41, float m42, float m43, float m44)
   1.356     {
   1.357         M[0][0] = m11; M[0][1] = m12; M[0][2] = m13; M[0][3] = m14;
   1.358         M[1][0] = m21; M[1][1] = m22; M[1][2] = m23; M[1][3] = m24;
   1.359         M[2][0] = m31; M[2][1] = m32; M[2][2] = m33; M[2][3] = m34;
   1.360         M[3][0] = m41; M[3][1] = m42; M[3][2] = m43; M[3][3] = m44;
   1.361     }
   1.362 
   1.363     Matrix4f(float m11, float m12, float m13,
   1.364              float m21, float m22, float m23,
   1.365              float m31, float m32, float m33)
   1.366     {
   1.367         M[0][0] = m11; M[0][1] = m12; M[0][2] = m13; M[0][3] = 0;
   1.368         M[1][0] = m21; M[1][1] = m22; M[1][2] = m23; M[1][3] = 0;
   1.369         M[2][0] = m31; M[2][1] = m32; M[2][2] = m33; M[2][3] = 0;
   1.370         M[3][0] = 0;   M[3][1] = 0;   M[3][2] = 0;   M[3][3] = 1;
   1.371     }
   1.372 
   1.373     static const Matrix4f& Identity()  { return IdentityValue; }
   1.374 
   1.375     void SetIdentity()
   1.376     {
   1.377         M[0][0] = M[1][1] = M[2][2] = M[3][3] = 1;
   1.378         M[0][1] = M[1][0] = M[2][3] = M[3][1] = 0;
   1.379         M[0][2] = M[1][2] = M[2][0] = M[3][2] = 0;
   1.380         M[0][3] = M[1][3] = M[2][1] = M[3][0] = 0;
   1.381     }
   1.382 
   1.383     // Multiplies two matrices into destination with minimum copying.
   1.384     static Matrix4f& Multiply(Matrix4f* d, const Matrix4f& a, const Matrix4f& b)
   1.385     {
   1.386         OVR_ASSERT((d != &a) && (d != &b));
   1.387         int i = 0;
   1.388         do {
   1.389             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];
   1.390             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];
   1.391             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];
   1.392             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];
   1.393         } while((++i) < 4);
   1.394 
   1.395         return *d;
   1.396     }
   1.397 
   1.398     Matrix4f operator* (const Matrix4f& b) const
   1.399     {
   1.400         Matrix4f result(Matrix4f::NoInit);
   1.401         Multiply(&result, *this, b);
   1.402         return result;
   1.403     }
   1.404 
   1.405     Matrix4f& operator*= (const Matrix4f& b)
   1.406     {
   1.407         return Multiply(this, Matrix4f(*this), b);
   1.408     }
   1.409 
   1.410     Matrix4f operator* (float s) const
   1.411     {
   1.412         return Matrix4f(M[0][0] * s, M[0][1] * s, M[0][2] * s, M[0][3] * s,
   1.413                         M[1][0] * s, M[1][1] * s, M[1][2] * s, M[1][3] * s,
   1.414                         M[2][0] * s, M[2][1] * s, M[2][2] * s, M[2][3] * s,
   1.415                         M[3][0] * s, M[3][1] * s, M[3][2] * s, M[3][3] * s);
   1.416     }
   1.417 
   1.418     Matrix4f& operator*= (float s)
   1.419     {
   1.420         M[0][0] *= s; M[0][1] *= s; M[0][2] *= s; M[0][3] *= s;
   1.421         M[1][0] *= s; M[1][1] *= s; M[1][2] *= s; M[1][3] *= s;
   1.422         M[2][0] *= s; M[2][1] *= s; M[2][2] *= s; M[2][3] *= s;
   1.423         M[3][0] *= s; M[3][1] *= s; M[3][2] *= s; M[3][3] *= s;
   1.424         return *this;
   1.425     }
   1.426 
   1.427     Vector3f Transform(const Vector3f& v) const
   1.428     {
   1.429         return Vector3f(M[0][0] * v.x + M[0][1] * v.y + M[0][2] * v.z + M[0][3],
   1.430                         M[1][0] * v.x + M[1][1] * v.y + M[1][2] * v.z + M[1][3],
   1.431                         M[2][0] * v.x + M[2][1] * v.y + M[2][2] * v.z + M[2][3]);
   1.432     }
   1.433 
   1.434     Matrix4f Transposed() const
   1.435     {
   1.436         return Matrix4f(M[0][0], M[1][0], M[2][0], M[3][0],
   1.437                         M[0][1], M[1][1], M[2][1], M[3][1],
   1.438                         M[0][2], M[1][2], M[2][2], M[3][2],
   1.439                         M[0][3], M[1][3], M[2][3], M[3][3]);
   1.440     }
   1.441 
   1.442     void     Transpose()
   1.443     {
   1.444         *this = Transposed();
   1.445     }
   1.446 
   1.447 
   1.448     float SubDet (const int* rows, const int* cols) const
   1.449     {
   1.450         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]])
   1.451              - 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]])
   1.452              + 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]]);
   1.453     }
   1.454 
   1.455     float Cofactor(int I, int J) const
   1.456     {
   1.457         const int indices[4][3] = {{1,2,3},{0,2,3},{0,1,3},{0,1,2}};
   1.458         return ((I+J)&1) ? -SubDet(indices[I],indices[J]) : SubDet(indices[I],indices[J]);
   1.459     }
   1.460 
   1.461     float    Determinant() const
   1.462     {
   1.463         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);
   1.464     }
   1.465 
   1.466     Matrix4f Adjugated() const
   1.467     {
   1.468         return Matrix4f(Cofactor(0,0), Cofactor(1,0), Cofactor(2,0), Cofactor(3,0), 
   1.469                         Cofactor(0,1), Cofactor(1,1), Cofactor(2,1), Cofactor(3,1), 
   1.470                         Cofactor(0,2), Cofactor(1,2), Cofactor(2,2), Cofactor(3,2),
   1.471                         Cofactor(0,3), Cofactor(1,3), Cofactor(2,3), Cofactor(3,3));
   1.472     }
   1.473 
   1.474     Matrix4f Inverted() const
   1.475     {
   1.476         float det = Determinant();
   1.477         assert(det != 0);
   1.478         return Adjugated() * (1.0f/det);
   1.479     }
   1.480 
   1.481     void Invert()
   1.482     {
   1.483         *this = Inverted();
   1.484     }
   1.485 
   1.486     //AnnaSteve:
   1.487     // a,b,c, are the YawPitchRoll angles to be returned
   1.488     // rotation a around axis A1
   1.489     // is followed by rotation b around axis A2
   1.490     // is followed by rotation c around axis A3
   1.491     // rotations are CCW or CW (D) in LH or RH coordinate system (S)
   1.492     template <Axis A1, Axis A2, Axis A3, RotateDirection D, HandedSystem S>
   1.493     void ToEulerAngles(float *a, float *b, float *c)
   1.494     {
   1.495         OVR_COMPILER_ASSERT((A1 != A2) && (A2 != A3) && (A1 != A3));
   1.496 
   1.497         float psign = -1.0f;
   1.498         if (((A1 + 1) % 3 == A2) && ((A2 + 1) % 3 == A3)) // Determine whether even permutation
   1.499         psign = 1.0f;
   1.500         
   1.501         float pm = psign*M[A1][A3];
   1.502         if (pm < -1.0f + Math<float>::SingularityRadius)
   1.503         { // South pole singularity
   1.504             *a = 0.0f;
   1.505             *b = -S*D*Math<float>::PiOver2;
   1.506             *c = S*D*atan2( psign*M[A2][A1], M[A2][A2] );
   1.507         }
   1.508         else if (pm > 1.0 - Math<float>::SingularityRadius)
   1.509         { // North pole singularity
   1.510             *a = 0.0f;
   1.511             *b = S*D*Math<float>::PiOver2;
   1.512             *c = S*D*atan2( psign*M[A2][A1], M[A2][A2] );
   1.513         }
   1.514         else
   1.515         { // Normal case (nonsingular)
   1.516             *a = S*D*atan2( -psign*M[A2][A3], M[A3][A3] );
   1.517             *b = S*D*asin(pm);
   1.518             *c = S*D*atan2( -psign*M[A1][A2], M[A1][A1] );
   1.519         }
   1.520 
   1.521         return;
   1.522     }
   1.523 
   1.524     //AnnaSteve:
   1.525     // a,b,c, are the YawPitchRoll angles to be returned
   1.526     // rotation a around axis A1
   1.527     // is followed by rotation b around axis A2
   1.528     // is followed by rotation c around axis A1
   1.529     // rotations are CCW or CW (D) in LH or RH coordinate system (S)
   1.530     template <Axis A1, Axis A2, RotateDirection D, HandedSystem S>
   1.531     void ToEulerAnglesABA(float *a, float *b, float *c)
   1.532     {        
   1.533          OVR_COMPILER_ASSERT(A1 != A2);
   1.534   
   1.535         // Determine the axis that was not supplied
   1.536         int m = 3 - A1 - A2;
   1.537 
   1.538         float psign = -1.0f;
   1.539         if ((A1 + 1) % 3 == A2) // Determine whether even permutation
   1.540             psign = 1.0f;
   1.541 
   1.542         float c2 = M[A1][A1];
   1.543         if (c2 < -1.0 + Math<float>::SingularityRadius)
   1.544         { // South pole singularity
   1.545             *a = 0.0f;
   1.546             *b = S*D*Math<float>::Pi;
   1.547             *c = S*D*atan2( -psign*M[A2][m],M[A2][A2]);
   1.548         }
   1.549         else if (c2 > 1.0 - Math<float>::SingularityRadius)
   1.550         { // North pole singularity
   1.551             *a = 0.0f;
   1.552             *b = 0.0f;
   1.553             *c = S*D*atan2( -psign*M[A2][m],M[A2][A2]);
   1.554         }
   1.555         else
   1.556         { // Normal case (nonsingular)
   1.557             *a = S*D*atan2( M[A2][A1],-psign*M[m][A1]);
   1.558             *b = S*D*acos(c2);
   1.559             *c = S*D*atan2( M[A1][A2],psign*M[A1][m]);
   1.560         }
   1.561         return;
   1.562     }
   1.563   
   1.564     // Creates a matrix that converts the vertices from one coordinate system
   1.565     // to another.
   1.566     // 
   1.567     static Matrix4f AxisConversion(const WorldAxes& to, const WorldAxes& from)
   1.568     {        
   1.569         // Holds axis values from the 'to' structure
   1.570         int toArray[3] = { to.XAxis, to.YAxis, to.ZAxis };
   1.571 
   1.572         // The inverse of the toArray
   1.573         int inv[4]; 
   1.574         inv[0] = inv[abs(to.XAxis)] = 0;
   1.575         inv[abs(to.YAxis)] = 1;
   1.576         inv[abs(to.ZAxis)] = 2;
   1.577 
   1.578         Matrix4f m(0,  0,  0, 
   1.579                    0,  0,  0,
   1.580                    0,  0,  0);
   1.581 
   1.582         // Only three values in the matrix need to be changed to 1 or -1.
   1.583         m.M[inv[abs(from.XAxis)]][0] = float(from.XAxis/toArray[inv[abs(from.XAxis)]]);
   1.584         m.M[inv[abs(from.YAxis)]][1] = float(from.YAxis/toArray[inv[abs(from.YAxis)]]);
   1.585         m.M[inv[abs(from.ZAxis)]][2] = float(from.ZAxis/toArray[inv[abs(from.ZAxis)]]);
   1.586         return m;
   1.587     } 
   1.588 
   1.589 
   1.590 
   1.591     static Matrix4f Translation(const Vector3f& v)
   1.592     {
   1.593         Matrix4f t;
   1.594         t.M[0][3] = v.x;
   1.595         t.M[1][3] = v.y;
   1.596         t.M[2][3] = v.z;
   1.597         return t;
   1.598     }
   1.599 
   1.600     static Matrix4f Translation(float x, float y, float z = 0.0f)
   1.601     {
   1.602         Matrix4f t;
   1.603         t.M[0][3] = x;
   1.604         t.M[1][3] = y;
   1.605         t.M[2][3] = z;
   1.606         return t;
   1.607     }
   1.608 
   1.609     static Matrix4f Scaling(const Vector3f& v)
   1.610     {
   1.611         Matrix4f t;
   1.612         t.M[0][0] = v.x;
   1.613         t.M[1][1] = v.y;
   1.614         t.M[2][2] = v.z;
   1.615         return t;
   1.616     }
   1.617 
   1.618     static Matrix4f Scaling(float x, float y, float z)
   1.619     {
   1.620         Matrix4f t;
   1.621         t.M[0][0] = x;
   1.622         t.M[1][1] = y;
   1.623         t.M[2][2] = z;
   1.624         return t;
   1.625     }
   1.626 
   1.627     static Matrix4f Scaling(float s)
   1.628     {
   1.629         Matrix4f t;
   1.630         t.M[0][0] = s;
   1.631         t.M[1][1] = s;
   1.632         t.M[2][2] = s;
   1.633         return t;
   1.634     }
   1.635 
   1.636   
   1.637 
   1.638     //AnnaSteve : Just for quick testing.  Not for final API.  Need to remove case.
   1.639     static Matrix4f RotationAxis(Axis A, float angle, RotateDirection d, HandedSystem s)
   1.640     {
   1.641         float sina = s * d *sin(angle);
   1.642         float cosa = cos(angle);
   1.643         
   1.644         switch(A)
   1.645         {
   1.646         case Axis_X:
   1.647             return Matrix4f(1,  0,     0, 
   1.648                             0,  cosa,  -sina,
   1.649                             0,  sina,  cosa);
   1.650         case Axis_Y:
   1.651             return Matrix4f(cosa,  0,   sina, 
   1.652                             0,     1,   0,
   1.653                             -sina, 0,   cosa);
   1.654         case Axis_Z:
   1.655             return Matrix4f(cosa,  -sina,  0, 
   1.656                             sina,  cosa,   0,
   1.657                             0,     0,      1);
   1.658         }
   1.659     }
   1.660 
   1.661 
   1.662     // Creates a rotation matrix rotating around the X axis by 'angle' radians.
   1.663     // Rotation direction is depends on the coordinate system:
   1.664     //  RHS (Oculus default): Positive angle values rotate Counter-clockwise (CCW),
   1.665     //                        while looking in the negative axis direction. This is the
   1.666     //                        same as looking down from positive axis values towards origin.
   1.667     //  LHS: Positive angle values rotate clock-wise (CW), while looking in the
   1.668     //       negative axis direction.
   1.669     static Matrix4f RotationX(float angle)
   1.670     {
   1.671         float sina = sin(angle);
   1.672         float cosa = cos(angle);
   1.673         return Matrix4f(1,  0,     0, 
   1.674                         0,  cosa,  -sina,
   1.675                         0,  sina,  cosa);
   1.676     }
   1.677 
   1.678     // Creates a rotation matrix rotating around the Y axis by 'angle' radians.
   1.679     // Rotation direction is depends on the coordinate system:
   1.680     //  RHS (Oculus default): Positive angle values rotate Counter-clockwise (CCW),
   1.681     //                        while looking in the negative axis direction. This is the
   1.682     //                        same as looking down from positive axis values towards origin.
   1.683     //  LHS: Positive angle values rotate clock-wise (CW), while looking in the
   1.684     //       negative axis direction.
   1.685     static Matrix4f RotationY(float angle)
   1.686     {
   1.687         float sina = sin(angle);
   1.688         float cosa = cos(angle);
   1.689         return Matrix4f(cosa,  0,   sina, 
   1.690                         0,     1,   0,
   1.691                         -sina, 0,   cosa);
   1.692     }
   1.693 
   1.694     // Creates a rotation matrix rotating around the Z axis by 'angle' radians.
   1.695     // Rotation direction is depends on the coordinate system:
   1.696     //  RHS (Oculus default): Positive angle values rotate Counter-clockwise (CCW),
   1.697     //                        while looking in the negative axis direction. This is the
   1.698     //                        same as looking down from positive axis values towards origin.
   1.699     //  LHS: Positive angle values rotate clock-wise (CW), while looking in the
   1.700     //       negative axis direction.
   1.701     static Matrix4f RotationZ(float angle)
   1.702     {
   1.703         float sina = sin(angle);
   1.704         float cosa = cos(angle);
   1.705         return Matrix4f(cosa,  -sina,  0, 
   1.706                         sina,  cosa,   0,
   1.707                         0,     0,      1);
   1.708     }
   1.709 
   1.710 
   1.711     // LookAtRH creates a View transformation matrix for right-handed coordinate system.
   1.712     // The resulting matrix points camera from 'eye' towards 'at' direction, with 'up'
   1.713     // specifying the up vector. The resulting matrix should be used with PerspectiveRH
   1.714     // projection.
   1.715     static Matrix4f LookAtRH(const Vector3f& eye, const Vector3f& at, const Vector3f& up);
   1.716 
   1.717     // LookAtLH creates a View transformation matrix for left-handed coordinate system.
   1.718     // The resulting matrix points camera from 'eye' towards 'at' direction, with 'up'
   1.719     // specifying the up vector. 
   1.720     static Matrix4f LookAtLH(const Vector3f& eye, const Vector3f& at, const Vector3f& up);
   1.721     
   1.722     
   1.723     // PerspectiveRH creates a right-handed perspective projection matrix that can be
   1.724     // used with the Oculus sample renderer. 
   1.725     //  yfov   - Specifies vertical field of view in radians.
   1.726     //  aspect - Screen aspect ration, which is usually width/height for square pixels.
   1.727     //           Note that xfov = yfov * aspect.
   1.728     //  znear  - Absolute value of near Z clipping clipping range.
   1.729     //  zfar   - Absolute value of far  Z clipping clipping range (larger then near).
   1.730     // Even though RHS usually looks in the direction of negative Z, positive values
   1.731     // are expected for znear and zfar.
   1.732     static Matrix4f PerspectiveRH(float yfov, float aspect, float znear, float zfar);
   1.733     
   1.734     
   1.735     // PerspectiveRH creates a left-handed perspective projection matrix that can be
   1.736     // used with the Oculus sample renderer. 
   1.737     //  yfov   - Specifies vertical field of view in radians.
   1.738     //  aspect - Screen aspect ration, which is usually width/height for square pixels.
   1.739     //           Note that xfov = yfov * aspect.
   1.740     //  znear  - Absolute value of near Z clipping clipping range.
   1.741     //  zfar   - Absolute value of far  Z clipping clipping range (larger then near).
   1.742     static Matrix4f PerspectiveLH(float yfov, float aspect, float znear, float zfar);
   1.743 
   1.744 
   1.745     static Matrix4f Ortho2D(float w, float h);
   1.746 };
   1.747 
   1.748 
   1.749 //-------------------------------------------------------------------------------------
   1.750 // ***** Quat
   1.751 
   1.752 // Quatf represents a quaternion class used for rotations.
   1.753 // 
   1.754 // Quaternion multiplications are done in right-to-left order, to match the
   1.755 // behavior of matrices.
   1.756 
   1.757 
   1.758 template<class T>
   1.759 class Quat
   1.760 {
   1.761 public:
   1.762     // w + Xi + Yj + Zk
   1.763     T x, y, z, w;    
   1.764 
   1.765     Quat() : x(0), y(0), z(0), w(1) {}
   1.766     Quat(T x_, T y_, T z_, T w_) : x(x_), y(y_), z(z_), w(w_) {}
   1.767 
   1.768 
   1.769     // Constructs rotation quaternion around the axis.
   1.770     Quat(const Vector3<T>& axis, T angle)
   1.771     {
   1.772         Vector3<T> unitAxis = axis.Normalized();
   1.773         T          sinHalfAngle = sin(angle * T(0.5));
   1.774 
   1.775         w = cos(angle * T(0.5));
   1.776         x = unitAxis.x * sinHalfAngle;
   1.777         y = unitAxis.y * sinHalfAngle;
   1.778         z = unitAxis.z * sinHalfAngle;
   1.779     }
   1.780 
   1.781     //AnnaSteve:
   1.782     void AxisAngle(Axis A, T angle, RotateDirection d, HandedSystem s)
   1.783     {
   1.784         T sinHalfAngle = s * d *sin(angle * (T)0.5);
   1.785         T v[3];
   1.786         v[0] = v[1] = v[2] = (T)0;
   1.787         v[A] = sinHalfAngle;
   1.788         //return Quat(v[0], v[1], v[2], cos(angle * (T)0.5));
   1.789         w = cos(angle * (T)0.5);
   1.790         x = v[0];
   1.791         y = v[1];
   1.792         z = v[2];
   1.793     }
   1.794 
   1.795 
   1.796     void GetAxisAngle(Vector3<T>* axis, T* angle) const
   1.797     {
   1.798         if (LengthSq() > Math<T>::Tolerance * Math<T>::Tolerance)
   1.799         {
   1.800             *axis  = Vector3<T>(x, y, z).Normalized();
   1.801             *angle = 2 * acos(w);
   1.802         }
   1.803         else
   1.804         {
   1.805             *axis = Vector3<T>(1, 0, 0);
   1.806             *angle= 0;
   1.807         }
   1.808     }
   1.809 
   1.810     bool operator== (const Quat& b) const   { return x == b.x && y == b.y && z == b.z && w == b.w; }
   1.811     bool operator!= (const Quat& b) const   { return x != b.x || y != b.y || z != b.z || w != b.w; }
   1.812 
   1.813     Quat  operator+  (const Quat& b) const  { return Quat(x + b.x, y + b.y, z + b.z, w + b.w); }
   1.814     Quat& operator+= (const Quat& b)        { w += b.w; x += b.x; y += b.y; z += b.z; return *this; }
   1.815     Quat  operator-  (const Quat& b) const  { return Quat(x - b.x, y - b.y, z - b.z, w - b.w); }
   1.816     Quat& operator-= (const Quat& b)        { w -= b.w; x -= b.x; y -= b.y; z -= b.z; return *this; }
   1.817 
   1.818     Quat  operator*  (T s) const            { return Quat(x * s, y * s, z * s, w * s); }
   1.819     Quat& operator*= (T s)                  { w *= s; x *= s; y *= s; z *= s; return *this; }
   1.820     Quat  operator/  (T s) const            { T rcp = T(1)/s; return Quat(x * rcp, y * rcp, z * rcp, w *rcp); }
   1.821     Quat& operator/= (T s)                  { T rcp = T(1)/s; w *= rcp; x *= rcp; y *= rcp; z *= rcp; return *this; }
   1.822 
   1.823     // Get Imaginary part vector
   1.824     Vector3<T> Imag() const                 { return Vector3<T>(x,y,z); }
   1.825 
   1.826     // Get quaternion length.
   1.827     T       Length() const                  { return sqrt(x * x + y * y + z * z + w * w); }
   1.828     // Get quaternion length squared.
   1.829     T       LengthSq() const                { return (x * x + y * y + z * z + w * w); }
   1.830     // Simple Eulidean distance in R^4 (not SLERP distance, but at least respects Haar measure)
   1.831     T       Distance(const Quat& q) const
   1.832     {
   1.833         T d1 = (*this - q).Length();
   1.834         T d2 = (*this + q).Length(); // Antipoldal point check
   1.835         return (d1 < d2) ? d1 : d2;
   1.836     }
   1.837     T       DistanceSq(const Quat& q) const
   1.838     {
   1.839         T d1 = (*this - q).LengthSq();
   1.840         T d2 = (*this + q).LengthSq(); // Antipoldal point check
   1.841         return (d1 < d2) ? d1 : d2;
   1.842     }
   1.843 
   1.844     // Normalize
   1.845     bool    IsNormalized() const            { return fabs(LengthSq() - 1) < Math<T>::Tolerance; }
   1.846     void    Normalize()                     { *this /= Length(); }
   1.847     Quat    Normalized() const              { return *this / Length(); }
   1.848 
   1.849     // Returns conjugate of the quaternion. Produces inverse rotation if quaternion is normalized.
   1.850     Quat    Conj() const                    { return Quat(-x, -y, -z, w); }
   1.851 
   1.852     // AnnaSteve fixed: order of quaternion multiplication
   1.853     // Quaternion multiplication. Combines quaternion rotations, performing the one on the 
   1.854     // right hand side first.
   1.855     Quat  operator* (const Quat& b) const   { return Quat(w * b.x + x * b.w + y * b.z - z * b.y,
   1.856                                                           w * b.y - x * b.z + y * b.w + z * b.x,
   1.857                                                           w * b.z + x * b.y - y * b.x + z * b.w,
   1.858                                                           w * b.w - x * b.x - y * b.y - z * b.z); }
   1.859 
   1.860     // 
   1.861     // this^p normalized; same as rotating by this p times.
   1.862     Quat PowNormalized(T p) const
   1.863     {
   1.864         Vector3<T> v;
   1.865         T          a;
   1.866         GetAxisAngle(&v, &a);
   1.867         return Quat(v, a * p);
   1.868     }
   1.869     
   1.870     // Rotate transforms vector in a manner that matches Matrix rotations (counter-clockwise,
   1.871     // assuming negative direction of the axis). Standard formula: q(t) * V * q(t)^-1. 
   1.872     Vector3<T> Rotate(const Vector3<T>& v) const
   1.873     {
   1.874         return ((*this * Quat<T>(v.x, v.y, v.z, 0)) * Inverted()).Imag();
   1.875     }
   1.876 
   1.877     
   1.878     // Inversed quaternion rotates in the opposite direction.
   1.879     Quat        Inverted() const
   1.880     {
   1.881         return Quat(-x, -y, -z, w);
   1.882     }
   1.883 
   1.884     // Sets this quaternion to the one rotates in the opposite direction.
   1.885     void        Invert()
   1.886     {
   1.887         *this = Quat(-x, -y, -z, w);
   1.888     }
   1.889     
   1.890     // Converting quaternion to matrix.
   1.891     operator Matrix4f() const
   1.892     {
   1.893         T ww = w*w;
   1.894         T xx = x*x;
   1.895         T yy = y*y;
   1.896         T zz = z*z;
   1.897 
   1.898         return Matrix4f(float(ww + xx - yy - zz),  float(T(2) * (x*y - w*z)), float(T(2) * (x*z + w*y)),
   1.899                         float(T(2) * (x*y + w*z)), float(ww - xx + yy - zz),  float(T(2) * (y*z - w*x)),
   1.900                         float(T(2) * (x*z - w*y)), float(T(2) * (y*z + w*x)), float(ww - xx - yy + zz) );
   1.901     }
   1.902 
   1.903     
   1.904     // GetEulerAngles extracts Euler angles from the quaternion, in the specified order of
   1.905     // axis rotations and the specified coordinate system. Right-handed coordinate system
   1.906     // is the default, with CCW rotations while looking in the negative axis direction.
   1.907     // Here a,b,c, are the Yaw/Pitch/Roll angles to be returned.
   1.908     // rotation a around axis A1
   1.909     // is followed by rotation b around axis A2
   1.910     // is followed by rotation c around axis A3
   1.911     // rotations are CCW or CW (D) in LH or RH coordinate system (S)
   1.912     template <Axis A1, Axis A2, Axis A3, RotateDirection D, HandedSystem S>
   1.913     void GetEulerAngles(T *a, T *b, T *c)
   1.914     {
   1.915         OVR_COMPILER_ASSERT((A1 != A2) && (A2 != A3) && (A1 != A3));
   1.916 
   1.917         T Q[3] = { x, y, z };  //Quaternion components x,y,z
   1.918 
   1.919         T ww  = w*w;
   1.920         T Q11 = Q[A1]*Q[A1];
   1.921         T Q22 = Q[A2]*Q[A2];
   1.922         T Q33 = Q[A3]*Q[A3];
   1.923 
   1.924         T psign = T(-1.0);
   1.925         // Determine whether even permutation
   1.926         if (((A1 + 1) % 3 == A2) && ((A2 + 1) % 3 == A3))
   1.927             psign = T(1.0);
   1.928         
   1.929         T s2 = psign * T(2.0) * (psign*w*Q[A2] + Q[A1]*Q[A3]);
   1.930 
   1.931         if (s2 < (T)-1.0 + Math<T>::SingularityRadius)
   1.932         { // South pole singularity
   1.933             *a = T(0.0);
   1.934             *b = -S*D*Math<T>::PiOver2;
   1.935             *c = S*D*atan2((T)2.0*(psign*Q[A1]*Q[A2] + w*Q[A3]),
   1.936 		                   ww + Q22 - Q11 - Q33 );
   1.937         }
   1.938         else if (s2 > (T)1.0 - Math<T>::SingularityRadius)
   1.939         {  // North pole singularity
   1.940             *a = (T)0.0;
   1.941             *b = S*D*Math<T>::PiOver2;
   1.942             *c = S*D*atan2((T)2.0*(psign*Q[A1]*Q[A2] + w*Q[A3]),
   1.943 		                   ww + Q22 - Q11 - Q33);
   1.944         }
   1.945         else
   1.946         {
   1.947             *a = -S*D*atan2((T)-2.0*(w*Q[A1] - psign*Q[A2]*Q[A3]),
   1.948 		                    ww + Q33 - Q11 - Q22);
   1.949             *b = S*D*asin(s2);
   1.950             *c = S*D*atan2((T)2.0*(w*Q[A3] - psign*Q[A1]*Q[A2]),
   1.951 		                   ww + Q11 - Q22 - Q33);
   1.952         }      
   1.953         return;
   1.954     }
   1.955 
   1.956     template <Axis A1, Axis A2, Axis A3, RotateDirection D>
   1.957     void GetEulerAngles(T *a, T *b, T *c)
   1.958     { GetEulerAngles<A1, A2, A3, D, Handed_R>(a, b, c); }
   1.959 
   1.960     template <Axis A1, Axis A2, Axis A3>
   1.961     void GetEulerAngles(T *a, T *b, T *c)
   1.962     { GetEulerAngles<A1, A2, A3, Rotate_CCW, Handed_R>(a, b, c); }
   1.963 
   1.964 
   1.965     // GetEulerAnglesABA extracts Euler angles from the quaternion, in the specified order of
   1.966     // axis rotations and the specified coordinate system. Right-handed coordinate system
   1.967     // is the default, with CCW rotations while looking in the negative axis direction.
   1.968     // Here a,b,c, are the Yaw/Pitch/Roll angles to be returned.
   1.969     // rotation a around axis A1
   1.970     // is followed by rotation b around axis A2
   1.971     // is followed by rotation c around axis A1
   1.972     // Rotations are CCW or CW (D) in LH or RH coordinate system (S)
   1.973     template <Axis A1, Axis A2, RotateDirection D, HandedSystem S>
   1.974     void GetEulerAnglesABA(T *a, T *b, T *c)
   1.975     {
   1.976         OVR_COMPILER_ASSERT(A1 != A2);
   1.977 
   1.978         T Q[3] = {x, y, z}; // Quaternion components
   1.979 
   1.980         // Determine the missing axis that was not supplied
   1.981         int m = 3 - A1 - A2;
   1.982 
   1.983         T ww = w*w;
   1.984         T Q11 = Q[A1]*Q[A1];
   1.985         T Q22 = Q[A2]*Q[A2];
   1.986         T Qmm = Q[m]*Q[m];
   1.987 
   1.988         T psign = T(-1.0);
   1.989         if ((A1 + 1) % 3 == A2) // Determine whether even permutation
   1.990         {
   1.991             psign = (T)1.0;
   1.992         }
   1.993 
   1.994         T c2 = ww + Q11 - Q22 - Qmm;
   1.995         if (c2 < (T)-1.0 + Math<T>::SingularityRadius)
   1.996         { // South pole singularity
   1.997             *a = (T)0.0;
   1.998             *b = S*D*Math<T>::Pi;
   1.999             *c = S*D*atan2( (T)2.0*(w*Q[A1] - psign*Q[A2]*Q[m]),
  1.1000 		                    ww + Q22 - Q11 - Qmm);
  1.1001         }
  1.1002         else if (c2 > (T)1.0 - Math<T>::SingularityRadius)
  1.1003         {  // North pole singularity
  1.1004             *a = (T)0.0;
  1.1005             *b = (T)0.0;
  1.1006             *c = S*D*atan2( (T)2.0*(w*Q[A1] - psign*Q[A2]*Q[m]),
  1.1007 		                   ww + Q22 - Q11 - Qmm);
  1.1008         }
  1.1009         else
  1.1010         {
  1.1011             *a = S*D*atan2( psign*w*Q[m] + Q[A1]*Q[A2],
  1.1012 		                   w*Q[A2] -psign*Q[A1]*Q[m]);
  1.1013             *b = S*D*acos(c2);
  1.1014             *c = S*D*atan2( -psign*w*Q[m] + Q[A1]*Q[A2],
  1.1015 		                   w*Q[A2] + psign*Q[A1]*Q[m]);
  1.1016         }
  1.1017         return;
  1.1018     }
  1.1019 };
  1.1020 
  1.1021 
  1.1022 typedef Quat<float>  Quatf;
  1.1023 typedef Quat<double> Quatd;
  1.1024 
  1.1025 
  1.1026 
  1.1027 //-------------------------------------------------------------------------------------
  1.1028 // ***** Angle
  1.1029 
  1.1030 // Cleanly representing the algebra of 2D rotations.
  1.1031 // The operations maintain the angle between -Pi and Pi, the same range as atan2.
  1.1032 // 
  1.1033 
  1.1034 template<class T>
  1.1035 class Angle
  1.1036 {
  1.1037 public:
  1.1038 	enum AngularUnits
  1.1039 	{
  1.1040 		Radians = 0,
  1.1041 		Degrees = 1
  1.1042 	};
  1.1043 
  1.1044     Angle() : a(0) {}
  1.1045     
  1.1046 	// Fix the range to be between -Pi and Pi
  1.1047 	Angle(T a_, AngularUnits u = Radians) : a((u == Radians) ? a_ : a_*Math<T>::DegreeToRadFactor) { FixRange(); }
  1.1048 
  1.1049 	T    Get(AngularUnits u = Radians) const       { return (u == Radians) ? a : a*Math<T>::RadToDegreeFactor; }
  1.1050 	void Set(const T& x, AngularUnits u = Radians) { a = (u == Radians) ? x : x*Math<T>::DegreeToRadFactor; FixRange(); }
  1.1051 	int Sign() const                               { if (a == 0) return 0; else return (a > 0) ? 1 : -1; }
  1.1052 	T   Abs() const                                { return (a > 0) ? a : -a; }
  1.1053 
  1.1054     bool operator== (const Angle& b) const    { return a == b.a; }
  1.1055     bool operator!= (const Angle& b) const    { return a != b.a; }
  1.1056 //	bool operator<  (const Angle& b) const    { return a < a.b; } 
  1.1057 //	bool operator>  (const Angle& b) const    { return a > a.b; } 
  1.1058 //	bool operator<= (const Angle& b) const    { return a <= a.b; } 
  1.1059 //	bool operator>= (const Angle& b) const    { return a >= a.b; } 
  1.1060 //	bool operator= (const T& x)               { a = x; FixRange(); }
  1.1061 
  1.1062 	// These operations assume a is already between -Pi and Pi.
  1.1063     Angle  operator+  (const Angle& b) const  { return Angle(a + b.a); }
  1.1064 	Angle  operator+  (const T& x) const      { return Angle(a + x); }
  1.1065 	Angle& operator+= (const Angle& b)        { a = a + b.a; FastFixRange(); return *this; }
  1.1066 	Angle& operator+= (const T& x)            { a = a + x; FixRange(); return *this; }
  1.1067 	Angle  operator-  (const Angle& b) const  { return Angle(a - b.a); }
  1.1068 	Angle  operator-  (const T& x) const      { return Angle(a - x); }
  1.1069 	Angle& operator-= (const Angle& b)        { a = a - b.a; FastFixRange(); return *this; }
  1.1070 	Angle& operator-= (const T& x)            { a = a - x; FixRange(); return *this; }
  1.1071 	
  1.1072 	T   Distance(const Angle& b)              { T c = fabs(a - b.a); return (c <= Math<T>::Pi) ? c : Math<T>::TwoPi - c; }
  1.1073 
  1.1074 private:
  1.1075 
  1.1076 	// The stored angle, which should be maintained between -Pi and Pi
  1.1077 	T a;
  1.1078 
  1.1079 	// Fixes the angle range to [-Pi,Pi], but assumes no more than 2Pi away on either side 
  1.1080 	inline void FastFixRange()
  1.1081 	{
  1.1082 		if (a < -Math<T>::Pi)
  1.1083 			a += Math<T>::TwoPi;
  1.1084 		else if (a > Math<T>::Pi)
  1.1085 			a -= Math<T>::TwoPi;
  1.1086 	}
  1.1087 
  1.1088 	// Fixes the angle range to [-Pi,Pi] for any given range, but slower then the fast method
  1.1089 	inline void FixRange()
  1.1090 	{
  1.1091 		a = fmod(a,Math<T>::TwoPi);
  1.1092 		if (a < -Math<T>::Pi)
  1.1093 			a += Math<T>::TwoPi;
  1.1094 		else if (a > Math<T>::Pi)
  1.1095 			a -= Math<T>::TwoPi;
  1.1096 	}
  1.1097 };
  1.1098 
  1.1099 
  1.1100 typedef Angle<float>  Anglef;
  1.1101 typedef Angle<double> Angled;
  1.1102 
  1.1103 
  1.1104 //-------------------------------------------------------------------------------------
  1.1105 // ***** Plane
  1.1106 
  1.1107 // Consists of a normal vector and distance from the origin where the plane is located.
  1.1108 
  1.1109 template<class T>
  1.1110 class Plane : public RefCountBase<Plane<T> >
  1.1111 {
  1.1112 public:
  1.1113     Vector3<T> N;
  1.1114     T          D;
  1.1115 
  1.1116     Plane() : D(0) {}
  1.1117 
  1.1118     // Normals must already be normalized
  1.1119     Plane(const Vector3<T>& n, T d) : N(n), D(d) {}
  1.1120     Plane(T x, T y, T z, T d) : N(x,y,z), D(d) {}
  1.1121 
  1.1122     // construct from a point on the plane and the normal
  1.1123     Plane(const Vector3<T>& p, const Vector3<T>& n) : N(n), D(-(p * n)) {}
  1.1124 
  1.1125     // Find the point to plane distance. The sign indicates what side of the plane the point is on (0 = point on plane).
  1.1126     T TestSide(const Vector3<T>& p) const
  1.1127     {
  1.1128         return (N * p) + D;
  1.1129     }
  1.1130 
  1.1131     Plane<T> Flipped() const
  1.1132     {
  1.1133         return Plane(-N, -D);
  1.1134     }
  1.1135 
  1.1136     void Flip()
  1.1137     {
  1.1138         N = -N;
  1.1139         D = -D;
  1.1140     }
  1.1141 
  1.1142 	bool operator==(const Plane<T>& rhs) const
  1.1143 	{
  1.1144 		return (this->D == rhs.D && this->N == rhs.N);
  1.1145 	}
  1.1146 };
  1.1147 
  1.1148 typedef Plane<float> Planef;
  1.1149 
  1.1150 }
  1.1151 
  1.1152 #endif
  1.1153 \ No newline at end of file