1.1 --- /dev/null	Thu Jan 01 00:00:00 1970 +0000
     1.2 +++ b/LibOVR/Src/Kernel/OVR_Math.h	Wed Jan 14 06:51:16 2015 +0200
     1.3 @@ -0,0 +1,2791 @@
     1.4 +/************************************************************************************
     1.5 +
     1.6 +PublicHeader:   OVR_Kernel.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, 
    1.11 +				Anna Yershova, Max Katsev, Dov Katz
    1.12 +
    1.13 +Copyright   :   Copyright 2014 Oculus VR, LLC All Rights reserved.
    1.14 +
    1.15 +Licensed under the Oculus VR Rift SDK License Version 3.2 (the "License"); 
    1.16 +you may not use the Oculus VR Rift SDK except in compliance with the License, 
    1.17 +which is provided at the time of installation or download, or which 
    1.18 +otherwise accompanies this software in either electronic or hard copy form.
    1.19 +
    1.20 +You may obtain a copy of the License at
    1.21 +
    1.22 +http://www.oculusvr.com/licenses/LICENSE-3.2 
    1.23 +
    1.24 +Unless required by applicable law or agreed to in writing, the Oculus VR SDK 
    1.25 +distributed under the License is distributed on an "AS IS" BASIS,
    1.26 +WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
    1.27 +See the License for the specific language governing permissions and
    1.28 +limitations under the License.
    1.29 +
    1.30 +*************************************************************************************/
    1.31 +
    1.32 +#ifndef OVR_Math_h
    1.33 +#define OVR_Math_h
    1.34 +
    1.35 +#include <assert.h>
    1.36 +#include <stdlib.h>
    1.37 +#include <math.h>
    1.38 +
    1.39 +#include "OVR_Types.h"
    1.40 +#include "OVR_RefCount.h"
    1.41 +#include "OVR_Std.h"
    1.42 +#include "OVR_Alg.h"
    1.43 +
    1.44 +
    1.45 +namespace OVR {
    1.46 +
    1.47 +//-------------------------------------------------------------------------------------
    1.48 +// ***** Constants for 3D world/axis definitions.
    1.49 +
    1.50 +// Definitions of axes for coordinate and rotation conversions.
    1.51 +enum Axis
    1.52 +{
    1.53 +    Axis_X = 0, Axis_Y = 1, Axis_Z = 2
    1.54 +};
    1.55 +
    1.56 +// RotateDirection describes the rotation direction around an axis, interpreted as follows:
    1.57 +//  CW  - Clockwise while looking "down" from positive axis towards the origin.
    1.58 +//  CCW - Counter-clockwise while looking from the positive axis towards the origin,
    1.59 +//        which is in the negative axis direction.
    1.60 +//  CCW is the default for the RHS coordinate system. Oculus standard RHS coordinate
    1.61 +//  system defines Y up, X right, and Z back (pointing out from the screen). In this
    1.62 +//  system Rotate_CCW around Z will specifies counter-clockwise rotation in XY plane.
    1.63 +enum RotateDirection
    1.64 +{
    1.65 +    Rotate_CCW = 1,
    1.66 +    Rotate_CW  = -1 
    1.67 +};
    1.68 +
    1.69 +// Constants for right handed and left handed coordinate systems
    1.70 +enum HandedSystem
    1.71 +{
    1.72 +    Handed_R = 1, Handed_L = -1
    1.73 +};
    1.74 +
    1.75 +// AxisDirection describes which way the coordinate axis points. Used by WorldAxes.
    1.76 +enum AxisDirection
    1.77 +{
    1.78 +    Axis_Up    =  2,
    1.79 +    Axis_Down  = -2,
    1.80 +    Axis_Right =  1,
    1.81 +    Axis_Left  = -1,
    1.82 +    Axis_In    =  3,
    1.83 +    Axis_Out   = -3
    1.84 +};
    1.85 +
    1.86 +struct WorldAxes
    1.87 +{
    1.88 +    AxisDirection XAxis, YAxis, ZAxis;
    1.89 +
    1.90 +    WorldAxes(AxisDirection x, AxisDirection y, AxisDirection z)
    1.91 +        : XAxis(x), YAxis(y), ZAxis(z) 
    1.92 +    { OVR_ASSERT(abs(x) != abs(y) && abs(y) != abs(z) && abs(z) != abs(x));}
    1.93 +};
    1.94 +
    1.95 +} // namespace OVR
    1.96 +
    1.97 +
    1.98 +//------------------------------------------------------------------------------------//
    1.99 +// ***** C Compatibility Types
   1.100 +
   1.101 +// These declarations are used to support conversion between C types used in
   1.102 +// LibOVR C interfaces and their C++ versions. As an example, they allow passing
   1.103 +// Vector3f into a function that expects ovrVector3f.
   1.104 +
   1.105 +typedef struct ovrQuatf_ ovrQuatf;
   1.106 +typedef struct ovrQuatd_ ovrQuatd;
   1.107 +typedef struct ovrSizei_ ovrSizei;
   1.108 +typedef struct ovrSizef_ ovrSizef;
   1.109 +typedef struct ovrRecti_ ovrRecti;
   1.110 +typedef struct ovrVector2i_ ovrVector2i;
   1.111 +typedef struct ovrVector2f_ ovrVector2f;
   1.112 +typedef struct ovrVector3f_ ovrVector3f;
   1.113 +typedef struct ovrVector3d_ ovrVector3d;
   1.114 +typedef struct ovrMatrix3d_ ovrMatrix3d;
   1.115 +typedef struct ovrMatrix4f_ ovrMatrix4f;
   1.116 +typedef struct ovrPosef_ ovrPosef;
   1.117 +typedef struct ovrPosed_ ovrPosed;
   1.118 +typedef struct ovrPoseStatef_ ovrPoseStatef;
   1.119 +typedef struct ovrPoseStated_ ovrPoseStated;
   1.120 +
   1.121 +namespace OVR {
   1.122 +
   1.123 +// Forward-declare our templates.
   1.124 +template<class T> class Quat;
   1.125 +template<class T> class Size;
   1.126 +template<class T> class Rect;
   1.127 +template<class T> class Vector2;
   1.128 +template<class T> class Vector3;
   1.129 +template<class T> class Matrix3;
   1.130 +template<class T> class Matrix4;
   1.131 +template<class T> class Pose;
   1.132 +template<class T> class PoseState;
   1.133 +
   1.134 +// CompatibleTypes::Type is used to lookup a compatible C-version of a C++ class.
   1.135 +template<class C>
   1.136 +struct CompatibleTypes
   1.137 +{    
   1.138 +    // Declaration here seems necessary for MSVC; specializations are
   1.139 +    // used instead.
   1.140 +    typedef struct {} Type;
   1.141 +};
   1.142 +
   1.143 +// Specializations providing CompatibleTypes::Type value.
   1.144 +template<> struct CompatibleTypes<Quat<float> >     { typedef ovrQuatf Type; };
   1.145 +template<> struct CompatibleTypes<Quat<double> >    { typedef ovrQuatd Type; };
   1.146 +template<> struct CompatibleTypes<Matrix3<double> > { typedef ovrMatrix3d Type; };
   1.147 +template<> struct CompatibleTypes<Matrix4<float> >  { typedef ovrMatrix4f Type; };
   1.148 +template<> struct CompatibleTypes<Size<int> >       { typedef ovrSizei Type; };
   1.149 +template<> struct CompatibleTypes<Size<float> >     { typedef ovrSizef Type; };
   1.150 +template<> struct CompatibleTypes<Rect<int> >       { typedef ovrRecti Type; };
   1.151 +template<> struct CompatibleTypes<Vector2<int> >    { typedef ovrVector2i Type; };
   1.152 +template<> struct CompatibleTypes<Vector2<float> >  { typedef ovrVector2f Type; };
   1.153 +template<> struct CompatibleTypes<Vector3<float> >  { typedef ovrVector3f Type; };
   1.154 +template<> struct CompatibleTypes<Vector3<double> > { typedef ovrVector3d Type; };
   1.155 +
   1.156 +template<> struct CompatibleTypes<Pose<float> > { typedef ovrPosef Type; };
   1.157 +template<> struct CompatibleTypes<Pose<double> > { typedef ovrPosed Type; };
   1.158 +
   1.159 +//------------------------------------------------------------------------------------//
   1.160 +// ***** Math
   1.161 +//
   1.162 +// Math class contains constants and functions. This class is a template specialized
   1.163 +// per type, with Math<float> and Math<double> being distinct.
   1.164 +template<class Type>
   1.165 +class Math
   1.166 +{  
   1.167 +public:
   1.168 +    // By default, support explicit conversion to float. This allows Vector2<int> to
   1.169 +    // compile, for example.
   1.170 +    typedef float OtherFloatType;
   1.171 +};
   1.172 +
   1.173 +
   1.174 +#define MATH_FLOAT_PI                (3.1415926f)
   1.175 +#define MATH_FLOAT_TWOPI             (2.0f *MATH_FLOAT_PI)
   1.176 +#define MATH_FLOAT_PIOVER2           (0.5f *MATH_FLOAT_PI)
   1.177 +#define MATH_FLOAT_PIOVER4           (0.25f*MATH_FLOAT_PI)
   1.178 +#define MATH_FLOAT_E                 (2.7182818f)
   1.179 +#define MATH_FLOAT_MAXVALUE			 (FLT_MAX) 
   1.180 +#define MATH_FLOAT MINPOSITIVEVALUE  (FLT_MIN)  
   1.181 +#define MATH_FLOAT_RADTODEGREEFACTOR (360.0f / MATH_FLOAT_TWOPI)
   1.182 +#define MATH_FLOAT_DEGREETORADFACTOR (MATH_FLOAT_TWOPI / 360.0f)
   1.183 +#define MATH_FLOAT_TOLERANCE		 (0.00001f)
   1.184 +#define MATH_FLOAT_SINGULARITYRADIUS (0.0000001f) // Use for Gimbal lock numerical problems
   1.185 +
   1.186 +#define MATH_DOUBLE_PI                (3.14159265358979)
   1.187 +#define MATH_DOUBLE_TWOPI             (2.0f *MATH_DOUBLE_PI)
   1.188 +#define MATH_DOUBLE_PIOVER2           (0.5f *MATH_DOUBLE_PI)
   1.189 +#define MATH_DOUBLE_PIOVER4           (0.25f*MATH_DOUBLE_PI)
   1.190 +#define MATH_DOUBLE_E                 (2.71828182845905)
   1.191 +#define MATH_DOUBLE_MAXVALUE		  (DBL_MAX)
   1.192 +#define MATH_DOUBLE MINPOSITIVEVALUE  (DBL_MIN)
   1.193 +#define MATH_DOUBLE_RADTODEGREEFACTOR (360.0f / MATH_DOUBLE_TWOPI)
   1.194 +#define MATH_DOUBLE_DEGREETORADFACTOR (MATH_DOUBLE_TWOPI / 360.0f)
   1.195 +#define MATH_DOUBLE_TOLERANCE		  (0.00001)
   1.196 +#define MATH_DOUBLE_SINGULARITYRADIUS (0.000000000001) // Use for Gimbal lock numerical problems
   1.197 +
   1.198 +
   1.199 +
   1.200 +
   1.201 +// Single-precision Math constants class.
   1.202 +template<>
   1.203 +class Math<float>
   1.204 +{
   1.205 +public:
   1.206 +     typedef double OtherFloatType;
   1.207 +};
   1.208 +
   1.209 +// Double-precision Math constants class.
   1.210 +template<>
   1.211 +class Math<double>
   1.212 +{
   1.213 +public:
   1.214 +    typedef float OtherFloatType;
   1.215 +};
   1.216 +
   1.217 +
   1.218 +typedef Math<float>  Mathf;
   1.219 +typedef Math<double> Mathd;
   1.220 +
   1.221 +// Conversion functions between degrees and radians
   1.222 +template<class T>
   1.223 +T RadToDegree(T rads) { return rads * ((T)MATH_DOUBLE_RADTODEGREEFACTOR); }
   1.224 +template<class T>
   1.225 +T DegreeToRad(T rads) { return rads * ((T)MATH_DOUBLE_DEGREETORADFACTOR); }
   1.226 +
   1.227 +// Numerically stable acos function
   1.228 +template<class T>
   1.229 +T Acos(T val) { 
   1.230 +		if (val > T(1))				return T(0);
   1.231 +		else if (val < T(-1))		return ((T)MATH_DOUBLE_PI);
   1.232 +		else						return acos(val); 
   1.233 +};
   1.234 +
   1.235 +// Numerically stable asin function
   1.236 +template<class T>
   1.237 +T Asin(T val) { 
   1.238 +	if (val > T(1))				return ((T)MATH_DOUBLE_PIOVER2);
   1.239 +	else if (val < T(-1))		return ((T)MATH_DOUBLE_PIOVER2) * T(3);
   1.240 +	else						return asin(val); 
   1.241 +};
   1.242 +
   1.243 +#ifdef OVR_CC_MSVC
   1.244 +inline int isnan(double x) { return _isnan(x); };
   1.245 +#endif
   1.246 +
   1.247 +template<class T>
   1.248 +class Quat;
   1.249 +
   1.250 +
   1.251 +//-------------------------------------------------------------------------------------
   1.252 +// ***** Vector2<>
   1.253 +
   1.254 +// Vector2f (Vector2d) represents a 2-dimensional vector or point in space,
   1.255 +// consisting of coordinates x and y
   1.256 +
   1.257 +template<class T>
   1.258 +class Vector2
   1.259 +{
   1.260 +public:
   1.261 +    T x, y;
   1.262 +
   1.263 +    Vector2() : x(0), y(0) { }
   1.264 +    Vector2(T x_, T y_) : x(x_), y(y_) { }
   1.265 +    explicit Vector2(T s) : x(s), y(s) { }
   1.266 +    explicit Vector2(const Vector2<typename Math<T>::OtherFloatType> &src)
   1.267 +        : x((T)src.x), y((T)src.y) { }
   1.268 +
   1.269 +
   1.270 +    // C-interop support.
   1.271 +    typedef  typename CompatibleTypes<Vector2<T> >::Type CompatibleType;
   1.272 +
   1.273 +    Vector2(const CompatibleType& s) : x(s.x), y(s.y) {  }
   1.274 +
   1.275 +    operator const CompatibleType& () const
   1.276 +    {
   1.277 +        static_assert(sizeof(Vector2<T>) == sizeof(CompatibleType), "sizeof(Vector2<T>) failure");
   1.278 +        return reinterpret_cast<const CompatibleType&>(*this);
   1.279 +    }
   1.280 +
   1.281 +        
   1.282 +    bool     operator== (const Vector2& b) const  { return x == b.x && y == b.y; }
   1.283 +    bool     operator!= (const Vector2& b) const  { return x != b.x || y != b.y; }
   1.284 +             
   1.285 +    Vector2  operator+  (const Vector2& b) const  { return Vector2(x + b.x, y + b.y); }
   1.286 +    Vector2& operator+= (const Vector2& b)        { x += b.x; y += b.y; return *this; }
   1.287 +    Vector2  operator-  (const Vector2& b) const  { return Vector2(x - b.x, y - b.y); }
   1.288 +    Vector2& operator-= (const Vector2& b)        { x -= b.x; y -= b.y; return *this; }
   1.289 +    Vector2  operator- () const                   { return Vector2(-x, -y); }
   1.290 +
   1.291 +    // Scalar multiplication/division scales vector.
   1.292 +    Vector2  operator*  (T s) const               { return Vector2(x*s, y*s); }
   1.293 +    Vector2& operator*= (T s)                     { x *= s; y *= s; return *this; }
   1.294 +
   1.295 +    Vector2  operator/  (T s) const               { T rcp = T(1)/s;
   1.296 +                                                    return Vector2(x*rcp, y*rcp); }
   1.297 +    Vector2& operator/= (T s)                     { T rcp = T(1)/s;
   1.298 +                                                    x *= rcp; y *= rcp;
   1.299 +                                                    return *this; }
   1.300 +
   1.301 +    static Vector2  Min(const Vector2& a, const Vector2& b) { return Vector2((a.x < b.x) ? a.x : b.x,
   1.302 +                                                                             (a.y < b.y) ? a.y : b.y); }
   1.303 +    static Vector2  Max(const Vector2& a, const Vector2& b) { return Vector2((a.x > b.x) ? a.x : b.x,
   1.304 +                                                                             (a.y > b.y) ? a.y : b.y); }
   1.305 +
   1.306 +    // Compare two vectors for equality with tolerance. Returns true if vectors match withing tolerance.
   1.307 +    bool	Compare(const Vector2&b, T tolerance = ((T)MATH_DOUBLE_TOLERANCE))  
   1.308 +    {
   1.309 +        return (fabs(b.x-x) < tolerance) && (fabs(b.y-y) < tolerance);
   1.310 +    }
   1.311 +    
   1.312 +	// Access element by index
   1.313 +	T& operator[] (int idx)
   1.314 +	{
   1.315 +		OVR_ASSERT(0 <= idx && idx < 2);
   1.316 +		return *(&x + idx);
   1.317 +	}
   1.318 +	const T& operator[] (int idx) const
   1.319 +	{
   1.320 +		OVR_ASSERT(0 <= idx && idx < 2);
   1.321 +		return *(&x + idx);
   1.322 +	}
   1.323 +
   1.324 +    // Entry-wise product of two vectors
   1.325 +    Vector2	EntrywiseMultiply(const Vector2& b) const	{ return Vector2(x * b.x, y * b.y);}
   1.326 +
   1.327 +
   1.328 +    // Multiply and divide operators do entry-wise math. Used Dot() for dot product.
   1.329 +    Vector2  operator*  (const Vector2& b) const        { return Vector2(x * b.x,  y * b.y); }
   1.330 +    Vector2  operator/  (const Vector2& b) const        { return Vector2(x / b.x,  y / b.y); }
   1.331 +
   1.332 +	// Dot product
   1.333 +    // Used to calculate angle q between two vectors among other things,
   1.334 +    // as (A dot B) = |a||b|cos(q).
   1.335 +    T		Dot(const Vector2& b) const                 { return x*b.x + y*b.y; }
   1.336 +
   1.337 +    // Returns the angle from this vector to b, in radians.
   1.338 +    T       Angle(const Vector2& b) const        
   1.339 +	{ 
   1.340 +		T div = LengthSq()*b.LengthSq();
   1.341 +		OVR_ASSERT(div != T(0));
   1.342 +		T result = Acos((this->Dot(b))/sqrt(div));
   1.343 +		return result;
   1.344 +	}
   1.345 +
   1.346 +    // Return Length of the vector squared.
   1.347 +    T       LengthSq() const                     { return (x * x + y * y); }
   1.348 +
   1.349 +    // Return vector length.
   1.350 +    T       Length() const                       { return sqrt(LengthSq()); }
   1.351 +
   1.352 +    // Returns squared distance between two points represented by vectors.
   1.353 +    T       DistanceSq(const Vector2& b) const   { return (*this - b).LengthSq(); }
   1.354 +
   1.355 +	// Returns distance between two points represented by vectors.
   1.356 +    T       Distance(const Vector2& b) const     { return (*this - b).Length(); }
   1.357 +
   1.358 +	// Determine if this a unit vector.
   1.359 +    bool    IsNormalized() const                 { return fabs(LengthSq() - T(1)) < ((T)MATH_DOUBLE_TOLERANCE); }
   1.360 +
   1.361 +    // Normalize, convention vector length to 1.    
   1.362 +    void    Normalize()                          
   1.363 +	{
   1.364 +		T l = Length();
   1.365 +		OVR_ASSERT(l != T(0));
   1.366 +		*this /= l; 
   1.367 +	}
   1.368 +    // Returns normalized (unit) version of the vector without modifying itself.
   1.369 +    Vector2 Normalized() const                   
   1.370 +	{ 
   1.371 +		T l = Length();
   1.372 +		OVR_ASSERT(l != T(0));
   1.373 +		return *this / l; 
   1.374 +	}
   1.375 +
   1.376 +    // Linearly interpolates from this vector to another.
   1.377 +    // Factor should be between 0.0 and 1.0, with 0 giving full value to this.
   1.378 +    Vector2 Lerp(const Vector2& b, T f) const    { return *this*(T(1) - f) + b*f; }
   1.379 +
   1.380 +    // Projects this vector onto the argument; in other words,
   1.381 +    // A.Project(B) returns projection of vector A onto B.
   1.382 +    Vector2 ProjectTo(const Vector2& b) const    
   1.383 +	{ 
   1.384 +		T l2 = b.LengthSq();
   1.385 +		OVR_ASSERT(l2 != T(0));
   1.386 +		return b * ( Dot(b) / l2 ); 
   1.387 +	}
   1.388 +};
   1.389 +
   1.390 +
   1.391 +typedef Vector2<float>  Vector2f;
   1.392 +typedef Vector2<double> Vector2d;
   1.393 +typedef Vector2<int>    Vector2i;
   1.394 +
   1.395 +typedef Vector2<float>  Point2f;
   1.396 +typedef Vector2<double> Point2d;
   1.397 +typedef Vector2<int>    Point2i;
   1.398 +
   1.399 +//-------------------------------------------------------------------------------------
   1.400 +// ***** Vector3<> - 3D vector of {x, y, z}
   1.401 +
   1.402 +//
   1.403 +// Vector3f (Vector3d) represents a 3-dimensional vector or point in space,
   1.404 +// consisting of coordinates x, y and z.
   1.405 +
   1.406 +template<class T>
   1.407 +class Vector3
   1.408 +{
   1.409 +public:
   1.410 +    T x, y, z;
   1.411 +
   1.412 +    // FIXME: default initialization of a vector class can be very expensive in a full-blown
   1.413 +    // application.  A few hundred thousand vector constructions is not unlikely and can add
   1.414 +    // up to milliseconds of time on processors like the PS3 PPU.
   1.415 +    Vector3() : x(0), y(0), z(0) { }
   1.416 +    Vector3(T x_, T y_, T z_ = 0) : x(x_), y(y_), z(z_) { }
   1.417 +    explicit Vector3(T s) : x(s), y(s), z(s) { }
   1.418 +    explicit Vector3(const Vector3<typename Math<T>::OtherFloatType> &src)
   1.419 +        : x((T)src.x), y((T)src.y), z((T)src.z) { }
   1.420 +
   1.421 +    static const Vector3 ZERO;
   1.422 +
   1.423 +    // C-interop support.
   1.424 +    typedef  typename CompatibleTypes<Vector3<T> >::Type CompatibleType;
   1.425 +
   1.426 +    Vector3(const CompatibleType& s) : x(s.x), y(s.y), z(s.z) {  }
   1.427 +
   1.428 +    operator const CompatibleType& () const
   1.429 +    {
   1.430 +        static_assert(sizeof(Vector3<T>) == sizeof(CompatibleType), "sizeof(Vector3<T>) failure");
   1.431 +        return reinterpret_cast<const CompatibleType&>(*this);
   1.432 +    }
   1.433 +
   1.434 +    bool     operator== (const Vector3& b) const  { return x == b.x && y == b.y && z == b.z; }
   1.435 +    bool     operator!= (const Vector3& b) const  { return x != b.x || y != b.y || z != b.z; }
   1.436 +             
   1.437 +    Vector3  operator+  (const Vector3& b) const  { return Vector3(x + b.x, y + b.y, z + b.z); }
   1.438 +    Vector3& operator+= (const Vector3& b)        { x += b.x; y += b.y; z += b.z; return *this; }
   1.439 +    Vector3  operator-  (const Vector3& b) const  { return Vector3(x - b.x, y - b.y, z - b.z); }
   1.440 +    Vector3& operator-= (const Vector3& b)        { x -= b.x; y -= b.y; z -= b.z; return *this; }
   1.441 +    Vector3  operator- () const                   { return Vector3(-x, -y, -z); }
   1.442 +
   1.443 +    // Scalar multiplication/division scales vector.
   1.444 +    Vector3  operator*  (T s) const               { return Vector3(x*s, y*s, z*s); }
   1.445 +    Vector3& operator*= (T s)                     { x *= s; y *= s; z *= s; return *this; }
   1.446 +
   1.447 +    Vector3  operator/  (T s) const               { T rcp = T(1)/s;
   1.448 +                                                    return Vector3(x*rcp, y*rcp, z*rcp); }
   1.449 +    Vector3& operator/= (T s)                     { T rcp = T(1)/s;
   1.450 +                                                    x *= rcp; y *= rcp; z *= rcp;
   1.451 +                                                    return *this; }
   1.452 +
   1.453 +    static Vector3  Min(const Vector3& a, const Vector3& b)
   1.454 +    {
   1.455 +        return Vector3((a.x < b.x) ? a.x : b.x,
   1.456 +                       (a.y < b.y) ? a.y : b.y,
   1.457 +                       (a.z < b.z) ? a.z : b.z);
   1.458 +    }
   1.459 +    static Vector3  Max(const Vector3& a, const Vector3& b)
   1.460 +    { 
   1.461 +        return Vector3((a.x > b.x) ? a.x : b.x,
   1.462 +                       (a.y > b.y) ? a.y : b.y,
   1.463 +                       (a.z > b.z) ? a.z : b.z);
   1.464 +    }        
   1.465 +
   1.466 +    // Compare two vectors for equality with tolerance. Returns true if vectors match withing tolerance.
   1.467 +    bool      Compare(const Vector3&b, T tolerance = ((T)MATH_DOUBLE_TOLERANCE)) 
   1.468 +    {
   1.469 +        return (fabs(b.x-x) < tolerance) && 
   1.470 +			   (fabs(b.y-y) < tolerance) && 
   1.471 +			   (fabs(b.z-z) < tolerance);
   1.472 +    }
   1.473 +    
   1.474 +    T& operator[] (int idx)
   1.475 +    {
   1.476 +        OVR_ASSERT(0 <= idx && idx < 3);
   1.477 +        return *(&x + idx);
   1.478 +    }
   1.479 +
   1.480 +    const T& operator[] (int idx) const
   1.481 +    {
   1.482 +        OVR_ASSERT(0 <= idx && idx < 3);
   1.483 +        return *(&x + idx);
   1.484 +    }
   1.485 +
   1.486 +    // Entrywise product of two vectors
   1.487 +    Vector3	EntrywiseMultiply(const Vector3& b) const	{ return Vector3(x * b.x, 
   1.488 +																		 y * b.y, 
   1.489 +																		 z * b.z);}
   1.490 +
   1.491 +    // Multiply and divide operators do entry-wise math
   1.492 +    Vector3  operator*  (const Vector3& b) const        { return Vector3(x * b.x, 
   1.493 +																		 y * b.y, 
   1.494 +																		 z * b.z); }
   1.495 +
   1.496 +    Vector3  operator/  (const Vector3& b) const        { return Vector3(x / b.x, 
   1.497 +																		 y / b.y, 
   1.498 +																		 z / b.z); }
   1.499 +
   1.500 +
   1.501 +	// Dot product
   1.502 +    // Used to calculate angle q between two vectors among other things,
   1.503 +    // as (A dot B) = |a||b|cos(q).
   1.504 +     T      Dot(const Vector3& b) const          { return x*b.x + y*b.y + z*b.z; }
   1.505 +
   1.506 +    // Compute cross product, which generates a normal vector.
   1.507 +    // Direction vector can be determined by right-hand rule: Pointing index finder in
   1.508 +    // direction a and middle finger in direction b, thumb will point in a.Cross(b).
   1.509 +    Vector3 Cross(const Vector3& b) const        { return Vector3(y*b.z - z*b.y,
   1.510 +                                                                  z*b.x - x*b.z,
   1.511 +                                                                  x*b.y - y*b.x); }
   1.512 +
   1.513 +    // Returns the angle from this vector to b, in radians.
   1.514 +    T       Angle(const Vector3& b) const 
   1.515 +	{
   1.516 +		T div = LengthSq()*b.LengthSq();
   1.517 +		OVR_ASSERT(div != T(0));
   1.518 +		T result = Acos((this->Dot(b))/sqrt(div));
   1.519 +		return result;
   1.520 +	}
   1.521 +
   1.522 +    // Return Length of the vector squared.
   1.523 +    T       LengthSq() const                     { return (x * x + y * y + z * z); }
   1.524 +
   1.525 +    // Return vector length.
   1.526 +    T       Length() const                       { return sqrt(LengthSq()); }
   1.527 +
   1.528 +    // Returns squared distance between two points represented by vectors.
   1.529 +    T       DistanceSq(Vector3 const& b) const         { return (*this - b).LengthSq(); }
   1.530 +
   1.531 +    // Returns distance between two points represented by vectors.
   1.532 +    T       Distance(Vector3 const& b) const     { return (*this - b).Length(); }
   1.533 +    
   1.534 +    // Determine if this a unit vector.
   1.535 +    bool    IsNormalized() const                 { return fabs(LengthSq() - T(1)) < ((T)MATH_DOUBLE_TOLERANCE); }
   1.536 +
   1.537 +    // Normalize, convention vector length to 1.    
   1.538 +    void    Normalize()                          
   1.539 +	{
   1.540 +		T l = Length();
   1.541 +		OVR_ASSERT(l != T(0));
   1.542 +		*this /= l; 
   1.543 +	}
   1.544 +
   1.545 +    // Returns normalized (unit) version of the vector without modifying itself.
   1.546 +    Vector3 Normalized() const                   
   1.547 +	{ 
   1.548 +		T l = Length();
   1.549 +		OVR_ASSERT(l != T(0));
   1.550 +		return *this / l; 
   1.551 +	}
   1.552 +
   1.553 +    // Linearly interpolates from this vector to another.
   1.554 +    // Factor should be between 0.0 and 1.0, with 0 giving full value to this.
   1.555 +    Vector3 Lerp(const Vector3& b, T f) const    { return *this*(T(1) - f) + b*f; }
   1.556 +
   1.557 +    // Projects this vector onto the argument; in other words,
   1.558 +    // A.Project(B) returns projection of vector A onto B.
   1.559 +    Vector3 ProjectTo(const Vector3& b) const    
   1.560 +	{ 
   1.561 +		T l2 = b.LengthSq();
   1.562 +		OVR_ASSERT(l2 != T(0));
   1.563 +		return b * ( Dot(b) / l2 ); 
   1.564 +	}
   1.565 +
   1.566 +    // Projects this vector onto a plane defined by a normal vector
   1.567 +    Vector3 ProjectToPlane(const Vector3& normal) const { return *this - this->ProjectTo(normal); }
   1.568 +};
   1.569 +
   1.570 +typedef Vector3<float>  Vector3f;
   1.571 +typedef Vector3<double> Vector3d;
   1.572 +typedef Vector3<int32_t>  Vector3i;
   1.573 +    
   1.574 +static_assert((sizeof(Vector3f) == 3*sizeof(float)), "sizeof(Vector3f) failure");
   1.575 +static_assert((sizeof(Vector3d) == 3*sizeof(double)), "sizeof(Vector3d) failure");
   1.576 +static_assert((sizeof(Vector3i) == 3*sizeof(int32_t)), "sizeof(Vector3i) failure");
   1.577 +
   1.578 +typedef Vector3<float>   Point3f;
   1.579 +typedef Vector3<double>  Point3d;
   1.580 +typedef Vector3<int32_t>  Point3i;
   1.581 +
   1.582 +
   1.583 +//-------------------------------------------------------------------------------------
   1.584 +// ***** Vector4<> - 4D vector of {x, y, z, w}
   1.585 +
   1.586 +//
   1.587 +// Vector4f (Vector4d) represents a 3-dimensional vector or point in space,
   1.588 +// consisting of coordinates x, y, z and w.
   1.589 +
   1.590 +template<class T>
   1.591 +class Vector4
   1.592 +{
   1.593 +public:
   1.594 +    T x, y, z, w;
   1.595 +
   1.596 +	// FIXME: default initialization of a vector class can be very expensive in a full-blown
   1.597 +	// application.  A few hundred thousand vector constructions is not unlikely and can add
   1.598 +	// up to milliseconds of time on processors like the PS3 PPU.
   1.599 +    Vector4() : x(0), y(0), z(0), w(0) { }
   1.600 +    Vector4(T x_, T y_, T z_, T w_) : x(x_), y(y_), z(z_), w(w_) { }
   1.601 +    explicit Vector4(T s) : x(s), y(s), z(s), w(s) { }
   1.602 +	explicit Vector4(const Vector3<T>& v, const float w_=1) : x(v.x), y(v.y), z(v.z), w(w_) { }
   1.603 +    explicit Vector4(const Vector4<typename Math<T>::OtherFloatType> &src)
   1.604 +        : x((T)src.x), y((T)src.y), z((T)src.z), w((T)src.w) { }
   1.605 +
   1.606 +    static const Vector4 ZERO;
   1.607 +
   1.608 +    // C-interop support.
   1.609 +    typedef  typename CompatibleTypes< Vector4<T> >::Type CompatibleType;
   1.610 +
   1.611 +    Vector4(const CompatibleType& s) : x(s.x), y(s.y), z(s.z), w(s.w) {  }
   1.612 +
   1.613 +    operator const CompatibleType& () const
   1.614 +    {
   1.615 +        static_assert(sizeof(Vector4<T>) == sizeof(CompatibleType), "sizeof(Vector4<T>) failure");
   1.616 +        return reinterpret_cast<const CompatibleType&>(*this);
   1.617 +    }
   1.618 +
   1.619 +	Vector4& operator= (const Vector3<T>& other)  { x=other.x; y=other.y; z=other.z; w=1; return *this; }
   1.620 +    bool     operator== (const Vector4& b) const  { return x == b.x && y == b.y && z == b.z && w == b.w; }
   1.621 +    bool     operator!= (const Vector4& b) const  { return x != b.x || y != b.y || z != b.z || w != b.w; }
   1.622 +             
   1.623 +    Vector4  operator+  (const Vector4& b) const  { return Vector4(x + b.x, y + b.y, z + b.z, w + b.w); }
   1.624 +    Vector4& operator+= (const Vector4& b)        { x += b.x; y += b.y; z += b.z; w += b.w; return *this; }
   1.625 +    Vector4  operator-  (const Vector4& b) const  { return Vector4(x - b.x, y - b.y, z - b.z, w - b.w); }
   1.626 +    Vector4& operator-= (const Vector4& b)        { x -= b.x; y -= b.y; z -= b.z; w -= b.w; return *this; }
   1.627 +    Vector4  operator- () const                   { return Vector4(-x, -y, -z, -w); }
   1.628 +
   1.629 +    // Scalar multiplication/division scales vector.
   1.630 +    Vector4  operator*  (T s) const               { return Vector4(x*s, y*s, z*s, w*s); }
   1.631 +    Vector4& operator*= (T s)                     { x *= s; y *= s; z *= s; w *= s;return *this; }
   1.632 +
   1.633 +    Vector4  operator/  (T s) const               { T rcp = T(1)/s;
   1.634 +                                                    return Vector4(x*rcp, y*rcp, z*rcp, w*rcp); }
   1.635 +    Vector4& operator/= (T s)                     { T rcp = T(1)/s;
   1.636 +                                                    x *= rcp; y *= rcp; z *= rcp; w *= rcp;
   1.637 +                                                    return *this; }
   1.638 +
   1.639 +    static Vector4  Min(const Vector4& a, const Vector4& b)
   1.640 +    {
   1.641 +        return Vector4((a.x < b.x) ? a.x : b.x,
   1.642 +                       (a.y < b.y) ? a.y : b.y,
   1.643 +                       (a.z < b.z) ? a.z : b.z,
   1.644 +					   (a.w < b.w) ? a.w : b.w);
   1.645 +    }
   1.646 +    static Vector4  Max(const Vector4& a, const Vector4& b)
   1.647 +    { 
   1.648 +        return Vector4((a.x > b.x) ? a.x : b.x,
   1.649 +                       (a.y > b.y) ? a.y : b.y,
   1.650 +                       (a.z > b.z) ? a.z : b.z,
   1.651 +					   (a.w > b.w) ? a.w : b.w);
   1.652 +    }        
   1.653 +
   1.654 +    // Compare two vectors for equality with tolerance. Returns true if vectors match withing tolerance.
   1.655 +    bool      Compare(const Vector4&b, T tolerance = ((T)MATH_DOUBLE_TOLERANCE))
   1.656 +    {
   1.657 +        return (fabs(b.x-x) < tolerance) && 
   1.658 +			   (fabs(b.y-y) < tolerance) && 
   1.659 +			   (fabs(b.z-z) < tolerance) &&
   1.660 +			   (fabs(b.w-w) < tolerance);
   1.661 +    }
   1.662 +    
   1.663 +    T& operator[] (int idx)
   1.664 +    {
   1.665 +        OVR_ASSERT(0 <= idx && idx < 4);
   1.666 +        return *(&x + idx);
   1.667 +    }
   1.668 +
   1.669 +    const T& operator[] (int idx) const
   1.670 +    {
   1.671 +        OVR_ASSERT(0 <= idx && idx < 4);
   1.672 +        return *(&x + idx);
   1.673 +    }
   1.674 +
   1.675 +    // Entry wise product of two vectors
   1.676 +    Vector4	EntrywiseMultiply(const Vector4& b) const	{ return Vector4(x * b.x, 
   1.677 +																		 y * b.y, 
   1.678 +																		 z * b.z);}
   1.679 +
   1.680 +    // Multiply and divide operators do entry-wise math
   1.681 +    Vector4  operator*  (const Vector4& b) const        { return Vector4(x * b.x, 
   1.682 +																		 y * b.y, 
   1.683 +																		 z * b.z,
   1.684 +																		 w * b.w); }
   1.685 +
   1.686 +    Vector4  operator/  (const Vector4& b) const        { return Vector4(x / b.x, 
   1.687 +																		 y / b.y, 
   1.688 +																		 z / b.z,
   1.689 +																		 w / b.w); }
   1.690 +
   1.691 +
   1.692 +	// Dot product
   1.693 +    T       Dot(const Vector4& b) const          { return x*b.x + y*b.y + z*b.z + w*b.w; }
   1.694 +
   1.695 +    // Return Length of the vector squared.
   1.696 +    T       LengthSq() const                     { return (x * x + y * y + z * z + w * w); }
   1.697 +
   1.698 +    // Return vector length.
   1.699 +    T       Length() const                       { return sqrt(LengthSq()); }
   1.700 +    
   1.701 +    // Determine if this a unit vector.
   1.702 +    bool    IsNormalized() const                 { return fabs(LengthSq() - T(1)) < Math<T>::Tolerance; }
   1.703 +
   1.704 +    // Normalize, convention vector length to 1.    
   1.705 +    void    Normalize()                          
   1.706 +	{
   1.707 +		T l = Length();
   1.708 +		OVR_ASSERT(l != T(0));
   1.709 +		*this /= l; 
   1.710 +	}
   1.711 +
   1.712 +    // Returns normalized (unit) version of the vector without modifying itself.
   1.713 +    Vector4 Normalized() const                   
   1.714 +	{ 
   1.715 +		T l = Length();
   1.716 +		OVR_ASSERT(l != T(0));
   1.717 +		return *this / l; 
   1.718 +	}
   1.719 +};
   1.720 +
   1.721 +typedef Vector4<float>  Vector4f;
   1.722 +typedef Vector4<double> Vector4d;
   1.723 +typedef Vector4<int>    Vector4i;
   1.724 +
   1.725 +
   1.726 +//-------------------------------------------------------------------------------------
   1.727 +// ***** Bounds3
   1.728 +
   1.729 +// Bounds class used to describe a 3D axis aligned bounding box.
   1.730 +
   1.731 +template<class T>
   1.732 +class Bounds3
   1.733 +{
   1.734 +public:
   1.735 +	Vector3<T>	b[2];
   1.736 +
   1.737 +	Bounds3()
   1.738 +	{
   1.739 +	}
   1.740 +
   1.741 +	Bounds3( const Vector3<T> & mins, const Vector3<T> & maxs )
   1.742 +{
   1.743 +		b[0] = mins;
   1.744 +		b[1] = maxs;
   1.745 +	}
   1.746 +
   1.747 +	void Clear()
   1.748 +	{
   1.749 +		b[0].x = b[0].y = b[0].z = Math<T>::MaxValue;
   1.750 +		b[1].x = b[1].y = b[1].z = -Math<T>::MaxValue;
   1.751 +	}
   1.752 +
   1.753 +	void AddPoint( const Vector3<T> & v )
   1.754 +	{
   1.755 +		b[0].x = Alg::Min( b[0].x, v.x );
   1.756 +		b[0].y = Alg::Min( b[0].y, v.y );
   1.757 +		b[0].z = Alg::Min( b[0].z, v.z );
   1.758 +		b[1].x = Alg::Max( b[1].x, v.x );
   1.759 +		b[1].y = Alg::Max( b[1].y, v.y );
   1.760 +		b[1].z = Alg::Max( b[1].z, v.z );
   1.761 +	}
   1.762 +
   1.763 +	const Vector3<T> & GetMins() const { return b[0]; }
   1.764 +	const Vector3<T> & GetMaxs() const { return b[1]; }
   1.765 +
   1.766 +	Vector3<T> & GetMins() { return b[0]; }
   1.767 +	Vector3<T> & GetMaxs() { return b[1]; }
   1.768 +};
   1.769 +
   1.770 +typedef Bounds3<float>	Bounds3f;
   1.771 +typedef Bounds3<double>	Bounds3d;
   1.772 +
   1.773 +
   1.774 +//-------------------------------------------------------------------------------------
   1.775 +// ***** Size
   1.776 +
   1.777 +// Size class represents 2D size with Width, Height components.
   1.778 +// Used to describe distentions of render targets, etc.
   1.779 +
   1.780 +template<class T>
   1.781 +class Size
   1.782 +{
   1.783 +public:
   1.784 +    T   w, h;
   1.785 +
   1.786 +    Size()              : w(0), h(0)   { }
   1.787 +    Size(T w_, T h_)    : w(w_), h(h_) { }
   1.788 +    explicit Size(T s)  : w(s), h(s)   { }
   1.789 +    explicit Size(const Size<typename Math<T>::OtherFloatType> &src)
   1.790 +        : w((T)src.w), h((T)src.h) { }
   1.791 +
   1.792 +    // C-interop support.
   1.793 +    typedef  typename CompatibleTypes<Size<T> >::Type CompatibleType;
   1.794 +
   1.795 +    Size(const CompatibleType& s) : w(s.w), h(s.h) {  }
   1.796 +
   1.797 +    operator const CompatibleType& () const
   1.798 +    {
   1.799 +        static_assert(sizeof(Size<T>) == sizeof(CompatibleType), "sizeof(Size<T>) failure");
   1.800 +        return reinterpret_cast<const CompatibleType&>(*this);
   1.801 +    }
   1.802 +
   1.803 +    bool     operator== (const Size& b) const  { return w == b.w && h == b.h; }
   1.804 +    bool     operator!= (const Size& b) const  { return w != b.w || h != b.h; }
   1.805 +             
   1.806 +    Size  operator+  (const Size& b) const  { return Size(w + b.w, h + b.h); }
   1.807 +    Size& operator+= (const Size& b)        { w += b.w; h += b.h; return *this; }
   1.808 +    Size  operator-  (const Size& b) const  { return Size(w - b.w, h - b.h); }
   1.809 +    Size& operator-= (const Size& b)        { w -= b.w; h -= b.h; return *this; }
   1.810 +    Size  operator- () const                { return Size(-w, -h); }
   1.811 +    Size  operator*  (const Size& b) const  { return Size(w * b.w, h * b.h); }
   1.812 +    Size& operator*= (const Size& b)        { w *= b.w; h *= b.h; return *this; }
   1.813 +    Size  operator/  (const Size& b) const  { return Size(w / b.w, h / b.h); }
   1.814 +    Size& operator/= (const Size& b)        { w /= b.w; h /= b.h; return *this; }
   1.815 +
   1.816 +    // Scalar multiplication/division scales both components.
   1.817 +    Size  operator*  (T s) const            { return Size(w*s, h*s); }
   1.818 +    Size& operator*= (T s)                  { w *= s; h *= s; return *this; }    
   1.819 +    Size  operator/  (T s) const            { return Size(w/s, h/s); }
   1.820 +    Size& operator/= (T s)                  { w /= s; h /= s; return *this; }
   1.821 +
   1.822 +    static Size Min(const Size& a, const Size& b)  { return Size((a.w  < b.w)  ? a.w  : b.w,
   1.823 +                                                                 (a.h < b.h) ? a.h : b.h); }
   1.824 +    static Size Max(const Size& a, const Size& b)  { return Size((a.w  > b.w)  ? a.w  : b.w,
   1.825 +                                                                 (a.h > b.h) ? a.h : b.h); }
   1.826 +    
   1.827 +    T       Area() const                    { return w * h; }
   1.828 +
   1.829 +    inline  Vector2<T> ToVector() const     { return Vector2<T>(w, h); }
   1.830 +};
   1.831 +
   1.832 +
   1.833 +typedef Size<int>       Sizei;
   1.834 +typedef Size<unsigned>  Sizeu;
   1.835 +typedef Size<float>     Sizef;
   1.836 +typedef Size<double>    Sized;
   1.837 +
   1.838 +
   1.839 +
   1.840 +//-----------------------------------------------------------------------------------
   1.841 +// ***** Rect
   1.842 +
   1.843 +// Rect describes a rectangular area for rendering, that includes position and size.
   1.844 +template<class T>
   1.845 +class Rect
   1.846 +{
   1.847 +public:
   1.848 +    T x, y;
   1.849 +    T w, h;
   1.850 +
   1.851 +    Rect() { }
   1.852 +    Rect(T x1, T y1, T w1, T h1)                   : x(x1), y(y1), w(w1), h(h1) { }    
   1.853 +    Rect(const Vector2<T>& pos, const Size<T>& sz) : x(pos.x), y(pos.y), w(sz.w), h(sz.h) { }
   1.854 +    Rect(const Size<T>& sz)                        : x(0), y(0), w(sz.w), h(sz.h) { }
   1.855 +    
   1.856 +    // C-interop support.
   1.857 +    typedef  typename CompatibleTypes<Rect<T> >::Type CompatibleType;
   1.858 +
   1.859 +    Rect(const CompatibleType& s) : x(s.Pos.x), y(s.Pos.y), w(s.Size.w), h(s.Size.h) {  }
   1.860 +
   1.861 +    operator const CompatibleType& () const
   1.862 +    {
   1.863 +        static_assert(sizeof(Rect<T>) == sizeof(CompatibleType), "sizeof(Rect<T>) failure");
   1.864 +        return reinterpret_cast<const CompatibleType&>(*this);
   1.865 +    }
   1.866 +
   1.867 +    Vector2<T> GetPos() const                { return Vector2<T>(x, y); }
   1.868 +    Size<T>    GetSize() const               { return Size<T>(w, h); }
   1.869 +    void       SetPos(const Vector2<T>& pos) { x = pos.x; y = pos.y; }
   1.870 +    void       SetSize(const Size<T>& sz)    { w = sz.w; h = sz.h; }
   1.871 +
   1.872 +    bool operator == (const Rect& vp) const
   1.873 +    { return (x == vp.x) && (y == vp.y) && (w == vp.w) && (h == vp.h); }
   1.874 +    bool operator != (const Rect& vp) const
   1.875 +    { return !operator == (vp); }
   1.876 +};
   1.877 +
   1.878 +typedef Rect<int> Recti;
   1.879 +
   1.880 +
   1.881 +//-------------------------------------------------------------------------------------//
   1.882 +// ***** Quat
   1.883 +//
   1.884 +// Quatf represents a quaternion class used for rotations.
   1.885 +// 
   1.886 +// Quaternion multiplications are done in right-to-left order, to match the
   1.887 +// behavior of matrices.
   1.888 +
   1.889 +
   1.890 +template<class T>
   1.891 +class Quat
   1.892 +{
   1.893 +public:
   1.894 +    // w + Xi + Yj + Zk
   1.895 +    T x, y, z, w;    
   1.896 +
   1.897 +    Quat() : x(0), y(0), z(0), w(1) { }
   1.898 +    Quat(T x_, T y_, T z_, T w_) : x(x_), y(y_), z(z_), w(w_) { }
   1.899 +    explicit Quat(const Quat<typename Math<T>::OtherFloatType> &src)
   1.900 +        : x((T)src.x), y((T)src.y), z((T)src.z), w((T)src.w) { }
   1.901 +
   1.902 +    typedef  typename CompatibleTypes<Quat<T> >::Type CompatibleType;
   1.903 +
   1.904 +    // C-interop support.
   1.905 +    Quat(const CompatibleType& s) : x(s.x), y(s.y), z(s.z), w(s.w) { }
   1.906 +
   1.907 +    operator CompatibleType () const
   1.908 +    {
   1.909 +        CompatibleType result;
   1.910 +        result.x = x;
   1.911 +        result.y = y;
   1.912 +        result.z = z;
   1.913 +        result.w = w;
   1.914 +        return result;
   1.915 +    }
   1.916 +
   1.917 +    // Constructs quaternion for rotation around the axis by an angle.
   1.918 +    Quat(const Vector3<T>& axis, T angle)
   1.919 +    {
   1.920 +        // Make sure we don't divide by zero. 
   1.921 +        if (axis.LengthSq() == 0)
   1.922 +        {
   1.923 +            // Assert if the axis is zero, but the angle isn't
   1.924 +            OVR_ASSERT(angle == 0);
   1.925 +            x = 0; y = 0; z = 0; w = 1;
   1.926 +            return;
   1.927 +        }
   1.928 +
   1.929 +		Vector3<T> unitAxis = axis.Normalized();
   1.930 +		T          sinHalfAngle = sin(angle * T(0.5));
   1.931 +
   1.932 +		w = cos(angle * T(0.5));
   1.933 +		x = unitAxis.x * sinHalfAngle;
   1.934 +		y = unitAxis.y * sinHalfAngle;
   1.935 +		z = unitAxis.z * sinHalfAngle;
   1.936 +    }
   1.937 +
   1.938 +    // Constructs quaternion for rotation around one of the coordinate axis by an angle.
   1.939 +    Quat(Axis A, T angle, RotateDirection d = Rotate_CCW, HandedSystem s = Handed_R)
   1.940 +    {
   1.941 +        T sinHalfAngle = s * d *sin(angle * T(0.5));
   1.942 +        T v[3];
   1.943 +        v[0] = v[1] = v[2] = T(0);
   1.944 +        v[A] = sinHalfAngle;
   1.945 +
   1.946 +        w = cos(angle * T(0.5));
   1.947 +        x = v[0];
   1.948 +        y = v[1];
   1.949 +        z = v[2];
   1.950 +    }
   1.951 +
   1.952 +    // Compute axis and angle from quaternion
   1.953 +    void GetAxisAngle(Vector3<T>* axis, T* angle) const
   1.954 +    {
   1.955 +		if ( x*x + y*y + z*z > ((T)MATH_DOUBLE_TOLERANCE) * ((T)MATH_DOUBLE_TOLERANCE) ) {
   1.956 +			*axis  = Vector3<T>(x, y, z).Normalized();
   1.957 +			*angle = 2 * Acos(w);
   1.958 +			if (*angle > ((T)MATH_DOUBLE_PI)) // Reduce the magnitude of the angle, if necessary
   1.959 +			{
   1.960 +				*angle = ((T)MATH_DOUBLE_TWOPI) - *angle;
   1.961 +				*axis = *axis * (-1);
   1.962 +			}
   1.963 +		}
   1.964 +		else 
   1.965 +		{
   1.966 +			*axis = Vector3<T>(1, 0, 0);
   1.967 +			*angle= 0;
   1.968 +		}
   1.969 +    }
   1.970 +
   1.971 +    // Constructs the quaternion from a rotation matrix
   1.972 +    explicit Quat(const Matrix4<T>& m)
   1.973 +    {
   1.974 +        T trace = m.M[0][0] + m.M[1][1] + m.M[2][2];
   1.975 +
   1.976 +        // In almost all cases, the first part is executed.
   1.977 +        // However, if the trace is not positive, the other
   1.978 +        // cases arise.
   1.979 +        if (trace > T(0)) 
   1.980 +        {
   1.981 +            T s = sqrt(trace + T(1)) * T(2); // s=4*qw
   1.982 +            w = T(0.25) * s;
   1.983 +            x = (m.M[2][1] - m.M[1][2]) / s;
   1.984 +            y = (m.M[0][2] - m.M[2][0]) / s;
   1.985 +            z = (m.M[1][0] - m.M[0][1]) / s; 
   1.986 +        } 
   1.987 +        else if ((m.M[0][0] > m.M[1][1])&&(m.M[0][0] > m.M[2][2])) 
   1.988 +        {
   1.989 +            T s = sqrt(T(1) + m.M[0][0] - m.M[1][1] - m.M[2][2]) * T(2);
   1.990 +            w = (m.M[2][1] - m.M[1][2]) / s;
   1.991 +            x = T(0.25) * s;
   1.992 +            y = (m.M[0][1] + m.M[1][0]) / s;
   1.993 +            z = (m.M[2][0] + m.M[0][2]) / s;
   1.994 +        } 
   1.995 +        else if (m.M[1][1] > m.M[2][2]) 
   1.996 +        {
   1.997 +            T s = sqrt(T(1) + m.M[1][1] - m.M[0][0] - m.M[2][2]) * T(2); // S=4*qy
   1.998 +            w = (m.M[0][2] - m.M[2][0]) / s;
   1.999 +            x = (m.M[0][1] + m.M[1][0]) / s;
  1.1000 +            y = T(0.25) * s;
  1.1001 +            z = (m.M[1][2] + m.M[2][1]) / s;
  1.1002 +        } 
  1.1003 +        else 
  1.1004 +        {
  1.1005 +            T s = sqrt(T(1) + m.M[2][2] - m.M[0][0] - m.M[1][1]) * T(2); // S=4*qz
  1.1006 +            w = (m.M[1][0] - m.M[0][1]) / s;
  1.1007 +            x = (m.M[0][2] + m.M[2][0]) / s;
  1.1008 +            y = (m.M[1][2] + m.M[2][1]) / s;
  1.1009 +            z = T(0.25) * s;
  1.1010 +        }
  1.1011 +    }
  1.1012 +
  1.1013 +	// Constructs the quaternion from a rotation matrix
  1.1014 +	explicit Quat(const Matrix3<T>& m)
  1.1015 +	{
  1.1016 +		T trace = m.M[0][0] + m.M[1][1] + m.M[2][2];
  1.1017 +
  1.1018 +		// In almost all cases, the first part is executed.
  1.1019 +		// However, if the trace is not positive, the other
  1.1020 +		// cases arise.
  1.1021 +		if (trace > T(0)) 
  1.1022 +		{
  1.1023 +			T s = sqrt(trace + T(1)) * T(2); // s=4*qw
  1.1024 +			w = T(0.25) * s;
  1.1025 +			x = (m.M[2][1] - m.M[1][2]) / s;
  1.1026 +			y = (m.M[0][2] - m.M[2][0]) / s;
  1.1027 +			z = (m.M[1][0] - m.M[0][1]) / s; 
  1.1028 +		} 
  1.1029 +		else if ((m.M[0][0] > m.M[1][1])&&(m.M[0][0] > m.M[2][2])) 
  1.1030 +		{
  1.1031 +			T s = sqrt(T(1) + m.M[0][0] - m.M[1][1] - m.M[2][2]) * T(2);
  1.1032 +			w = (m.M[2][1] - m.M[1][2]) / s;
  1.1033 +			x = T(0.25) * s;
  1.1034 +			y = (m.M[0][1] + m.M[1][0]) / s;
  1.1035 +			z = (m.M[2][0] + m.M[0][2]) / s;
  1.1036 +		} 
  1.1037 +		else if (m.M[1][1] > m.M[2][2]) 
  1.1038 +		{
  1.1039 +			T s = sqrt(T(1) + m.M[1][1] - m.M[0][0] - m.M[2][2]) * T(2); // S=4*qy
  1.1040 +			w = (m.M[0][2] - m.M[2][0]) / s;
  1.1041 +			x = (m.M[0][1] + m.M[1][0]) / s;
  1.1042 +			y = T(0.25) * s;
  1.1043 +			z = (m.M[1][2] + m.M[2][1]) / s;
  1.1044 +		} 
  1.1045 +		else 
  1.1046 +		{
  1.1047 +			T s = sqrt(T(1) + m.M[2][2] - m.M[0][0] - m.M[1][1]) * T(2); // S=4*qz
  1.1048 +			w = (m.M[1][0] - m.M[0][1]) / s;
  1.1049 +			x = (m.M[0][2] + m.M[2][0]) / s;
  1.1050 +			y = (m.M[1][2] + m.M[2][1]) / s;
  1.1051 +			z = T(0.25) * s;
  1.1052 +		}
  1.1053 +	}
  1.1054 +
  1.1055 +    bool operator== (const Quat& b) const   { return x == b.x && y == b.y && z == b.z && w == b.w; }
  1.1056 +    bool operator!= (const Quat& b) const   { return x != b.x || y != b.y || z != b.z || w != b.w; }
  1.1057 +
  1.1058 +    Quat  operator+  (const Quat& b) const  { return Quat(x + b.x, y + b.y, z + b.z, w + b.w); }
  1.1059 +    Quat& operator+= (const Quat& b)        { w += b.w; x += b.x; y += b.y; z += b.z; return *this; }
  1.1060 +    Quat  operator-  (const Quat& b) const  { return Quat(x - b.x, y - b.y, z - b.z, w - b.w); }
  1.1061 +    Quat& operator-= (const Quat& b)        { w -= b.w; x -= b.x; y -= b.y; z -= b.z; return *this; }
  1.1062 +
  1.1063 +    Quat  operator*  (T s) const            { return Quat(x * s, y * s, z * s, w * s); }
  1.1064 +    Quat& operator*= (T s)                  { w *= s; x *= s; y *= s; z *= s; return *this; }
  1.1065 +    Quat  operator/  (T s) const            { T rcp = T(1)/s; return Quat(x * rcp, y * rcp, z * rcp, w *rcp); }
  1.1066 +    Quat& operator/= (T s)                  { T rcp = T(1)/s; w *= rcp; x *= rcp; y *= rcp; z *= rcp; return *this; }
  1.1067 +
  1.1068 +
  1.1069 +    // Get Imaginary part vector
  1.1070 +    Vector3<T> Imag() const                 { return Vector3<T>(x,y,z); }
  1.1071 +
  1.1072 +    // Get quaternion length.
  1.1073 +    T       Length() const                  { return sqrt(LengthSq()); }
  1.1074 +
  1.1075 +    // Get quaternion length squared.
  1.1076 +    T       LengthSq() const                { return (x * x + y * y + z * z + w * w); }
  1.1077 +
  1.1078 +    // Simple Euclidean distance in R^4 (not SLERP distance, but at least respects Haar measure)
  1.1079 +    T       Distance(const Quat& q) const	
  1.1080 +	{ 
  1.1081 +        T d1 = (*this - q).Length();
  1.1082 +        T d2 = (*this + q).Length(); // Antipodal point check
  1.1083 +        return (d1 < d2) ? d1 : d2;
  1.1084 +	}
  1.1085 +
  1.1086 +    T       DistanceSq(const Quat& q) const
  1.1087 +    {
  1.1088 +        T d1 = (*this - q).LengthSq();
  1.1089 +        T d2 = (*this + q).LengthSq(); // Antipodal point check
  1.1090 +        return (d1 < d2) ? d1 : d2;
  1.1091 +    }
  1.1092 +
  1.1093 +    T       Dot(const Quat& q) const
  1.1094 +    {
  1.1095 +        return x * q.x + y * q.y + z * q.z + w * q.w;
  1.1096 +    }
  1.1097 +
  1.1098 +	// Angle between two quaternions in radians
  1.1099 +    T       Angle(const Quat& q) const
  1.1100 +	{
  1.1101 +		return 2 * Acos(Alg::Abs(Dot(q)));
  1.1102 +	}
  1.1103 +
  1.1104 +    // Normalize
  1.1105 +    bool    IsNormalized() const            { return fabs(LengthSq() - T(1)) < ((T)MATH_DOUBLE_TOLERANCE); }
  1.1106 +
  1.1107 +    void    Normalize()                     
  1.1108 +	{
  1.1109 + 		T l = Length();
  1.1110 +		OVR_ASSERT(l != T(0));
  1.1111 +		*this /= l; 
  1.1112 +	}
  1.1113 +
  1.1114 +	Quat    Normalized() const              
  1.1115 +	{ 
  1.1116 +		T l = Length();
  1.1117 +		OVR_ASSERT(l != T(0));
  1.1118 +		return *this / l; 
  1.1119 +	}
  1.1120 +
  1.1121 +    // Returns conjugate of the quaternion. Produces inverse rotation if quaternion is normalized.
  1.1122 +    Quat    Conj() const                    { return Quat(-x, -y, -z, w); }
  1.1123 +
  1.1124 +    // Quaternion multiplication. Combines quaternion rotations, performing the one on the 
  1.1125 +    // right hand side first.
  1.1126 +    Quat  operator* (const Quat& b) const   { return Quat(w * b.x + x * b.w + y * b.z - z * b.y,
  1.1127 +                                                          w * b.y - x * b.z + y * b.w + z * b.x,
  1.1128 +                                                          w * b.z + x * b.y - y * b.x + z * b.w,
  1.1129 +                                                          w * b.w - x * b.x - y * b.y - z * b.z); }
  1.1130 +
  1.1131 +    // 
  1.1132 +    // this^p normalized; same as rotating by this p times.
  1.1133 +    Quat PowNormalized(T p) const
  1.1134 +    {
  1.1135 +        Vector3<T> v;
  1.1136 +        T          a;
  1.1137 +        GetAxisAngle(&v, &a);
  1.1138 +        return Quat(v, a * p);
  1.1139 +    }
  1.1140 +
  1.1141 +    // Normalized linear interpolation of quaternions
  1.1142 +    Quat Nlerp(const Quat& other, T a)
  1.1143 +    {
  1.1144 +        T sign = (Dot(other) >= 0) ? 1 : -1;
  1.1145 +        return (*this * sign * a + other * (1-a)).Normalized();
  1.1146 +    }
  1.1147 +    
  1.1148 +    // Rotate transforms vector in a manner that matches Matrix rotations (counter-clockwise,
  1.1149 +    // assuming negative direction of the axis). Standard formula: q(t) * V * q(t)^-1. 
  1.1150 +    Vector3<T> Rotate(const Vector3<T>& v) const
  1.1151 +    {
  1.1152 +        return ((*this * Quat<T>(v.x, v.y, v.z, T(0))) * Inverted()).Imag();
  1.1153 +    }
  1.1154 +    
  1.1155 +    // Inversed quaternion rotates in the opposite direction.
  1.1156 +    Quat        Inverted() const
  1.1157 +    {
  1.1158 +        return Quat(-x, -y, -z, w);
  1.1159 +    }
  1.1160 +
  1.1161 +    // Sets this quaternion to the one rotates in the opposite direction.
  1.1162 +    void        Invert()
  1.1163 +    {
  1.1164 +        *this = Quat(-x, -y, -z, w);
  1.1165 +    }
  1.1166 +    
  1.1167 +    // GetEulerAngles extracts Euler angles from the quaternion, in the specified order of
  1.1168 +    // axis rotations and the specified coordinate system. Right-handed coordinate system
  1.1169 +    // is the default, with CCW rotations while looking in the negative axis direction.
  1.1170 +    // Here a,b,c, are the Yaw/Pitch/Roll angles to be returned.
  1.1171 +    // rotation a around axis A1
  1.1172 +    // is followed by rotation b around axis A2
  1.1173 +    // is followed by rotation c around axis A3
  1.1174 +    // rotations are CCW or CW (D) in LH or RH coordinate system (S)
  1.1175 +	template <Axis A1, Axis A2, Axis A3, RotateDirection D, HandedSystem S>
  1.1176 +    void GetEulerAngles(T *a, T *b, T *c) const 
  1.1177 +    {
  1.1178 +        static_assert((A1 != A2) && (A2 != A3) && (A1 != A3), "(A1 != A2) && (A2 != A3) && (A1 != A3)");
  1.1179 +
  1.1180 +        T Q[3] = { x, y, z };  //Quaternion components x,y,z
  1.1181 +
  1.1182 +        T ww  = w*w;
  1.1183 +        T Q11 = Q[A1]*Q[A1];
  1.1184 +        T Q22 = Q[A2]*Q[A2];
  1.1185 +        T Q33 = Q[A3]*Q[A3];
  1.1186 +
  1.1187 +        T psign = T(-1);
  1.1188 +        // Determine whether even permutation
  1.1189 +        if (((A1 + 1) % 3 == A2) && ((A2 + 1) % 3 == A3))
  1.1190 +            psign = T(1);
  1.1191 +        
  1.1192 +        T s2 = psign * T(2) * (psign*w*Q[A2] + Q[A1]*Q[A3]);
  1.1193 +
  1.1194 +        if (s2 < T(-1) + ((T)MATH_DOUBLE_SINGULARITYRADIUS))
  1.1195 +        { // South pole singularity
  1.1196 +            *a = T(0);
  1.1197 +            *b = -S*D*((T)MATH_DOUBLE_PIOVER2);
  1.1198 +            *c = S*D*atan2(T(2)*(psign*Q[A1]*Q[A2] + w*Q[A3]),
  1.1199 +		                   ww + Q22 - Q11 - Q33 );
  1.1200 +        }
  1.1201 +        else if (s2 > T(1) - ((T)MATH_DOUBLE_SINGULARITYRADIUS))
  1.1202 +        {  // North pole singularity
  1.1203 +            *a = T(0);
  1.1204 +            *b = S*D*((T)MATH_DOUBLE_PIOVER2);
  1.1205 +            *c = S*D*atan2(T(2)*(psign*Q[A1]*Q[A2] + w*Q[A3]),
  1.1206 +		                   ww + Q22 - Q11 - Q33);
  1.1207 +        }
  1.1208 +        else
  1.1209 +        {
  1.1210 +            *a = -S*D*atan2(T(-2)*(w*Q[A1] - psign*Q[A2]*Q[A3]),
  1.1211 +		                    ww + Q33 - Q11 - Q22);
  1.1212 +            *b = S*D*asin(s2);
  1.1213 +            *c = S*D*atan2(T(2)*(w*Q[A3] - psign*Q[A1]*Q[A2]),
  1.1214 +		                   ww + Q11 - Q22 - Q33);
  1.1215 +        }      
  1.1216 +        return;
  1.1217 +    }
  1.1218 +
  1.1219 +    template <Axis A1, Axis A2, Axis A3, RotateDirection D>
  1.1220 +    void GetEulerAngles(T *a, T *b, T *c) const
  1.1221 +    { GetEulerAngles<A1, A2, A3, D, Handed_R>(a, b, c); }
  1.1222 +
  1.1223 +    template <Axis A1, Axis A2, Axis A3>
  1.1224 +    void GetEulerAngles(T *a, T *b, T *c) const
  1.1225 +    { GetEulerAngles<A1, A2, A3, Rotate_CCW, Handed_R>(a, b, c); }
  1.1226 +
  1.1227 +    // GetEulerAnglesABA extracts Euler angles from the quaternion, in the specified order of
  1.1228 +    // axis rotations and the specified coordinate system. Right-handed coordinate system
  1.1229 +    // is the default, with CCW rotations while looking in the negative axis direction.
  1.1230 +    // Here a,b,c, are the Yaw/Pitch/Roll angles to be returned.
  1.1231 +    // rotation a around axis A1
  1.1232 +    // is followed by rotation b around axis A2
  1.1233 +    // is followed by rotation c around axis A1
  1.1234 +    // Rotations are CCW or CW (D) in LH or RH coordinate system (S)
  1.1235 +    template <Axis A1, Axis A2, RotateDirection D, HandedSystem S>
  1.1236 +    void GetEulerAnglesABA(T *a, T *b, T *c) const
  1.1237 +    {
  1.1238 +        static_assert(A1 != A2, "A1 != A2");
  1.1239 +
  1.1240 +        T Q[3] = {x, y, z}; // Quaternion components
  1.1241 +
  1.1242 +        // Determine the missing axis that was not supplied
  1.1243 +        int m = 3 - A1 - A2;
  1.1244 +
  1.1245 +        T ww = w*w;
  1.1246 +        T Q11 = Q[A1]*Q[A1];
  1.1247 +        T Q22 = Q[A2]*Q[A2];
  1.1248 +        T Qmm = Q[m]*Q[m];
  1.1249 +
  1.1250 +        T psign = T(-1);
  1.1251 +        if ((A1 + 1) % 3 == A2) // Determine whether even permutation
  1.1252 +        {
  1.1253 +            psign = T(1);
  1.1254 +        }
  1.1255 +
  1.1256 +        T c2 = ww + Q11 - Q22 - Qmm;
  1.1257 +        if (c2 < T(-1) + Math<T>::SingularityRadius)
  1.1258 +        { // South pole singularity
  1.1259 +            *a = T(0);
  1.1260 +            *b = S*D*((T)MATH_DOUBLE_PI);
  1.1261 +            *c = S*D*atan2( T(2)*(w*Q[A1] - psign*Q[A2]*Q[m]),
  1.1262 +		                    ww + Q22 - Q11 - Qmm);
  1.1263 +        }
  1.1264 +        else if (c2 > T(1) - Math<T>::SingularityRadius)
  1.1265 +        {  // North pole singularity
  1.1266 +            *a = T(0);
  1.1267 +            *b = T(0);
  1.1268 +            *c = S*D*atan2( T(2)*(w*Q[A1] - psign*Q[A2]*Q[m]),
  1.1269 +		                   ww + Q22 - Q11 - Qmm);
  1.1270 +        }
  1.1271 +        else
  1.1272 +        {
  1.1273 +            *a = S*D*atan2( psign*w*Q[m] + Q[A1]*Q[A2],
  1.1274 +		                   w*Q[A2] -psign*Q[A1]*Q[m]);
  1.1275 +            *b = S*D*acos(c2);
  1.1276 +            *c = S*D*atan2( -psign*w*Q[m] + Q[A1]*Q[A2],
  1.1277 +		                   w*Q[A2] + psign*Q[A1]*Q[m]);
  1.1278 +        }
  1.1279 +        return;
  1.1280 +    }
  1.1281 +};
  1.1282 +
  1.1283 +typedef Quat<float>  Quatf;
  1.1284 +typedef Quat<double> Quatd;
  1.1285 +
  1.1286 +static_assert((sizeof(Quatf) == 4*sizeof(float)), "sizeof(Quatf) failure");
  1.1287 +static_assert((sizeof(Quatd) == 4*sizeof(double)), "sizeof(Quatd) failure");
  1.1288 +
  1.1289 +//-------------------------------------------------------------------------------------
  1.1290 +// ***** Pose
  1.1291 +
  1.1292 +// Position and orientation combined.
  1.1293 +
  1.1294 +template<class T>
  1.1295 +class Pose
  1.1296 +{
  1.1297 +public:
  1.1298 +    typedef typename CompatibleTypes<Pose<T> >::Type CompatibleType;
  1.1299 +
  1.1300 +    Pose() { }
  1.1301 +    Pose(const Quat<T>& orientation, const Vector3<T>& pos)
  1.1302 +        : Rotation(orientation), Translation(pos) {  }
  1.1303 +    Pose(const Pose& s)
  1.1304 +        : Rotation(s.Rotation), Translation(s.Translation) {  }
  1.1305 +    Pose(const CompatibleType& s)
  1.1306 +        : Rotation(s.Orientation), Translation(s.Position) {  }
  1.1307 +    explicit Pose(const Pose<typename Math<T>::OtherFloatType> &s)
  1.1308 +        : Rotation(s.Rotation), Translation(s.Translation) {  }
  1.1309 +
  1.1310 +    operator typename CompatibleTypes<Pose<T> >::Type () const
  1.1311 +    {
  1.1312 +        typename CompatibleTypes<Pose<T> >::Type result;
  1.1313 +        result.Orientation = Rotation;
  1.1314 +        result.Position = Translation;
  1.1315 +        return result;
  1.1316 +    }
  1.1317 +
  1.1318 +    Quat<T>    Rotation;
  1.1319 +    Vector3<T> Translation;
  1.1320 +    
  1.1321 +    static_assert((sizeof(T) == sizeof(double) || sizeof(T) == sizeof(float)), "(sizeof(T) == sizeof(double) || sizeof(T) == sizeof(float))");
  1.1322 +
  1.1323 +    void ToArray(T* arr) const
  1.1324 +    {
  1.1325 +        T temp[7] =  { Rotation.x, Rotation.y, Rotation.z, Rotation.w, Translation.x, Translation.y, Translation.z };
  1.1326 +        for (int i = 0; i < 7; i++) arr[i] = temp[i];
  1.1327 +    }
  1.1328 +
  1.1329 +    static Pose<T> FromArray(const T* v)
  1.1330 +    {
  1.1331 +        Quat<T> rotation(v[0], v[1], v[2], v[3]);
  1.1332 +        Vector3<T> translation(v[4], v[5], v[6]);
  1.1333 +        return Pose<T>(rotation, translation);
  1.1334 +    }
  1.1335 +
  1.1336 +    Vector3<T> Rotate(const Vector3<T>& v) const
  1.1337 +    {
  1.1338 +        return Rotation.Rotate(v);
  1.1339 +    }
  1.1340 +
  1.1341 +    Vector3<T> Translate(const Vector3<T>& v) const
  1.1342 +    {
  1.1343 +        return v + Translation;
  1.1344 +    }
  1.1345 +
  1.1346 +    Vector3<T> Apply(const Vector3<T>& v) const
  1.1347 +    {
  1.1348 +        return Translate(Rotate(v));
  1.1349 +    }
  1.1350 +
  1.1351 +    Pose operator*(const Pose& other) const   
  1.1352 +    {
  1.1353 +        return Pose(Rotation * other.Rotation, Apply(other.Translation));
  1.1354 +    }
  1.1355 +
  1.1356 +    Pose Inverted() const   
  1.1357 +    {
  1.1358 +        Quat<T> inv = Rotation.Inverted();
  1.1359 +        return Pose(inv, inv.Rotate(-Translation));
  1.1360 +    }
  1.1361 +};
  1.1362 +
  1.1363 +typedef Pose<float>  Posef;
  1.1364 +typedef Pose<double> Posed;
  1.1365 +
  1.1366 +static_assert((sizeof(Posed) == sizeof(Quatd) + sizeof(Vector3d)), "sizeof(Posed) failure");
  1.1367 +static_assert((sizeof(Posef) == sizeof(Quatf) + sizeof(Vector3f)), "sizeof(Posef) failure");
  1.1368 +    
  1.1369 +
  1.1370 +//-------------------------------------------------------------------------------------
  1.1371 +// ***** Matrix4
  1.1372 +//
  1.1373 +// Matrix4 is a 4x4 matrix used for 3d transformations and projections.
  1.1374 +// Translation stored in the last column.
  1.1375 +// The matrix is stored in row-major order in memory, meaning that values
  1.1376 +// of the first row are stored before the next one.
  1.1377 +//
  1.1378 +// The arrangement of the matrix is chosen to be in Right-Handed 
  1.1379 +// coordinate system and counterclockwise rotations when looking down
  1.1380 +// the axis
  1.1381 +//
  1.1382 +// Transformation Order:
  1.1383 +//   - Transformations are applied from right to left, so the expression
  1.1384 +//     M1 * M2 * M3 * V means that the vector V is transformed by M3 first,
  1.1385 +//     followed by M2 and M1. 
  1.1386 +//
  1.1387 +// Coordinate system: Right Handed
  1.1388 +//
  1.1389 +// Rotations: Counterclockwise when looking down the axis. All angles are in radians.
  1.1390 +//    
  1.1391 +//  | sx   01   02   tx |    // First column  (sx, 10, 20): Axis X basis vector.
  1.1392 +//  | 10   sy   12   ty |    // Second column (01, sy, 21): Axis Y basis vector.
  1.1393 +//  | 20   21   sz   tz |    // Third columnt (02, 12, sz): Axis Z basis vector.
  1.1394 +//  | 30   31   32   33 |
  1.1395 +//
  1.1396 +//  The basis vectors are first three columns.
  1.1397 +
  1.1398 +template<class T>
  1.1399 +class Matrix4
  1.1400 +{
  1.1401 +    static const Matrix4 IdentityValue;
  1.1402 +
  1.1403 +public:
  1.1404 +    T M[4][4];    
  1.1405 +
  1.1406 +    enum NoInitType { NoInit };
  1.1407 +
  1.1408 +    // Construct with no memory initialization.
  1.1409 +    Matrix4(NoInitType) { }
  1.1410 +
  1.1411 +    // By default, we construct identity matrix.
  1.1412 +    Matrix4()
  1.1413 +    {
  1.1414 +        SetIdentity();        
  1.1415 +    }
  1.1416 +
  1.1417 +    Matrix4(T m11, T m12, T m13, T m14,
  1.1418 +            T m21, T m22, T m23, T m24,
  1.1419 +            T m31, T m32, T m33, T m34,
  1.1420 +            T m41, T m42, T m43, T m44)
  1.1421 +    {
  1.1422 +        M[0][0] = m11; M[0][1] = m12; M[0][2] = m13; M[0][3] = m14;
  1.1423 +        M[1][0] = m21; M[1][1] = m22; M[1][2] = m23; M[1][3] = m24;
  1.1424 +        M[2][0] = m31; M[2][1] = m32; M[2][2] = m33; M[2][3] = m34;
  1.1425 +        M[3][0] = m41; M[3][1] = m42; M[3][2] = m43; M[3][3] = m44;
  1.1426 +    }
  1.1427 +
  1.1428 +    Matrix4(T m11, T m12, T m13,
  1.1429 +            T m21, T m22, T m23,
  1.1430 +            T m31, T m32, T m33)
  1.1431 +    {
  1.1432 +        M[0][0] = m11; M[0][1] = m12; M[0][2] = m13; M[0][3] = 0;
  1.1433 +        M[1][0] = m21; M[1][1] = m22; M[1][2] = m23; M[1][3] = 0;
  1.1434 +        M[2][0] = m31; M[2][1] = m32; M[2][2] = m33; M[2][3] = 0;
  1.1435 +        M[3][0] = 0;   M[3][1] = 0;   M[3][2] = 0;   M[3][3] = 1;
  1.1436 +    }
  1.1437 +
  1.1438 +    explicit Matrix4(const Quat<T>& q)
  1.1439 +    {
  1.1440 +        T ww = q.w*q.w;
  1.1441 +        T xx = q.x*q.x;
  1.1442 +        T yy = q.y*q.y;
  1.1443 +        T zz = q.z*q.z;
  1.1444 +
  1.1445 +        M[0][0] = ww + xx - yy - zz;       M[0][1] = 2 * (q.x*q.y - q.w*q.z); M[0][2] = 2 * (q.x*q.z + q.w*q.y); M[0][3] = 0;
  1.1446 +        M[1][0] = 2 * (q.x*q.y + q.w*q.z); M[1][1] = ww - xx + yy - zz;       M[1][2] = 2 * (q.y*q.z - q.w*q.x); M[1][3] = 0;
  1.1447 +        M[2][0] = 2 * (q.x*q.z - q.w*q.y); M[2][1] = 2 * (q.y*q.z + q.w*q.x); M[2][2] = ww - xx - yy + zz;       M[2][3] = 0;
  1.1448 +        M[3][0] = 0;                       M[3][1] = 0;                       M[3][2] = 0;                       M[3][3] = 1;
  1.1449 +    }
  1.1450 +
  1.1451 +    explicit Matrix4(const Pose<T>& p)
  1.1452 +    {
  1.1453 +        Matrix4 result(p.Rotation);
  1.1454 +        result.SetTranslation(p.Translation);
  1.1455 +        *this = result;
  1.1456 +    }
  1.1457 +
  1.1458 +    // C-interop support
  1.1459 +    explicit Matrix4(const Matrix4<typename Math<T>::OtherFloatType> &src)
  1.1460 +    {
  1.1461 +        for (int i = 0; i < 4; i++)
  1.1462 +            for (int j = 0; j < 4; j++)
  1.1463 +                M[i][j] = (T)src.M[i][j];
  1.1464 +    }
  1.1465 +
  1.1466 +    // C-interop support.
  1.1467 +    Matrix4(const typename CompatibleTypes<Matrix4<T> >::Type& s) 
  1.1468 +    {
  1.1469 +        static_assert(sizeof(s) == sizeof(Matrix4), "sizeof(s) == sizeof(Matrix4)");
  1.1470 +        memcpy(M, s.M, sizeof(M));
  1.1471 +    }
  1.1472 +
  1.1473 +    operator typename CompatibleTypes<Matrix4<T> >::Type () const
  1.1474 +    {
  1.1475 +        typename CompatibleTypes<Matrix4<T> >::Type result;
  1.1476 +        static_assert(sizeof(result) == sizeof(Matrix4), "sizeof(result) == sizeof(Matrix4)");
  1.1477 +        memcpy(result.M, M, sizeof(M));
  1.1478 +        return result;
  1.1479 +    }
  1.1480 +
  1.1481 +    void ToString(char* dest, size_t destsize) const
  1.1482 +    {
  1.1483 +        size_t pos = 0;
  1.1484 +        for (int r=0; r<4; r++)
  1.1485 +            for (int c=0; c<4; c++)
  1.1486 +                pos += OVR_sprintf(dest+pos, destsize-pos, "%g ", M[r][c]);
  1.1487 +    }
  1.1488 +
  1.1489 +    static Matrix4 FromString(const char* src)
  1.1490 +    {
  1.1491 +        Matrix4 result;
  1.1492 +		if (src)
  1.1493 +		{
  1.1494 +        for (int r=0; r<4; r++)
  1.1495 +			{
  1.1496 +            for (int c=0; c<4; c++)
  1.1497 +            {
  1.1498 +                result.M[r][c] = (T)atof(src);
  1.1499 +                while (src && *src != ' ')
  1.1500 +					{
  1.1501 +                    src++;
  1.1502 +					}
  1.1503 +                while (src && *src == ' ')
  1.1504 +					{
  1.1505 +                    src++;
  1.1506 +            }
  1.1507 +				}
  1.1508 +			}
  1.1509 +		}
  1.1510 +        return result;
  1.1511 +    }
  1.1512 +
  1.1513 +    static const Matrix4& Identity()  { return IdentityValue; }
  1.1514 +
  1.1515 +    void SetIdentity()
  1.1516 +    {
  1.1517 +        M[0][0] = M[1][1] = M[2][2] = M[3][3] = 1;
  1.1518 +        M[0][1] = M[1][0] = M[2][3] = M[3][1] = 0;
  1.1519 +        M[0][2] = M[1][2] = M[2][0] = M[3][2] = 0;
  1.1520 +        M[0][3] = M[1][3] = M[2][1] = M[3][0] = 0;
  1.1521 +    }
  1.1522 +
  1.1523 +	void SetXBasis(const Vector3f & v)
  1.1524 +	{
  1.1525 +		M[0][0] = v.x;
  1.1526 +		M[1][0] = v.y;
  1.1527 +		M[2][0] = v.z;
  1.1528 +	}
  1.1529 +	Vector3f GetXBasis() const
  1.1530 +	{
  1.1531 +		return Vector3f(M[0][0], M[1][0], M[2][0]);
  1.1532 +	}
  1.1533 +
  1.1534 +	void SetYBasis(const Vector3f & v)
  1.1535 +	{
  1.1536 +		M[0][1] = v.x;
  1.1537 +		M[1][1] = v.y;
  1.1538 +		M[2][1] = v.z;
  1.1539 +	}
  1.1540 +	Vector3f GetYBasis() const
  1.1541 +	{
  1.1542 +		return Vector3f(M[0][1], M[1][1], M[2][1]);
  1.1543 +	}
  1.1544 +
  1.1545 +	void SetZBasis(const Vector3f & v)
  1.1546 +	{
  1.1547 +		M[0][2] = v.x;
  1.1548 +		M[1][2] = v.y;
  1.1549 +		M[2][2] = v.z;
  1.1550 +	}
  1.1551 +	Vector3f GetZBasis() const
  1.1552 +	{
  1.1553 +		return Vector3f(M[0][2], M[1][2], M[2][2]);
  1.1554 +	}
  1.1555 +
  1.1556 +	bool operator== (const Matrix4& b) const
  1.1557 +	{
  1.1558 +		bool isEqual = true;
  1.1559 +        for (int i = 0; i < 4; i++)
  1.1560 +            for (int j = 0; j < 4; j++)
  1.1561 +                isEqual &= (M[i][j] == b.M[i][j]);
  1.1562 +
  1.1563 +		return isEqual;
  1.1564 +	}
  1.1565 +
  1.1566 +    Matrix4 operator+ (const Matrix4& b) const
  1.1567 +    {
  1.1568 +        Matrix4 result(*this);
  1.1569 +        result += b;
  1.1570 +        return result;
  1.1571 +    }
  1.1572 +
  1.1573 +    Matrix4& operator+= (const Matrix4& b)
  1.1574 +    {
  1.1575 +        for (int i = 0; i < 4; i++)
  1.1576 +            for (int j = 0; j < 4; j++)
  1.1577 +                M[i][j] += b.M[i][j];
  1.1578 +        return *this;
  1.1579 +    }
  1.1580 +
  1.1581 +    Matrix4 operator- (const Matrix4& b) const
  1.1582 +    {
  1.1583 +        Matrix4 result(*this);
  1.1584 +        result -= b;
  1.1585 +        return result;
  1.1586 +    }
  1.1587 +
  1.1588 +    Matrix4& operator-= (const Matrix4& b)
  1.1589 +    {
  1.1590 +        for (int i = 0; i < 4; i++)
  1.1591 +            for (int j = 0; j < 4; j++)
  1.1592 +                M[i][j] -= b.M[i][j];
  1.1593 +        return *this;
  1.1594 +    }
  1.1595 +
  1.1596 +    // Multiplies two matrices into destination with minimum copying.
  1.1597 +    static Matrix4& Multiply(Matrix4* d, const Matrix4& a, const Matrix4& b)
  1.1598 +    {
  1.1599 +        OVR_ASSERT((d != &a) && (d != &b));
  1.1600 +        int i = 0;
  1.1601 +        do {
  1.1602 +            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.1603 +            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.1604 +            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.1605 +            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.1606 +        } while((++i) < 4);
  1.1607 +
  1.1608 +        return *d;
  1.1609 +    }
  1.1610 +
  1.1611 +    Matrix4 operator* (const Matrix4& b) const
  1.1612 +    {
  1.1613 +        Matrix4 result(Matrix4::NoInit);
  1.1614 +        Multiply(&result, *this, b);
  1.1615 +        return result;
  1.1616 +    }
  1.1617 +
  1.1618 +    Matrix4& operator*= (const Matrix4& b)
  1.1619 +    {
  1.1620 +        return Multiply(this, Matrix4(*this), b);
  1.1621 +    }
  1.1622 +
  1.1623 +    Matrix4 operator* (T s) const
  1.1624 +    {
  1.1625 +        Matrix4 result(*this);
  1.1626 +        result *= s;
  1.1627 +        return result;
  1.1628 +    }
  1.1629 +
  1.1630 +    Matrix4& operator*= (T s)
  1.1631 +    {
  1.1632 +        for (int i = 0; i < 4; i++)
  1.1633 +            for (int j = 0; j < 4; j++)
  1.1634 +                M[i][j] *= s;
  1.1635 +        return *this;
  1.1636 +    }
  1.1637 +
  1.1638 +
  1.1639 +    Matrix4 operator/ (T s) const
  1.1640 +    {
  1.1641 +        Matrix4 result(*this);
  1.1642 +        result /= s;
  1.1643 +        return result;
  1.1644 +    }
  1.1645 +
  1.1646 +    Matrix4& operator/= (T s)
  1.1647 +    {
  1.1648 +        for (int i = 0; i < 4; i++)
  1.1649 +            for (int j = 0; j < 4; j++)
  1.1650 +                M[i][j] /= s;
  1.1651 +        return *this;
  1.1652 +    }
  1.1653 +
  1.1654 +    Vector3<T> Transform(const Vector3<T>& v) const
  1.1655 +    {
  1.1656 +		const T rcpW = 1.0f / (M[3][0] * v.x + M[3][1] * v.y + M[3][2] * v.z + M[3][3]);
  1.1657 +		return Vector3<T>((M[0][0] * v.x + M[0][1] * v.y + M[0][2] * v.z + M[0][3]) * rcpW,
  1.1658 +						  (M[1][0] * v.x + M[1][1] * v.y + M[1][2] * v.z + M[1][3]) * rcpW,
  1.1659 +						  (M[2][0] * v.x + M[2][1] * v.y + M[2][2] * v.z + M[2][3]) * rcpW);
  1.1660 +	}
  1.1661 +
  1.1662 +	Vector4<T> Transform(const Vector4<T>& v) const
  1.1663 +	{
  1.1664 +		return Vector4<T>(M[0][0] * v.x + M[0][1] * v.y + M[0][2] * v.z + M[0][3] * v.w,
  1.1665 +						  M[1][0] * v.x + M[1][1] * v.y + M[1][2] * v.z + M[1][3] * v.w,
  1.1666 +						  M[2][0] * v.x + M[2][1] * v.y + M[2][2] * v.z + M[2][3] * v.w,
  1.1667 +						  M[3][0] * v.x + M[3][1] * v.y + M[3][2] * v.z + M[3][3] * v.w);
  1.1668 +    }
  1.1669 +
  1.1670 +    Matrix4 Transposed() const
  1.1671 +    {
  1.1672 +        return Matrix4(M[0][0], M[1][0], M[2][0], M[3][0],
  1.1673 +                        M[0][1], M[1][1], M[2][1], M[3][1],
  1.1674 +                        M[0][2], M[1][2], M[2][2], M[3][2],
  1.1675 +                        M[0][3], M[1][3], M[2][3], M[3][3]);
  1.1676 +    }
  1.1677 +
  1.1678 +    void     Transpose()
  1.1679 +    {
  1.1680 +        *this = Transposed();
  1.1681 +    }
  1.1682 +
  1.1683 +
  1.1684 +    T SubDet (const size_t* rows, const size_t* cols) const
  1.1685 +    {
  1.1686 +        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.1687 +             - 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.1688 +             + 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.1689 +    }
  1.1690 +
  1.1691 +    T Cofactor(size_t I, size_t J) const
  1.1692 +    {
  1.1693 +        const size_t indices[4][3] = {{1,2,3},{0,2,3},{0,1,3},{0,1,2}};
  1.1694 +        return ((I+J)&1) ? -SubDet(indices[I],indices[J]) : SubDet(indices[I],indices[J]);
  1.1695 +    }
  1.1696 +
  1.1697 +    T    Determinant() const
  1.1698 +    {
  1.1699 +        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.1700 +    }
  1.1701 +
  1.1702 +    Matrix4 Adjugated() const
  1.1703 +    {
  1.1704 +        return Matrix4(Cofactor(0,0), Cofactor(1,0), Cofactor(2,0), Cofactor(3,0), 
  1.1705 +                        Cofactor(0,1), Cofactor(1,1), Cofactor(2,1), Cofactor(3,1), 
  1.1706 +                        Cofactor(0,2), Cofactor(1,2), Cofactor(2,2), Cofactor(3,2),
  1.1707 +                        Cofactor(0,3), Cofactor(1,3), Cofactor(2,3), Cofactor(3,3));
  1.1708 +    }
  1.1709 +
  1.1710 +    Matrix4 Inverted() const
  1.1711 +    {
  1.1712 +        T det = Determinant();
  1.1713 +        assert(det != 0);
  1.1714 +        return Adjugated() * (1.0f/det);
  1.1715 +    }
  1.1716 +
  1.1717 +    void Invert()
  1.1718 +    {
  1.1719 +        *this = Inverted();
  1.1720 +    }
  1.1721 +
  1.1722 +	// This is more efficient than general inverse, but ONLY works
  1.1723 +	// correctly if it is a homogeneous transform matrix (rot + trans)
  1.1724 +	Matrix4 InvertedHomogeneousTransform() const
  1.1725 +	{
  1.1726 +		// Make the inverse rotation matrix
  1.1727 +		Matrix4 rinv = this->Transposed();
  1.1728 +		rinv.M[3][0] = rinv.M[3][1] = rinv.M[3][2] = 0.0f;
  1.1729 +		// Make the inverse translation matrix
  1.1730 +		Vector3<T> tvinv(-M[0][3],-M[1][3],-M[2][3]);
  1.1731 +		Matrix4 tinv = Matrix4::Translation(tvinv);
  1.1732 +		return rinv * tinv;  // "untranslate", then "unrotate"
  1.1733 +	}
  1.1734 +
  1.1735 +	// This is more efficient than general inverse, but ONLY works
  1.1736 +	// correctly if it is a homogeneous transform matrix (rot + trans)
  1.1737 +	void InvertHomogeneousTransform()
  1.1738 +	{
  1.1739 +        *this = InvertedHomogeneousTransform();
  1.1740 +	}
  1.1741 +
  1.1742 +	// Matrix to Euler Angles conversion
  1.1743 +    // a,b,c, are the YawPitchRoll angles to be returned
  1.1744 +    // rotation a around axis A1
  1.1745 +    // is followed by rotation b around axis A2
  1.1746 +    // is followed by rotation c around axis A3
  1.1747 +    // rotations are CCW or CW (D) in LH or RH coordinate system (S)
  1.1748 +    template <Axis A1, Axis A2, Axis A3, RotateDirection D, HandedSystem S>
  1.1749 +    void ToEulerAngles(T *a, T *b, T *c) const
  1.1750 +    {
  1.1751 +        static_assert((A1 != A2) && (A2 != A3) && (A1 != A3), "(A1 != A2) && (A2 != A3) && (A1 != A3)");
  1.1752 +
  1.1753 +        T psign = -1;
  1.1754 +        if (((A1 + 1) % 3 == A2) && ((A2 + 1) % 3 == A3)) // Determine whether even permutation
  1.1755 +        psign = 1;
  1.1756 +        
  1.1757 +        T pm = psign*M[A1][A3];
  1.1758 +        if (pm < -1.0f + Math<T>::SingularityRadius)
  1.1759 +        { // South pole singularity
  1.1760 +            *a = 0;
  1.1761 +            *b = -S*D*((T)MATH_DOUBLE_PIOVER2);
  1.1762 +            *c = S*D*atan2( psign*M[A2][A1], M[A2][A2] );
  1.1763 +        }
  1.1764 +        else if (pm > 1.0f - Math<T>::SingularityRadius)
  1.1765 +        { // North pole singularity
  1.1766 +            *a = 0;
  1.1767 +            *b = S*D*((T)MATH_DOUBLE_PIOVER2);
  1.1768 +            *c = S*D*atan2( psign*M[A2][A1], M[A2][A2] );
  1.1769 +        }
  1.1770 +        else
  1.1771 +        { // Normal case (nonsingular)
  1.1772 +            *a = S*D*atan2( -psign*M[A2][A3], M[A3][A3] );
  1.1773 +            *b = S*D*asin(pm);
  1.1774 +            *c = S*D*atan2( -psign*M[A1][A2], M[A1][A1] );
  1.1775 +        }
  1.1776 +
  1.1777 +        return;
  1.1778 +    }
  1.1779 +
  1.1780 +	// Matrix to Euler Angles conversion
  1.1781 +    // a,b,c, are the YawPitchRoll angles to be returned
  1.1782 +    // rotation a around axis A1
  1.1783 +    // is followed by rotation b around axis A2
  1.1784 +    // is followed by rotation c around axis A1
  1.1785 +    // rotations are CCW or CW (D) in LH or RH coordinate system (S)
  1.1786 +    template <Axis A1, Axis A2, RotateDirection D, HandedSystem S>
  1.1787 +    void ToEulerAnglesABA(T *a, T *b, T *c) const
  1.1788 +    {        
  1.1789 +         static_assert(A1 != A2, "A1 != A2");
  1.1790 +  
  1.1791 +        // Determine the axis that was not supplied
  1.1792 +        int m = 3 - A1 - A2;
  1.1793 +
  1.1794 +        T psign = -1;
  1.1795 +        if ((A1 + 1) % 3 == A2) // Determine whether even permutation
  1.1796 +            psign = 1.0f;
  1.1797 +
  1.1798 +        T c2 = M[A1][A1];
  1.1799 +        if (c2 < -1 + Math<T>::SingularityRadius)
  1.1800 +        { // South pole singularity
  1.1801 +            *a = 0;
  1.1802 +            *b = S*D*((T)MATH_DOUBLE_PI);
  1.1803 +            *c = S*D*atan2( -psign*M[A2][m],M[A2][A2]);
  1.1804 +        }
  1.1805 +        else if (c2 > 1.0f - Math<T>::SingularityRadius)
  1.1806 +        { // North pole singularity
  1.1807 +            *a = 0;
  1.1808 +            *b = 0;
  1.1809 +            *c = S*D*atan2( -psign*M[A2][m],M[A2][A2]);
  1.1810 +        }
  1.1811 +        else
  1.1812 +        { // Normal case (nonsingular)
  1.1813 +            *a = S*D*atan2( M[A2][A1],-psign*M[m][A1]);
  1.1814 +            *b = S*D*acos(c2);
  1.1815 +            *c = S*D*atan2( M[A1][A2],psign*M[A1][m]);
  1.1816 +        }
  1.1817 +        return;
  1.1818 +    }
  1.1819 +  
  1.1820 +    // Creates a matrix that converts the vertices from one coordinate system
  1.1821 +    // to another.
  1.1822 +    static Matrix4 AxisConversion(const WorldAxes& to, const WorldAxes& from)
  1.1823 +    {        
  1.1824 +        // Holds axis values from the 'to' structure
  1.1825 +        int toArray[3] = { to.XAxis, to.YAxis, to.ZAxis };
  1.1826 +
  1.1827 +        // The inverse of the toArray
  1.1828 +        int inv[4]; 
  1.1829 +        inv[0] = inv[abs(to.XAxis)] = 0;
  1.1830 +        inv[abs(to.YAxis)] = 1;
  1.1831 +        inv[abs(to.ZAxis)] = 2;
  1.1832 +
  1.1833 +        Matrix4 m(0,  0,  0, 
  1.1834 +                  0,  0,  0,
  1.1835 +                  0,  0,  0);
  1.1836 +
  1.1837 +        // Only three values in the matrix need to be changed to 1 or -1.
  1.1838 +        m.M[inv[abs(from.XAxis)]][0] = T(from.XAxis/toArray[inv[abs(from.XAxis)]]);
  1.1839 +        m.M[inv[abs(from.YAxis)]][1] = T(from.YAxis/toArray[inv[abs(from.YAxis)]]);
  1.1840 +        m.M[inv[abs(from.ZAxis)]][2] = T(from.ZAxis/toArray[inv[abs(from.ZAxis)]]);
  1.1841 +        return m;
  1.1842 +    } 
  1.1843 +
  1.1844 +
  1.1845 +	// Creates a matrix for translation by vector
  1.1846 +    static Matrix4 Translation(const Vector3<T>& v)
  1.1847 +    {
  1.1848 +        Matrix4 t;
  1.1849 +        t.M[0][3] = v.x;
  1.1850 +        t.M[1][3] = v.y;
  1.1851 +        t.M[2][3] = v.z;
  1.1852 +        return t;
  1.1853 +    }
  1.1854 +
  1.1855 +	// Creates a matrix for translation by vector
  1.1856 +    static Matrix4 Translation(T x, T y, T z = 0.0f)
  1.1857 +    {
  1.1858 +        Matrix4 t;
  1.1859 +        t.M[0][3] = x;
  1.1860 +        t.M[1][3] = y;
  1.1861 +        t.M[2][3] = z;
  1.1862 +        return t;
  1.1863 +    }
  1.1864 +
  1.1865 +	// Sets the translation part
  1.1866 +    void SetTranslation(const Vector3<T>& v)
  1.1867 +    {
  1.1868 +        M[0][3] = v.x;
  1.1869 +        M[1][3] = v.y;
  1.1870 +        M[2][3] = v.z;
  1.1871 +    }
  1.1872 +
  1.1873 +    Vector3<T> GetTranslation() const
  1.1874 +    {
  1.1875 +        return Vector3<T>( M[0][3], M[1][3], M[2][3] );
  1.1876 +    }
  1.1877 +
  1.1878 +	// Creates a matrix for scaling by vector
  1.1879 +    static Matrix4 Scaling(const Vector3<T>& v)
  1.1880 +    {
  1.1881 +        Matrix4 t;
  1.1882 +        t.M[0][0] = v.x;
  1.1883 +        t.M[1][1] = v.y;
  1.1884 +        t.M[2][2] = v.z;
  1.1885 +        return t;
  1.1886 +    }
  1.1887 +
  1.1888 +	// Creates a matrix for scaling by vector
  1.1889 +    static Matrix4 Scaling(T x, T y, T z)
  1.1890 +    {
  1.1891 +        Matrix4 t;
  1.1892 +        t.M[0][0] = x;
  1.1893 +        t.M[1][1] = y;
  1.1894 +        t.M[2][2] = z;
  1.1895 +        return t;
  1.1896 +    }
  1.1897 +
  1.1898 +	// Creates a matrix for scaling by constant
  1.1899 +    static Matrix4 Scaling(T s)
  1.1900 +    {
  1.1901 +        Matrix4 t;
  1.1902 +        t.M[0][0] = s;
  1.1903 +        t.M[1][1] = s;
  1.1904 +        t.M[2][2] = s;
  1.1905 +        return t;
  1.1906 +    }
  1.1907 +
  1.1908 +    // Simple L1 distance in R^12
  1.1909 +	T Distance(const Matrix4& m2) const           
  1.1910 +	{ 
  1.1911 +		T d = fabs(M[0][0] - m2.M[0][0]) + fabs(M[0][1] - m2.M[0][1]);
  1.1912 +		d += fabs(M[0][2] - m2.M[0][2]) + fabs(M[0][3] - m2.M[0][3]);
  1.1913 +		d += fabs(M[1][0] - m2.M[1][0]) + fabs(M[1][1] - m2.M[1][1]);
  1.1914 +		d += fabs(M[1][2] - m2.M[1][2]) + fabs(M[1][3] - m2.M[1][3]);
  1.1915 +		d += fabs(M[2][0] - m2.M[2][0]) + fabs(M[2][1] - m2.M[2][1]);
  1.1916 +		d += fabs(M[2][2] - m2.M[2][2]) + fabs(M[2][3] - m2.M[2][3]);
  1.1917 +		d += fabs(M[3][0] - m2.M[3][0]) + fabs(M[3][1] - m2.M[3][1]);
  1.1918 +		d += fabs(M[3][2] - m2.M[3][2]) + fabs(M[3][3] - m2.M[3][3]);
  1.1919 +		return d; 
  1.1920 +	}
  1.1921 +
  1.1922 +    // Creates a rotation matrix rotating around the X axis by 'angle' radians.
  1.1923 +    // Just for quick testing.  Not for final API.  Need to remove case.
  1.1924 +    static Matrix4 RotationAxis(Axis A, T angle, RotateDirection d, HandedSystem s)
  1.1925 +    {
  1.1926 +        T sina = s * d *sin(angle);
  1.1927 +        T cosa = cos(angle);
  1.1928 +        
  1.1929 +        switch(A)
  1.1930 +        {
  1.1931 +        case Axis_X:
  1.1932 +            return Matrix4(1,  0,     0, 
  1.1933 +                           0,  cosa,  -sina,
  1.1934 +                           0,  sina,  cosa);
  1.1935 +        case Axis_Y:
  1.1936 +            return Matrix4(cosa,  0,   sina, 
  1.1937 +                           0,     1,   0,
  1.1938 +                           -sina, 0,   cosa);
  1.1939 +        case Axis_Z:
  1.1940 +            return Matrix4(cosa,  -sina,  0, 
  1.1941 +                           sina,  cosa,   0,
  1.1942 +                           0,     0,      1);
  1.1943 +        }
  1.1944 +    }
  1.1945 +
  1.1946 +
  1.1947 +    // Creates a rotation matrix rotating around the X axis by 'angle' radians.
  1.1948 +    // Rotation direction is depends on the coordinate system:
  1.1949 +    // RHS (Oculus default): Positive angle values rotate Counter-clockwise (CCW),
  1.1950 +    //                        while looking in the negative axis direction. This is the
  1.1951 +    //                        same as looking down from positive axis values towards origin.
  1.1952 +    // LHS: Positive angle values rotate clock-wise (CW), while looking in the
  1.1953 +    //       negative axis direction.
  1.1954 +    static Matrix4 RotationX(T angle)
  1.1955 +    {
  1.1956 +        T sina = sin(angle);
  1.1957 +        T cosa = cos(angle);
  1.1958 +        return Matrix4(1,  0,     0, 
  1.1959 +                       0,  cosa,  -sina,
  1.1960 +                       0,  sina,  cosa);
  1.1961 +    }
  1.1962 +
  1.1963 +    // Creates a rotation matrix rotating around the Y axis by 'angle' radians.
  1.1964 +    // Rotation direction is depends on the coordinate system:
  1.1965 +    //  RHS (Oculus default): Positive angle values rotate Counter-clockwise (CCW),
  1.1966 +    //                        while looking in the negative axis direction. This is the
  1.1967 +    //                        same as looking down from positive axis values towards origin.
  1.1968 +    //  LHS: Positive angle values rotate clock-wise (CW), while looking in the
  1.1969 +    //       negative axis direction.
  1.1970 +    static Matrix4 RotationY(T angle)
  1.1971 +    {
  1.1972 +        T sina = sin(angle);
  1.1973 +        T cosa = cos(angle);
  1.1974 +        return Matrix4(cosa,  0,   sina, 
  1.1975 +                       0,     1,   0,
  1.1976 +                       -sina, 0,   cosa);
  1.1977 +    }
  1.1978 +
  1.1979 +    // Creates a rotation matrix rotating around the Z axis by 'angle' radians.
  1.1980 +    // Rotation direction is depends on the coordinate system:
  1.1981 +    //  RHS (Oculus default): Positive angle values rotate Counter-clockwise (CCW),
  1.1982 +    //                        while looking in the negative axis direction. This is the
  1.1983 +    //                        same as looking down from positive axis values towards origin.
  1.1984 +    //  LHS: Positive angle values rotate clock-wise (CW), while looking in the
  1.1985 +    //       negative axis direction.
  1.1986 +    static Matrix4 RotationZ(T angle)
  1.1987 +    {
  1.1988 +        T sina = sin(angle);
  1.1989 +        T cosa = cos(angle);
  1.1990 +        return Matrix4(cosa,  -sina,  0, 
  1.1991 +                       sina,  cosa,   0,
  1.1992 +                       0,     0,      1);
  1.1993 +    }
  1.1994 +
  1.1995 +    // LookAtRH creates a View transformation matrix for right-handed coordinate system.
  1.1996 +    // The resulting matrix points camera from 'eye' towards 'at' direction, with 'up'
  1.1997 +    // specifying the up vector. The resulting matrix should be used with PerspectiveRH
  1.1998 +    // projection.
  1.1999 +    static Matrix4 LookAtRH(const Vector3<T>& eye, const Vector3<T>& at, const Vector3<T>& up)
  1.2000 +    {
  1.2001 +        Vector3<T> z = (eye - at).Normalized();  // Forward
  1.2002 +        Vector3<T> x = up.Cross(z).Normalized(); // Right
  1.2003 +        Vector3<T> y = z.Cross(x);
  1.2004 +
  1.2005 +        Matrix4 m(x.x,  x.y,  x.z,  -(x.Dot(eye)),
  1.2006 +                  y.x,  y.y,  y.z,  -(y.Dot(eye)),
  1.2007 +                  z.x,  z.y,  z.z,  -(z.Dot(eye)),
  1.2008 +                  0,    0,    0,    1 );
  1.2009 +        return m;
  1.2010 +    }
  1.2011 +    
  1.2012 +    // LookAtLH creates a View transformation matrix for left-handed coordinate system.
  1.2013 +    // The resulting matrix points camera from 'eye' towards 'at' direction, with 'up'
  1.2014 +    // specifying the up vector. 
  1.2015 +    static Matrix4 LookAtLH(const Vector3<T>& eye, const Vector3<T>& at, const Vector3<T>& up)
  1.2016 +    {
  1.2017 +        Vector3<T> z = (at - eye).Normalized();  // Forward
  1.2018 +        Vector3<T> x = up.Cross(z).Normalized(); // Right
  1.2019 +        Vector3<T> y = z.Cross(x);
  1.2020 +
  1.2021 +        Matrix4 m(x.x,  x.y,  x.z,  -(x.Dot(eye)),
  1.2022 +                  y.x,  y.y,  y.z,  -(y.Dot(eye)),
  1.2023 +                  z.x,  z.y,  z.z,  -(z.Dot(eye)),
  1.2024 +                  0,    0,    0,    1 ); 
  1.2025 +        return m;
  1.2026 +    }
  1.2027 +    
  1.2028 +    // PerspectiveRH creates a right-handed perspective projection matrix that can be
  1.2029 +    // used with the Oculus sample renderer. 
  1.2030 +    //  yfov   - Specifies vertical field of view in radians.
  1.2031 +    //  aspect - Screen aspect ration, which is usually width/height for square pixels.
  1.2032 +    //           Note that xfov = yfov * aspect.
  1.2033 +    //  znear  - Absolute value of near Z clipping clipping range.
  1.2034 +    //  zfar   - Absolute value of far  Z clipping clipping range (larger then near).
  1.2035 +    // Even though RHS usually looks in the direction of negative Z, positive values
  1.2036 +    // are expected for znear and zfar.
  1.2037 +    static Matrix4 PerspectiveRH(T yfov, T aspect, T znear, T zfar)
  1.2038 +    {
  1.2039 +        Matrix4 m;
  1.2040 +        T tanHalfFov = tan(yfov * 0.5f);
  1.2041 +
  1.2042 +        m.M[0][0] = 1. / (aspect * tanHalfFov);
  1.2043 +        m.M[1][1] = 1. / tanHalfFov;
  1.2044 +        m.M[2][2] = zfar / (znear - zfar);
  1.2045 +        m.M[3][2] = -1.;
  1.2046 +        m.M[2][3] = (zfar * znear) / (znear - zfar);
  1.2047 +        m.M[3][3] = 0.;
  1.2048 +
  1.2049 +        // Note: Post-projection matrix result assumes Left-Handed coordinate system,
  1.2050 +        //       with Y up, X right and Z forward. This supports positive z-buffer values.
  1.2051 +		// This is the case even for RHS coordinate input.
  1.2052 +        return m;
  1.2053 +    }
  1.2054 +    
  1.2055 +    // PerspectiveLH creates a left-handed perspective projection matrix that can be
  1.2056 +    // used with the Oculus sample renderer. 
  1.2057 +    //  yfov   - Specifies vertical field of view in radians.
  1.2058 +    //  aspect - Screen aspect ration, which is usually width/height for square pixels.
  1.2059 +    //           Note that xfov = yfov * aspect.
  1.2060 +    //  znear  - Absolute value of near Z clipping clipping range.
  1.2061 +    //  zfar   - Absolute value of far  Z clipping clipping range (larger then near).
  1.2062 +    static Matrix4 PerspectiveLH(T yfov, T aspect, T znear, T zfar)
  1.2063 +    {
  1.2064 +        Matrix4 m;
  1.2065 +        T tanHalfFov = tan(yfov * 0.5f);
  1.2066 +
  1.2067 +        m.M[0][0] = 1. / (aspect * tanHalfFov);
  1.2068 +        m.M[1][1] = 1. / tanHalfFov;
  1.2069 +        //m.M[2][2] = zfar / (znear - zfar);
  1.2070 +         m.M[2][2] = zfar / (zfar - znear);
  1.2071 +        m.M[3][2] = -1.;
  1.2072 +        m.M[2][3] = (zfar * znear) / (znear - zfar);
  1.2073 +        m.M[3][3] = 0.;
  1.2074 +
  1.2075 +        // Note: Post-projection matrix result assumes Left-Handed coordinate system,    
  1.2076 +        //       with Y up, X right and Z forward. This supports positive z-buffer values.
  1.2077 +        // This is the case even for RHS coordinate input. 
  1.2078 +        return m;
  1.2079 +    }
  1.2080 +
  1.2081 +    static Matrix4 Ortho2D(T w, T h)
  1.2082 +    {
  1.2083 +        Matrix4 m;
  1.2084 +        m.M[0][0] = 2.0/w;
  1.2085 +        m.M[1][1] = -2.0/h;
  1.2086 +        m.M[0][3] = -1.0;
  1.2087 +        m.M[1][3] = 1.0;
  1.2088 +        m.M[2][2] = 0;
  1.2089 +        return m;
  1.2090 +    }
  1.2091 +};
  1.2092 +
  1.2093 +typedef Matrix4<float>  Matrix4f;
  1.2094 +typedef Matrix4<double> Matrix4d;
  1.2095 +
  1.2096 +//-------------------------------------------------------------------------------------
  1.2097 +// ***** Matrix3
  1.2098 +//
  1.2099 +// Matrix3 is a 3x3 matrix used for representing a rotation matrix.
  1.2100 +// The matrix is stored in row-major order in memory, meaning that values
  1.2101 +// of the first row are stored before the next one.
  1.2102 +//
  1.2103 +// The arrangement of the matrix is chosen to be in Right-Handed 
  1.2104 +// coordinate system and counterclockwise rotations when looking down
  1.2105 +// the axis
  1.2106 +//
  1.2107 +// Transformation Order:
  1.2108 +//   - Transformations are applied from right to left, so the expression
  1.2109 +//     M1 * M2 * M3 * V means that the vector V is transformed by M3 first,
  1.2110 +//     followed by M2 and M1. 
  1.2111 +//
  1.2112 +// Coordinate system: Right Handed
  1.2113 +//
  1.2114 +// Rotations: Counterclockwise when looking down the axis. All angles are in radians.
  1.2115 +
  1.2116 +template<typename T>
  1.2117 +class SymMat3;
  1.2118 +
  1.2119 +template<class T>
  1.2120 +class Matrix3
  1.2121 +{
  1.2122 +	static const Matrix3 IdentityValue;
  1.2123 +
  1.2124 +public:
  1.2125 +	T M[3][3];    
  1.2126 +
  1.2127 +	enum NoInitType { NoInit };
  1.2128 +
  1.2129 +	// Construct with no memory initialization.
  1.2130 +	Matrix3(NoInitType) { }
  1.2131 +
  1.2132 +	// By default, we construct identity matrix.
  1.2133 +	Matrix3()
  1.2134 +	{
  1.2135 +		SetIdentity();        
  1.2136 +	}
  1.2137 +
  1.2138 +	Matrix3(T m11, T m12, T m13,
  1.2139 +			T m21, T m22, T m23,
  1.2140 +			T m31, T m32, T m33)
  1.2141 +	{
  1.2142 +		M[0][0] = m11; M[0][1] = m12; M[0][2] = m13;
  1.2143 +		M[1][0] = m21; M[1][1] = m22; M[1][2] = m23;
  1.2144 +		M[2][0] = m31; M[2][1] = m32; M[2][2] = m33;
  1.2145 +	}
  1.2146 +	
  1.2147 +	/*
  1.2148 +	explicit Matrix3(const Quat<T>& q)
  1.2149 +	{
  1.2150 +		T ww = q.w*q.w;
  1.2151 +		T xx = q.x*q.x;
  1.2152 +		T yy = q.y*q.y;
  1.2153 +		T zz = q.z*q.z;
  1.2154 +
  1.2155 +		M[0][0] = ww + xx - yy - zz;       M[0][1] = 2 * (q.x*q.y - q.w*q.z); M[0][2] = 2 * (q.x*q.z + q.w*q.y);
  1.2156 +		M[1][0] = 2 * (q.x*q.y + q.w*q.z); M[1][1] = ww - xx + yy - zz;       M[1][2] = 2 * (q.y*q.z - q.w*q.x);
  1.2157 +		M[2][0] = 2 * (q.x*q.z - q.w*q.y); M[2][1] = 2 * (q.y*q.z + q.w*q.x); M[2][2] = ww - xx - yy + zz;      
  1.2158 +	}
  1.2159 +	*/
  1.2160 +	
  1.2161 +	explicit Matrix3(const Quat<T>& q)
  1.2162 +	{
  1.2163 +		const T tx  = q.x+q.x,  ty  = q.y+q.y,  tz  = q.z+q.z;
  1.2164 +		const T twx = q.w*tx,   twy = q.w*ty,   twz = q.w*tz;
  1.2165 +		const T txx = q.x*tx,   txy = q.x*ty,   txz = q.x*tz;
  1.2166 +		const T tyy = q.y*ty,   tyz = q.y*tz,   tzz = q.z*tz;
  1.2167 +		M[0][0] = T(1) - (tyy + tzz);	M[0][1] = txy - twz;			M[0][2] = txz + twy;
  1.2168 +		M[1][0] = txy + twz;			M[1][1] = T(1) - (txx + tzz);	M[1][2] = tyz - twx;
  1.2169 +		M[2][0] = txz - twy;			M[2][1] = tyz + twx;			M[2][2] = T(1) - (txx + tyy);
  1.2170 +	}
  1.2171 +	
  1.2172 +	inline explicit Matrix3(T s)
  1.2173 +    {
  1.2174 +        M[0][0] = M[1][1] = M[2][2] = s;
  1.2175 +        M[0][1] = M[0][2] = M[1][0] = M[1][2] = M[2][0] = M[2][1] = 0;
  1.2176 +    }
  1.2177 +
  1.2178 +	explicit Matrix3(const Pose<T>& p)
  1.2179 +	{
  1.2180 +		Matrix3 result(p.Rotation);
  1.2181 +		result.SetTranslation(p.Translation);
  1.2182 +		*this = result;
  1.2183 +	}
  1.2184 +
  1.2185 +	// C-interop support
  1.2186 +	explicit Matrix3(const Matrix4<typename Math<T>::OtherFloatType> &src)
  1.2187 +	{
  1.2188 +		for (int i = 0; i < 3; i++)
  1.2189 +			for (int j = 0; j < 3; j++)
  1.2190 +				M[i][j] = (T)src.M[i][j];
  1.2191 +	}
  1.2192 +
  1.2193 +	// C-interop support.
  1.2194 +	Matrix3(const typename CompatibleTypes<Matrix3<T> >::Type& s) 
  1.2195 +	{
  1.2196 +		static_assert(sizeof(s) == sizeof(Matrix3), "sizeof(s) == sizeof(Matrix3)");
  1.2197 +		memcpy(M, s.M, sizeof(M));
  1.2198 +	}
  1.2199 +
  1.2200 +	operator const typename CompatibleTypes<Matrix3<T> >::Type () const
  1.2201 +	{
  1.2202 +		typename CompatibleTypes<Matrix3<T> >::Type result;
  1.2203 +		static_assert(sizeof(result) == sizeof(Matrix3), "sizeof(result) == sizeof(Matrix3)");
  1.2204 +		memcpy(result.M, M, sizeof(M));
  1.2205 +		return result;
  1.2206 +	}
  1.2207 +
  1.2208 +	void ToString(char* dest, size_t destsize) const
  1.2209 +	{
  1.2210 +		size_t pos = 0;
  1.2211 +		for (int r=0; r<3; r++)
  1.2212 +			for (int c=0; c<3; c++)
  1.2213 +				pos += OVR_sprintf(dest+pos, destsize-pos, "%g ", M[r][c]);
  1.2214 +	}
  1.2215 +
  1.2216 +	static Matrix3 FromString(const char* src)
  1.2217 +	{
  1.2218 +		Matrix3 result;
  1.2219 +		for (int r=0; r<3; r++)
  1.2220 +			for (int c=0; c<3; c++)
  1.2221 +			{
  1.2222 +				result.M[r][c] = (T)atof(src);
  1.2223 +				while (src && *src != ' ')
  1.2224 +					src++;
  1.2225 +				while (src && *src == ' ')
  1.2226 +					src++;
  1.2227 +			}
  1.2228 +			return result;
  1.2229 +	}
  1.2230 +
  1.2231 +	static const Matrix3& Identity()  { return IdentityValue; }
  1.2232 +
  1.2233 +	void SetIdentity()
  1.2234 +	{
  1.2235 +		M[0][0] = M[1][1] = M[2][2] = 1;
  1.2236 +		M[0][1] = M[1][0] = M[2][0] = 0;
  1.2237 +		M[0][2] = M[1][2] = M[2][1] = 0;
  1.2238 +	}
  1.2239 +
  1.2240 +	bool operator== (const Matrix3& b) const
  1.2241 +	{
  1.2242 +		bool isEqual = true;
  1.2243 +		for (int i = 0; i < 3; i++)
  1.2244 +			for (int j = 0; j < 3; j++)
  1.2245 +				isEqual &= (M[i][j] == b.M[i][j]);
  1.2246 +
  1.2247 +		return isEqual;
  1.2248 +	}
  1.2249 +
  1.2250 +	Matrix3 operator+ (const Matrix3& b) const
  1.2251 +	{
  1.2252 +        Matrix4<T> result(*this);
  1.2253 +		result += b;
  1.2254 +		return result;
  1.2255 +	}
  1.2256 +
  1.2257 +	Matrix3& operator+= (const Matrix3& b)
  1.2258 +	{
  1.2259 +		for (int i = 0; i < 3; i++)
  1.2260 +			for (int j = 0; j < 3; j++)
  1.2261 +				M[i][j] += b.M[i][j];
  1.2262 +		return *this;
  1.2263 +	}
  1.2264 +
  1.2265 +	void operator= (const Matrix3& b)
  1.2266 +	{
  1.2267 +		for (int i = 0; i < 3; i++)
  1.2268 +			for (int j = 0; j < 3; j++)
  1.2269 +				M[i][j] = b.M[i][j];
  1.2270 +		return;
  1.2271 +	}
  1.2272 +
  1.2273 +	void operator= (const SymMat3<T>& b)
  1.2274 +	{
  1.2275 +		for (int i = 0; i < 3; i++)
  1.2276 +			for (int j = 0; j < 3; j++)
  1.2277 +				M[i][j] = 0;
  1.2278 +
  1.2279 +		M[0][0] = b.v[0];
  1.2280 +		M[0][1] = b.v[1];
  1.2281 +		M[0][2] = b.v[2];
  1.2282 +		M[1][1] = b.v[3];
  1.2283 +		M[1][2] = b.v[4];
  1.2284 +		M[2][2] = b.v[5];
  1.2285 +
  1.2286 +		return;
  1.2287 +	}
  1.2288 +
  1.2289 +	Matrix3 operator- (const Matrix3& b) const
  1.2290 +	{
  1.2291 +		Matrix3 result(*this);
  1.2292 +		result -= b;
  1.2293 +		return result;
  1.2294 +	}
  1.2295 +
  1.2296 +	Matrix3& operator-= (const Matrix3& b)
  1.2297 +	{
  1.2298 +		for (int i = 0; i < 3; i++)
  1.2299 +			for (int j = 0; j < 3; j++)
  1.2300 +				M[i][j] -= b.M[i][j];
  1.2301 +		return *this;
  1.2302 +	}
  1.2303 +
  1.2304 +	// Multiplies two matrices into destination with minimum copying.
  1.2305 +	static Matrix3& Multiply(Matrix3* d, const Matrix3& a, const Matrix3& b)
  1.2306 +	{
  1.2307 +		OVR_ASSERT((d != &a) && (d != &b));
  1.2308 +		int i = 0;
  1.2309 +		do {
  1.2310 +			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];
  1.2311 +			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];
  1.2312 +			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];
  1.2313 +		} while((++i) < 3);
  1.2314 +
  1.2315 +		return *d;
  1.2316 +	}
  1.2317 +
  1.2318 +	Matrix3 operator* (const Matrix3& b) const
  1.2319 +	{
  1.2320 +		Matrix3 result(Matrix3::NoInit);
  1.2321 +		Multiply(&result, *this, b);
  1.2322 +		return result;
  1.2323 +	}
  1.2324 +
  1.2325 +	Matrix3& operator*= (const Matrix3& b)
  1.2326 +	{
  1.2327 +		return Multiply(this, Matrix3(*this), b);
  1.2328 +	}
  1.2329 +
  1.2330 +	Matrix3 operator* (T s) const
  1.2331 +	{
  1.2332 +		Matrix3 result(*this);
  1.2333 +		result *= s;
  1.2334 +		return result;
  1.2335 +	}
  1.2336 +
  1.2337 +	Matrix3& operator*= (T s)
  1.2338 +	{
  1.2339 +		for (int i = 0; i < 3; i++)
  1.2340 +			for (int j = 0; j < 3; j++)
  1.2341 +				M[i][j] *= s;
  1.2342 +		return *this;
  1.2343 +	}
  1.2344 +
  1.2345 +	Vector3<T> operator* (const Vector3<T> &b) const
  1.2346 +	{
  1.2347 +		Vector3<T> result;
  1.2348 +		result.x = M[0][0]*b.x + M[0][1]*b.y + M[0][2]*b.z;
  1.2349 +		result.y = M[1][0]*b.x + M[1][1]*b.y + M[1][2]*b.z;
  1.2350 +		result.z = M[2][0]*b.x + M[2][1]*b.y + M[2][2]*b.z;
  1.2351 +
  1.2352 +		return result;
  1.2353 +	}
  1.2354 +
  1.2355 +	Matrix3 operator/ (T s) const
  1.2356 +	{
  1.2357 +		Matrix3 result(*this);
  1.2358 +		result /= s;
  1.2359 +		return result;
  1.2360 +	}
  1.2361 +
  1.2362 +	Matrix3& operator/= (T s)
  1.2363 +	{
  1.2364 +		for (int i = 0; i < 3; i++)
  1.2365 +			for (int j = 0; j < 3; j++)
  1.2366 +				M[i][j] /= s;
  1.2367 +		return *this;
  1.2368 +	}
  1.2369 +
  1.2370 +	Vector2<T> Transform(const Vector2<T>& v) const
  1.2371 +	{
  1.2372 +		const float rcpZ = 1.0f / (M[2][0] * v.x + M[2][1] * v.y + M[2][2]);
  1.2373 +		return Vector2<T>((M[0][0] * v.x + M[0][1] * v.y + M[0][2]) * rcpZ,
  1.2374 +						  (M[1][0] * v.x + M[1][1] * v.y + M[1][2]) * rcpZ);
  1.2375 +	}
  1.2376 +
  1.2377 +	Vector3<T> Transform(const Vector3<T>& v) const
  1.2378 +	{
  1.2379 +		return Vector3<T>(M[0][0] * v.x + M[0][1] * v.y + M[0][2] * v.z,
  1.2380 +						  M[1][0] * v.x + M[1][1] * v.y + M[1][2] * v.z,
  1.2381 +						  M[2][0] * v.x + M[2][1] * v.y + M[2][2] * v.z);
  1.2382 +	}
  1.2383 +
  1.2384 +	Matrix3 Transposed() const
  1.2385 +	{
  1.2386 +		return Matrix3(M[0][0], M[1][0], M[2][0],
  1.2387 +					   M[0][1], M[1][1], M[2][1],
  1.2388 +					   M[0][2], M[1][2], M[2][2]);
  1.2389 +	}
  1.2390 +
  1.2391 +	void     Transpose()
  1.2392 +	{
  1.2393 +		*this = Transposed();
  1.2394 +	}
  1.2395 +
  1.2396 +
  1.2397 +	T SubDet (const size_t* rows, const size_t* cols) const
  1.2398 +	{
  1.2399 +		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.2400 +			 - 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.2401 +			 + 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.2402 +	}
  1.2403 +
  1.2404 +	// M += a*b.t()
  1.2405 +	inline void Rank1Add(const Vector3<T> &a, const Vector3<T> &b)
  1.2406 +	{
  1.2407 +		M[0][0] += a.x*b.x;		M[0][1] += a.x*b.y;		M[0][2] += a.x*b.z;
  1.2408 +		M[1][0] += a.y*b.x;		M[1][1] += a.y*b.y;		M[1][2] += a.y*b.z;
  1.2409 +		M[2][0] += a.z*b.x;		M[2][1] += a.z*b.y;		M[2][2] += a.z*b.z;
  1.2410 +	}
  1.2411 +
  1.2412 +	// M -= a*b.t()
  1.2413 +	inline void Rank1Sub(const Vector3<T> &a, const Vector3<T> &b)
  1.2414 +	{
  1.2415 +		M[0][0] -= a.x*b.x;		M[0][1] -= a.x*b.y;		M[0][2] -= a.x*b.z;
  1.2416 +		M[1][0] -= a.y*b.x;		M[1][1] -= a.y*b.y;		M[1][2] -= a.y*b.z;
  1.2417 +		M[2][0] -= a.z*b.x;		M[2][1] -= a.z*b.y;		M[2][2] -= a.z*b.z;
  1.2418 +	}
  1.2419 +
  1.2420 +	inline Vector3<T> Col(int c) const
  1.2421 +	{
  1.2422 +		return Vector3<T>(M[0][c], M[1][c], M[2][c]);
  1.2423 +	}
  1.2424 +
  1.2425 +	inline Vector3<T> Row(int r) const
  1.2426 +	{
  1.2427 +        return Vector3<T>(M[r][0], M[r][1], M[r][2]);
  1.2428 +	}
  1.2429 +
  1.2430 +	inline T Determinant() const
  1.2431 +	{
  1.2432 +		const Matrix3<T>& m = *this;
  1.2433 +		T d; 
  1.2434 +
  1.2435 +		d  = m.M[0][0] * (m.M[1][1]*m.M[2][2] - m.M[1][2] * m.M[2][1]);
  1.2436 +		d -= m.M[0][1] * (m.M[1][0]*m.M[2][2] - m.M[1][2] * m.M[2][0]);
  1.2437 +		d += m.M[0][2] * (m.M[1][0]*m.M[2][1] - m.M[1][1] * m.M[2][0]);
  1.2438 +
  1.2439 +		return d;
  1.2440 +	}
  1.2441 +	
  1.2442 +	inline Matrix3<T> Inverse() const
  1.2443 +    {
  1.2444 +        Matrix3<T> a;
  1.2445 +        const  Matrix3<T>& m = *this;
  1.2446 +        T d = Determinant();
  1.2447 +
  1.2448 +        assert(d != 0);
  1.2449 +        T s = T(1)/d;
  1.2450 +
  1.2451 +        a.M[0][0] = s * (m.M[1][1] * m.M[2][2] - m.M[1][2] * m.M[2][1]);   
  1.2452 +        a.M[1][0] = s * (m.M[1][2] * m.M[2][0] - m.M[1][0] * m.M[2][2]);   
  1.2453 +        a.M[2][0] = s * (m.M[1][0] * m.M[2][1] - m.M[1][1] * m.M[2][0]);   
  1.2454 +
  1.2455 +		a.M[0][1] = s * (m.M[0][2] * m.M[2][1] - m.M[0][1] * m.M[2][2]);   
  1.2456 +		a.M[1][1] = s * (m.M[0][0] * m.M[2][2] - m.M[0][2] * m.M[2][0]);   
  1.2457 +		a.M[2][1] = s * (m.M[0][1] * m.M[2][0] - m.M[0][0] * m.M[2][1]);   
  1.2458 +        
  1.2459 +		a.M[0][2] = s * (m.M[0][1] * m.M[1][2] - m.M[0][2] * m.M[1][1]);   
  1.2460 +		a.M[1][2] = s * (m.M[0][2] * m.M[1][0] - m.M[0][0] * m.M[1][2]);   
  1.2461 +		a.M[2][2] = s * (m.M[0][0] * m.M[1][1] - m.M[0][1] * m.M[1][0]);   
  1.2462 +        
  1.2463 +        return a;
  1.2464 +    }
  1.2465 +	
  1.2466 +};
  1.2467 +
  1.2468 +typedef Matrix3<float>  Matrix3f;
  1.2469 +typedef Matrix3<double> Matrix3d;
  1.2470 +
  1.2471 +//-------------------------------------------------------------------------------------
  1.2472 +
  1.2473 +template<typename T>
  1.2474 +class SymMat3
  1.2475 +{
  1.2476 +private:
  1.2477 +	typedef SymMat3<T> this_type;
  1.2478 +
  1.2479 +public:
  1.2480 +	typedef T Value_t;
  1.2481 +	// Upper symmetric
  1.2482 +	T v[6]; // _00 _01 _02 _11 _12 _22
  1.2483 +
  1.2484 +	inline SymMat3() {}
  1.2485 +
  1.2486 +	inline explicit SymMat3(T s)
  1.2487 +	{
  1.2488 +		v[0] = v[3] = v[5] = s;
  1.2489 +		v[1] = v[2] = v[4] = 0;
  1.2490 +	}
  1.2491 +
  1.2492 +	inline explicit SymMat3(T a00, T a01, T a02, T a11, T a12, T a22)
  1.2493 +	{
  1.2494 +		v[0] = a00; v[1] = a01; v[2] = a02;
  1.2495 +		v[3] = a11; v[4] = a12;
  1.2496 +		v[5] = a22;
  1.2497 +	}
  1.2498 +
  1.2499 +	static inline int Index(unsigned int i, unsigned int j)
  1.2500 +	{
  1.2501 +		return (i <= j) ? (3*i - i*(i+1)/2 + j) : (3*j - j*(j+1)/2 + i);
  1.2502 +	}
  1.2503 +
  1.2504 +	inline T operator()(int i, int j) const { return v[Index(i,j)]; }
  1.2505 +	
  1.2506 +	inline T &operator()(int i, int j) { return v[Index(i,j)]; }
  1.2507 +
  1.2508 +	template<typename U>
  1.2509 +	inline SymMat3<U> CastTo() const
  1.2510 +	{
  1.2511 +		return SymMat3<U>(static_cast<U>(v[0]), static_cast<U>(v[1]), static_cast<U>(v[2]),
  1.2512 +						  static_cast<U>(v[3]), static_cast<U>(v[4]), static_cast<U>(v[5]));
  1.2513 +	}
  1.2514 +
  1.2515 +	inline this_type& operator+=(const this_type& b)
  1.2516 +	{
  1.2517 +		v[0]+=b.v[0];
  1.2518 +		v[1]+=b.v[1];
  1.2519 +		v[2]+=b.v[2];
  1.2520 +		v[3]+=b.v[3];
  1.2521 +		v[4]+=b.v[4];
  1.2522 +		v[5]+=b.v[5];
  1.2523 +		return *this;
  1.2524 +	}
  1.2525 +
  1.2526 +	inline this_type& operator-=(const this_type& b)
  1.2527 +	{
  1.2528 +		v[0]-=b.v[0];
  1.2529 +		v[1]-=b.v[1];
  1.2530 +		v[2]-=b.v[2];
  1.2531 +		v[3]-=b.v[3];
  1.2532 +		v[4]-=b.v[4];
  1.2533 +		v[5]-=b.v[5];
  1.2534 +
  1.2535 +		return *this;
  1.2536 +	}
  1.2537 +
  1.2538 +	inline this_type& operator*=(T s)
  1.2539 +	{
  1.2540 +		v[0]*=s;
  1.2541 +		v[1]*=s;
  1.2542 +		v[2]*=s;
  1.2543 +		v[3]*=s;
  1.2544 +		v[4]*=s;
  1.2545 +		v[5]*=s;
  1.2546 +
  1.2547 +		return *this;
  1.2548 +	}
  1.2549 +		
  1.2550 +	inline SymMat3 operator*(T s) const
  1.2551 +	{
  1.2552 +		SymMat3 d;
  1.2553 +		d.v[0] = v[0]*s; 
  1.2554 +		d.v[1] = v[1]*s; 
  1.2555 +		d.v[2] = v[2]*s; 
  1.2556 +		d.v[3] = v[3]*s; 
  1.2557 +		d.v[4] = v[4]*s; 
  1.2558 +		d.v[5] = v[5]*s; 
  1.2559 +						
  1.2560 +		return d;
  1.2561 +	}
  1.2562 +
  1.2563 +	// Multiplies two matrices into destination with minimum copying.
  1.2564 +	static SymMat3& Multiply(SymMat3* d, const SymMat3& a, const SymMat3& b)
  1.2565 +	{		
  1.2566 +		// _00 _01 _02 _11 _12 _22
  1.2567 +
  1.2568 +		d->v[0] = a.v[0] * b.v[0];
  1.2569 +		d->v[1] = a.v[0] * b.v[1] + a.v[1] * b.v[3];
  1.2570 +		d->v[2] = a.v[0] * b.v[2] + a.v[1] * b.v[4];
  1.2571 +					
  1.2572 +		d->v[3] = a.v[3] * b.v[3];
  1.2573 +		d->v[4] = a.v[3] * b.v[4] + a.v[4] * b.v[5];
  1.2574 +				
  1.2575 +		d->v[5] = a.v[5] * b.v[5];
  1.2576 +	
  1.2577 +		return *d;
  1.2578 +	}
  1.2579 +	
  1.2580 +	inline T Determinant() const
  1.2581 +	{
  1.2582 +		const this_type& m = *this;
  1.2583 +		T d; 
  1.2584 +
  1.2585 +		d  = m(0,0) * (m(1,1)*m(2,2) - m(1,2) * m(2,1));
  1.2586 +		d -= m(0,1) * (m(1,0)*m(2,2) - m(1,2) * m(2,0));
  1.2587 +		d += m(0,2) * (m(1,0)*m(2,1) - m(1,1) * m(2,0));
  1.2588 +
  1.2589 +		return d;
  1.2590 +	}
  1.2591 +
  1.2592 +	inline this_type Inverse() const
  1.2593 +	{
  1.2594 +		this_type a;
  1.2595 +		const this_type& m = *this;
  1.2596 +		T d = Determinant();
  1.2597 +
  1.2598 +		assert(d != 0);
  1.2599 +		T s = T(1)/d;
  1.2600 +
  1.2601 +		a(0,0) = s * (m(1,1) * m(2,2) - m(1,2) * m(2,1));   
  1.2602 +
  1.2603 +		a(0,1) = s * (m(0,2) * m(2,1) - m(0,1) * m(2,2));   
  1.2604 +		a(1,1) = s * (m(0,0) * m(2,2) - m(0,2) * m(2,0));   
  1.2605 +
  1.2606 +		a(0,2) = s * (m(0,1) * m(1,2) - m(0,2) * m(1,1));   
  1.2607 +		a(1,2) = s * (m(0,2) * m(1,0) - m(0,0) * m(1,2));   
  1.2608 +		a(2,2) = s * (m(0,0) * m(1,1) - m(0,1) * m(1,0));   
  1.2609 +
  1.2610 +		return a;
  1.2611 +	}
  1.2612 +
  1.2613 +	inline T Trace() const { return v[0] + v[3] + v[5]; }
  1.2614 +
  1.2615 +	// M = a*a.t()
  1.2616 +	inline void Rank1(const Vector3<T> &a)
  1.2617 +	{
  1.2618 +		v[0] = a.x*a.x; v[1] = a.x*a.y; v[2] = a.x*a.z;
  1.2619 +		v[3] = a.y*a.y; v[4] = a.y*a.z;
  1.2620 +		v[5] = a.z*a.z;
  1.2621 +	}
  1.2622 +
  1.2623 +	// M += a*a.t()
  1.2624 +	inline void Rank1Add(const Vector3<T> &a)
  1.2625 +	{
  1.2626 +		v[0] += a.x*a.x; v[1] += a.x*a.y; v[2] += a.x*a.z;
  1.2627 +		v[3] += a.y*a.y; v[4] += a.y*a.z;
  1.2628 +		v[5] += a.z*a.z;
  1.2629 +	}
  1.2630 +
  1.2631 +	// M -= a*a.t()
  1.2632 +	inline void Rank1Sub(const Vector3<T> &a)
  1.2633 +	{
  1.2634 +		v[0] -= a.x*a.x; v[1] -= a.x*a.y; v[2] -= a.x*a.z;
  1.2635 +		v[3] -= a.y*a.y; v[4] -= a.y*a.z;
  1.2636 +		v[5] -= a.z*a.z;
  1.2637 +	}
  1.2638 +};
  1.2639 +
  1.2640 +typedef SymMat3<float>  SymMat3f;
  1.2641 +typedef SymMat3<double> SymMat3d;
  1.2642 +
  1.2643 +template<typename T>
  1.2644 +inline Matrix3<T> operator*(const SymMat3<T>& a, const SymMat3<T>& b)
  1.2645 +{
  1.2646 +	#define AJB_ARBC(r,c) (a(r,0)*b(0,c)+a(r,1)*b(1,c)+a(r,2)*b(2,c))
  1.2647 +    return Matrix3<T>(
  1.2648 +		AJB_ARBC(0,0), AJB_ARBC(0,1), AJB_ARBC(0,2),
  1.2649 +		AJB_ARBC(1,0), AJB_ARBC(1,1), AJB_ARBC(1,2),
  1.2650 +		AJB_ARBC(2,0), AJB_ARBC(2,1), AJB_ARBC(2,2));
  1.2651 +	#undef AJB_ARBC
  1.2652 +}
  1.2653 +
  1.2654 +template<typename T>
  1.2655 +inline Matrix3<T> operator*(const Matrix3<T>& a, const SymMat3<T>& b)
  1.2656 +{
  1.2657 +	#define AJB_ARBC(r,c) (a(r,0)*b(0,c)+a(r,1)*b(1,c)+a(r,2)*b(2,c))
  1.2658 +	return Matrix3<T>(
  1.2659 +		AJB_ARBC(0,0), AJB_ARBC(0,1), AJB_ARBC(0,2),
  1.2660 +		AJB_ARBC(1,0), AJB_ARBC(1,1), AJB_ARBC(1,2),
  1.2661 +		AJB_ARBC(2,0), AJB_ARBC(2,1), AJB_ARBC(2,2));
  1.2662 +	#undef AJB_ARBC
  1.2663 +}
  1.2664 +
  1.2665 +//-------------------------------------------------------------------------------------
  1.2666 +// ***** Angle
  1.2667 +
  1.2668 +// Cleanly representing the algebra of 2D rotations.
  1.2669 +// The operations maintain the angle between -Pi and Pi, the same range as atan2.
  1.2670 +
  1.2671 +template<class T>
  1.2672 +class Angle
  1.2673 +{
  1.2674 +public:
  1.2675 +	enum AngularUnits
  1.2676 +	{
  1.2677 +		Radians = 0,
  1.2678 +		Degrees = 1
  1.2679 +	};
  1.2680 +
  1.2681 +    Angle() : a(0) {}
  1.2682 +    
  1.2683 +	// Fix the range to be between -Pi and Pi
  1.2684 +	Angle(T a_, AngularUnits u = Radians) : a((u == Radians) ? a_ : a_*((T)MATH_DOUBLE_DEGREETORADFACTOR)) { FixRange(); }
  1.2685 +
  1.2686 +	T    Get(AngularUnits u = Radians) const       { return (u == Radians) ? a : a*((T)MATH_DOUBLE_RADTODEGREEFACTOR); }
  1.2687 +	void Set(const T& x, AngularUnits u = Radians) { a = (u == Radians) ? x : x*((T)MATH_DOUBLE_DEGREETORADFACTOR); FixRange(); }
  1.2688 +	int Sign() const                               { if (a == 0) return 0; else return (a > 0) ? 1 : -1; }
  1.2689 +	T   Abs() const                                { return (a > 0) ? a : -a; }
  1.2690 +
  1.2691 +    bool operator== (const Angle& b) const    { return a == b.a; }
  1.2692 +    bool operator!= (const Angle& b) const    { return a != b.a; }
  1.2693 +//	bool operator<  (const Angle& b) const    { return a < a.b; } 
  1.2694 +//	bool operator>  (const Angle& b) const    { return a > a.b; } 
  1.2695 +//	bool operator<= (const Angle& b) const    { return a <= a.b; } 
  1.2696 +//	bool operator>= (const Angle& b) const    { return a >= a.b; } 
  1.2697 +//	bool operator= (const T& x)               { a = x; FixRange(); }
  1.2698 +
  1.2699 +	// These operations assume a is already between -Pi and Pi.
  1.2700 +	Angle& operator+= (const Angle& b)        { a = a + b.a; FastFixRange(); return *this; }
  1.2701 +	Angle& operator+= (const T& x)            { a = a + x; FixRange(); return *this; }
  1.2702 +    Angle  operator+  (const Angle& b) const  { Angle res = *this; res += b; return res; }
  1.2703 +	Angle  operator+  (const T& x) const      { Angle res = *this; res += x; return res; }
  1.2704 +	Angle& operator-= (const Angle& b)        { a = a - b.a; FastFixRange(); return *this; }
  1.2705 +	Angle& operator-= (const T& x)            { a = a - x; FixRange(); return *this; }
  1.2706 +	Angle  operator-  (const Angle& b) const  { Angle res = *this; res -= b; return res; }
  1.2707 +	Angle  operator-  (const T& x) const      { Angle res = *this; res -= x; return res; }
  1.2708 +	
  1.2709 +	T   Distance(const Angle& b)              { T c = fabs(a - b.a); return (c <= ((T)MATH_DOUBLE_PI)) ? c : ((T)MATH_DOUBLE_TWOPI) - c; }
  1.2710 +
  1.2711 +private:
  1.2712 +
  1.2713 +	// The stored angle, which should be maintained between -Pi and Pi
  1.2714 +	T a;
  1.2715 +
  1.2716 +	// Fixes the angle range to [-Pi,Pi], but assumes no more than 2Pi away on either side 
  1.2717 +	inline void FastFixRange()
  1.2718 +	{
  1.2719 +		if (a < -((T)MATH_DOUBLE_PI))
  1.2720 +			a += ((T)MATH_DOUBLE_TWOPI);
  1.2721 +		else if (a > ((T)MATH_DOUBLE_PI))
  1.2722 +			a -= ((T)MATH_DOUBLE_TWOPI);
  1.2723 +	}
  1.2724 +
  1.2725 +	// Fixes the angle range to [-Pi,Pi] for any given range, but slower then the fast method
  1.2726 +	inline void FixRange()
  1.2727 +	{
  1.2728 +        // do nothing if the value is already in the correct range, since fmod call is expensive
  1.2729 +        if (a >= -((T)MATH_DOUBLE_PI) && a <= ((T)MATH_DOUBLE_PI))
  1.2730 +            return;
  1.2731 +		a = fmod(a,((T)MATH_DOUBLE_TWOPI));
  1.2732 +		if (a < -((T)MATH_DOUBLE_PI))
  1.2733 +			a += ((T)MATH_DOUBLE_TWOPI);
  1.2734 +		else if (a > ((T)MATH_DOUBLE_PI))
  1.2735 +			a -= ((T)MATH_DOUBLE_TWOPI);
  1.2736 +	}
  1.2737 +};
  1.2738 +
  1.2739 +
  1.2740 +typedef Angle<float>  Anglef;
  1.2741 +typedef Angle<double> Angled;
  1.2742 +
  1.2743 +
  1.2744 +//-------------------------------------------------------------------------------------
  1.2745 +// ***** Plane
  1.2746 +
  1.2747 +// Consists of a normal vector and distance from the origin where the plane is located.
  1.2748 +
  1.2749 +template<class T>
  1.2750 +class Plane
  1.2751 +{
  1.2752 +public:
  1.2753 +    Vector3<T> N;
  1.2754 +    T          D;
  1.2755 +
  1.2756 +    Plane() : D(0) {}
  1.2757 +
  1.2758 +    // Normals must already be normalized
  1.2759 +    Plane(const Vector3<T>& n, T d) : N(n), D(d) {}
  1.2760 +    Plane(T x, T y, T z, T d) : N(x,y,z), D(d) {}
  1.2761 +
  1.2762 +    // construct from a point on the plane and the normal
  1.2763 +    Plane(const Vector3<T>& p, const Vector3<T>& n) : N(n), D(-(p * n)) {}
  1.2764 +
  1.2765 +    // Find the point to plane distance. The sign indicates what side of the plane the point is on (0 = point on plane).
  1.2766 +    T TestSide(const Vector3<T>& p) const
  1.2767 +    {
  1.2768 +        return (N.Dot(p)) + D;
  1.2769 +    }
  1.2770 +
  1.2771 +    Plane<T> Flipped() const
  1.2772 +    {
  1.2773 +        return Plane(-N, -D);
  1.2774 +    }
  1.2775 +
  1.2776 +    void Flip()
  1.2777 +    {
  1.2778 +        N = -N;
  1.2779 +        D = -D;
  1.2780 +    }
  1.2781 +
  1.2782 +	bool operator==(const Plane<T>& rhs) const
  1.2783 +	{
  1.2784 +		return (this->D == rhs.D && this->N == rhs.N);
  1.2785 +	}
  1.2786 +};
  1.2787 +
  1.2788 +typedef Plane<float> Planef;
  1.2789 +typedef Plane<double> Planed;
  1.2790 +
  1.2791 +
  1.2792 +} // Namespace OVR
  1.2793 +
  1.2794 +#endif