Include/Extras/OVR_Math.h

   1 /********************************************************************************/ /**\r
   2  \file      OVR_Math.h\r
   3  \brief     Implementation of 3D primitives such as vectors, matrices.\r
   4  \copyright Copyright 2014-2016 Oculus VR, LLC All Rights reserved.\r
   5  *************************************************************************************/\r
   6 \r
   7 #ifndef OVR_Math_h\r
   8 #define OVR_Math_h\r
   9 \r
  10 // This file is intended to be independent of the rest of LibOVR and LibOVRKernel and thus\r
  11 // has no #include dependencies on either.\r
  12 \r
  13 #include <math.h>\r
  14 #include <stdint.h>\r
  15 #include <stdlib.h>\r
  16 #include <stdio.h>\r
  17 #include <string.h>\r
  18 #include <float.h>\r
  19 \r
  20 #ifndef OVR_EXCLUDE_CAPI_FROM_MATH\r
  21 #include "../OVR_CAPI.h" // Required due to a dependence on the ovrFovPort_ declaration.\r
  22 #endif\r
  23 \r
  24 #if defined(_MSC_VER)\r
  25 #pragma warning(push)\r
  26 #pragma warning(disable : 4127) // conditional expression is constant\r
  27 \r
  28 #if _MSC_VER < 1800 // isfinite was introduced in VS2013\r
  29 #define isfinite(x) _finite((x))\r
  30 #endif\r
  31 #endif\r
  32 \r
  33 #if defined(_MSC_VER)\r
  34 #define OVRMath_sprintf sprintf_s\r
  35 #else\r
  36 #define OVRMath_sprintf snprintf\r
  37 #endif\r
  38 \r
  39 //-------------------------------------------------------------------------------------\r
  40 // ***** OVR_MATH_ASSERT\r
  41 //\r
  42 // Independent debug break implementation for OVR_Math.h.\r
  43 \r
  44 #if !defined(OVR_MATH_DEBUG_BREAK)\r
  45 #if defined(_DEBUG)\r
  46 #if defined(_MSC_VER)\r
  47 #define OVR_MATH_DEBUG_BREAK __debugbreak()\r
  48 #else\r
  49 #define OVR_MATH_DEBUG_BREAK __builtin_trap()\r
  50 #endif\r
  51 #else\r
  52 #define OVR_MATH_DEBUG_BREAK ((void)0)\r
  53 #endif\r
  54 #endif\r
  55 \r
  56 //-------------------------------------------------------------------------------------\r
  57 // ***** OVR_MATH_ASSERT\r
  58 //\r
  59 // Independent OVR_MATH_ASSERT implementation for OVR_Math.h.\r
  60 \r
  61 #if !defined(OVR_MATH_ASSERT)\r
  62 #if defined(_DEBUG)\r
  63 #define OVR_MATH_ASSERT(p) \\r
  64   if (!(p)) {              \\r
  65     OVR_MATH_DEBUG_BREAK;  \\r
  66   }\r
  67 #else\r
  68 #define OVR_MATH_ASSERT(p) ((void)0)\r
  69 #endif\r
  70 #endif\r
  71 \r
  72 //-------------------------------------------------------------------------------------\r
  73 // ***** OVR_MATH_STATIC_ASSERT\r
  74 //\r
  75 // Independent OVR_MATH_ASSERT implementation for OVR_Math.h.\r
  76 \r
  77 #if !defined(OVR_MATH_STATIC_ASSERT)\r
  78 #if defined(__cplusplus) &&                                                                       \\r
  79     ((defined(_MSC_VER) && (defined(_MSC_VER) >= 1600)) || defined(__GXX_EXPERIMENTAL_CXX0X__) || \\r
  80      (__cplusplus >= 201103L))\r
  81 #define OVR_MATH_STATIC_ASSERT static_assert\r
  82 #else\r
  83 #if !defined(OVR_SA_UNUSED)\r
  84 #if defined(__GNUC__) || defined(__clang__)\r
  85 #define OVR_SA_UNUSED __attribute__((unused))\r
  86 #else\r
  87 #define OVR_SA_UNUSED\r
  88 #endif\r
  89 #define OVR_SA_PASTE(a, b) a##b\r
  90 #define OVR_SA_HELP(a, b) OVR_SA_PASTE(a, b)\r
  91 #endif\r
  92 \r
  93 #define OVR_MATH_STATIC_ASSERT(expression, msg) \\r
  94   typedef char OVR_SA_HELP(compileTimeAssert, __LINE__)[((expression) != 0) ? 1 : -1] OVR_SA_UNUSED\r
  95 #endif\r
  96 #endif\r
  97 \r
  98 namespace OVR {\r
  99 \r
 100 template <class T>\r
 101 const T OVRMath_Min(const T a, const T b) {\r
 102   return (a < b) ? a : b;\r
 103 }\r
 104 \r
 105 template <class T>\r
 106 const T OVRMath_Max(const T a, const T b) {\r
 107   return (b < a) ? a : b;\r
 108 }\r
 109 \r
 110 template <class T>\r
 111 void OVRMath_Swap(T& a, T& b) {\r
 112   T temp(a);\r
 113   a = b;\r
 114   b = temp;\r
 115 }\r
 116 \r
 117 //-------------------------------------------------------------------------------------\r
 118 // ***** Constants for 3D world/axis definitions.\r
 119 \r
 120 // Definitions of axes for coordinate and rotation conversions.\r
 121 enum Axis { Axis_X = 0, Axis_Y = 1, Axis_Z = 2 };\r
 122 \r
 123 // RotateDirection describes the rotation direction around an axis, interpreted as follows:\r
 124 //  CW  - Clockwise while looking "down" from positive axis towards the origin.\r
 125 //  CCW - Counter-clockwise while looking from the positive axis towards the origin,\r
 126 //        which is in the negative axis direction.\r
 127 //  CCW is the default for the RHS coordinate system. Oculus standard RHS coordinate\r
 128 //  system defines Y up, X right, and Z back (pointing out from the screen). In this\r
 129 //  system Rotate_CCW around Z will specifies counter-clockwise rotation in XY plane.\r
 130 enum RotateDirection { Rotate_CCW = 1, Rotate_CW = -1 };\r
 131 \r
 132 // Constants for right handed and left handed coordinate systems\r
 133 enum HandedSystem { Handed_R = 1, Handed_L = -1 };\r
 134 \r
 135 // AxisDirection describes which way the coordinate axis points. Used by WorldAxes.\r
 136 enum AxisDirection {\r
 137   Axis_Up = 2,\r
 138   Axis_Down = -2,\r
 139   Axis_Right = 1,\r
 140   Axis_Left = -1,\r
 141   Axis_In = 3,\r
 142   Axis_Out = -3\r
 143 };\r
 144 \r
 145 struct WorldAxes {\r
 146   AxisDirection XAxis, YAxis, ZAxis;\r
 147 \r
 148   WorldAxes(AxisDirection x, AxisDirection y, AxisDirection z) : XAxis(x), YAxis(y), ZAxis(z) {\r
 149     OVR_MATH_ASSERT(abs(x) != abs(y) && abs(y) != abs(z) && abs(z) != abs(x));\r
 150   }\r
 151 };\r
 152 \r
 153 } // namespace OVR\r
 154 \r
 155 //------------------------------------------------------------------------------------//\r
 156 // ***** C Compatibility Types\r
 157 \r
 158 // These declarations are used to support conversion between C types used in\r
 159 // LibOVR C interfaces and their C++ versions. As an example, they allow passing\r
 160 // Vector3f into a function that expects ovrVector3f.\r
 161 \r
 162 typedef struct ovrQuatf_ ovrQuatf;\r
 163 typedef struct ovrQuatd_ ovrQuatd;\r
 164 typedef struct ovrSizei_ ovrSizei;\r
 165 typedef struct ovrSizef_ ovrSizef;\r
 166 typedef struct ovrSized_ ovrSized;\r
 167 typedef struct ovrRecti_ ovrRecti;\r
 168 typedef struct ovrVector2i_ ovrVector2i;\r
 169 typedef struct ovrVector2f_ ovrVector2f;\r
 170 typedef struct ovrVector2d_ ovrVector2d;\r
 171 typedef struct ovrVector3f_ ovrVector3f;\r
 172 typedef struct ovrVector3d_ ovrVector3d;\r
 173 typedef struct ovrVector4f_ ovrVector4f;\r
 174 typedef struct ovrVector4d_ ovrVector4d;\r
 175 typedef struct ovrMatrix2f_ ovrMatrix2f;\r
 176 typedef struct ovrMatrix2d_ ovrMatrix2d;\r
 177 typedef struct ovrMatrix3f_ ovrMatrix3f;\r
 178 typedef struct ovrMatrix3d_ ovrMatrix3d;\r
 179 typedef struct ovrMatrix4f_ ovrMatrix4f;\r
 180 typedef struct ovrMatrix4d_ ovrMatrix4d;\r
 181 typedef struct ovrPosef_ ovrPosef;\r
 182 typedef struct ovrPosed_ ovrPosed;\r
 183 typedef struct ovrPoseStatef_ ovrPoseStatef;\r
 184 typedef struct ovrPoseStated_ ovrPoseStated;\r
 185 typedef struct ovrFovPort_ ovrFovPort;\r
 186 \r
 187 namespace OVR {\r
 188 \r
 189 // Forward-declare our templates.\r
 190 template <class T>\r
 191 class Quat;\r
 192 template <class T>\r
 193 class Size;\r
 194 template <class T>\r
 195 class Rect;\r
 196 template <class T>\r
 197 class Vector2;\r
 198 template <class T>\r
 199 class Vector3;\r
 200 template <class T>\r
 201 class Vector4;\r
 202 template <class T>\r
 203 class Matrix2;\r
 204 template <class T>\r
 205 class Matrix3;\r
 206 template <class T>\r
 207 class Matrix4;\r
 208 template <class T>\r
 209 class Pose;\r
 210 template <class T>\r
 211 class PoseState;\r
 212 struct FovPort;\r
 213 \r
 214 // CompatibleTypes::Type is used to lookup a compatible C-version of a C++ class.\r
 215 template <class C>\r
 216 struct CompatibleTypes {\r
 217   // Declaration here seems necessary for MSVC; specializations are\r
 218   // used instead.\r
 219   typedef struct {\r
 220   } Type;\r
 221 };\r
 222 \r
 223 // Specializations providing CompatibleTypes::Type value.\r
 224 template <>\r
 225 struct CompatibleTypes<Quat<float>> {\r
 226   typedef ovrQuatf Type;\r
 227 };\r
 228 template <>\r
 229 struct CompatibleTypes<Quat<double>> {\r
 230   typedef ovrQuatd Type;\r
 231 };\r
 232 template <>\r
 233 struct CompatibleTypes<Matrix2<float>> {\r
 234   typedef ovrMatrix2f Type;\r
 235 };\r
 236 template <>\r
 237 struct CompatibleTypes<Matrix2<double>> {\r
 238   typedef ovrMatrix2d Type;\r
 239 };\r
 240 template <>\r
 241 struct CompatibleTypes<Matrix3<float>> {\r
 242   typedef ovrMatrix3f Type;\r
 243 };\r
 244 template <>\r
 245 struct CompatibleTypes<Matrix3<double>> {\r
 246   typedef ovrMatrix3d Type;\r
 247 };\r
 248 template <>\r
 249 struct CompatibleTypes<Matrix4<float>> {\r
 250   typedef ovrMatrix4f Type;\r
 251 };\r
 252 template <>\r
 253 struct CompatibleTypes<Matrix4<double>> {\r
 254   typedef ovrMatrix4d Type;\r
 255 };\r
 256 template <>\r
 257 struct CompatibleTypes<Size<int>> {\r
 258   typedef ovrSizei Type;\r
 259 };\r
 260 template <>\r
 261 struct CompatibleTypes<Size<float>> {\r
 262   typedef ovrSizef Type;\r
 263 };\r
 264 template <>\r
 265 struct CompatibleTypes<Size<double>> {\r
 266   typedef ovrSized Type;\r
 267 };\r
 268 template <>\r
 269 struct CompatibleTypes<Rect<int>> {\r
 270   typedef ovrRecti Type;\r
 271 };\r
 272 template <>\r
 273 struct CompatibleTypes<Vector2<int>> {\r
 274   typedef ovrVector2i Type;\r
 275 };\r
 276 template <>\r
 277 struct CompatibleTypes<Vector2<float>> {\r
 278   typedef ovrVector2f Type;\r
 279 };\r
 280 template <>\r
 281 struct CompatibleTypes<Vector2<double>> {\r
 282   typedef ovrVector2d Type;\r
 283 };\r
 284 template <>\r
 285 struct CompatibleTypes<Vector3<float>> {\r
 286   typedef ovrVector3f Type;\r
 287 };\r
 288 template <>\r
 289 struct CompatibleTypes<Vector3<double>> {\r
 290   typedef ovrVector3d Type;\r
 291 };\r
 292 template <>\r
 293 struct CompatibleTypes<Vector4<float>> {\r
 294   typedef ovrVector4f Type;\r
 295 };\r
 296 template <>\r
 297 struct CompatibleTypes<Vector4<double>> {\r
 298   typedef ovrVector4d Type;\r
 299 };\r
 300 template <>\r
 301 struct CompatibleTypes<Pose<float>> {\r
 302   typedef ovrPosef Type;\r
 303 };\r
 304 template <>\r
 305 struct CompatibleTypes<Pose<double>> {\r
 306   typedef ovrPosed Type;\r
 307 };\r
 308 template <>\r
 309 struct CompatibleTypes<FovPort> {\r
 310   typedef ovrFovPort Type;\r
 311 };\r
 312 \r
 313 //------------------------------------------------------------------------------------//\r
 314 // ***** Math\r
 315 //\r
 316 // Math class contains constants and functions. This class is a template specialized\r
 317 // per type, with Math<float> and Math<double> being distinct.\r
 318 template <class T>\r
 319 class Math {\r
 320  public:\r
 321   // By default, support explicit conversion to float. This allows Vector2<int> to\r
 322   // compile, for example.\r
 323   typedef float OtherFloatType;\r
 324 \r
 325   static int Tolerance() {\r
 326     return 0;\r
 327   } // Default value so integer types compile\r
 328 };\r
 329 \r
 330 //------------------------------------------------------------------------------------//\r
 331 // ***** double constants\r
 332 #define MATH_DOUBLE_PI 3.14159265358979323846\r
 333 #define MATH_DOUBLE_TWOPI (2 * MATH_DOUBLE_PI)\r
 334 #define MATH_DOUBLE_PIOVER2 (0.5 * MATH_DOUBLE_PI)\r
 335 #define MATH_DOUBLE_PIOVER4 (0.25 * MATH_DOUBLE_PI)\r
 336 #define MATH_FLOAT_MAXVALUE (FLT_MAX)\r
 337 \r
 338 #define MATH_DOUBLE_RADTODEGREEFACTOR (360.0 / MATH_DOUBLE_TWOPI)\r
 339 #define MATH_DOUBLE_DEGREETORADFACTOR (MATH_DOUBLE_TWOPI / 360.0)\r
 340 \r
 341 #define MATH_DOUBLE_E 2.71828182845904523536\r
 342 #define MATH_DOUBLE_LOG2E 1.44269504088896340736\r
 343 #define MATH_DOUBLE_LOG10E 0.434294481903251827651\r
 344 #define MATH_DOUBLE_LN2 0.693147180559945309417\r
 345 #define MATH_DOUBLE_LN10 2.30258509299404568402\r
 346 \r
 347 #define MATH_DOUBLE_SQRT2 1.41421356237309504880\r
 348 #define MATH_DOUBLE_SQRT1_2 0.707106781186547524401\r
 349 \r
 350 #define MATH_DOUBLE_TOLERANCE \\r
 351   1e-12 // a default number for value equality tolerance: about 4500*Epsilon;\r
 352 #define MATH_DOUBLE_SINGULARITYRADIUS \\r
 353   1e-12 // about 1-cos(.0001 degree), for gimbal lock numerical problems\r
 354 \r
 355 #define MATH_DOUBLE_HUGENUMBER 1.3407807929942596e+154\r
 356 #define MATH_DOUBLE_SMALLESTNONDENORMAL 2.2250738585072014e-308\r
 357 \r
 358 //------------------------------------------------------------------------------------//\r
 359 // ***** float constants\r
 360 #define MATH_FLOAT_PI float(MATH_DOUBLE_PI)\r
 361 #define MATH_FLOAT_TWOPI float(MATH_DOUBLE_TWOPI)\r
 362 #define MATH_FLOAT_PIOVER2 float(MATH_DOUBLE_PIOVER2)\r
 363 #define MATH_FLOAT_PIOVER4 float(MATH_DOUBLE_PIOVER4)\r
 364 \r
 365 #define MATH_FLOAT_RADTODEGREEFACTOR float(MATH_DOUBLE_RADTODEGREEFACTOR)\r
 366 #define MATH_FLOAT_DEGREETORADFACTOR float(MATH_DOUBLE_DEGREETORADFACTOR)\r
 367 \r
 368 #define MATH_FLOAT_E float(MATH_DOUBLE_E)\r
 369 #define MATH_FLOAT_LOG2E float(MATH_DOUBLE_LOG2E)\r
 370 #define MATH_FLOAT_LOG10E float(MATH_DOUBLE_LOG10E)\r
 371 #define MATH_FLOAT_LN2 float(MATH_DOUBLE_LN2)\r
 372 #define MATH_FLOAT_LN10 float(MATH_DOUBLE_LN10)\r
 373 \r
 374 #define MATH_FLOAT_SQRT2 float(MATH_DOUBLE_SQRT2)\r
 375 #define MATH_FLOAT_SQRT1_2 float(MATH_DOUBLE_SQRT1_2)\r
 376 \r
 377 #define MATH_FLOAT_TOLERANCE \\r
 378   1e-5f // a default number for value equality tolerance: 1e-5, about 84*EPSILON;\r
 379 #define MATH_FLOAT_SINGULARITYRADIUS \\r
 380   1e-7f // about 1-cos(.025 degree), for gimbal lock numerical problems\r
 381 \r
 382 #define MATH_FLOAT_HUGENUMBER 1.8446742974197924e+019f\r
 383 #define MATH_FLOAT_SMALLESTNONDENORMAL 1.1754943508222875e-038f\r
 384 \r
 385 // Single-precision Math constants class.\r
 386 template <>\r
 387 class Math<float> {\r
 388  public:\r
 389   typedef double OtherFloatType;\r
 390 \r
 391   static inline float MaxValue() {\r
 392     return FLT_MAX;\r
 393   };\r
 394   static inline float Tolerance() {\r
 395     return MATH_FLOAT_TOLERANCE;\r
 396   }; // a default number for value equality tolerance\r
 397   static inline float SingularityRadius() {\r
 398     return MATH_FLOAT_SINGULARITYRADIUS;\r
 399   }; // for gimbal lock numerical problems\r
 400   static inline float HugeNumber() {\r
 401     return MATH_FLOAT_HUGENUMBER;\r
 402   }\r
 403   static inline float SmallestNonDenormal() {\r
 404     return MATH_FLOAT_SMALLESTNONDENORMAL;\r
 405   }\r
 406 };\r
 407 \r
 408 // Double-precision Math constants class\r
 409 template <>\r
 410 class Math<double> {\r
 411  public:\r
 412   typedef float OtherFloatType;\r
 413 \r
 414   static inline double Tolerance() {\r
 415     return MATH_DOUBLE_TOLERANCE;\r
 416   }; // a default number for value equality tolerance\r
 417   static inline double SingularityRadius() {\r
 418     return MATH_DOUBLE_SINGULARITYRADIUS;\r
 419   }; // for gimbal lock numerical problems\r
 420   static inline double HugeNumber() {\r
 421     return MATH_DOUBLE_HUGENUMBER;\r
 422   }\r
 423   static inline double SmallestNonDenormal() {\r
 424     return MATH_DOUBLE_SMALLESTNONDENORMAL;\r
 425   }\r
 426 };\r
 427 \r
 428 typedef Math<float> Mathf;\r
 429 typedef Math<double> Mathd;\r
 430 \r
 431 // Conversion functions between degrees and radians\r
 432 // (non-templated to ensure passing int arguments causes warning)\r
 433 inline float RadToDegree(float rad) {\r
 434   return rad * MATH_FLOAT_RADTODEGREEFACTOR;\r
 435 }\r
 436 inline double RadToDegree(double rad) {\r
 437   return rad * MATH_DOUBLE_RADTODEGREEFACTOR;\r
 438 }\r
 439 \r
 440 inline float DegreeToRad(float deg) {\r
 441   return deg * MATH_FLOAT_DEGREETORADFACTOR;\r
 442 }\r
 443 inline double DegreeToRad(double deg) {\r
 444   return deg * MATH_DOUBLE_DEGREETORADFACTOR;\r
 445 }\r
 446 \r
 447 // Square function\r
 448 template <class T>\r
 449 inline T Sqr(T x) {\r
 450   return x * x;\r
 451 }\r
 452 \r
 453 // MERGE_MOBILE_SDK\r
 454 // Safe reciprocal square root.\r
 455 template <class T>\r
 456 T RcpSqrt(const T f) {\r
 457   return (f >= Math<T>::SmallestNonDenormal()) ? static_cast<T>(1.0 / sqrt(f))\r
 458                                                : Math<T>::HugeNumber();\r
 459 }\r
 460 // MERGE_MOBILE_SDK\r
 461 \r
 462 // Sign: returns 0 if x == 0, -1 if x < 0, and 1 if x > 0\r
 463 template <class T>\r
 464 inline T Sign(T x) {\r
 465   return (x != T(0)) ? (x < T(0) ? T(-1) : T(1)) : T(0);\r
 466 }\r
 467 \r
 468 // Numerically stable acos function\r
 469 inline float Acos(float x) {\r
 470   return (x > 1.0f) ? 0.0f : (x < -1.0f) ? MATH_FLOAT_PI : acosf(x);\r
 471 }\r
 472 inline double Acos(double x) {\r
 473   return (x > 1.0) ? 0.0 : (x < -1.0) ? MATH_DOUBLE_PI : acos(x);\r
 474 }\r
 475 \r
 476 // Numerically stable asin function\r
 477 inline float Asin(float x) {\r
 478   return (x > 1.0f) ? MATH_FLOAT_PIOVER2 : (x < -1.0f) ? -MATH_FLOAT_PIOVER2 : asinf(x);\r
 479 }\r
 480 inline double Asin(double x) {\r
 481   return (x > 1.0) ? MATH_DOUBLE_PIOVER2 : (x < -1.0) ? -MATH_DOUBLE_PIOVER2 : asin(x);\r
 482 }\r
 483 \r
 484 template <class T>\r
 485 class Quat;\r
 486 \r
 487 //-------------------------------------------------------------------------------------\r
 488 // ***** Vector2<>\r
 489 \r
 490 // Vector2f (Vector2d) represents a 2-dimensional vector or point in space,\r
 491 // consisting of coordinates x and y\r
 492 \r
 493 template <class T>\r
 494 class Vector2 {\r
 495  public:\r
 496   typedef T ElementType;\r
 497   static const size_t ElementCount = 2;\r
 498 \r
 499   T x, y;\r
 500 \r
 501   Vector2() : x(0), y(0) {}\r
 502   Vector2(T x_, T y_) : x(x_), y(y_) {}\r
 503   explicit Vector2(T s) : x(s), y(s) {}\r
 504   explicit Vector2(const Vector2<typename Math<T>::OtherFloatType>& src)\r
 505       : x((T)src.x), y((T)src.y) {}\r
 506 \r
 507   static Vector2 Zero() {\r
 508     return Vector2(0, 0);\r
 509   }\r
 510 \r
 511   // C-interop support.\r
 512   typedef typename CompatibleTypes<Vector2<T>>::Type CompatibleType;\r
 513 \r
 514   Vector2(const CompatibleType& s) : x(s.x), y(s.y) {}\r
 515 \r
 516   operator const CompatibleType&() const {\r
 517     OVR_MATH_STATIC_ASSERT(\r
 518         sizeof(Vector2<T>) == sizeof(CompatibleType), "sizeof(Vector2<T>) failure");\r
 519     return reinterpret_cast<const CompatibleType&>(*this);\r
 520   }\r
 521 \r
 522   bool operator==(const Vector2& b) const {\r
 523     return x == b.x && y == b.y;\r
 524   }\r
 525   bool operator!=(const Vector2& b) const {\r
 526     return x != b.x || y != b.y;\r
 527   }\r
 528 \r
 529   Vector2 operator+(const Vector2& b) const {\r
 530     return Vector2(x + b.x, y + b.y);\r
 531   }\r
 532   Vector2& operator+=(const Vector2& b) {\r
 533     x += b.x;\r
 534     y += b.y;\r
 535     return *this;\r
 536   }\r
 537   Vector2 operator-(const Vector2& b) const {\r
 538     return Vector2(x - b.x, y - b.y);\r
 539   }\r
 540   Vector2& operator-=(const Vector2& b) {\r
 541     x -= b.x;\r
 542     y -= b.y;\r
 543     return *this;\r
 544   }\r
 545   Vector2 operator-() const {\r
 546     return Vector2(-x, -y);\r
 547   }\r
 548 \r
 549   // Scalar multiplication/division scales vector.\r
 550   Vector2 operator*(T s) const {\r
 551     return Vector2(x * s, y * s);\r
 552   }\r
 553   Vector2& operator*=(T s) {\r
 554     x *= s;\r
 555     y *= s;\r
 556     return *this;\r
 557   }\r
 558 \r
 559   Vector2 operator/(T s) const {\r
 560     T rcp = T(1) / s;\r
 561     return Vector2(x * rcp, y * rcp);\r
 562   }\r
 563   Vector2& operator/=(T s) {\r
 564     T rcp = T(1) / s;\r
 565     x *= rcp;\r
 566     y *= rcp;\r
 567     return *this;\r
 568   }\r
 569 \r
 570   static Vector2 Min(const Vector2& a, const Vector2& b) {\r
 571     return Vector2((a.x < b.x) ? a.x : b.x, (a.y < b.y) ? a.y : b.y);\r
 572   }\r
 573   static Vector2 Max(const Vector2& a, const Vector2& b) {\r
 574     return Vector2((a.x > b.x) ? a.x : b.x, (a.y > b.y) ? a.y : b.y);\r
 575   }\r
 576 \r
 577   Vector2 Clamped(T maxMag) const {\r
 578     T magSquared = LengthSq();\r
 579     if (magSquared <= Sqr(maxMag))\r
 580       return *this;\r
 581     else\r
 582       return *this * (maxMag / sqrt(magSquared));\r
 583   }\r
 584 \r
 585   // Compare two vectors for equality with tolerance. Returns true if vectors match within\r
 586   // tolerance.\r
 587   bool IsEqual(const Vector2& b, T tolerance = Math<T>::Tolerance()) const {\r
 588     return (fabs(b.x - x) <= tolerance) && (fabs(b.y - y) <= tolerance);\r
 589   }\r
 590   bool Compare(const Vector2& b, T tolerance = Math<T>::Tolerance()) const {\r
 591     return IsEqual(b, tolerance);\r
 592   }\r
 593 \r
 594   // Access element by index\r
 595   T& operator[](int idx) {\r
 596     OVR_MATH_ASSERT(0 <= idx && idx < 2);\r
 597     return *(&x + idx);\r
 598   }\r
 599   const T& operator[](int idx) const {\r
 600     OVR_MATH_ASSERT(0 <= idx && idx < 2);\r
 601     return *(&x + idx);\r
 602   }\r
 603 \r
 604   // Entry-wise product of two vectors\r
 605   Vector2 EntrywiseMultiply(const Vector2& b) const {\r
 606     return Vector2(x * b.x, y * b.y);\r
 607   }\r
 608 \r
 609   // Multiply and divide operators do entry-wise math. Used Dot() for dot product.\r
 610   Vector2 operator*(const Vector2& b) const {\r
 611     return Vector2(x * b.x, y * b.y);\r
 612   }\r
 613   Vector2 operator/(const Vector2& b) const {\r
 614     return Vector2(x / b.x, y / b.y);\r
 615   }\r
 616 \r
 617   // Dot product\r
 618   // Used to calculate angle q between two vectors among other things,\r
 619   // as (A dot B) = |a||b|cos(q).\r
 620   T Dot(const Vector2& b) const {\r
 621     return x * b.x + y * b.y;\r
 622   }\r
 623 \r
 624   // Returns the angle from this vector to b, in radians.\r
 625   T Angle(const Vector2& b) const {\r
 626     T div = LengthSq() * b.LengthSq();\r
 627     OVR_MATH_ASSERT(div != T(0));\r
 628     T result = Acos((this->Dot(b)) / sqrt(div));\r
 629     return result;\r
 630   }\r
 631 \r
 632   // Return Length of the vector squared.\r
 633   T LengthSq() const {\r
 634     return (x * x + y * y);\r
 635   }\r
 636 \r
 637   // Return vector length.\r
 638   T Length() const {\r
 639     return sqrt(LengthSq());\r
 640   }\r
 641 \r
 642   // Returns squared distance between two points represented by vectors.\r
 643   T DistanceSq(const Vector2& b) const {\r
 644     return (*this - b).LengthSq();\r
 645   }\r
 646 \r
 647   // Returns distance between two points represented by vectors.\r
 648   T Distance(const Vector2& b) const {\r
 649     return (*this - b).Length();\r
 650   }\r
 651 \r
 652   // Determine if this a unit vector.\r
 653   bool IsNormalized() const {\r
 654     return fabs(LengthSq() - T(1)) < Math<T>::Tolerance();\r
 655   }\r
 656 \r
 657   // Normalize, convention vector length to 1.\r
 658   void Normalize() {\r
 659     T s = Length();\r
 660     if (s != T(0))\r
 661       s = T(1) / s;\r
 662     *this *= s;\r
 663   }\r
 664 \r
 665   // Returns normalized (unit) version of the vector without modifying itself.\r
 666   Vector2 Normalized() const {\r
 667     T s = Length();\r
 668     if (s != T(0))\r
 669       s = T(1) / s;\r
 670     return *this * s;\r
 671   }\r
 672 \r
 673   // Linearly interpolates from this vector to another.\r
 674   // Factor should be between 0.0 and 1.0, with 0 giving full value to this.\r
 675   Vector2 Lerp(const Vector2& b, T f) const {\r
 676     return *this * (T(1) - f) + b * f;\r
 677   }\r
 678 \r
 679   // Projects this vector onto the argument; in other words,\r
 680   // A.Project(B) returns projection of vector A onto B.\r
 681   Vector2 ProjectTo(const Vector2& b) const {\r
 682     T l2 = b.LengthSq();\r
 683     OVR_MATH_ASSERT(l2 != T(0));\r
 684     return b * (Dot(b) / l2);\r
 685   }\r
 686 \r
 687   // returns true if vector b is clockwise from this vector\r
 688   bool IsClockwise(const Vector2& b) const {\r
 689     return (x * b.y - y * b.x) < 0;\r
 690   }\r
 691 };\r
 692 \r
 693 typedef Vector2<float> Vector2f;\r
 694 typedef Vector2<double> Vector2d;\r
 695 typedef Vector2<int> Vector2i;\r
 696 \r
 697 typedef Vector2<float> Point2f;\r
 698 typedef Vector2<double> Point2d;\r
 699 typedef Vector2<int> Point2i;\r
 700 \r
 701 //-------------------------------------------------------------------------------------\r
 702 // ***** Vector3<> - 3D vector of {x, y, z}\r
 703 \r
 704 //\r
 705 // Vector3f (Vector3d) represents a 3-dimensional vector or point in space,\r
 706 // consisting of coordinates x, y and z.\r
 707 \r
 708 template <class T>\r
 709 class Vector3 {\r
 710  public:\r
 711   typedef T ElementType;\r
 712   static const size_t ElementCount = 3;\r
 713 \r
 714   T x, y, z;\r
 715 \r
 716   // FIXME: default initialization of a vector class can be very expensive in a full-blown\r
 717   // application.  A few hundred thousand vector constructions is not unlikely and can add\r
 718   // up to milliseconds of time on processors like the PS3 PPU.\r
 719   Vector3() : x(0), y(0), z(0) {}\r
 720   Vector3(T x_, T y_, T z_ = 0) : x(x_), y(y_), z(z_) {}\r
 721   explicit Vector3(T s) : x(s), y(s), z(s) {}\r
 722   explicit Vector3(const Vector3<typename Math<T>::OtherFloatType>& src)\r
 723       : x((T)src.x), y((T)src.y), z((T)src.z) {}\r
 724 \r
 725   static Vector3 Zero() {\r
 726     return Vector3(0, 0, 0);\r
 727   }\r
 728 \r
 729   // C-interop support.\r
 730   typedef typename CompatibleTypes<Vector3<T>>::Type CompatibleType;\r
 731 \r
 732   Vector3(const CompatibleType& s) : x(s.x), y(s.y), z(s.z) {}\r
 733 \r
 734   operator const CompatibleType&() const {\r
 735     OVR_MATH_STATIC_ASSERT(\r
 736         sizeof(Vector3<T>) == sizeof(CompatibleType), "sizeof(Vector3<T>) failure");\r
 737     return reinterpret_cast<const CompatibleType&>(*this);\r
 738   }\r
 739 \r
 740   bool operator==(const Vector3& b) const {\r
 741     return x == b.x && y == b.y && z == b.z;\r
 742   }\r
 743   bool operator!=(const Vector3& b) const {\r
 744     return x != b.x || y != b.y || z != b.z;\r
 745   }\r
 746 \r
 747   Vector3 operator+(const Vector3& b) const {\r
 748     return Vector3(x + b.x, y + b.y, z + b.z);\r
 749   }\r
 750   Vector3& operator+=(const Vector3& b) {\r
 751     x += b.x;\r
 752     y += b.y;\r
 753     z += b.z;\r
 754     return *this;\r
 755   }\r
 756   Vector3 operator-(const Vector3& b) const {\r
 757     return Vector3(x - b.x, y - b.y, z - b.z);\r
 758   }\r
 759   Vector3& operator-=(const Vector3& b) {\r
 760     x -= b.x;\r
 761     y -= b.y;\r
 762     z -= b.z;\r
 763     return *this;\r
 764   }\r
 765   Vector3 operator-() const {\r
 766     return Vector3(-x, -y, -z);\r
 767   }\r
 768 \r
 769   // Scalar multiplication/division scales vector.\r
 770   Vector3 operator*(T s) const {\r
 771     return Vector3(x * s, y * s, z * s);\r
 772   }\r
 773   Vector3& operator*=(T s) {\r
 774     x *= s;\r
 775     y *= s;\r
 776     z *= s;\r
 777     return *this;\r
 778   }\r
 779 \r
 780   Vector3 operator/(T s) const {\r
 781     T rcp = T(1) / s;\r
 782     return Vector3(x * rcp, y * rcp, z * rcp);\r
 783   }\r
 784   Vector3& operator/=(T s) {\r
 785     T rcp = T(1) / s;\r
 786     x *= rcp;\r
 787     y *= rcp;\r
 788     z *= rcp;\r
 789     return *this;\r
 790   }\r
 791 \r
 792   static Vector3 Min(const Vector3& a, const Vector3& b) {\r
 793     return Vector3((a.x < b.x) ? a.x : b.x, (a.y < b.y) ? a.y : b.y, (a.z < b.z) ? a.z : b.z);\r
 794   }\r
 795   static Vector3 Max(const Vector3& a, const Vector3& b) {\r
 796     return Vector3((a.x > b.x) ? a.x : b.x, (a.y > b.y) ? a.y : b.y, (a.z > b.z) ? a.z : b.z);\r
 797   }\r
 798 \r
 799   Vector3 Clamped(T maxMag) const {\r
 800     T magSquared = LengthSq();\r
 801     if (magSquared <= Sqr(maxMag))\r
 802       return *this;\r
 803     else\r
 804       return *this * (maxMag / sqrt(magSquared));\r
 805   }\r
 806 \r
 807   // Compare two vectors for equality with tolerance. Returns true if vectors match within\r
 808   // tolerance.\r
 809   bool IsEqual(const Vector3& b, T tolerance = Math<T>::Tolerance()) const {\r
 810     return (fabs(b.x - x) <= tolerance) && (fabs(b.y - y) <= tolerance) &&\r
 811         (fabs(b.z - z) <= tolerance);\r
 812   }\r
 813   bool Compare(const Vector3& b, T tolerance = Math<T>::Tolerance()) const {\r
 814     return IsEqual(b, tolerance);\r
 815   }\r
 816 \r
 817   T& operator[](int idx) {\r
 818     OVR_MATH_ASSERT(0 <= idx && idx < 3);\r
 819     return *(&x + idx);\r
 820   }\r
 821 \r
 822   const T& operator[](int idx) const {\r
 823     OVR_MATH_ASSERT(0 <= idx && idx < 3);\r
 824     return *(&x + idx);\r
 825   }\r
 826 \r
 827   // Entrywise product of two vectors\r
 828   Vector3 EntrywiseMultiply(const Vector3& b) const {\r
 829     return Vector3(x * b.x, y * b.y, z * b.z);\r
 830   }\r
 831 \r
 832   // Multiply and divide operators do entry-wise math\r
 833   Vector3 operator*(const Vector3& b) const {\r
 834     return Vector3(x * b.x, y * b.y, z * b.z);\r
 835   }\r
 836 \r
 837   Vector3 operator/(const Vector3& b) const {\r
 838     return Vector3(x / b.x, y / b.y, z / b.z);\r
 839   }\r
 840 \r
 841   // Dot product\r
 842   // Used to calculate angle q between two vectors among other things,\r
 843   // as (A dot B) = |a||b|cos(q).\r
 844   T Dot(const Vector3& b) const {\r
 845     return x * b.x + y * b.y + z * b.z;\r
 846   }\r
 847 \r
 848   // Compute cross product, which generates a normal vector.\r
 849   // Direction vector can be determined by right-hand rule: Pointing index finder in\r
 850   // direction a and middle finger in direction b, thumb will point in a.Cross(b).\r
 851   Vector3 Cross(const Vector3& b) const {\r
 852     return Vector3(y * b.z - z * b.y, z * b.x - x * b.z, x * b.y - y * b.x);\r
 853   }\r
 854 \r
 855   // Returns the angle from this vector to b, in radians.\r
 856   T Angle(const Vector3& b) const {\r
 857     T div = LengthSq() * b.LengthSq();\r
 858     OVR_MATH_ASSERT(div != T(0));\r
 859     T result = Acos((this->Dot(b)) / sqrt(div));\r
 860     return result;\r
 861   }\r
 862 \r
 863   // Return Length of the vector squared.\r
 864   T LengthSq() const {\r
 865     return (x * x + y * y + z * z);\r
 866   }\r
 867 \r
 868   // Return vector length.\r
 869   T Length() const {\r
 870     return (T)sqrt(LengthSq());\r
 871   }\r
 872 \r
 873   // Returns squared distance between two points represented by vectors.\r
 874   T DistanceSq(Vector3 const& b) const {\r
 875     return (*this - b).LengthSq();\r
 876   }\r
 877 \r
 878   // Returns distance between two points represented by vectors.\r
 879   T Distance(Vector3 const& b) const {\r
 880     return (*this - b).Length();\r
 881   }\r
 882 \r
 883   bool IsNormalized() const {\r
 884     return fabs(LengthSq() - T(1)) < Math<T>::Tolerance();\r
 885   }\r
 886 \r
 887   // Normalize, convention vector length to 1.\r
 888   void Normalize() {\r
 889     T s = Length();\r
 890     if (s != T(0))\r
 891       s = T(1) / s;\r
 892     *this *= s;\r
 893   }\r
 894 \r
 895   // Returns normalized (unit) version of the vector without modifying itself.\r
 896   Vector3 Normalized() const {\r
 897     T s = Length();\r
 898     if (s != T(0))\r
 899       s = T(1) / s;\r
 900     return *this * s;\r
 901   }\r
 902 \r
 903   // Linearly interpolates from this vector to another.\r
 904   // Factor should be between 0.0 and 1.0, with 0 giving full value to this.\r
 905   Vector3 Lerp(const Vector3& b, T f) const {\r
 906     return *this * (T(1) - f) + b * f;\r
 907   }\r
 908 \r
 909   // Projects this vector onto the argument; in other words,\r
 910   // A.Project(B) returns projection of vector A onto B.\r
 911   Vector3 ProjectTo(const Vector3& b) const {\r
 912     T l2 = b.LengthSq();\r
 913     OVR_MATH_ASSERT(l2 != T(0));\r
 914     return b * (Dot(b) / l2);\r
 915   }\r
 916 \r
 917   // Projects this vector onto a plane defined by a normal vector\r
 918   Vector3 ProjectToPlane(const Vector3& normal) const {\r
 919     return *this - this->ProjectTo(normal);\r
 920   }\r
 921 \r
 922   bool IsNan() const {\r
 923     return !isfinite(x + y + z);\r
 924   }\r
 925   bool IsFinite() const {\r
 926     return isfinite(x + y + z);\r
 927   }\r
 928 };\r
 929 \r
 930 typedef Vector3<float> Vector3f;\r
 931 typedef Vector3<double> Vector3d;\r
 932 typedef Vector3<int32_t> Vector3i;\r
 933 \r
 934 OVR_MATH_STATIC_ASSERT((sizeof(Vector3f) == 3 * sizeof(float)), "sizeof(Vector3f) failure");\r
 935 OVR_MATH_STATIC_ASSERT((sizeof(Vector3d) == 3 * sizeof(double)), "sizeof(Vector3d) failure");\r
 936 OVR_MATH_STATIC_ASSERT((sizeof(Vector3i) == 3 * sizeof(int32_t)), "sizeof(Vector3i) failure");\r
 937 \r
 938 typedef Vector3<float> Point3f;\r
 939 typedef Vector3<double> Point3d;\r
 940 typedef Vector3<int32_t> Point3i;\r
 941 \r
 942 //-------------------------------------------------------------------------------------\r
 943 // ***** Vector4<> - 4D vector of {x, y, z, w}\r
 944 \r
 945 //\r
 946 // Vector4f (Vector4d) represents a 3-dimensional vector or point in space,\r
 947 // consisting of coordinates x, y, z and w.\r
 948 \r
 949 template <class T>\r
 950 class Vector4 {\r
 951  public:\r
 952   typedef T ElementType;\r
 953   static const size_t ElementCount = 4;\r
 954 \r
 955   T x, y, z, w;\r
 956 \r
 957   // FIXME: default initialization of a vector class can be very expensive in a full-blown\r
 958   // application.  A few hundred thousand vector constructions is not unlikely and can add\r
 959   // up to milliseconds of time on processors like the PS3 PPU.\r
 960   Vector4() : x(0), y(0), z(0), w(0) {}\r
 961   Vector4(T x_, T y_, T z_, T w_) : x(x_), y(y_), z(z_), w(w_) {}\r
 962   explicit Vector4(T s) : x(s), y(s), z(s), w(s) {}\r
 963   explicit Vector4(const Vector3<T>& v, const T w_ = T(1)) : x(v.x), y(v.y), z(v.z), w(w_) {}\r
 964   explicit Vector4(const Vector4<typename Math<T>::OtherFloatType>& src)\r
 965       : x((T)src.x), y((T)src.y), z((T)src.z), w((T)src.w) {}\r
 966 \r
 967   static Vector4 Zero() {\r
 968     return Vector4(0, 0, 0, 0);\r
 969   }\r
 970 \r
 971   // C-interop support.\r
 972   typedef typename CompatibleTypes<Vector4<T>>::Type CompatibleType;\r
 973 \r
 974   Vector4(const CompatibleType& s) : x(s.x), y(s.y), z(s.z), w(s.w) {}\r
 975 \r
 976   operator const CompatibleType&() const {\r
 977     OVR_MATH_STATIC_ASSERT(\r
 978         sizeof(Vector4<T>) == sizeof(CompatibleType), "sizeof(Vector4<T>) failure");\r
 979     return reinterpret_cast<const CompatibleType&>(*this);\r
 980   }\r
 981 \r
 982   Vector4& operator=(const Vector3<T>& other) {\r
 983     x = other.x;\r
 984     y = other.y;\r
 985     z = other.z;\r
 986     w = 1;\r
 987     return *this;\r
 988   }\r
 989   bool operator==(const Vector4& b) const {\r
 990     return x == b.x && y == b.y && z == b.z && w == b.w;\r
 991   }\r
 992   bool operator!=(const Vector4& b) const {\r
 993     return x != b.x || y != b.y || z != b.z || w != b.w;\r
 994   }\r
 995 \r
 996   Vector4 operator+(const Vector4& b) const {\r
 997     return Vector4(x + b.x, y + b.y, z + b.z, w + b.w);\r
 998   }\r
 999   Vector4& operator+=(const Vector4& b) {\r
1000     x += b.x;\r
1001     y += b.y;\r
1002     z += b.z;\r
1003     w += b.w;\r
1004     return *this;\r
1005   }\r
1006   Vector4 operator-(const Vector4& b) const {\r
1007     return Vector4(x - b.x, y - b.y, z - b.z, w - b.w);\r
1008   }\r
1009   Vector4& operator-=(const Vector4& b) {\r
1010     x -= b.x;\r
1011     y -= b.y;\r
1012     z -= b.z;\r
1013     w -= b.w;\r
1014     return *this;\r
1015   }\r
1016   Vector4 operator-() const {\r
1017     return Vector4(-x, -y, -z, -w);\r
1018   }\r
1019 \r
1020   // Scalar multiplication/division scales vector.\r
1021   Vector4 operator*(T s) const {\r
1022     return Vector4(x * s, y * s, z * s, w * s);\r
1023   }\r
1024   Vector4& operator*=(T s) {\r
1025     x *= s;\r
1026     y *= s;\r
1027     z *= s;\r
1028     w *= s;\r
1029     return *this;\r
1030   }\r
1031 \r
1032   Vector4 operator/(T s) const {\r
1033     T rcp = T(1) / s;\r
1034     return Vector4(x * rcp, y * rcp, z * rcp, w * rcp);\r
1035   }\r
1036   Vector4& operator/=(T s) {\r
1037     T rcp = T(1) / s;\r
1038     x *= rcp;\r
1039     y *= rcp;\r
1040     z *= rcp;\r
1041     w *= rcp;\r
1042     return *this;\r
1043   }\r
1044 \r
1045   static Vector4 Min(const Vector4& a, const Vector4& b) {\r
1046     return Vector4(\r
1047         (a.x < b.x) ? a.x : b.x,\r
1048         (a.y < b.y) ? a.y : b.y,\r
1049         (a.z < b.z) ? a.z : b.z,\r
1050         (a.w < b.w) ? a.w : b.w);\r
1051   }\r
1052   static Vector4 Max(const Vector4& a, const Vector4& b) {\r
1053     return Vector4(\r
1054         (a.x > b.x) ? a.x : b.x,\r
1055         (a.y > b.y) ? a.y : b.y,\r
1056         (a.z > b.z) ? a.z : b.z,\r
1057         (a.w > b.w) ? a.w : b.w);\r
1058   }\r
1059 \r
1060   Vector4 Clamped(T maxMag) const {\r
1061     T magSquared = LengthSq();\r
1062     if (magSquared <= Sqr(maxMag))\r
1063       return *this;\r
1064     else\r
1065       return *this * (maxMag / sqrt(magSquared));\r
1066   }\r
1067 \r
1068   // Compare two vectors for equality with tolerance. Returns true if vectors match within\r
1069   // tolerance.\r
1070   bool IsEqual(const Vector4& b, T tolerance = Math<T>::Tolerance()) const {\r
1071     return (fabs(b.x - x) <= tolerance) && (fabs(b.y - y) <= tolerance) &&\r
1072         (fabs(b.z - z) <= tolerance) && (fabs(b.w - w) <= tolerance);\r
1073   }\r
1074   bool Compare(const Vector4& b, T tolerance = Math<T>::Tolerance()) const {\r
1075     return IsEqual(b, tolerance);\r
1076   }\r
1077 \r
1078   T& operator[](int idx) {\r
1079     OVR_MATH_ASSERT(0 <= idx && idx < 4);\r
1080     return *(&x + idx);\r
1081   }\r
1082 \r
1083   const T& operator[](int idx) const {\r
1084     OVR_MATH_ASSERT(0 <= idx && idx < 4);\r
1085     return *(&x + idx);\r
1086   }\r
1087 \r
1088   // Entry wise product of two vectors\r
1089   Vector4 EntrywiseMultiply(const Vector4& b) const {\r
1090     return Vector4(x * b.x, y * b.y, z * b.z, w * b.w);\r
1091   }\r
1092 \r
1093   // Multiply and divide operators do entry-wise math\r
1094   Vector4 operator*(const Vector4& b) const {\r
1095     return Vector4(x * b.x, y * b.y, z * b.z, w * b.w);\r
1096   }\r
1097 \r
1098   Vector4 operator/(const Vector4& b) const {\r
1099     return Vector4(x / b.x, y / b.y, z / b.z, w / b.w);\r
1100   }\r
1101 \r
1102   // Dot product\r
1103   T Dot(const Vector4& b) const {\r
1104     return x * b.x + y * b.y + z * b.z + w * b.w;\r
1105   }\r
1106 \r
1107   // Return Length of the vector squared.\r
1108   T LengthSq() const {\r
1109     return (x * x + y * y + z * z + w * w);\r
1110   }\r
1111 \r
1112   // Return vector length.\r
1113   T Length() const {\r
1114     return sqrt(LengthSq());\r
1115   }\r
1116 \r
1117   bool IsNormalized() const {\r
1118     return fabs(LengthSq() - T(1)) < Math<T>::Tolerance();\r
1119   }\r
1120 \r
1121   // Normalize, convention vector length to 1.\r
1122   void Normalize() {\r
1123     T s = Length();\r
1124     if (s != T(0))\r
1125       s = T(1) / s;\r
1126     *this *= s;\r
1127   }\r
1128 \r
1129   // Returns normalized (unit) version of the vector without modifying itself.\r
1130   Vector4 Normalized() const {\r
1131     T s = Length();\r
1132     if (s != T(0))\r
1133       s = T(1) / s;\r
1134     return *this * s;\r
1135   }\r
1136 \r
1137   // Linearly interpolates from this vector to another.\r
1138   // Factor should be between 0.0 and 1.0, with 0 giving full value to this.\r
1139   Vector4 Lerp(const Vector4& b, T f) const {\r
1140     return *this * (T(1) - f) + b * f;\r
1141   }\r
1142 };\r
1143 \r
1144 typedef Vector4<float> Vector4f;\r
1145 typedef Vector4<double> Vector4d;\r
1146 typedef Vector4<int> Vector4i;\r
1147 \r
1148 //-------------------------------------------------------------------------------------\r
1149 // ***** Bounds3\r
1150 \r
1151 // Bounds class used to describe a 3D axis aligned bounding box.\r
1152 \r
1153 template <class T>\r
1154 class Bounds3 {\r
1155  public:\r
1156   Vector3<T> b[2];\r
1157 \r
1158   Bounds3() {\r
1159     Clear();\r
1160   }\r
1161 \r
1162   Bounds3(const Vector3<T>& mins, const Vector3<T>& maxs) {\r
1163     b[0] = mins;\r
1164     b[1] = maxs;\r
1165   }\r
1166 \r
1167   void Clear() {\r
1168     b[0].x = b[0].y = b[0].z = Math<T>::MaxValue();\r
1169     b[1].x = b[1].y = b[1].z = -Math<T>::MaxValue();\r
1170   }\r
1171 \r
1172   void AddPoint(const Vector3<T>& v) {\r
1173     b[0].x = (b[0].x < v.x ? b[0].x : v.x);\r
1174     b[0].y = (b[0].y < v.y ? b[0].y : v.y);\r
1175     b[0].z = (b[0].z < v.z ? b[0].z : v.z);\r
1176     b[1].x = (v.x < b[1].x ? b[1].x : v.x);\r
1177     b[1].y = (v.y < b[1].y ? b[1].y : v.y);\r
1178     b[1].z = (v.z < b[1].z ? b[1].z : v.z);\r
1179   }\r
1180 \r
1181   bool Excludes(const Vector3<T>& v) const {\r
1182     bool testing = false;\r
1183     for (int32_t t = 0; t < 3; ++t) {\r
1184       testing |= v[t] > b[1][t];\r
1185       testing |= v[t] < b[0][t];\r
1186     }\r
1187     return testing;\r
1188   }\r
1189 \r
1190   // exludes, ignoring vertical\r
1191   bool ExcludesXZ(const Vector3<T>& v) const {\r
1192     bool testing = false;\r
1193     testing |= v[0] > b[1][0];\r
1194     testing |= v[0] < b[0][0];\r
1195     testing |= v[2] > b[1][2];\r
1196     testing |= v[2] < b[0][2];\r
1197     return testing;\r
1198   }\r
1199 \r
1200   bool Excludes(const Bounds3<T>& bounds) const {\r
1201     bool testing = false;\r
1202     for (int32_t t = 0; t < 3; ++t) {\r
1203       testing |= bounds.b[0][t] > b[1][t];\r
1204       testing |= bounds.b[1][t] < b[0][t];\r
1205     }\r
1206     return testing;\r
1207   }\r
1208 \r
1209   const Vector3<T>& GetMins() const {\r
1210     return b[0];\r
1211   }\r
1212   const Vector3<T>& GetMaxs() const {\r
1213     return b[1];\r
1214   }\r
1215 \r
1216   Vector3<T>& GetMins() {\r
1217     return b[0];\r
1218   }\r
1219   Vector3<T>& GetMaxs() {\r
1220     return b[1];\r
1221   }\r
1222 };\r
1223 \r
1224 typedef Bounds3<float> Bounds3f;\r
1225 typedef Bounds3<double> Bounds3d;\r
1226 \r
1227 //-------------------------------------------------------------------------------------\r
1228 // ***** Size\r
1229 \r
1230 // Size class represents 2D size with Width, Height components.\r
1231 // Used to describe distentions of render targets, etc.\r
1232 \r
1233 template <class T>\r
1234 class Size {\r
1235  public:\r
1236   T w, h;\r
1237 \r
1238   Size() : w(0), h(0) {}\r
1239   Size(T w_, T h_) : w(w_), h(h_) {}\r
1240   explicit Size(T s) : w(s), h(s) {}\r
1241   explicit Size(const Size<typename Math<T>::OtherFloatType>& src) : w((T)src.w), h((T)src.h) {}\r
1242 \r
1243   // C-interop support.\r
1244   typedef typename CompatibleTypes<Size<T>>::Type CompatibleType;\r
1245 \r
1246   Size(const CompatibleType& s) : w(s.w), h(s.h) {}\r
1247 \r
1248   operator const CompatibleType&() const {\r
1249     OVR_MATH_STATIC_ASSERT(sizeof(Size<T>) == sizeof(CompatibleType), "sizeof(Size<T>) failure");\r
1250     return reinterpret_cast<const CompatibleType&>(*this);\r
1251   }\r
1252 \r
1253   bool operator==(const Size& b) const {\r
1254     return w == b.w && h == b.h;\r
1255   }\r
1256   bool operator!=(const Size& b) const {\r
1257     return w != b.w || h != b.h;\r
1258   }\r
1259 \r
1260   Size operator+(const Size& b) const {\r
1261     return Size(w + b.w, h + b.h);\r
1262   }\r
1263   Size& operator+=(const Size& b) {\r
1264     w += b.w;\r
1265     h += b.h;\r
1266     return *this;\r
1267   }\r
1268   Size operator-(const Size& b) const {\r
1269     return Size(w - b.w, h - b.h);\r
1270   }\r
1271   Size& operator-=(const Size& b) {\r
1272     w -= b.w;\r
1273     h -= b.h;\r
1274     return *this;\r
1275   }\r
1276   Size operator-() const {\r
1277     return Size(-w, -h);\r
1278   }\r
1279   Size operator*(const Size& b) const {\r
1280     return Size(w * b.w, h * b.h);\r
1281   }\r
1282   Size& operator*=(const Size& b) {\r
1283     w *= b.w;\r
1284     h *= b.h;\r
1285     return *this;\r
1286   }\r
1287   Size operator/(const Size& b) const {\r
1288     return Size(w / b.w, h / b.h);\r
1289   }\r
1290   Size& operator/=(const Size& b) {\r
1291     w /= b.w;\r
1292     h /= b.h;\r
1293     return *this;\r
1294   }\r
1295 \r
1296   // Scalar multiplication/division scales both components.\r
1297   Size operator*(T s) const {\r
1298     return Size(w * s, h * s);\r
1299   }\r
1300   Size& operator*=(T s) {\r
1301     w *= s;\r
1302     h *= s;\r
1303     return *this;\r
1304   }\r
1305   Size operator/(T s) const {\r
1306     return Size(w / s, h / s);\r
1307   }\r
1308   Size& operator/=(T s) {\r
1309     w /= s;\r
1310     h /= s;\r
1311     return *this;\r
1312   }\r
1313 \r
1314   static Size Min(const Size& a, const Size& b) {\r
1315     return Size((a.w < b.w) ? a.w : b.w, (a.h < b.h) ? a.h : b.h);\r
1316   }\r
1317   static Size Max(const Size& a, const Size& b) {\r
1318     return Size((a.w > b.w) ? a.w : b.w, (a.h > b.h) ? a.h : b.h);\r
1319   }\r
1320 \r
1321   T Area() const {\r
1322     return w * h;\r
1323   }\r
1324 \r
1325   inline Vector2<T> ToVector() const {\r
1326     return Vector2<T>(w, h);\r
1327   }\r
1328 };\r
1329 \r
1330 typedef Size<int> Sizei;\r
1331 typedef Size<unsigned> Sizeu;\r
1332 typedef Size<float> Sizef;\r
1333 typedef Size<double> Sized;\r
1334 \r
1335 //-----------------------------------------------------------------------------------\r
1336 // ***** Rect\r
1337 \r
1338 // Rect describes a rectangular area for rendering, that includes position and size.\r
1339 template <class T>\r
1340 class Rect {\r
1341  public:\r
1342   T x, y;\r
1343   T w, h;\r
1344 \r
1345   Rect() {}\r
1346   Rect(T x1, T y1, T w1, T h1) : x(x1), y(y1), w(w1), h(h1) {}\r
1347   Rect(const Vector2<T>& pos, const Size<T>& sz) : x(pos.x), y(pos.y), w(sz.w), h(sz.h) {}\r
1348   Rect(const Size<T>& sz) : x(0), y(0), w(sz.w), h(sz.h) {}\r
1349 \r
1350   // C-interop support.\r
1351   typedef typename CompatibleTypes<Rect<T>>::Type CompatibleType;\r
1352 \r
1353   Rect(const CompatibleType& s) : x(s.Pos.x), y(s.Pos.y), w(s.Size.w), h(s.Size.h) {}\r
1354 \r
1355   operator const CompatibleType&() const {\r
1356     OVR_MATH_STATIC_ASSERT(sizeof(Rect<T>) == sizeof(CompatibleType), "sizeof(Rect<T>) failure");\r
1357     return reinterpret_cast<const CompatibleType&>(*this);\r
1358   }\r
1359 \r
1360   Vector2<T> GetPos() const {\r
1361     return Vector2<T>(x, y);\r
1362   }\r
1363   Size<T> GetSize() const {\r
1364     return Size<T>(w, h);\r
1365   }\r
1366   void SetPos(const Vector2<T>& pos) {\r
1367     x = pos.x;\r
1368     y = pos.y;\r
1369   }\r
1370   void SetSize(const Size<T>& sz) {\r
1371     w = sz.w;\r
1372     h = sz.h;\r
1373   }\r
1374 \r
1375   bool operator==(const Rect& vp) const {\r
1376     return (x == vp.x) && (y == vp.y) && (w == vp.w) && (h == vp.h);\r
1377   }\r
1378   bool operator!=(const Rect& vp) const {\r
1379     return !operator==(vp);\r
1380   }\r
1381 };\r
1382 \r
1383 typedef Rect<int> Recti;\r
1384 \r
1385 //-------------------------------------------------------------------------------------//\r
1386 // ***** Quat\r
1387 //\r
1388 // Quatf represents a quaternion class used for rotations.\r
1389 //\r
1390 // Quaternion multiplications are done in right-to-left order, to match the\r
1391 // behavior of matrices.\r
1392 \r
1393 template <class T>\r
1394 class Quat {\r
1395  public:\r
1396   typedef T ElementType;\r
1397   static const size_t ElementCount = 4;\r
1398 \r
1399   // x,y,z = axis*sin(angle), w = cos(angle)\r
1400   T x, y, z, w;\r
1401 \r
1402   Quat() : x(0), y(0), z(0), w(1) {}\r
1403   Quat(T x_, T y_, T z_, T w_) : x(x_), y(y_), z(z_), w(w_) {}\r
1404   explicit Quat(const Quat<typename Math<T>::OtherFloatType>& src)\r
1405       : x((T)src.x), y((T)src.y), z((T)src.z), w((T)src.w) {\r
1406     // NOTE: Converting a normalized Quat<float> to Quat<double>\r
1407     // will generally result in an un-normalized quaternion.\r
1408     // But we don't normalize here in case the quaternion\r
1409     // being converted is not a normalized rotation quaternion.\r
1410   }\r
1411 \r
1412   typedef typename CompatibleTypes<Quat<T>>::Type CompatibleType;\r
1413 \r
1414   // C-interop support.\r
1415   Quat(const CompatibleType& s) : x(s.x), y(s.y), z(s.z), w(s.w) {}\r
1416 \r
1417   operator CompatibleType() const {\r
1418     CompatibleType result;\r
1419     result.x = x;\r
1420     result.y = y;\r
1421     result.z = z;\r
1422     result.w = w;\r
1423     return result;\r
1424   }\r
1425 \r
1426   // Constructs quaternion for rotation around the axis by an angle.\r
1427   Quat(const Vector3<T>& axis, T angle) {\r
1428     // Make sure we don't divide by zero.\r
1429     if (axis.LengthSq() == T(0)) {\r
1430       // Assert if the axis is zero, but the angle isn't\r
1431       OVR_MATH_ASSERT(angle == T(0));\r
1432       x = y = z = T(0);\r
1433       w = T(1);\r
1434       return;\r
1435     }\r
1436 \r
1437     Vector3<T> unitAxis = axis.Normalized();\r
1438     T sinHalfAngle = sin(angle * T(0.5));\r
1439 \r
1440     w = cos(angle * T(0.5));\r
1441     x = unitAxis.x * sinHalfAngle;\r
1442     y = unitAxis.y * sinHalfAngle;\r
1443     z = unitAxis.z * sinHalfAngle;\r
1444   }\r
1445 \r
1446   // Constructs quaternion for rotation around one of the coordinate axis by an angle.\r
1447   Quat(Axis A, T angle, RotateDirection d = Rotate_CCW, HandedSystem s = Handed_R) {\r
1448     T sinHalfAngle = s * d * sin(angle * T(0.5));\r
1449     T v[3];\r
1450     v[0] = v[1] = v[2] = T(0);\r
1451     v[A] = sinHalfAngle;\r
1452 \r
1453     w = cos(angle * T(0.5));\r
1454     x = v[0];\r
1455     y = v[1];\r
1456     z = v[2];\r
1457   }\r
1458 \r
1459   Quat operator-() {\r
1460     return Quat(-x, -y, -z, -w);\r
1461   } // unary minus\r
1462 \r
1463   static Quat Identity() {\r
1464     return Quat(0, 0, 0, 1);\r
1465   }\r
1466 \r
1467   // Compute axis and angle from quaternion\r
1468   void GetAxisAngle(Vector3<T>* axis, T* angle) const {\r
1469     if (x * x + y * y + z * z > Math<T>::Tolerance() * Math<T>::Tolerance()) {\r
1470       *axis = Vector3<T>(x, y, z).Normalized();\r
1471       *angle = 2 * Acos(w);\r
1472       if (*angle > ((T)MATH_DOUBLE_PI)) // Reduce the magnitude of the angle, if necessary\r
1473       {\r
1474         *angle = ((T)MATH_DOUBLE_TWOPI) - *angle;\r
1475         *axis = *axis * (-1);\r
1476       }\r
1477     } else {\r
1478       *axis = Vector3<T>(1, 0, 0);\r
1479       *angle = T(0);\r
1480     }\r
1481   }\r
1482 \r
1483   // Convert a quaternion to a rotation vector, also known as\r
1484   // Rodrigues vector, AxisAngle vector, SORA vector, exponential map.\r
1485   // A rotation vector describes a rotation about an axis:\r
1486   // the axis of rotation is the vector normalized,\r
1487   // the angle of rotation is the magnitude of the vector.\r
1488   Vector3<T> ToRotationVector() const {\r
1489     // OVR_MATH_ASSERT(IsNormalized()); // If this fires, caller has a quat math bug\r
1490     T s = T(0);\r
1491     T sinHalfAngle = sqrt(x * x + y * y + z * z);\r
1492     if (sinHalfAngle > T(0)) {\r
1493       T cosHalfAngle = w;\r
1494       T halfAngle = atan2(sinHalfAngle, cosHalfAngle);\r
1495 \r
1496       // Ensure minimum rotation magnitude\r
1497       if (cosHalfAngle < 0)\r
1498         halfAngle -= T(MATH_DOUBLE_PI);\r
1499 \r
1500       s = T(2) * halfAngle / sinHalfAngle;\r
1501     }\r
1502     return Vector3<T>(x * s, y * s, z * s);\r
1503   }\r
1504 \r
1505   // Faster version of the above, optimized for use with small rotations, where rotation angle ~=\r
1506   // sin(angle)\r
1507   inline OVR::Vector3<T> FastToRotationVector() const {\r
1508     OVR_MATH_ASSERT(IsNormalized()); // If this fires, caller has a quat math bug\r
1509     T s;\r
1510     T sinHalfSquared = x * x + y * y + z * z;\r
1511     if (sinHalfSquared < T(.0037)) // =~ sin(7/2 degrees)^2\r
1512     {\r
1513       // Max rotation magnitude error is about .062% at 7 degrees rotation, or about .0043 degrees\r
1514       s = T(2) * Sign(w);\r
1515     } else {\r
1516       T sinHalfAngle = sqrt(sinHalfSquared);\r
1517       T cosHalfAngle = w;\r
1518       T halfAngle = atan2(sinHalfAngle, cosHalfAngle);\r
1519 \r
1520       // Ensure minimum rotation magnitude\r
1521       if (cosHalfAngle < 0)\r
1522         halfAngle -= T(MATH_DOUBLE_PI);\r
1523 \r
1524       s = T(2) * halfAngle / sinHalfAngle;\r
1525     }\r
1526     return Vector3<T>(x * s, y * s, z * s);\r
1527   }\r
1528 \r
1529   // Given a rotation vector of form unitRotationAxis * angle,\r
1530   // returns the equivalent quaternion (unitRotationAxis * sin(angle), cos(Angle)).\r
1531   static Quat FromRotationVector(const Vector3<T>& v) {\r
1532     T angleSquared = v.LengthSq();\r
1533     T s = T(0);\r
1534     T c = T(1);\r
1535     if (angleSquared > T(0)) {\r
1536       T angle = sqrt(angleSquared);\r
1537       s = sin(angle * T(0.5)) / angle; // normalize\r
1538       c = cos(angle * T(0.5));\r
1539     }\r
1540     return Quat(s * v.x, s * v.y, s * v.z, c);\r
1541   }\r
1542 \r
1543   // Faster version of above, optimized for use with small rotation magnitudes, where rotation angle\r
1544   // =~ sin(angle).\r
1545   // If normalize is false, small-angle quaternions are returned un-normalized.\r
1546   inline static Quat FastFromRotationVector(const OVR::Vector3<T>& v, bool normalize = true) {\r
1547     T s, c;\r
1548     T angleSquared = v.LengthSq();\r
1549     if (angleSquared < T(0.0076)) // =~ (5 degrees*pi/180)^2\r
1550     {\r
1551       s = T(0.5);\r
1552       c = T(1.0);\r
1553       // Max rotation magnitude error (after normalization) is about .064% at 5 degrees rotation, or\r
1554       // .0032 degrees\r
1555       if (normalize && angleSquared > 0) {\r
1556         // sin(angle/2)^2 ~= (angle/2)^2 and cos(angle/2)^2 ~= 1\r
1557         T invLen = T(1) / sqrt(angleSquared * T(0.25) + T(1)); // normalize\r
1558         s = s * invLen;\r
1559         c = c * invLen;\r
1560       }\r
1561     } else {\r
1562       T angle = sqrt(angleSquared);\r
1563       s = sin(angle * T(0.5)) / angle;\r
1564       c = cos(angle * T(0.5));\r
1565     }\r
1566     return Quat(s * v.x, s * v.y, s * v.z, c);\r
1567   }\r
1568 \r
1569   // Constructs the quaternion from a rotation matrix\r
1570   explicit Quat(const Matrix4<T>& m) {\r
1571     T trace = m.M[0][0] + m.M[1][1] + m.M[2][2];\r
1572 \r
1573     // In almost all cases, the first part is executed.\r
1574     // However, if the trace is not positive, the other\r
1575     // cases arise.\r
1576     if (trace > T(0)) {\r
1577       T s = sqrt(trace + T(1)) * T(2); // s=4*qw\r
1578       w = T(0.25) * s;\r
1579       x = (m.M[2][1] - m.M[1][2]) / s;\r
1580       y = (m.M[0][2] - m.M[2][0]) / s;\r
1581       z = (m.M[1][0] - m.M[0][1]) / s;\r
1582     } else if ((m.M[0][0] > m.M[1][1]) && (m.M[0][0] > m.M[2][2])) {\r
1583       T s = sqrt(T(1) + m.M[0][0] - m.M[1][1] - m.M[2][2]) * T(2);\r
1584       w = (m.M[2][1] - m.M[1][2]) / s;\r
1585       x = T(0.25) * s;\r
1586       y = (m.M[0][1] + m.M[1][0]) / s;\r
1587       z = (m.M[2][0] + m.M[0][2]) / s;\r
1588     } else if (m.M[1][1] > m.M[2][2]) {\r
1589       T s = sqrt(T(1) + m.M[1][1] - m.M[0][0] - m.M[2][2]) * T(2); // S=4*qy\r
1590       w = (m.M[0][2] - m.M[2][0]) / s;\r
1591       x = (m.M[0][1] + m.M[1][0]) / s;\r
1592       y = T(0.25) * s;\r
1593       z = (m.M[1][2] + m.M[2][1]) / s;\r
1594     } else {\r
1595       T s = sqrt(T(1) + m.M[2][2] - m.M[0][0] - m.M[1][1]) * T(2); // S=4*qz\r
1596       w = (m.M[1][0] - m.M[0][1]) / s;\r
1597       x = (m.M[0][2] + m.M[2][0]) / s;\r
1598       y = (m.M[1][2] + m.M[2][1]) / s;\r
1599       z = T(0.25) * s;\r
1600     }\r
1601     OVR_MATH_ASSERT(IsNormalized()); // Ensure input matrix is orthogonal\r
1602   }\r
1603 \r
1604   // Constructs the quaternion from a rotation matrix\r
1605   explicit Quat(const Matrix3<T>& m) {\r
1606     T trace = m.M[0][0] + m.M[1][1] + m.M[2][2];\r
1607 \r
1608     // In almost all cases, the first part is executed.\r
1609     // However, if the trace is not positive, the other\r
1610     // cases arise.\r
1611     if (trace > T(0)) {\r
1612       T s = sqrt(trace + T(1)) * T(2); // s=4*qw\r
1613       w = T(0.25) * s;\r
1614       x = (m.M[2][1] - m.M[1][2]) / s;\r
1615       y = (m.M[0][2] - m.M[2][0]) / s;\r
1616       z = (m.M[1][0] - m.M[0][1]) / s;\r
1617     } else if ((m.M[0][0] > m.M[1][1]) && (m.M[0][0] > m.M[2][2])) {\r
1618       T s = sqrt(T(1) + m.M[0][0] - m.M[1][1] - m.M[2][2]) * T(2);\r
1619       w = (m.M[2][1] - m.M[1][2]) / s;\r
1620       x = T(0.25) * s;\r
1621       y = (m.M[0][1] + m.M[1][0]) / s;\r
1622       z = (m.M[2][0] + m.M[0][2]) / s;\r
1623     } else if (m.M[1][1] > m.M[2][2]) {\r
1624       T s = sqrt(T(1) + m.M[1][1] - m.M[0][0] - m.M[2][2]) * T(2); // S=4*qy\r
1625       w = (m.M[0][2] - m.M[2][0]) / s;\r
1626       x = (m.M[0][1] + m.M[1][0]) / s;\r
1627       y = T(0.25) * s;\r
1628       z = (m.M[1][2] + m.M[2][1]) / s;\r
1629     } else {\r
1630       T s = sqrt(T(1) + m.M[2][2] - m.M[0][0] - m.M[1][1]) * T(2); // S=4*qz\r
1631       w = (m.M[1][0] - m.M[0][1]) / s;\r
1632       x = (m.M[0][2] + m.M[2][0]) / s;\r
1633       y = (m.M[1][2] + m.M[2][1]) / s;\r
1634       z = T(0.25) * s;\r
1635     }\r
1636     OVR_MATH_ASSERT(IsNormalized()); // Ensure input matrix is orthogonal\r
1637   }\r
1638 \r
1639   // MERGE_MOBILE_SDK\r
1640   // Constructs a quaternion that rotates 'from' to line up with 'to'.\r
1641   explicit Quat(const Vector3<T>& from, const Vector3<T>& to) {\r
1642     const T cx = from.y * to.z - from.z * to.y;\r
1643     const T cy = from.z * to.x - from.x * to.z;\r
1644     const T cz = from.x * to.y - from.y * to.x;\r
1645     const T dot = from.x * to.x + from.y * to.y + from.z * to.z;\r
1646     const T crossLengthSq = cx * cx + cy * cy + cz * cz;\r
1647     const T magnitude = static_cast<T>(sqrt(crossLengthSq + dot * dot));\r
1648     const T cw = dot + magnitude;\r
1649     if (cw < Math<T>::SmallestNonDenormal()) {\r
1650       const T sx = to.y * to.y + to.z * to.z;\r
1651       const T sz = to.x * to.x + to.y * to.y;\r
1652       if (sx > sz) {\r
1653         const T rcpLength = RcpSqrt(sx);\r
1654         x = T(0);\r
1655         y = to.z * rcpLength;\r
1656         z = -to.y * rcpLength;\r
1657         w = T(0);\r
1658       } else {\r
1659         const T rcpLength = RcpSqrt(sz);\r
1660         x = to.y * rcpLength;\r
1661         y = -to.x * rcpLength;\r
1662         z = T(0);\r
1663         w = T(0);\r
1664       }\r
1665       return;\r
1666     }\r
1667     const T rcpLength = RcpSqrt(crossLengthSq + cw * cw);\r
1668     x = cx * rcpLength;\r
1669     y = cy * rcpLength;\r
1670     z = cz * rcpLength;\r
1671     w = cw * rcpLength;\r
1672   }\r
1673   // MERGE_MOBILE_SDK\r
1674 \r
1675   bool operator==(const Quat& b) const {\r
1676     return x == b.x && y == b.y && z == b.z && w == b.w;\r
1677   }\r
1678   bool operator!=(const Quat& b) const {\r
1679     return x != b.x || y != b.y || z != b.z || w != b.w;\r
1680   }\r
1681 \r
1682   Quat operator+(const Quat& b) const {\r
1683     return Quat(x + b.x, y + b.y, z + b.z, w + b.w);\r
1684   }\r
1685   Quat& operator+=(const Quat& b) {\r
1686     w += b.w;\r
1687     x += b.x;\r
1688     y += b.y;\r
1689     z += b.z;\r
1690     return *this;\r
1691   }\r
1692   Quat operator-(const Quat& b) const {\r
1693     return Quat(x - b.x, y - b.y, z - b.z, w - b.w);\r
1694   }\r
1695   Quat& operator-=(const Quat& b) {\r
1696     w -= b.w;\r
1697     x -= b.x;\r
1698     y -= b.y;\r
1699     z -= b.z;\r
1700     return *this;\r
1701   }\r
1702 \r
1703   Quat operator*(T s) const {\r
1704     return Quat(x * s, y * s, z * s, w * s);\r
1705   }\r
1706   Quat& operator*=(T s) {\r
1707     w *= s;\r
1708     x *= s;\r
1709     y *= s;\r
1710     z *= s;\r
1711     return *this;\r
1712   }\r
1713   Quat operator/(T s) const {\r
1714     T rcp = T(1) / s;\r
1715     return Quat(x * rcp, y * rcp, z * rcp, w * rcp);\r
1716   }\r
1717   Quat& operator/=(T s) {\r
1718     T rcp = T(1) / s;\r
1719     w *= rcp;\r
1720     x *= rcp;\r
1721     y *= rcp;\r
1722     z *= rcp;\r
1723     return *this;\r
1724   }\r
1725 \r
1726   // MERGE_MOBILE_SDK\r
1727   Vector3<T> operator*(const Vector3<T>& v) const {\r
1728     return Rotate(v);\r
1729   }\r
1730   // MERGE_MOBILE_SDK\r
1731 \r
1732   // Compare two quats for equality within tolerance. Returns true if quats match within tolerance.\r
1733   bool IsEqual(const Quat& b, T tolerance = Math<T>::Tolerance()) const {\r
1734     return Abs(Dot(b)) >= T(1) - tolerance;\r
1735   }\r
1736 \r
1737   // Compare two quats for equality within tolerance while checking matching hemispheres. Returns\r
1738   // true if quats match within tolerance.\r
1739   bool IsEqualMatchHemisphere(Quat b, T tolerance = Math<T>::Tolerance()) const {\r
1740     b.EnsureSameHemisphere(*this);\r
1741     return Abs(Dot(b)) >= T(1) - tolerance;\r
1742   }\r
1743 \r
1744   static T Abs(const T v) {\r
1745     return (v >= 0) ? v : -v;\r
1746   }\r
1747 \r
1748   // Get Imaginary part vector\r
1749   Vector3<T> Imag() const {\r
1750     return Vector3<T>(x, y, z);\r
1751   }\r
1752 \r
1753   // Get quaternion length.\r
1754   T Length() const {\r
1755     return sqrt(LengthSq());\r
1756   }\r
1757 \r
1758   // Get quaternion length squared.\r
1759   T LengthSq() const {\r
1760     return (x * x + y * y + z * z + w * w);\r
1761   }\r
1762 \r
1763   // Simple Euclidean distance in R^4 (not SLERP distance, but at least respects Haar measure)\r
1764   T Distance(const Quat& q) const {\r
1765     T d1 = (*this - q).Length();\r
1766     T d2 = (*this + q).Length(); // Antipodal point check\r
1767     return (d1 < d2) ? d1 : d2;\r
1768   }\r
1769 \r
1770   T DistanceSq(const Quat& q) const {\r
1771     T d1 = (*this - q).LengthSq();\r
1772     T d2 = (*this + q).LengthSq(); // Antipodal point check\r
1773     return (d1 < d2) ? d1 : d2;\r
1774   }\r
1775 \r
1776   T Dot(const Quat& q) const {\r
1777     return x * q.x + y * q.y + z * q.z + w * q.w;\r
1778   }\r
1779 \r
1780   // Angle between two quaternions in radians\r
1781   T Angle(const Quat& q) const {\r
1782     return T(2) * Acos(Abs(Dot(q)));\r
1783   }\r
1784 \r
1785   // Angle of quaternion\r
1786   T Angle() const {\r
1787     return T(2) * Acos(Abs(w));\r
1788   }\r
1789 \r
1790   // Normalize\r
1791   bool IsNormalized() const {\r
1792     return fabs(LengthSq() - T(1)) < Math<T>::Tolerance();\r
1793   }\r
1794 \r
1795   void Normalize() {\r
1796     T s = Length();\r
1797     if (s != T(0))\r
1798       s = T(1) / s;\r
1799     *this *= s;\r
1800   }\r
1801 \r
1802   Quat Normalized() const {\r
1803     T s = Length();\r
1804     if (s != T(0))\r
1805       s = T(1) / s;\r
1806     return *this * s;\r
1807   }\r
1808 \r
1809   inline void EnsureSameHemisphere(const Quat& o) {\r
1810     if (Dot(o) < T(0)) {\r
1811       x = -x;\r
1812       y = -y;\r
1813       z = -z;\r
1814       w = -w;\r
1815     }\r
1816   }\r
1817 \r
1818   // Returns conjugate of the quaternion. Produces inverse rotation if quaternion is normalized.\r
1819   Quat Conj() const {\r
1820     return Quat(-x, -y, -z, w);\r
1821   }\r
1822 \r
1823   // Quaternion multiplication. Combines quaternion rotations, performing the one on the\r
1824   // right hand side first.\r
1825   Quat operator*(const Quat& b) const {\r
1826     return Quat(\r
1827         w * b.x + x * b.w + y * b.z - z * b.y,\r
1828         w * b.y - x * b.z + y * b.w + z * b.x,\r
1829         w * b.z + x * b.y - y * b.x + z * b.w,\r
1830         w * b.w - x * b.x - y * b.y - z * b.z);\r
1831   }\r
1832   const Quat& operator*=(const Quat& b) {\r
1833     *this = *this * b;\r
1834     return *this;\r
1835   }\r
1836 \r
1837   //\r
1838   // this^p normalized; same as rotating by this p times.\r
1839   Quat PowNormalized(T p) const {\r
1840     Vector3<T> v;\r
1841     T a;\r
1842     GetAxisAngle(&v, &a);\r
1843     return Quat(v, a * p);\r
1844   }\r
1845 \r
1846   // Compute quaternion that rotates v into alignTo: alignTo = Quat::Align(alignTo, v).Rotate(v).\r
1847   // NOTE: alignTo and v must be normalized.\r
1848   static Quat Align(const Vector3<T>& alignTo, const Vector3<T>& v) {\r
1849     OVR_MATH_ASSERT(alignTo.IsNormalized() && v.IsNormalized());\r
1850     Vector3<T> bisector = (v + alignTo);\r
1851     bisector.Normalize();\r
1852     T cosHalfAngle = v.Dot(bisector); // 0..1\r
1853     if (cosHalfAngle > T(0)) {\r
1854       Vector3<T> imag = v.Cross(bisector);\r
1855       return Quat(imag.x, imag.y, imag.z, cosHalfAngle);\r
1856     } else {\r
1857       // cosHalfAngle == 0: a 180 degree rotation.\r
1858       // sinHalfAngle == 1, rotation axis is any axis perpendicular\r
1859       // to alignTo.  Choose axis to include largest magnitude components\r
1860       if (fabs(v.x) > fabs(v.y)) {\r
1861         // x or z is max magnitude component\r
1862         // = Cross(v, (0,1,0)).Normalized();\r
1863         T invLen = sqrt(v.x * v.x + v.z * v.z);\r
1864         if (invLen > T(0))\r
1865           invLen = T(1) / invLen;\r
1866         return Quat(-v.z * invLen, 0, v.x * invLen, 0);\r
1867       } else {\r
1868         // y or z is max magnitude component\r
1869         // = Cross(v, (1,0,0)).Normalized();\r
1870         T invLen = sqrt(v.y * v.y + v.z * v.z);\r
1871         if (invLen > T(0))\r
1872           invLen = T(1) / invLen;\r
1873         return Quat(0, v.z * invLen, -v.y * invLen, 0);\r
1874       }\r
1875     }\r
1876   }\r
1877 \r
1878   // Decompose a quat into quat = swing * twist, where twist is a rotation about axis,\r
1879   // and swing is a rotation perpendicular to axis.\r
1880   Quat GetSwingTwist(const Vector3<T>& axis, Quat* twist) const {\r
1881     OVR_MATH_ASSERT(twist);\r
1882     OVR_MATH_ASSERT(axis.IsNormalized());\r
1883 \r
1884     // Create a normalized quaternion from projection of (x,y,z) onto axis\r
1885     T d = axis.Dot(Vector3<T>(x, y, z));\r
1886     *twist = Quat(axis.x * d, axis.y * d, axis.z * d, w);\r
1887     T len = twist->Length();\r
1888     if (len == 0)\r
1889       twist->w = T(1); // identity\r
1890     else\r
1891       *twist /= len; // normalize\r
1892 \r
1893     return *this * twist->Inverted();\r
1894   }\r
1895 \r
1896   // Normalized linear interpolation of quaternions\r
1897   // NOTE: This function is a bad approximation of Slerp()\r
1898   // when the angle between the *this and b is large.\r
1899   // Use FastSlerp() or Slerp() instead.\r
1900   Quat Lerp(const Quat& b, T s) const {\r
1901     return (*this * (T(1) - s) + b * (Dot(b) < 0 ? -s : s)).Normalized();\r
1902   }\r
1903 \r
1904   // Spherical linear interpolation between rotations\r
1905   Quat Slerp(const Quat& b, T s) const {\r
1906     Vector3<T> delta = (b * this->Inverted()).ToRotationVector();\r
1907     return (FromRotationVector(delta * s) * *this)\r
1908         .Normalized(); // normalize so errors don't accumulate\r
1909   }\r
1910 \r
1911   // Spherical linear interpolation: much faster for small rotations, accurate for large rotations.\r
1912   // See FastTo/FromRotationVector\r
1913   Quat FastSlerp(const Quat& b, T s) const {\r
1914     Vector3<T> delta = (b * this->Inverted()).FastToRotationVector();\r
1915     return (FastFromRotationVector(delta * s, false) * *this).Normalized();\r
1916   }\r
1917 \r
1918   // MERGE_MOBILE_SDK\r
1919   // FIXME: This is opposite of Lerp for some reason.  It goes from 1 to 0 instead of 0 to 1.\r
1920   // Leaving it as a gift for future generations to deal with.\r
1921   Quat Nlerp(const Quat& other, T a) const {\r
1922     T sign = (Dot(other) >= 0.0f) ? 1.0f : -1.0f;\r
1923     return (*this * sign * a + other * (1 - a)).Normalized();\r
1924   }\r
1925   // MERGE_MOBILE_SDK\r
1926 \r
1927   // Rotate transforms vector in a manner that matches Matrix rotations (counter-clockwise,\r
1928   // assuming negative direction of the axis). Standard formula: q(t) * V * q(t)^-1.\r
1929   Vector3<T> Rotate(const Vector3<T>& v) const {\r
1930     OVR_MATH_ASSERT(IsNormalized()); // If this fires, caller has a quat math bug\r
1931 \r
1932     // rv = q * (v,0) * q'\r
1933     // Same as rv = v + real * cross(imag,v)*2 + cross(imag, cross(imag,v)*2);\r
1934 \r
1935     // uv = 2 * Imag().Cross(v);\r
1936     T uvx = T(2) * (y * v.z - z * v.y);\r
1937     T uvy = T(2) * (z * v.x - x * v.z);\r
1938     T uvz = T(2) * (x * v.y - y * v.x);\r
1939 \r
1940     // return v + Real()*uv + Imag().Cross(uv);\r
1941     return Vector3<T>(\r
1942         v.x + w * uvx + y * uvz - z * uvy,\r
1943         v.y + w * uvy + z * uvx - x * uvz,\r
1944         v.z + w * uvz + x * uvy - y * uvx);\r
1945   }\r
1946 \r
1947   // Rotation by inverse of *this\r
1948   Vector3<T> InverseRotate(const Vector3<T>& v) const {\r
1949     OVR_MATH_ASSERT(IsNormalized()); // If this fires, caller has a quat math bug\r
1950 \r
1951     // rv = q' * (v,0) * q\r
1952     // Same as rv = v + real * cross(-imag,v)*2 + cross(-imag, cross(-imag,v)*2);\r
1953     //      or rv = v - real * cross(imag,v)*2 + cross(imag, cross(imag,v)*2);\r
1954 \r
1955     // uv = 2 * Imag().Cross(v);\r
1956     T uvx = T(2) * (y * v.z - z * v.y);\r
1957     T uvy = T(2) * (z * v.x - x * v.z);\r
1958     T uvz = T(2) * (x * v.y - y * v.x);\r
1959 \r
1960     // return v - Real()*uv + Imag().Cross(uv);\r
1961     return Vector3<T>(\r
1962         v.x - w * uvx + y * uvz - z * uvy,\r
1963         v.y - w * uvy + z * uvx - x * uvz,\r
1964         v.z - w * uvz + x * uvy - y * uvx);\r
1965   }\r
1966 \r
1967   // Inversed quaternion rotates in the opposite direction.\r
1968   Quat Inverted() const {\r
1969     return Quat(-x, -y, -z, w);\r
1970   }\r
1971 \r
1972   Quat Inverse() const {\r
1973     return Quat(-x, -y, -z, w);\r
1974   }\r
1975 \r
1976   // Sets this quaternion to the one rotates in the opposite direction.\r
1977   void Invert() {\r
1978     *this = Quat(-x, -y, -z, w);\r
1979   }\r
1980 \r
1981   // Time integration of constant angular velocity over dt\r
1982   Quat TimeIntegrate(const Vector3<T>& angularVelocity, T dt) const {\r
1983     // solution is: this * exp( omega*dt/2 ); FromRotationVector(v) gives exp(v*.5).\r
1984     return (*this * FastFromRotationVector(angularVelocity * dt, false)).Normalized();\r
1985   }\r
1986 \r
1987   // Time integration of constant angular acceleration and velocity over dt\r
1988   // These are the first two terms of the "Magnus expansion" of the solution\r
1989   //\r
1990   //   o = o * exp( W=(W1 + W2 + W3+...) * 0.5 );\r
1991   //\r
1992   //  omega1 = (omega + omegaDot*dt)\r
1993   //  W1 = (omega + omega1)*dt/2\r
1994   //  W2 = cross(omega, omega1)/12*dt^2 % (= -cross(omega_dot, omega)/12*dt^3)\r
1995   // Terms 3 and beyond are vanishingly small:\r
1996   //  W3 = cross(omega_dot, cross(omega_dot, omega))/240*dt^5\r
1997   //\r
1998   Quat TimeIntegrate(const Vector3<T>& angularVelocity, const Vector3<T>& angularAcceleration, T dt)\r
1999       const {\r
2000     const Vector3<T>& omega = angularVelocity;\r
2001     const Vector3<T>& omegaDot = angularAcceleration;\r
2002 \r
2003     Vector3<T> omega1 = (omega + omegaDot * dt);\r
2004     Vector3<T> W = ((omega + omega1) + omega.Cross(omega1) * (dt / T(6))) * (dt / T(2));\r
2005 \r
2006     // FromRotationVector(v) is exp(v*.5)\r
2007     return (*this * FastFromRotationVector(W, false)).Normalized();\r
2008   }\r
2009 \r
2010   // Decompose rotation into three rotations:\r
2011   // roll radians about Z axis, then pitch radians about X axis, then yaw radians about Y axis.\r
2012   // Call with nullptr if a return value is not needed.\r
2013   void GetYawPitchRoll(T* yaw, T* pitch, T* roll) const {\r
2014     return GetEulerAngles<Axis_Y, Axis_X, Axis_Z, Rotate_CCW, Handed_R>(yaw, pitch, roll);\r
2015   }\r
2016 \r
2017   // GetEulerAngles extracts Euler angles from the quaternion, in the specified order of\r
2018   // axis rotations and the specified coordinate system. Right-handed coordinate system\r
2019   // is the default, with CCW rotations while looking in the negative axis direction.\r
2020   // Here a,b,c, are the Yaw/Pitch/Roll angles to be returned.\r
2021   // Rotation order is c, b, a:\r
2022   // rotation c around axis A3\r
2023   // is followed by rotation b around axis A2\r
2024   // is followed by rotation a around axis A1\r
2025   // rotations are CCW or CW (D) in LH or RH coordinate system (S)\r
2026   //\r
2027   template <Axis A1, Axis A2, Axis A3, RotateDirection D, HandedSystem S>\r
2028   void GetEulerAngles(T* a, T* b, T* c) const {\r
2029     OVR_MATH_ASSERT(IsNormalized()); // If this fires, caller has a quat math bug\r
2030     OVR_MATH_STATIC_ASSERT(\r
2031         (A1 != A2) && (A2 != A3) && (A1 != A3), "(A1 != A2) && (A2 != A3) && (A1 != A3)");\r
2032 \r
2033     T Q[3] = {x, y, z}; // Quaternion components x,y,z\r
2034 \r
2035     T ww = w * w;\r
2036     T Q11 = Q[A1] * Q[A1];\r
2037     T Q22 = Q[A2] * Q[A2];\r
2038     T Q33 = Q[A3] * Q[A3];\r
2039 \r
2040     T psign = T(-1);\r
2041     // Determine whether even permutation\r
2042     if (((A1 + 1) % 3 == A2) && ((A2 + 1) % 3 == A3))\r
2043       psign = T(1);\r
2044 \r
2045     T s2 = psign * T(2) * (psign * w * Q[A2] + Q[A1] * Q[A3]);\r
2046 \r
2047     T singularityRadius = Math<T>::SingularityRadius();\r
2048     if (s2 < T(-1) + singularityRadius) { // South pole singularity\r
2049       if (a)\r
2050         *a = T(0);\r
2051       if (b)\r
2052         *b = -S * D * ((T)MATH_DOUBLE_PIOVER2);\r
2053       if (c)\r
2054         *c = S * D * atan2(T(2) * (psign * Q[A1] * Q[A2] + w * Q[A3]), ww + Q22 - Q11 - Q33);\r
2055     } else if (s2 > T(1) - singularityRadius) { // North pole singularity\r
2056       if (a)\r
2057         *a = T(0);\r
2058       if (b)\r
2059         *b = S * D * ((T)MATH_DOUBLE_PIOVER2);\r
2060       if (c)\r
2061         *c = S * D * atan2(T(2) * (psign * Q[A1] * Q[A2] + w * Q[A3]), ww + Q22 - Q11 - Q33);\r
2062     } else {\r
2063       if (a)\r
2064         *a = -S * D * atan2(T(-2) * (w * Q[A1] - psign * Q[A2] * Q[A3]), ww + Q33 - Q11 - Q22);\r
2065       if (b)\r
2066         *b = S * D * asin(s2);\r
2067       if (c)\r
2068         *c = S * D * atan2(T(2) * (w * Q[A3] - psign * Q[A1] * Q[A2]), ww + Q11 - Q22 - Q33);\r
2069     }\r
2070   }\r
2071 \r
2072   template <Axis A1, Axis A2, Axis A3, RotateDirection D>\r
2073   void GetEulerAngles(T* a, T* b, T* c) const {\r
2074     GetEulerAngles<A1, A2, A3, D, Handed_R>(a, b, c);\r
2075   }\r
2076 \r
2077   template <Axis A1, Axis A2, Axis A3>\r
2078   void GetEulerAngles(T* a, T* b, T* c) const {\r
2079     GetEulerAngles<A1, A2, A3, Rotate_CCW, Handed_R>(a, b, c);\r
2080   }\r
2081 \r
2082   // GetEulerAnglesABA extracts Euler angles from the quaternion, in the specified order of\r
2083   // axis rotations and the specified coordinate system. Right-handed coordinate system\r
2084   // is the default, with CCW rotations while looking in the negative axis direction.\r
2085   // Here a,b,c, are the Yaw/Pitch/Roll angles to be returned.\r
2086   // rotation a around axis A1\r
2087   // is followed by rotation b around axis A2\r
2088   // is followed by rotation c around axis A1\r
2089   // Rotations are CCW or CW (D) in LH or RH coordinate system (S)\r
2090   template <Axis A1, Axis A2, RotateDirection D, HandedSystem S>\r
2091   void GetEulerAnglesABA(T* a, T* b, T* c) const {\r
2092     OVR_MATH_ASSERT(IsNormalized()); // If this fires, caller has a quat math bug\r
2093     OVR_MATH_STATIC_ASSERT(A1 != A2, "A1 != A2");\r
2094 \r
2095     T Q[3] = {x, y, z}; // Quaternion components\r
2096 \r
2097     // Determine the missing axis that was not supplied\r
2098     int m = 3 - A1 - A2;\r
2099 \r
2100     T ww = w * w;\r
2101     T Q11 = Q[A1] * Q[A1];\r
2102     T Q22 = Q[A2] * Q[A2];\r
2103     T Qmm = Q[m] * Q[m];\r
2104 \r
2105     T psign = T(-1);\r
2106     if ((A1 + 1) % 3 == A2) // Determine whether even permutation\r
2107     {\r
2108       psign = T(1);\r
2109     }\r
2110 \r
2111     T c2 = ww + Q11 - Q22 - Qmm;\r
2112     T singularityRadius = Math<T>::SingularityRadius();\r
2113     if (c2 < T(-1) + singularityRadius) { // South pole singularity\r
2114       if (a)\r
2115         *a = T(0);\r
2116       if (b)\r
2117         *b = S * D * ((T)MATH_DOUBLE_PI);\r
2118       if (c)\r
2119         *c = S * D * atan2(T(2) * (w * Q[A1] - psign * Q[A2] * Q[m]), ww + Q22 - Q11 - Qmm);\r
2120     } else if (c2 > T(1) - singularityRadius) { // North pole singularity\r
2121       if (a)\r
2122         *a = T(0);\r
2123       if (b)\r
2124         *b = T(0);\r
2125       if (c)\r
2126         *c = S * D * atan2(T(2) * (w * Q[A1] - psign * Q[A2] * Q[m]), ww + Q22 - Q11 - Qmm);\r
2127     } else {\r
2128       if (a)\r
2129         *a = S * D * atan2(psign * w * Q[m] + Q[A1] * Q[A2], w * Q[A2] - psign * Q[A1] * Q[m]);\r
2130       if (b)\r
2131         *b = S * D * acos(c2);\r
2132       if (c)\r
2133         *c = S * D * atan2(-psign * w * Q[m] + Q[A1] * Q[A2], w * Q[A2] + psign * Q[A1] * Q[m]);\r
2134     }\r
2135   }\r
2136 \r
2137   bool IsNan() const {\r
2138     return !isfinite(x + y + z + w);\r
2139   }\r
2140   bool IsFinite() const {\r
2141     return isfinite(x + y + z + w);\r
2142   }\r
2143 };\r
2144 \r
2145 typedef Quat<float> Quatf;\r
2146 typedef Quat<double> Quatd;\r
2147 \r
2148 OVR_MATH_STATIC_ASSERT((sizeof(Quatf) == 4 * sizeof(float)), "sizeof(Quatf) failure");\r
2149 OVR_MATH_STATIC_ASSERT((sizeof(Quatd) == 4 * sizeof(double)), "sizeof(Quatd) failure");\r
2150 \r
2151 //-------------------------------------------------------------------------------------\r
2152 // ***** Pose\r
2153 //\r
2154 // Position and orientation combined.\r
2155 //\r
2156 // This structure needs to be the same size and layout on 32-bit and 64-bit arch.\r
2157 // Update OVR_PadCheck.cpp when updating this object.\r
2158 template <class T>\r
2159 class Pose {\r
2160  public:\r
2161   typedef typename CompatibleTypes<Pose<T>>::Type CompatibleType;\r
2162 \r
2163   Pose() {}\r
2164   Pose(const Quat<T>& orientation, const Vector3<T>& pos)\r
2165       : Rotation(orientation), Translation(pos) {}\r
2166   Pose(const Pose& s) : Rotation(s.Rotation), Translation(s.Translation) {}\r
2167   Pose(const Matrix3<T>& R, const Vector3<T>& t) : Rotation((Quat<T>)R), Translation(t) {}\r
2168   Pose(const CompatibleType& s) : Rotation(s.Orientation), Translation(s.Position) {}\r
2169 \r
2170   explicit Pose(const Pose<typename Math<T>::OtherFloatType>& s)\r
2171       : Rotation(s.Rotation), Translation(s.Translation) {\r
2172     // Ensure normalized rotation if converting from float to double\r
2173     if (sizeof(T) > sizeof(typename Math<T>::OtherFloatType))\r
2174       Rotation.Normalize();\r
2175   }\r
2176 \r
2177   static Pose Identity() {\r
2178     return Pose(Quat<T>(0, 0, 0, 1), Vector3<T>(0, 0, 0));\r
2179   }\r
2180 \r
2181   void SetIdentity() {\r
2182     Rotation = Quat<T>(0, 0, 0, 1);\r
2183     Translation = Vector3<T>(0, 0, 0);\r
2184   }\r
2185 \r
2186   // used to make things obviously broken if someone tries to use the value\r
2187   void SetInvalid() {\r
2188     Rotation = Quat<T>(NAN, NAN, NAN, NAN);\r
2189     Translation = Vector3<T>(NAN, NAN, NAN);\r
2190   }\r
2191 \r
2192   bool IsEqual(const Pose& b, T tolerance = Math<T>::Tolerance()) const {\r
2193     return Translation.IsEqual(b.Translation, tolerance) && Rotation.IsEqual(b.Rotation, tolerance);\r
2194   }\r
2195 \r
2196   bool IsEqualMatchHemisphere(const Pose& b, T tolerance = Math<T>::Tolerance()) const {\r
2197     return Translation.IsEqual(b.Translation, tolerance) &&\r
2198         Rotation.IsEqualMatchHemisphere(b.Rotation, tolerance);\r
2199   }\r
2200 \r
2201   operator typename CompatibleTypes<Pose<T>>::Type() const {\r
2202     typename CompatibleTypes<Pose<T>>::Type result;\r
2203     result.Orientation = Rotation;\r
2204     result.Position = Translation;\r
2205     return result;\r
2206   }\r
2207 \r
2208   Quat<T> Rotation;\r
2209   Vector3<T> Translation;\r
2210 \r
2211   OVR_MATH_STATIC_ASSERT(\r
2212       (sizeof(T) == sizeof(double) || sizeof(T) == sizeof(float)),\r
2213       "(sizeof(T) == sizeof(double) || sizeof(T) == sizeof(float))");\r
2214 \r
2215   void ToArray(T* arr) const {\r
2216     T temp[7] = {Rotation.x,\r
2217                  Rotation.y,\r
2218                  Rotation.z,\r
2219                  Rotation.w,\r
2220                  Translation.x,\r
2221                  Translation.y,\r
2222                  Translation.z};\r
2223     for (int i = 0; i < 7; i++)\r
2224       arr[i] = temp[i];\r
2225   }\r
2226 \r
2227   static Pose<T> FromArray(const T* v) {\r
2228     Quat<T> rotation(v[0], v[1], v[2], v[3]);\r
2229     Vector3<T> translation(v[4], v[5], v[6]);\r
2230     // Ensure rotation is normalized, in case it was originally a float, stored in a .json file,\r
2231     // etc.\r
2232     return Pose<T>(rotation.Normalized(), translation);\r
2233   }\r
2234 \r
2235   Vector3<T> Rotate(const Vector3<T>& v) const {\r
2236     return Rotation.Rotate(v);\r
2237   }\r
2238 \r
2239   Vector3<T> InverseRotate(const Vector3<T>& v) const {\r
2240     return Rotation.InverseRotate(v);\r
2241   }\r
2242 \r
2243   Vector3<T> Translate(const Vector3<T>& v) const {\r
2244     return v + Translation;\r
2245   }\r
2246 \r
2247   Vector3<T> Transform(const Vector3<T>& v) const {\r
2248     return Rotate(v) + Translation;\r
2249   }\r
2250 \r
2251   Vector3<T> InverseTransform(const Vector3<T>& v) const {\r
2252     return InverseRotate(v - Translation);\r
2253   }\r
2254 \r
2255   Vector3<T> TransformNormal(const Vector3<T>& v) const {\r
2256     return Rotate(v);\r
2257   }\r
2258 \r
2259   Vector3<T> InverseTransformNormal(const Vector3<T>& v) const {\r
2260     return InverseRotate(v);\r
2261   }\r
2262 \r
2263   Vector3<T> Apply(const Vector3<T>& v) const {\r
2264     return Transform(v);\r
2265   }\r
2266 \r
2267   Pose operator*(const Pose& other) const {\r
2268     return Pose(Rotation * other.Rotation, Apply(other.Translation));\r
2269   }\r
2270 \r
2271   Pose Inverted() const {\r
2272     Quat<T> inv = Rotation.Inverted();\r
2273     return Pose(inv, inv.Rotate(-Translation));\r
2274   }\r
2275 \r
2276   // Interpolation between two poses: translation is interpolated with Lerp(),\r
2277   // and rotations are interpolated with Slerp().\r
2278   Pose Lerp(const Pose& b, T s) const {\r
2279     return Pose(Rotation.Slerp(b.Rotation, s), Translation.Lerp(b.Translation, s));\r
2280   }\r
2281 \r
2282   // Similar to Lerp above, except faster in case of small rotation differences.  See\r
2283   // Quat<T>::FastSlerp.\r
2284   Pose FastLerp(const Pose& b, T s) const {\r
2285     return Pose(Rotation.FastSlerp(b.Rotation, s), Translation.Lerp(b.Translation, s));\r
2286   }\r
2287 \r
2288   Pose TimeIntegrate(const Vector3<T>& linearVelocity, const Vector3<T>& angularVelocity, T dt)\r
2289       const {\r
2290     return Pose(\r
2291         (Rotation * Quat<T>::FastFromRotationVector(angularVelocity * dt, false)).Normalized(),\r
2292         Translation + linearVelocity * dt);\r
2293   }\r
2294 \r
2295   Pose TimeIntegrate(\r
2296       const Vector3<T>& linearVelocity,\r
2297       const Vector3<T>& linearAcceleration,\r
2298       const Vector3<T>& angularVelocity,\r
2299       const Vector3<T>& angularAcceleration,\r
2300       T dt) const {\r
2301     return Pose(\r
2302         Rotation.TimeIntegrate(angularVelocity, angularAcceleration, dt),\r
2303         Translation + linearVelocity * dt + linearAcceleration * dt * dt * T(0.5));\r
2304   }\r
2305 \r
2306   Pose Normalized() const {\r
2307     return Pose(Rotation.Normalized(), Translation);\r
2308   }\r
2309   void Normalize() {\r
2310     Rotation.Normalize();\r
2311   }\r
2312 \r
2313   bool IsNan() const {\r
2314     return Translation.IsNan() || Rotation.IsNan();\r
2315   }\r
2316   bool IsFinite() const {\r
2317     return Translation.IsFinite() && Rotation.IsFinite();\r
2318   }\r
2319 };\r
2320 \r
2321 typedef Pose<float> Posef;\r
2322 typedef Pose<double> Posed;\r
2323 \r
2324 OVR_MATH_STATIC_ASSERT(\r
2325     (sizeof(Posed) == sizeof(Quatd) + sizeof(Vector3d)),\r
2326     "sizeof(Posed) failure");\r
2327 OVR_MATH_STATIC_ASSERT(\r
2328     (sizeof(Posef) == sizeof(Quatf) + sizeof(Vector3f)),\r
2329     "sizeof(Posef) failure");\r
2330 \r
2331 //-------------------------------------------------------------------------------------\r
2332 // ***** Matrix4\r
2333 //\r
2334 // Matrix4 is a 4x4 matrix used for 3d transformations and projections.\r
2335 // Translation stored in the last column.\r
2336 // The matrix is stored in row-major order in memory, meaning that values\r
2337 // of the first row are stored before the next one.\r
2338 //\r
2339 // The arrangement of the matrix is chosen to be in Right-Handed\r
2340 // coordinate system and counterclockwise rotations when looking down\r
2341 // the axis\r
2342 //\r
2343 // Transformation Order:\r
2344 //   - Transformations are applied from right to left, so the expression\r
2345 //     M1 * M2 * M3 * V means that the vector V is transformed by M3 first,\r
2346 //     followed by M2 and M1.\r
2347 //\r
2348 // Coordinate system: Right Handed\r
2349 //\r
2350 // Rotations: Counterclockwise when looking down the axis. All angles are in radians.\r
2351 //\r
2352 //  | sx   01   02   tx |    // First column  (sx, 10, 20): Axis X basis vector.\r
2353 //  | 10   sy   12   ty |    // Second column (01, sy, 21): Axis Y basis vector.\r
2354 //  | 20   21   sz   tz |    // Third columnt (02, 12, sz): Axis Z basis vector.\r
2355 //  | 30   31   32   33 |\r
2356 //\r
2357 //  The basis vectors are first three columns.\r
2358 \r
2359 template <class T>\r
2360 class Matrix4 {\r
2361  public:\r
2362   typedef T ElementType;\r
2363   static const size_t Dimension = 4;\r
2364 \r
2365   T M[4][4];\r
2366 \r
2367   enum NoInitType { NoInit };\r
2368 \r
2369   // Construct with no memory initialization.\r
2370   Matrix4(NoInitType) {}\r
2371 \r
2372   // By default, we construct identity matrix.\r
2373   Matrix4() {\r
2374     M[0][0] = M[1][1] = M[2][2] = M[3][3] = T(1);\r
2375     M[0][1] = M[1][0] = M[2][3] = M[3][1] = T(0);\r
2376     M[0][2] = M[1][2] = M[2][0] = M[3][2] = T(0);\r
2377     M[0][3] = M[1][3] = M[2][1] = M[3][0] = T(0);\r
2378   }\r
2379 \r
2380   Matrix4(\r
2381       T m11,\r
2382       T m12,\r
2383       T m13,\r
2384       T m14,\r
2385       T m21,\r
2386       T m22,\r
2387       T m23,\r
2388       T m24,\r
2389       T m31,\r
2390       T m32,\r
2391       T m33,\r
2392       T m34,\r
2393       T m41,\r
2394       T m42,\r
2395       T m43,\r
2396       T m44) {\r
2397     M[0][0] = m11;\r
2398     M[0][1] = m12;\r
2399     M[0][2] = m13;\r
2400     M[0][3] = m14;\r
2401     M[1][0] = m21;\r
2402     M[1][1] = m22;\r
2403     M[1][2] = m23;\r
2404     M[1][3] = m24;\r
2405     M[2][0] = m31;\r
2406     M[2][1] = m32;\r
2407     M[2][2] = m33;\r
2408     M[2][3] = m34;\r
2409     M[3][0] = m41;\r
2410     M[3][1] = m42;\r
2411     M[3][2] = m43;\r
2412     M[3][3] = m44;\r
2413   }\r
2414 \r
2415   Matrix4(T m11, T m12, T m13, T m21, T m22, T m23, T m31, T m32, T m33) {\r
2416     M[0][0] = m11;\r
2417     M[0][1] = m12;\r
2418     M[0][2] = m13;\r
2419     M[0][3] = T(0);\r
2420     M[1][0] = m21;\r
2421     M[1][1] = m22;\r
2422     M[1][2] = m23;\r
2423     M[1][3] = T(0);\r
2424     M[2][0] = m31;\r
2425     M[2][1] = m32;\r
2426     M[2][2] = m33;\r
2427     M[2][3] = T(0);\r
2428     M[3][0] = T(0);\r
2429     M[3][1] = T(0);\r
2430     M[3][2] = T(0);\r
2431     M[3][3] = T(1);\r
2432   }\r
2433 \r
2434   explicit Matrix4(const Matrix3<T>& m) {\r
2435     M[0][0] = m.M[0][0];\r
2436     M[0][1] = m.M[0][1];\r
2437     M[0][2] = m.M[0][2];\r
2438     M[0][3] = T(0);\r
2439     M[1][0] = m.M[1][0];\r
2440     M[1][1] = m.M[1][1];\r
2441     M[1][2] = m.M[1][2];\r
2442     M[1][3] = T(0);\r
2443     M[2][0] = m.M[2][0];\r
2444     M[2][1] = m.M[2][1];\r
2445     M[2][2] = m.M[2][2];\r
2446     M[2][3] = T(0);\r
2447     M[3][0] = T(0);\r
2448     M[3][1] = T(0);\r
2449     M[3][2] = T(0);\r
2450     M[3][3] = T(1);\r
2451   }\r
2452 \r
2453   explicit Matrix4(const Quat<T>& q) {\r
2454     OVR_MATH_ASSERT(q.IsNormalized()); // If this fires, caller has a quat math bug\r
2455     T ww = q.w * q.w;\r
2456     T xx = q.x * q.x;\r
2457     T yy = q.y * q.y;\r
2458     T zz = q.z * q.z;\r
2459 \r
2460     M[0][0] = ww + xx - yy - zz;\r
2461     M[0][1] = 2 * (q.x * q.y - q.w * q.z);\r
2462     M[0][2] = 2 * (q.x * q.z + q.w * q.y);\r
2463     M[0][3] = T(0);\r
2464     M[1][0] = 2 * (q.x * q.y + q.w * q.z);\r
2465     M[1][1] = ww - xx + yy - zz;\r
2466     M[1][2] = 2 * (q.y * q.z - q.w * q.x);\r
2467     M[1][3] = T(0);\r
2468     M[2][0] = 2 * (q.x * q.z - q.w * q.y);\r
2469     M[2][1] = 2 * (q.y * q.z + q.w * q.x);\r
2470     M[2][2] = ww - xx - yy + zz;\r
2471     M[2][3] = T(0);\r
2472     M[3][0] = T(0);\r
2473     M[3][1] = T(0);\r
2474     M[3][2] = T(0);\r
2475     M[3][3] = T(1);\r
2476   }\r
2477 \r
2478   explicit Matrix4(const Pose<T>& p) {\r
2479     Matrix4 result(p.Rotation);\r
2480     result.SetTranslation(p.Translation);\r
2481     *this = result;\r
2482   }\r
2483 \r
2484   // C-interop support\r
2485   explicit Matrix4(const Matrix4<typename Math<T>::OtherFloatType>& src) {\r
2486     for (int i = 0; i < 4; i++)\r
2487       for (int j = 0; j < 4; j++)\r
2488         M[i][j] = (T)src.M[i][j];\r
2489   }\r
2490 \r
2491   // C-interop support.\r
2492   Matrix4(const typename CompatibleTypes<Matrix4<T>>::Type& s) {\r
2493     OVR_MATH_STATIC_ASSERT(sizeof(s) == sizeof(Matrix4), "sizeof(s) == sizeof(Matrix4)");\r
2494     memcpy(M, s.M, sizeof(M));\r
2495   }\r
2496 \r
2497   operator typename CompatibleTypes<Matrix4<T>>::Type() const {\r
2498     typename CompatibleTypes<Matrix4<T>>::Type result;\r
2499     OVR_MATH_STATIC_ASSERT(sizeof(result) == sizeof(Matrix4), "sizeof(result) == sizeof(Matrix4)");\r
2500     memcpy(result.M, M, sizeof(M));\r
2501     return result;\r
2502   }\r
2503 \r
2504   void ToString(char* dest, size_t destsize) const {\r
2505     size_t pos = 0;\r
2506     for (int r = 0; r < 4; r++) {\r
2507       for (int c = 0; c < 4; c++) {\r
2508         pos += OVRMath_sprintf(dest + pos, destsize - pos, "%g ", M[r][c]);\r
2509       }\r
2510     }\r
2511   }\r
2512 \r
2513   static Matrix4 FromString(const char* src) {\r
2514     Matrix4 result;\r
2515     if (src) {\r
2516       for (int r = 0; r < 4; r++) {\r
2517         for (int c = 0; c < 4; c++) {\r
2518           result.M[r][c] = (T)atof(src);\r
2519           while (*src && *src != ' ') {\r
2520             src++;\r
2521           }\r
2522           while (*src && *src == ' ') {\r
2523             src++;\r
2524           }\r
2525         }\r
2526       }\r
2527     }\r
2528     return result;\r
2529   }\r
2530 \r
2531   static Matrix4 Identity() {\r
2532     return Matrix4();\r
2533   }\r
2534 \r
2535   void SetIdentity() {\r
2536     M[0][0] = M[1][1] = M[2][2] = M[3][3] = T(1);\r
2537     M[0][1] = M[1][0] = M[2][3] = M[3][1] = T(0);\r
2538     M[0][2] = M[1][2] = M[2][0] = M[3][2] = T(0);\r
2539     M[0][3] = M[1][3] = M[2][1] = M[3][0] = T(0);\r
2540   }\r
2541 \r
2542   void SetXBasis(const Vector3<T>& v) {\r
2543     M[0][0] = v.x;\r
2544     M[1][0] = v.y;\r
2545     M[2][0] = v.z;\r
2546   }\r
2547   Vector3<T> GetXBasis() const {\r
2548     return Vector3<T>(M[0][0], M[1][0], M[2][0]);\r
2549   }\r
2550 \r
2551   void SetYBasis(const Vector3<T>& v) {\r
2552     M[0][1] = v.x;\r
2553     M[1][1] = v.y;\r
2554     M[2][1] = v.z;\r
2555   }\r
2556   Vector3<T> GetYBasis() const {\r
2557     return Vector3<T>(M[0][1], M[1][1], M[2][1]);\r
2558   }\r
2559 \r
2560   void SetZBasis(const Vector3<T>& v) {\r
2561     M[0][2] = v.x;\r
2562     M[1][2] = v.y;\r
2563     M[2][2] = v.z;\r
2564   }\r
2565   Vector3<T> GetZBasis() const {\r
2566     return Vector3<T>(M[0][2], M[1][2], M[2][2]);\r
2567   }\r
2568 \r
2569   bool operator==(const Matrix4& b) const {\r
2570     bool isEqual = true;\r
2571     for (int i = 0; i < 4; i++)\r
2572       for (int j = 0; j < 4; j++)\r
2573         isEqual &= (M[i][j] == b.M[i][j]);\r
2574 \r
2575     return isEqual;\r
2576   }\r
2577 \r
2578   Matrix4 operator+(const Matrix4& b) const {\r
2579     Matrix4 result(*this);\r
2580     result += b;\r
2581     return result;\r
2582   }\r
2583 \r
2584   Matrix4& operator+=(const Matrix4& b) {\r
2585     for (int i = 0; i < 4; i++)\r
2586       for (int j = 0; j < 4; j++)\r
2587         M[i][j] += b.M[i][j];\r
2588     return *this;\r
2589   }\r
2590 \r
2591   Matrix4 operator-(const Matrix4& b) const {\r
2592     Matrix4 result(*this);\r
2593     result -= b;\r
2594     return result;\r
2595   }\r
2596 \r
2597   Matrix4& operator-=(const Matrix4& b) {\r
2598     for (int i = 0; i < 4; i++)\r
2599       for (int j = 0; j < 4; j++)\r
2600         M[i][j] -= b.M[i][j];\r
2601     return *this;\r
2602   }\r
2603 \r
2604   // Multiplies two matrices into destination with minimum copying.\r
2605   static Matrix4& Multiply(Matrix4* d, const Matrix4& a, const Matrix4& b) {\r
2606     OVR_MATH_ASSERT((d != &a) && (d != &b));\r
2607     int i = 0;\r
2608     do {\r
2609       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] +\r
2610           a.M[i][3] * b.M[3][0];\r
2611       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] +\r
2612           a.M[i][3] * b.M[3][1];\r
2613       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] +\r
2614           a.M[i][3] * b.M[3][2];\r
2615       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] +\r
2616           a.M[i][3] * b.M[3][3];\r
2617     } while ((++i) < 4);\r
2618 \r
2619     return *d;\r
2620   }\r
2621 \r
2622   Matrix4 operator*(const Matrix4& b) const {\r
2623     Matrix4 result(Matrix4::NoInit);\r
2624     Multiply(&result, *this, b);\r
2625     return result;\r
2626   }\r
2627 \r
2628   Matrix4& operator*=(const Matrix4& b) {\r
2629     return Multiply(this, Matrix4(*this), b);\r
2630   }\r
2631 \r
2632   Matrix4 operator*(T s) const {\r
2633     Matrix4 result(*this);\r
2634     result *= s;\r
2635     return result;\r
2636   }\r
2637 \r
2638   Matrix4& operator*=(T s) {\r
2639     for (int i = 0; i < 4; i++)\r
2640       for (int j = 0; j < 4; j++)\r
2641         M[i][j] *= s;\r
2642     return *this;\r
2643   }\r
2644 \r
2645   Matrix4 operator/(T s) const {\r
2646     Matrix4 result(*this);\r
2647     result /= s;\r
2648     return result;\r
2649   }\r
2650 \r
2651   Matrix4& operator/=(T s) {\r
2652     for (int i = 0; i < 4; i++)\r
2653       for (int j = 0; j < 4; j++)\r
2654         M[i][j] /= s;\r
2655     return *this;\r
2656   }\r
2657 \r
2658   T operator()(int i, int j) const {\r
2659     return M[i][j];\r
2660   }\r
2661   T& operator()(int i, int j) {\r
2662     return M[i][j];\r
2663   }\r
2664 \r
2665   Vector4<T> operator*(const Vector4<T>& b) const {\r
2666     return Transform(b);\r
2667   }\r
2668 \r
2669   Vector3<T> Transform(const Vector3<T>& v) const {\r
2670     const T rcpW = T(1) / (M[3][0] * v.x + M[3][1] * v.y + M[3][2] * v.z + M[3][3]);\r
2671     return Vector3<T>(\r
2672         (M[0][0] * v.x + M[0][1] * v.y + M[0][2] * v.z + M[0][3]) * rcpW,\r
2673         (M[1][0] * v.x + M[1][1] * v.y + M[1][2] * v.z + M[1][3]) * rcpW,\r
2674         (M[2][0] * v.x + M[2][1] * v.y + M[2][2] * v.z + M[2][3]) * rcpW);\r
2675   }\r
2676 \r
2677   Vector4<T> Transform(const Vector4<T>& v) const {\r
2678     return Vector4<T>(\r
2679         M[0][0] * v.x + M[0][1] * v.y + M[0][2] * v.z + M[0][3] * v.w,\r
2680         M[1][0] * v.x + M[1][1] * v.y + M[1][2] * v.z + M[1][3] * v.w,\r
2681         M[2][0] * v.x + M[2][1] * v.y + M[2][2] * v.z + M[2][3] * v.w,\r
2682         M[3][0] * v.x + M[3][1] * v.y + M[3][2] * v.z + M[3][3] * v.w);\r
2683   }\r
2684 \r
2685   Matrix4 Transposed() const {\r
2686     return Matrix4(\r
2687         M[0][0],\r
2688         M[1][0],\r
2689         M[2][0],\r
2690         M[3][0],\r
2691         M[0][1],\r
2692         M[1][1],\r
2693         M[2][1],\r
2694         M[3][1],\r
2695         M[0][2],\r
2696         M[1][2],\r
2697         M[2][2],\r
2698         M[3][2],\r
2699         M[0][3],\r
2700         M[1][3],\r
2701         M[2][3],\r
2702         M[3][3]);\r
2703   }\r
2704 \r
2705   void Transpose() {\r
2706     *this = Transposed();\r
2707   }\r
2708 \r
2709   T SubDet(const size_t* rows, const size_t* cols) const {\r
2710     return M[rows[0]][cols[0]] *\r
2711         (M[rows[1]][cols[1]] * M[rows[2]][cols[2]] - M[rows[1]][cols[2]] * M[rows[2]][cols[1]]) -\r
2712         M[rows[0]][cols[1]] *\r
2713         (M[rows[1]][cols[0]] * M[rows[2]][cols[2]] - M[rows[1]][cols[2]] * M[rows[2]][cols[0]]) +\r
2714         M[rows[0]][cols[2]] *\r
2715         (M[rows[1]][cols[0]] * M[rows[2]][cols[1]] - M[rows[1]][cols[1]] * M[rows[2]][cols[0]]);\r
2716   }\r
2717 \r
2718   T Cofactor(size_t I, size_t J) const {\r
2719     const size_t indices[4][3] = {{1, 2, 3}, {0, 2, 3}, {0, 1, 3}, {0, 1, 2}};\r
2720     return ((I + J) & 1) ? -SubDet(indices[I], indices[J]) : SubDet(indices[I], indices[J]);\r
2721   }\r
2722 \r
2723   T Determinant() const {\r
2724     return M[0][0] * Cofactor(0, 0) + M[0][1] * Cofactor(0, 1) + M[0][2] * Cofactor(0, 2) +\r
2725         M[0][3] * Cofactor(0, 3);\r
2726   }\r
2727 \r
2728   Matrix4 Adjugated() const {\r
2729     return Matrix4(\r
2730         Cofactor(0, 0),\r
2731         Cofactor(1, 0),\r
2732         Cofactor(2, 0),\r
2733         Cofactor(3, 0),\r
2734         Cofactor(0, 1),\r
2735         Cofactor(1, 1),\r
2736         Cofactor(2, 1),\r
2737         Cofactor(3, 1),\r
2738         Cofactor(0, 2),\r
2739         Cofactor(1, 2),\r
2740         Cofactor(2, 2),\r
2741         Cofactor(3, 2),\r
2742         Cofactor(0, 3),\r
2743         Cofactor(1, 3),\r
2744         Cofactor(2, 3),\r
2745         Cofactor(3, 3));\r
2746   }\r
2747 \r
2748   Matrix4 Inverted() const {\r
2749     T det = Determinant();\r
2750     OVR_MATH_ASSERT(det != 0);\r
2751     return Adjugated() * (T(1) / det);\r
2752   }\r
2753 \r
2754   void Invert() {\r
2755     *this = Inverted();\r
2756   }\r
2757 \r
2758   // This is more efficient than general inverse, but ONLY works\r
2759   // correctly if it is a homogeneous transform matrix (rot + trans)\r
2760   Matrix4 InvertedHomogeneousTransform() const {\r
2761     // Make the inverse rotation matrix\r
2762     Matrix4 rinv = this->Transposed();\r
2763     rinv.M[3][0] = rinv.M[3][1] = rinv.M[3][2] = T(0);\r
2764     // Make the inverse translation matrix\r
2765     Vector3<T> tvinv(-M[0][3], -M[1][3], -M[2][3]);\r
2766     Matrix4 tinv = Matrix4::Translation(tvinv);\r
2767     return rinv * tinv; // "untranslate", then "unrotate"\r
2768   }\r
2769 \r
2770   // This is more efficient than general inverse, but ONLY works\r
2771   // correctly if it is a homogeneous transform matrix (rot + trans)\r
2772   void InvertHomogeneousTransform() {\r
2773     *this = InvertedHomogeneousTransform();\r
2774   }\r
2775 \r
2776   // Matrix to Euler Angles conversion\r
2777   // a,b,c, are the YawPitchRoll angles to be returned\r
2778   // rotation a around axis A1\r
2779   // is followed by rotation b around axis A2\r
2780   // is followed by rotation c around axis A3\r
2781   // rotations are CCW or CW (D) in LH or RH coordinate system (S)\r
2782   template <Axis A1, Axis A2, Axis A3, RotateDirection D, HandedSystem S>\r
2783   void ToEulerAngles(T* a, T* b, T* c) const {\r
2784     OVR_MATH_STATIC_ASSERT(\r
2785         (A1 != A2) && (A2 != A3) && (A1 != A3), "(A1 != A2) && (A2 != A3) && (A1 != A3)");\r
2786 \r
2787     T psign = T(-1);\r
2788     if (((A1 + 1) % 3 == A2) && ((A2 + 1) % 3 == A3)) // Determine whether even permutation\r
2789       psign = T(1);\r
2790 \r
2791     T pm = psign * M[A1][A3];\r
2792     T singularityRadius = Math<T>::SingularityRadius();\r
2793     if (pm < T(-1) + singularityRadius) { // South pole singularity\r
2794       *a = T(0);\r
2795       *b = -S * D * ((T)MATH_DOUBLE_PIOVER2);\r
2796       *c = S * D * atan2(psign * M[A2][A1], M[A2][A2]);\r
2797     } else if (pm > T(1) - singularityRadius) { // North pole singularity\r
2798       *a = T(0);\r
2799       *b = S * D * ((T)MATH_DOUBLE_PIOVER2);\r
2800       *c = S * D * atan2(psign * M[A2][A1], M[A2][A2]);\r
2801     } else { // Normal case (nonsingular)\r
2802       *a = S * D * atan2(-psign * M[A2][A3], M[A3][A3]);\r
2803       *b = S * D * asin(pm);\r
2804       *c = S * D * atan2(-psign * M[A1][A2], M[A1][A1]);\r
2805     }\r
2806   }\r
2807 \r
2808   // Matrix to Euler Angles conversion\r
2809   // a,b,c, are the YawPitchRoll angles to be returned\r
2810   // rotation a around axis A1\r
2811   // is followed by rotation b around axis A2\r
2812   // is followed by rotation c around axis A1\r
2813   // rotations are CCW or CW (D) in LH or RH coordinate system (S)\r
2814   template <Axis A1, Axis A2, RotateDirection D, HandedSystem S>\r
2815   void ToEulerAnglesABA(T* a, T* b, T* c) const {\r
2816     OVR_MATH_STATIC_ASSERT(A1 != A2, "A1 != A2");\r
2817 \r
2818     // Determine the axis that was not supplied\r
2819     int m = 3 - A1 - A2;\r
2820 \r
2821     T psign = T(-1);\r
2822     if ((A1 + 1) % 3 == A2) // Determine whether even permutation\r
2823       psign = T(1);\r
2824 \r
2825     T c2 = M[A1][A1];\r
2826     T singularityRadius = Math<T>::SingularityRadius();\r
2827     if (c2 < T(-1) + singularityRadius) { // South pole singularity\r
2828       *a = T(0);\r
2829       *b = S * D * ((T)MATH_DOUBLE_PI);\r
2830       *c = S * D * atan2(-psign * M[A2][m], M[A2][A2]);\r
2831     } else if (c2 > T(1) - singularityRadius) { // North pole singularity\r
2832       *a = T(0);\r
2833       *b = T(0);\r
2834       *c = S * D * atan2(-psign * M[A2][m], M[A2][A2]);\r
2835     } else { // Normal case (nonsingular)\r
2836       *a = S * D * atan2(M[A2][A1], -psign * M[m][A1]);\r
2837       *b = S * D * acos(c2);\r
2838       *c = S * D * atan2(M[A1][A2], psign * M[A1][m]);\r
2839     }\r
2840   }\r
2841 \r
2842   // Creates a matrix that converts the vertices from one coordinate system\r
2843   // to another.\r
2844   static Matrix4 AxisConversion(const WorldAxes& to, const WorldAxes& from) {\r
2845     // Holds axis values from the 'to' structure\r
2846     int toArray[3] = {to.XAxis, to.YAxis, to.ZAxis};\r
2847 \r
2848     // The inverse of the toArray\r
2849     int inv[4];\r
2850     inv[0] = inv[abs(to.XAxis)] = 0;\r
2851     inv[abs(to.YAxis)] = 1;\r
2852     inv[abs(to.ZAxis)] = 2;\r
2853 \r
2854     Matrix4 m(0, 0, 0, 0, 0, 0, 0, 0, 0);\r
2855 \r
2856     // Only three values in the matrix need to be changed to 1 or -1.\r
2857     m.M[inv[abs(from.XAxis)]][0] = T(from.XAxis / toArray[inv[abs(from.XAxis)]]);\r
2858     m.M[inv[abs(from.YAxis)]][1] = T(from.YAxis / toArray[inv[abs(from.YAxis)]]);\r
2859     m.M[inv[abs(from.ZAxis)]][2] = T(from.ZAxis / toArray[inv[abs(from.ZAxis)]]);\r
2860     return m;\r
2861   }\r
2862 \r
2863   // Creates a matrix for translation by vector\r
2864   static Matrix4 Translation(const Vector3<T>& v) {\r
2865     Matrix4 t;\r
2866     t.M[0][3] = v.x;\r
2867     t.M[1][3] = v.y;\r
2868     t.M[2][3] = v.z;\r
2869     return t;\r
2870   }\r
2871 \r
2872   // Creates a matrix for translation by vector\r
2873   static Matrix4 Translation(T x, T y, T z = T(0)) {\r
2874     Matrix4 t;\r
2875     t.M[0][3] = x;\r
2876     t.M[1][3] = y;\r
2877     t.M[2][3] = z;\r
2878     return t;\r
2879   }\r
2880 \r
2881   // Sets the translation part\r
2882   void SetTranslation(const Vector3<T>& v) {\r
2883     M[0][3] = v.x;\r
2884     M[1][3] = v.y;\r
2885     M[2][3] = v.z;\r
2886   }\r
2887 \r
2888   Vector3<T> GetTranslation() const {\r
2889     return Vector3<T>(M[0][3], M[1][3], M[2][3]);\r
2890   }\r
2891 \r
2892   // Creates a matrix for scaling by vector\r
2893   static Matrix4 Scaling(const Vector3<T>& v) {\r
2894     Matrix4 t;\r
2895     t.M[0][0] = v.x;\r
2896     t.M[1][1] = v.y;\r
2897     t.M[2][2] = v.z;\r
2898     return t;\r
2899   }\r
2900 \r
2901   // Creates a matrix for scaling by vector\r
2902   static Matrix4 Scaling(T x, T y, T z) {\r
2903     Matrix4 t;\r
2904     t.M[0][0] = x;\r
2905     t.M[1][1] = y;\r
2906     t.M[2][2] = z;\r
2907     return t;\r
2908   }\r
2909 \r
2910   // Creates a matrix for scaling by constant\r
2911   static Matrix4 Scaling(T s) {\r
2912     Matrix4 t;\r
2913     t.M[0][0] = s;\r
2914     t.M[1][1] = s;\r
2915     t.M[2][2] = s;\r
2916     return t;\r
2917   }\r
2918 \r
2919   // Simple L1 distance in R^12\r
2920   T Distance(const Matrix4& m2) const {\r
2921     T d = fabs(M[0][0] - m2.M[0][0]) + fabs(M[0][1] - m2.M[0][1]);\r
2922     d += fabs(M[0][2] - m2.M[0][2]) + fabs(M[0][3] - m2.M[0][3]);\r
2923     d += fabs(M[1][0] - m2.M[1][0]) + fabs(M[1][1] - m2.M[1][1]);\r
2924     d += fabs(M[1][2] - m2.M[1][2]) + fabs(M[1][3] - m2.M[1][3]);\r
2925     d += fabs(M[2][0] - m2.M[2][0]) + fabs(M[2][1] - m2.M[2][1]);\r
2926     d += fabs(M[2][2] - m2.M[2][2]) + fabs(M[2][3] - m2.M[2][3]);\r
2927     d += fabs(M[3][0] - m2.M[3][0]) + fabs(M[3][1] - m2.M[3][1]);\r
2928     d += fabs(M[3][2] - m2.M[3][2]) + fabs(M[3][3] - m2.M[3][3]);\r
2929     return d;\r
2930   }\r
2931 \r
2932   // Creates a rotation matrix rotating around the X axis by 'angle' radians.\r
2933   // Just for quick testing.  Not for final API.  Need to remove case.\r
2934   static Matrix4 RotationAxis(Axis A, T angle, RotateDirection d, HandedSystem s) {\r
2935     T sina = s * d * sin(angle);\r
2936     T cosa = cos(angle);\r
2937 \r
2938     switch (A) {\r
2939       case Axis_X:\r
2940         return Matrix4(1, 0, 0, 0, cosa, -sina, 0, sina, cosa);\r
2941       case Axis_Y:\r
2942         return Matrix4(cosa, 0, sina, 0, 1, 0, -sina, 0, cosa);\r
2943       case Axis_Z:\r
2944         return Matrix4(cosa, -sina, 0, sina, cosa, 0, 0, 0, 1);\r
2945       default:\r
2946         return Matrix4();\r
2947     }\r
2948   }\r
2949 \r
2950   // Creates a rotation matrix rotating around the X axis by 'angle' radians.\r
2951   // Rotation direction is depends on the coordinate system:\r
2952   // RHS (Oculus default): Positive angle values rotate Counter-clockwise (CCW),\r
2953   //                        while looking in the negative axis direction. This is the\r
2954   //                        same as looking down from positive axis values towards origin.\r
2955   // LHS: Positive angle values rotate clock-wise (CW), while looking in the\r
2956   //       negative axis direction.\r
2957   static Matrix4 RotationX(T angle) {\r
2958     T sina = sin(angle);\r
2959     T cosa = cos(angle);\r
2960     return Matrix4(1, 0, 0, 0, cosa, -sina, 0, sina, cosa);\r
2961   }\r
2962 \r
2963   // Creates a rotation matrix rotating around the Y axis by 'angle' radians.\r
2964   // Rotation direction is depends on the coordinate system:\r
2965   //  RHS (Oculus default): Positive angle values rotate Counter-clockwise (CCW),\r
2966   //                        while looking in the negative axis direction. This is the\r
2967   //                        same as looking down from positive axis values towards origin.\r
2968   //  LHS: Positive angle values rotate clock-wise (CW), while looking in the\r
2969   //       negative axis direction.\r
2970   static Matrix4 RotationY(T angle) {\r
2971     T sina = (T)sin(angle);\r
2972     T cosa = (T)cos(angle);\r
2973     return Matrix4(cosa, 0, sina, 0, 1, 0, -sina, 0, cosa);\r
2974   }\r
2975 \r
2976   // Creates a rotation matrix rotating around the Z axis by 'angle' radians.\r
2977   // Rotation direction is depends on the coordinate system:\r
2978   //  RHS (Oculus default): Positive angle values rotate Counter-clockwise (CCW),\r
2979   //                        while looking in the negative axis direction. This is the\r
2980   //                        same as looking down from positive axis values towards origin.\r
2981   //  LHS: Positive angle values rotate clock-wise (CW), while looking in the\r
2982   //       negative axis direction.\r
2983   static Matrix4 RotationZ(T angle) {\r
2984     T sina = sin(angle);\r
2985     T cosa = cos(angle);\r
2986     return Matrix4(cosa, -sina, 0, sina, cosa, 0, 0, 0, 1);\r
2987   }\r
2988 \r
2989   // LookAtRH creates a View transformation matrix for right-handed coordinate system.\r
2990   // The resulting matrix points camera from 'eye' towards 'at' direction, with 'up'\r
2991   // specifying the up vector. The resulting matrix should be used with PerspectiveRH\r
2992   // projection.\r
2993   static Matrix4 LookAtRH(const Vector3<T>& eye, const Vector3<T>& at, const Vector3<T>& up) {\r
2994     Vector3<T> z = (eye - at).Normalized(); // Forward\r
2995     Vector3<T> x = up.Cross(z).Normalized(); // Right\r
2996     Vector3<T> y = z.Cross(x);\r
2997 \r
2998     Matrix4 m(\r
2999         x.x,\r
3000         x.y,\r
3001         x.z,\r
3002         -(x.Dot(eye)),\r
3003         y.x,\r
3004         y.y,\r
3005         y.z,\r
3006         -(y.Dot(eye)),\r
3007         z.x,\r
3008         z.y,\r
3009         z.z,\r
3010         -(z.Dot(eye)),\r
3011         0,\r
3012         0,\r
3013         0,\r
3014         1);\r
3015     return m;\r
3016   }\r
3017 \r
3018   // LookAtLH creates a View transformation matrix for left-handed coordinate system.\r
3019   // The resulting matrix points camera from 'eye' towards 'at' direction, with 'up'\r
3020   // specifying the up vector.\r
3021   static Matrix4 LookAtLH(const Vector3<T>& eye, const Vector3<T>& at, const Vector3<T>& up) {\r
3022     Vector3<T> z = (at - eye).Normalized(); // Forward\r
3023     Vector3<T> x = up.Cross(z).Normalized(); // Right\r
3024     Vector3<T> y = z.Cross(x);\r
3025 \r
3026     Matrix4 m(\r
3027         x.x,\r
3028         x.y,\r
3029         x.z,\r
3030         -(x.Dot(eye)),\r
3031         y.x,\r
3032         y.y,\r
3033         y.z,\r
3034         -(y.Dot(eye)),\r
3035         z.x,\r
3036         z.y,\r
3037         z.z,\r
3038         -(z.Dot(eye)),\r
3039         0,\r
3040         0,\r
3041         0,\r
3042         1);\r
3043     return m;\r
3044   }\r
3045 \r
3046   // PerspectiveRH creates a right-handed perspective projection matrix that can be\r
3047   // used with the Oculus sample renderer.\r
3048   //  yfov   - Specifies vertical field of view in radians.\r
3049   //  aspect - Screen aspect ration, which is usually width/height for square pixels.\r
3050   //           Note that xfov = yfov * aspect.\r
3051   //  znear  - Absolute value of near Z clipping clipping range.\r
3052   //  zfar   - Absolute value of far  Z clipping clipping range (larger then near).\r
3053   // Even though RHS usually looks in the direction of negative Z, positive values\r
3054   // are expected for znear and zfar.\r
3055   static Matrix4 PerspectiveRH(T yfov, T aspect, T znear, T zfar) {\r
3056     Matrix4 m;\r
3057     T tanHalfFov = (T)tan(yfov * T(0.5));\r
3058 \r
3059     m.M[0][0] = T(1) / (aspect * tanHalfFov);\r
3060     m.M[1][1] = T(1) / tanHalfFov;\r
3061     m.M[2][2] = zfar / (znear - zfar);\r
3062     m.M[3][2] = T(-1);\r
3063     m.M[2][3] = (zfar * znear) / (znear - zfar);\r
3064     m.M[3][3] = T(0);\r
3065 \r
3066     // Note: Post-projection matrix result assumes Left-Handed coordinate system,\r
3067     //       with Y up, X right and Z forward. This supports positive z-buffer values.\r
3068     // This is the case even for RHS coordinate input.\r
3069     return m;\r
3070   }\r
3071 \r
3072   // PerspectiveLH creates a left-handed perspective projection matrix that can be\r
3073   // used with the Oculus sample renderer.\r
3074   //  yfov   - Specifies vertical field of view in radians.\r
3075   //  aspect - Screen aspect ration, which is usually width/height for square pixels.\r
3076   //           Note that xfov = yfov * aspect.\r
3077   //  znear  - Absolute value of near Z clipping clipping range.\r
3078   //  zfar   - Absolute value of far  Z clipping clipping range (larger then near).\r
3079   static Matrix4 PerspectiveLH(T yfov, T aspect, T znear, T zfar) {\r
3080     Matrix4 m;\r
3081     T tanHalfFov = (T)tan(yfov * T(0.5));\r
3082 \r
3083     m.M[0][0] = T(1) / (aspect * tanHalfFov);\r
3084     m.M[1][1] = T(1) / tanHalfFov;\r
3085     // m.M[2][2] = zfar / (znear - zfar);\r
3086     m.M[2][2] = zfar / (zfar - znear);\r
3087     m.M[3][2] = T(-1);\r
3088     m.M[2][3] = (zfar * znear) / (znear - zfar);\r
3089     m.M[3][3] = T(0);\r
3090 \r
3091     // Note: Post-projection matrix result assumes Left-Handed coordinate system,\r
3092     //       with Y up, X right and Z forward. This supports positive z-buffer values.\r
3093     // This is the case even for RHS coordinate input.\r
3094     return m;\r
3095   }\r
3096 \r
3097   static Matrix4 Ortho2D(T w, T h) {\r
3098     Matrix4 m;\r
3099     m.M[0][0] = T(2.0) / w;\r
3100     m.M[1][1] = T(-2.0) / h;\r
3101     m.M[0][3] = T(-1.0);\r
3102     m.M[1][3] = T(1.0);\r
3103     m.M[2][2] = T(0);\r
3104     return m;\r
3105   }\r
3106 };\r
3107 \r
3108 typedef Matrix4<float> Matrix4f;\r
3109 typedef Matrix4<double> Matrix4d;\r
3110 \r
3111 //-------------------------------------------------------------------------------------\r
3112 // ***** Matrix3\r
3113 //\r
3114 // Matrix3 is a 3x3 matrix used for representing a rotation matrix.\r
3115 // The matrix is stored in row-major order in memory, meaning that values\r
3116 // of the first row are stored before the next one.\r
3117 //\r
3118 // The arrangement of the matrix is chosen to be in Right-Handed\r
3119 // coordinate system and counterclockwise rotations when looking down\r
3120 // the axis\r
3121 //\r
3122 // Transformation Order:\r
3123 //   - Transformations are applied from right to left, so the expression\r
3124 //     M1 * M2 * M3 * V means that the vector V is transformed by M3 first,\r
3125 //     followed by M2 and M1.\r
3126 //\r
3127 // Coordinate system: Right Handed\r
3128 //\r
3129 // Rotations: Counterclockwise when looking down the axis. All angles are in radians.\r
3130 \r
3131 template <class T>\r
3132 class Matrix3 {\r
3133  public:\r
3134   typedef T ElementType;\r
3135   static const size_t Dimension = 3;\r
3136 \r
3137   T M[3][3];\r
3138 \r
3139   enum NoInitType { NoInit };\r
3140 \r
3141   // Construct with no memory initialization.\r
3142   Matrix3(NoInitType) {}\r
3143 \r
3144   // By default, we construct identity matrix.\r
3145   Matrix3() {\r
3146     M[0][0] = M[1][1] = M[2][2] = T(1);\r
3147     M[0][1] = M[1][0] = M[2][0] = T(0);\r
3148     M[0][2] = M[1][2] = M[2][1] = T(0);\r
3149   }\r
3150 \r
3151   Matrix3(T m11, T m12, T m13, T m21, T m22, T m23, T m31, T m32, T m33) {\r
3152     M[0][0] = m11;\r
3153     M[0][1] = m12;\r
3154     M[0][2] = m13;\r
3155     M[1][0] = m21;\r
3156     M[1][1] = m22;\r
3157     M[1][2] = m23;\r
3158     M[2][0] = m31;\r
3159     M[2][1] = m32;\r
3160     M[2][2] = m33;\r
3161   }\r
3162 \r
3163   // Construction from X, Y, Z basis vectors\r
3164   Matrix3(const Vector3<T>& xBasis, const Vector3<T>& yBasis, const Vector3<T>& zBasis) {\r
3165     M[0][0] = xBasis.x;\r
3166     M[0][1] = yBasis.x;\r
3167     M[0][2] = zBasis.x;\r
3168     M[1][0] = xBasis.y;\r
3169     M[1][1] = yBasis.y;\r
3170     M[1][2] = zBasis.y;\r
3171     M[2][0] = xBasis.z;\r
3172     M[2][1] = yBasis.z;\r
3173     M[2][2] = zBasis.z;\r
3174   }\r
3175 \r
3176   explicit Matrix3(const Quat<T>& q) {\r
3177     OVR_MATH_ASSERT(q.IsNormalized()); // If this fires, caller has a quat math bug\r
3178     const T tx = q.x + q.x, ty = q.y + q.y, tz = q.z + q.z;\r
3179     const T twx = q.w * tx, twy = q.w * ty, twz = q.w * tz;\r
3180     const T txx = q.x * tx, txy = q.x * ty, txz = q.x * tz;\r
3181     const T tyy = q.y * ty, tyz = q.y * tz, tzz = q.z * tz;\r
3182     M[0][0] = T(1) - (tyy + tzz);\r
3183     M[0][1] = txy - twz;\r
3184     M[0][2] = txz + twy;\r
3185     M[1][0] = txy + twz;\r
3186     M[1][1] = T(1) - (txx + tzz);\r
3187     M[1][2] = tyz - twx;\r
3188     M[2][0] = txz - twy;\r
3189     M[2][1] = tyz + twx;\r
3190     M[2][2] = T(1) - (txx + tyy);\r
3191   }\r
3192 \r
3193   inline explicit Matrix3(T s) {\r
3194     M[0][0] = M[1][1] = M[2][2] = s;\r
3195     M[0][1] = M[0][2] = M[1][0] = M[1][2] = M[2][0] = M[2][1] = T(0);\r
3196   }\r
3197 \r
3198   Matrix3(T m11, T m22, T m33) {\r
3199     M[0][0] = m11;\r
3200     M[0][1] = T(0);\r
3201     M[0][2] = T(0);\r
3202     M[1][0] = T(0);\r
3203     M[1][1] = m22;\r
3204     M[1][2] = T(0);\r
3205     M[2][0] = T(0);\r
3206     M[2][1] = T(0);\r
3207     M[2][2] = m33;\r
3208   }\r
3209 \r
3210   explicit Matrix3(const Matrix3<typename Math<T>::OtherFloatType>& src) {\r
3211     for (int i = 0; i < 3; i++)\r
3212       for (int j = 0; j < 3; j++)\r
3213         M[i][j] = (T)src.M[i][j];\r
3214   }\r
3215 \r
3216   // C-interop support.\r
3217   Matrix3(const typename CompatibleTypes<Matrix3<T>>::Type& s) {\r
3218     OVR_MATH_STATIC_ASSERT(sizeof(s) == sizeof(Matrix3), "sizeof(s) == sizeof(Matrix3)");\r
3219     memcpy(M, s.M, sizeof(M));\r
3220   }\r
3221 \r
3222   operator const typename CompatibleTypes<Matrix3<T>>::Type() const {\r
3223     typename CompatibleTypes<Matrix3<T>>::Type result;\r
3224     OVR_MATH_STATIC_ASSERT(sizeof(result) == sizeof(Matrix3), "sizeof(result) == sizeof(Matrix3)");\r
3225     memcpy(result.M, M, sizeof(M));\r
3226     return result;\r
3227   }\r
3228 \r
3229   T operator()(int i, int j) const {\r
3230     return M[i][j];\r
3231   }\r
3232   T& operator()(int i, int j) {\r
3233     return M[i][j];\r
3234   }\r
3235 \r
3236   void ToString(char* dest, size_t destsize) const {\r
3237     size_t pos = 0;\r
3238     for (int r = 0; r < 3; r++) {\r
3239       for (int c = 0; c < 3; c++)\r
3240         pos += OVRMath_sprintf(dest + pos, destsize - pos, "%g ", M[r][c]);\r
3241     }\r
3242   }\r
3243 \r
3244   static Matrix3 FromString(const char* src) {\r
3245     Matrix3 result;\r
3246     if (src) {\r
3247       for (int r = 0; r < 3; r++) {\r
3248         for (int c = 0; c < 3; c++) {\r
3249           result.M[r][c] = (T)atof(src);\r
3250           while (*src && *src != ' ')\r
3251             src++;\r
3252           while (*src && *src == ' ')\r
3253             src++;\r
3254         }\r
3255       }\r
3256     }\r
3257     return result;\r
3258   }\r
3259 \r
3260   static Matrix3 Identity() {\r
3261     return Matrix3();\r
3262   }\r
3263 \r
3264   void SetIdentity() {\r
3265     M[0][0] = M[1][1] = M[2][2] = T(1);\r
3266     M[0][1] = M[1][0] = M[2][0] = T(0);\r
3267     M[0][2] = M[1][2] = M[2][1] = T(0);\r
3268   }\r
3269 \r
3270   static Matrix3 Diagonal(T m00, T m11, T m22) {\r
3271     return Matrix3(m00, 0, 0, 0, m11, 0, 0, 0, m22);\r
3272   }\r
3273   static Matrix3 Diagonal(const Vector3<T>& v) {\r
3274     return Diagonal(v.x, v.y, v.z);\r
3275   }\r
3276 \r
3277   T Trace() const {\r
3278     return M[0][0] + M[1][1] + M[2][2];\r
3279   }\r
3280 \r
3281   bool operator==(const Matrix3& b) const {\r
3282     bool isEqual = true;\r
3283     for (int i = 0; i < 3; i++) {\r
3284       for (int j = 0; j < 3; j++)\r
3285         isEqual &= (M[i][j] == b.M[i][j]);\r
3286     }\r
3287 \r
3288     return isEqual;\r
3289   }\r
3290 \r
3291   Matrix3 operator+(const Matrix3& b) const {\r
3292     Matrix3<T> result(*this);\r
3293     result += b;\r
3294     return result;\r
3295   }\r
3296 \r
3297   Matrix3& operator+=(const Matrix3& b) {\r
3298     for (int i = 0; i < 3; i++)\r
3299       for (int j = 0; j < 3; j++)\r
3300         M[i][j] += b.M[i][j];\r
3301     return *this;\r
3302   }\r
3303 \r
3304   void operator=(const Matrix3& b) {\r
3305     for (int i = 0; i < 3; i++)\r
3306       for (int j = 0; j < 3; j++)\r
3307         M[i][j] = b.M[i][j];\r
3308   }\r
3309 \r
3310   Matrix3 operator-(const Matrix3& b) const {\r
3311     Matrix3 result(*this);\r
3312     result -= b;\r
3313     return result;\r
3314   }\r
3315 \r
3316   Matrix3& operator-=(const Matrix3& b) {\r
3317     for (int i = 0; i < 3; i++) {\r
3318       for (int j = 0; j < 3; j++)\r
3319         M[i][j] -= b.M[i][j];\r
3320     }\r
3321 \r
3322     return *this;\r
3323   }\r
3324 \r
3325   // Multiplies two matrices into destination with minimum copying.\r
3326   static Matrix3& Multiply(Matrix3* d, const Matrix3& a, const Matrix3& b) {\r
3327     OVR_MATH_ASSERT((d != &a) && (d != &b));\r
3328     int i = 0;\r
3329     do {\r
3330       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];\r
3331       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];\r
3332       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];\r
3333     } while ((++i) < 3);\r
3334 \r
3335     return *d;\r
3336   }\r
3337 \r
3338   Matrix3 operator*(const Matrix3& b) const {\r
3339     Matrix3 result(Matrix3::NoInit);\r
3340     Multiply(&result, *this, b);\r
3341     return result;\r
3342   }\r
3343 \r
3344   Matrix3& operator*=(const Matrix3& b) {\r
3345     return Multiply(this, Matrix3(*this), b);\r
3346   }\r
3347 \r
3348   Matrix3 operator*(T s) const {\r
3349     Matrix3 result(*this);\r
3350     result *= s;\r
3351     return result;\r
3352   }\r
3353 \r
3354   Matrix3& operator*=(T s) {\r
3355     for (int i = 0; i < 3; i++) {\r
3356       for (int j = 0; j < 3; j++)\r
3357         M[i][j] *= s;\r
3358     }\r
3359 \r
3360     return *this;\r
3361   }\r
3362 \r
3363   Vector3<T> operator*(const Vector3<T>& b) const {\r
3364     Vector3<T> result;\r
3365     result.x = M[0][0] * b.x + M[0][1] * b.y + M[0][2] * b.z;\r
3366     result.y = M[1][0] * b.x + M[1][1] * b.y + M[1][2] * b.z;\r
3367     result.z = M[2][0] * b.x + M[2][1] * b.y + M[2][2] * b.z;\r
3368 \r
3369     return result;\r
3370   }\r
3371 \r
3372   Matrix3 operator/(T s) const {\r
3373     Matrix3 result(*this);\r
3374     result /= s;\r
3375     return result;\r
3376   }\r
3377 \r
3378   Matrix3& operator/=(T s) {\r
3379     for (int i = 0; i < 3; i++) {\r
3380       for (int j = 0; j < 3; j++)\r
3381         M[i][j] /= s;\r
3382     }\r
3383 \r
3384     return *this;\r
3385   }\r
3386 \r
3387   Vector2<T> Transform(const Vector2<T>& v) const {\r
3388     const T rcpZ = T(1) / (M[2][0] * v.x + M[2][1] * v.y + M[2][2]);\r
3389     return Vector2<T>(\r
3390         (M[0][0] * v.x + M[0][1] * v.y + M[0][2]) * rcpZ,\r
3391         (M[1][0] * v.x + M[1][1] * v.y + M[1][2]) * rcpZ);\r
3392   }\r
3393 \r
3394   Vector3<T> Transform(const Vector3<T>& v) const {\r
3395     return Vector3<T>(\r
3396         M[0][0] * v.x + M[0][1] * v.y + M[0][2] * v.z,\r
3397         M[1][0] * v.x + M[1][1] * v.y + M[1][2] * v.z,\r
3398         M[2][0] * v.x + M[2][1] * v.y + M[2][2] * v.z);\r
3399   }\r
3400 \r
3401   Matrix3 Transposed() const {\r
3402     return Matrix3(M[0][0], M[1][0], M[2][0], M[0][1], M[1][1], M[2][1], M[0][2], M[1][2], M[2][2]);\r
3403   }\r
3404 \r
3405   void Transpose() {\r
3406     *this = Transposed();\r
3407   }\r
3408 \r
3409   T SubDet(const size_t* rows, const size_t* cols) const {\r
3410     return M[rows[0]][cols[0]] *\r
3411         (M[rows[1]][cols[1]] * M[rows[2]][cols[2]] - M[rows[1]][cols[2]] * M[rows[2]][cols[1]]) -\r
3412         M[rows[0]][cols[1]] *\r
3413         (M[rows[1]][cols[0]] * M[rows[2]][cols[2]] - M[rows[1]][cols[2]] * M[rows[2]][cols[0]]) +\r
3414         M[rows[0]][cols[2]] *\r
3415         (M[rows[1]][cols[0]] * M[rows[2]][cols[1]] - M[rows[1]][cols[1]] * M[rows[2]][cols[0]]);\r
3416   }\r
3417 \r
3418   // M += a*b.t()\r
3419   inline void Rank1Add(const Vector3<T>& a, const Vector3<T>& b) {\r
3420     M[0][0] += a.x * b.x;\r
3421     M[0][1] += a.x * b.y;\r
3422     M[0][2] += a.x * b.z;\r
3423     M[1][0] += a.y * b.x;\r
3424     M[1][1] += a.y * b.y;\r
3425     M[1][2] += a.y * b.z;\r
3426     M[2][0] += a.z * b.x;\r
3427     M[2][1] += a.z * b.y;\r
3428     M[2][2] += a.z * b.z;\r
3429   }\r
3430 \r
3431   // M -= a*b.t()\r
3432   inline void Rank1Sub(const Vector3<T>& a, const Vector3<T>& b) {\r
3433     M[0][0] -= a.x * b.x;\r
3434     M[0][1] -= a.x * b.y;\r
3435     M[0][2] -= a.x * b.z;\r
3436     M[1][0] -= a.y * b.x;\r
3437     M[1][1] -= a.y * b.y;\r
3438     M[1][2] -= a.y * b.z;\r
3439     M[2][0] -= a.z * b.x;\r
3440     M[2][1] -= a.z * b.y;\r
3441     M[2][2] -= a.z * b.z;\r
3442   }\r
3443 \r
3444   inline Vector3<T> Col(int c) const {\r
3445     return Vector3<T>(M[0][c], M[1][c], M[2][c]);\r
3446   }\r
3447 \r
3448   inline Vector3<T> Row(int r) const {\r
3449     return Vector3<T>(M[r][0], M[r][1], M[r][2]);\r
3450   }\r
3451 \r
3452   inline Vector3<T> GetColumn(int c) const {\r
3453     return Vector3<T>(M[0][c], M[1][c], M[2][c]);\r
3454   }\r
3455 \r
3456   inline Vector3<T> GetRow(int r) const {\r
3457     return Vector3<T>(M[r][0], M[r][1], M[r][2]);\r
3458   }\r
3459 \r
3460   inline void SetColumn(int c, const Vector3<T>& v) {\r
3461     M[0][c] = v.x;\r
3462     M[1][c] = v.y;\r
3463     M[2][c] = v.z;\r
3464   }\r
3465 \r
3466   inline void SetRow(int r, const Vector3<T>& v) {\r
3467     M[r][0] = v.x;\r
3468     M[r][1] = v.y;\r
3469     M[r][2] = v.z;\r
3470   }\r
3471 \r
3472   inline T Determinant() const {\r
3473     const Matrix3<T>& m = *this;\r
3474     T d;\r
3475 \r
3476     d = m.M[0][0] * (m.M[1][1] * m.M[2][2] - m.M[1][2] * m.M[2][1]);\r
3477     d -= m.M[0][1] * (m.M[1][0] * m.M[2][2] - m.M[1][2] * m.M[2][0]);\r
3478     d += m.M[0][2] * (m.M[1][0] * m.M[2][1] - m.M[1][1] * m.M[2][0]);\r
3479 \r
3480     return d;\r
3481   }\r
3482 \r
3483   inline Matrix3<T> Inverse() const {\r
3484     Matrix3<T> a;\r
3485     const Matrix3<T>& m = *this;\r
3486     T d = Determinant();\r
3487 \r
3488     OVR_MATH_ASSERT(d != 0);\r
3489     T s = T(1) / d;\r
3490 \r
3491     a.M[0][0] = s * (m.M[1][1] * m.M[2][2] - m.M[1][2] * m.M[2][1]);\r
3492     a.M[1][0] = s * (m.M[1][2] * m.M[2][0] - m.M[1][0] * m.M[2][2]);\r
3493     a.M[2][0] = s * (m.M[1][0] * m.M[2][1] - m.M[1][1] * m.M[2][0]);\r
3494 \r
3495     a.M[0][1] = s * (m.M[0][2] * m.M[2][1] - m.M[0][1] * m.M[2][2]);\r
3496     a.M[1][1] = s * (m.M[0][0] * m.M[2][2] - m.M[0][2] * m.M[2][0]);\r
3497     a.M[2][1] = s * (m.M[0][1] * m.M[2][0] - m.M[0][0] * m.M[2][1]);\r
3498 \r
3499     a.M[0][2] = s * (m.M[0][1] * m.M[1][2] - m.M[0][2] * m.M[1][1]);\r
3500     a.M[1][2] = s * (m.M[0][2] * m.M[1][0] - m.M[0][0] * m.M[1][2]);\r
3501     a.M[2][2] = s * (m.M[0][0] * m.M[1][1] - m.M[0][1] * m.M[1][0]);\r
3502 \r
3503     return a;\r
3504   }\r
3505 \r
3506   // Outer Product of two column vectors: a * b.Transpose()\r
3507   static Matrix3 OuterProduct(const Vector3<T>& a, const Vector3<T>& b) {\r
3508     return Matrix3(\r
3509         a.x * b.x,\r
3510         a.x * b.y,\r
3511         a.x * b.z,\r
3512         a.y * b.x,\r
3513         a.y * b.y,\r
3514         a.y * b.z,\r
3515         a.z * b.x,\r
3516         a.z * b.y,\r
3517         a.z * b.z);\r
3518   }\r
3519 \r
3520   // Vector cross product as a premultiply matrix:\r
3521   // L.Cross(R) = LeftCrossAsMatrix(L) * R\r
3522   static Matrix3 LeftCrossAsMatrix(const Vector3<T>& L) {\r
3523     return Matrix3(T(0), -L.z, +L.y, +L.z, T(0), -L.x, -L.y, +L.x, T(0));\r
3524   }\r
3525 \r
3526   // Vector cross product as a premultiply matrix:\r
3527   // L.Cross(R) = RightCrossAsMatrix(R) * L\r
3528   static Matrix3 RightCrossAsMatrix(const Vector3<T>& R) {\r
3529     return Matrix3(T(0), +R.z, -R.y, -R.z, T(0), +R.x, +R.y, -R.x, T(0));\r
3530   }\r
3531 \r
3532   // Angle in radians of a rotation matrix\r
3533   // Uses identity trace(a) = 2*cos(theta) + 1\r
3534   T Angle() const {\r
3535     return Acos((Trace() - T(1)) * T(0.5));\r
3536   }\r
3537 \r
3538   // Angle in radians between two rotation matrices\r
3539   T Angle(const Matrix3& b) const {\r
3540     // Compute trace of (this->Transposed() * b)\r
3541     // This works out to sum of products of elements.\r
3542     T trace = T(0);\r
3543     for (int i = 0; i < 3; i++) {\r
3544       for (int j = 0; j < 3; j++) {\r
3545         trace += M[i][j] * b.M[i][j];\r
3546       }\r
3547     }\r
3548     return Acos((trace - T(1)) * T(0.5));\r
3549   }\r
3550 };\r
3551 \r
3552 typedef Matrix3<float> Matrix3f;\r
3553 typedef Matrix3<double> Matrix3d;\r
3554 \r
3555 //-------------------------------------------------------------------------------------\r
3556 // ***** Matrix2\r
3557 \r
3558 template <class T>\r
3559 class Matrix2 {\r
3560  public:\r
3561   typedef T ElementType;\r
3562   static const size_t Dimension = 2;\r
3563 \r
3564   T M[2][2];\r
3565 \r
3566   enum NoInitType { NoInit };\r
3567 \r
3568   // Construct with no memory initialization.\r
3569   Matrix2(NoInitType) {}\r
3570 \r
3571   // By default, we construct identity matrix.\r
3572   Matrix2() {\r
3573     M[0][0] = M[1][1] = T(1);\r
3574     M[0][1] = M[1][0] = T(0);\r
3575   }\r
3576 \r
3577   Matrix2(T m11, T m12, T m21, T m22) {\r
3578     M[0][0] = m11;\r
3579     M[0][1] = m12;\r
3580     M[1][0] = m21;\r
3581     M[1][1] = m22;\r
3582   }\r
3583 \r
3584   // Construction from X, Y basis vectors\r
3585   Matrix2(const Vector2<T>& xBasis, const Vector2<T>& yBasis) {\r
3586     M[0][0] = xBasis.x;\r
3587     M[0][1] = yBasis.x;\r
3588     M[1][0] = xBasis.y;\r
3589     M[1][1] = yBasis.y;\r
3590   }\r
3591 \r
3592   explicit Matrix2(T s) {\r
3593     M[0][0] = M[1][1] = s;\r
3594     M[0][1] = M[1][0] = T(0);\r
3595   }\r
3596 \r
3597   Matrix2(T m11, T m22) {\r
3598     M[0][0] = m11;\r
3599     M[0][1] = T(0);\r
3600     M[1][0] = T(0);\r
3601     M[1][1] = m22;\r
3602   }\r
3603 \r
3604   explicit Matrix2(const Matrix2<typename Math<T>::OtherFloatType>& src) {\r
3605     M[0][0] = T(src.M[0][0]);\r
3606     M[0][1] = T(src.M[0][1]);\r
3607     M[1][0] = T(src.M[1][0]);\r
3608     M[1][1] = T(src.M[1][1]);\r
3609   }\r
3610 \r
3611   // C-interop support\r
3612   Matrix2(const typename CompatibleTypes<Matrix2<T>>::Type& s) {\r
3613     OVR_MATH_STATIC_ASSERT(sizeof(s) == sizeof(Matrix2), "sizeof(s) == sizeof(Matrix2)");\r
3614     memcpy(M, s.M, sizeof(M));\r
3615   }\r
3616 \r
3617   operator const typename CompatibleTypes<Matrix2<T>>::Type() const {\r
3618     typename CompatibleTypes<Matrix2<T>>::Type result;\r
3619     OVR_MATH_STATIC_ASSERT(sizeof(result) == sizeof(Matrix2), "sizeof(result) == sizeof(Matrix2)");\r
3620     memcpy(result.M, M, sizeof(M));\r
3621     return result;\r
3622   }\r
3623 \r
3624   T operator()(int i, int j) const {\r
3625     return M[i][j];\r
3626   }\r
3627   T& operator()(int i, int j) {\r
3628     return M[i][j];\r
3629   }\r
3630   const T* operator[](int i) const {\r
3631     return M[i];\r
3632   }\r
3633   T* operator[](int i) {\r
3634     return M[i];\r
3635   }\r
3636 \r
3637   static Matrix2 Identity() {\r
3638     return Matrix2();\r
3639   }\r
3640 \r
3641   void SetIdentity() {\r
3642     M[0][0] = M[1][1] = T(1);\r
3643     M[0][1] = M[1][0] = T(0);\r
3644   }\r
3645 \r
3646   static Matrix2 Diagonal(T m00, T m11) {\r
3647     return Matrix2(m00, m11);\r
3648   }\r
3649   static Matrix2 Diagonal(const Vector2<T>& v) {\r
3650     return Matrix2(v.x, v.y);\r
3651   }\r
3652 \r
3653   T Trace() const {\r
3654     return M[0][0] + M[1][1];\r
3655   }\r
3656 \r
3657   bool operator==(const Matrix2& b) const {\r
3658     return M[0][0] == b.M[0][0] && M[0][1] == b.M[0][1] && M[1][0] == b.M[1][0] &&\r
3659         M[1][1] == b.M[1][1];\r
3660   }\r
3661 \r
3662   Matrix2 operator+(const Matrix2& b) const {\r
3663     return Matrix2(\r
3664         M[0][0] + b.M[0][0], M[0][1] + b.M[0][1], M[1][0] + b.M[1][0], M[1][1] + b.M[1][1]);\r
3665   }\r
3666 \r
3667   Matrix2& operator+=(const Matrix2& b) {\r
3668     M[0][0] += b.M[0][0];\r
3669     M[0][1] += b.M[0][1];\r
3670     M[1][0] += b.M[1][0];\r
3671     M[1][1] += b.M[1][1];\r
3672     return *this;\r
3673   }\r
3674 \r
3675   void operator=(const Matrix2& b) {\r
3676     M[0][0] = b.M[0][0];\r
3677     M[0][1] = b.M[0][1];\r
3678     M[1][0] = b.M[1][0];\r
3679     M[1][1] = b.M[1][1];\r
3680   }\r
3681 \r
3682   Matrix2 operator-(const Matrix2& b) const {\r
3683     return Matrix2(\r
3684         M[0][0] - b.M[0][0], M[0][1] - b.M[0][1], M[1][0] - b.M[1][0], M[1][1] - b.M[1][1]);\r
3685   }\r
3686 \r
3687   Matrix2& operator-=(const Matrix2& b) {\r
3688     M[0][0] -= b.M[0][0];\r
3689     M[0][1] -= b.M[0][1];\r
3690     M[1][0] -= b.M[1][0];\r
3691     M[1][1] -= b.M[1][1];\r
3692     return *this;\r
3693   }\r
3694 \r
3695   Matrix2 operator*(const Matrix2& b) const {\r
3696     return Matrix2(\r
3697         M[0][0] * b.M[0][0] + M[0][1] * b.M[1][0],\r
3698         M[0][0] * b.M[0][1] + M[0][1] * b.M[1][1],\r
3699         M[1][0] * b.M[0][0] + M[1][1] * b.M[1][0],\r
3700         M[1][0] * b.M[0][1] + M[1][1] * b.M[1][1]);\r
3701   }\r
3702 \r
3703   Matrix2& operator*=(const Matrix2& b) {\r
3704     *this = *this * b;\r
3705     return *this;\r
3706   }\r
3707 \r
3708   Matrix2 operator*(T s) const {\r
3709     return Matrix2(M[0][0] * s, M[0][1] * s, M[1][0] * s, M[1][1] * s);\r
3710   }\r
3711 \r
3712   Matrix2& operator*=(T s) {\r
3713     M[0][0] *= s;\r
3714     M[0][1] *= s;\r
3715     M[1][0] *= s;\r
3716     M[1][1] *= s;\r
3717     return *this;\r
3718   }\r
3719 \r
3720   Matrix2 operator/(T s) const {\r
3721     return *this * (T(1) / s);\r
3722   }\r
3723 \r
3724   Matrix2& operator/=(T s) {\r
3725     return *this *= (T(1) / s);\r
3726   }\r
3727 \r
3728   Vector2<T> operator*(const Vector2<T>& b) const {\r
3729     return Vector2<T>(M[0][0] * b.x + M[0][1] * b.y, M[1][0] * b.x + M[1][1] * b.y);\r
3730   }\r
3731 \r
3732   Vector2<T> Transform(const Vector2<T>& v) const {\r
3733     return Vector2<T>(M[0][0] * v.x + M[0][1] * v.y, M[1][0] * v.x + M[1][1] * v.y);\r
3734   }\r
3735 \r
3736   Matrix2 Transposed() const {\r
3737     return Matrix2(M[0][0], M[1][0], M[0][1], M[1][1]);\r
3738   }\r
3739 \r
3740   void Transpose() {\r
3741     OVRMath_Swap(M[1][0], M[0][1]);\r
3742   }\r
3743 \r
3744   Vector2<T> GetColumn(int c) const {\r
3745     return Vector2<T>(M[0][c], M[1][c]);\r
3746   }\r
3747 \r
3748   Vector2<T> GetRow(int r) const {\r
3749     return Vector2<T>(M[r][0], M[r][1]);\r
3750   }\r
3751 \r
3752   void SetColumn(int c, const Vector2<T>& v) {\r
3753     M[0][c] = v.x;\r
3754     M[1][c] = v.y;\r
3755   }\r
3756 \r
3757   void SetRow(int r, const Vector2<T>& v) {\r
3758     M[r][0] = v.x;\r
3759     M[r][1] = v.y;\r
3760   }\r
3761 \r
3762   T Determinant() const {\r
3763     return M[0][0] * M[1][1] - M[0][1] * M[1][0];\r
3764   }\r
3765 \r
3766   Matrix2 Inverse() const {\r
3767     T rcpDet = T(1) / Determinant();\r
3768     return Matrix2(M[1][1] * rcpDet, -M[0][1] * rcpDet, -M[1][0] * rcpDet, M[0][0] * rcpDet);\r
3769   }\r
3770 \r
3771   // Outer Product of two column vectors: a * b.Transpose()\r
3772   static Matrix2 OuterProduct(const Vector2<T>& a, const Vector2<T>& b) {\r
3773     return Matrix2(a.x * b.x, a.x * b.y, a.y * b.x, a.y * b.y);\r
3774   }\r
3775 \r
3776   // Angle in radians between two rotation matrices\r
3777   T Angle(const Matrix2& b) const {\r
3778     const Matrix2& a = *this;\r
3779     return Acos(a(0, 0) * b(0, 0) + a(1, 0) * b(1, 0));\r
3780   }\r
3781 };\r
3782 \r
3783 typedef Matrix2<float> Matrix2f;\r
3784 typedef Matrix2<double> Matrix2d;\r
3785 \r
3786 //-------------------------------------------------------------------------------------\r
3787 \r
3788 template <class T>\r
3789 class SymMat3 {\r
3790  private:\r
3791   typedef SymMat3<T> this_type;\r
3792 \r
3793  public:\r
3794   typedef T Value_t;\r
3795   // Upper symmetric\r
3796   T v[6]; // _00 _01 _02 _11 _12 _22\r
3797 \r
3798   inline SymMat3() {}\r
3799 \r
3800   inline explicit SymMat3(T s) {\r
3801     v[0] = v[3] = v[5] = s;\r
3802     v[1] = v[2] = v[4] = T(0);\r
3803   }\r
3804 \r
3805   inline explicit SymMat3(T a00, T a01, T a02, T a11, T a12, T a22) {\r
3806     v[0] = a00;\r
3807     v[1] = a01;\r
3808     v[2] = a02;\r
3809     v[3] = a11;\r
3810     v[4] = a12;\r
3811     v[5] = a22;\r
3812   }\r
3813 \r
3814   // Cast to symmetric Matrix3\r
3815   operator Matrix3<T>() const {\r
3816     return Matrix3<T>(v[0], v[1], v[2], v[1], v[3], v[4], v[2], v[4], v[5]);\r
3817   }\r
3818 \r
3819   static inline int Index(unsigned int i, unsigned int j) {\r
3820     return (i <= j) ? (3 * i - i * (i + 1) / 2 + j) : (3 * j - j * (j + 1) / 2 + i);\r
3821   }\r
3822 \r
3823   inline T operator()(int i, int j) const {\r
3824     return v[Index(i, j)];\r
3825   }\r
3826 \r
3827   inline T& operator()(int i, int j) {\r
3828     return v[Index(i, j)];\r
3829   }\r
3830 \r
3831   inline this_type& operator+=(const this_type& b) {\r
3832     v[0] += b.v[0];\r
3833     v[1] += b.v[1];\r
3834     v[2] += b.v[2];\r
3835     v[3] += b.v[3];\r
3836     v[4] += b.v[4];\r
3837     v[5] += b.v[5];\r
3838     return *this;\r
3839   }\r
3840 \r
3841   inline this_type& operator-=(const this_type& b) {\r
3842     v[0] -= b.v[0];\r
3843     v[1] -= b.v[1];\r
3844     v[2] -= b.v[2];\r
3845     v[3] -= b.v[3];\r
3846     v[4] -= b.v[4];\r
3847     v[5] -= b.v[5];\r
3848 \r
3849     return *this;\r
3850   }\r
3851 \r
3852   inline this_type& operator*=(T s) {\r
3853     v[0] *= s;\r
3854     v[1] *= s;\r
3855     v[2] *= s;\r
3856     v[3] *= s;\r
3857     v[4] *= s;\r
3858     v[5] *= s;\r
3859 \r
3860     return *this;\r
3861   }\r
3862 \r
3863   inline SymMat3 operator*(T s) const {\r
3864     SymMat3 d;\r
3865     d.v[0] = v[0] * s;\r
3866     d.v[1] = v[1] * s;\r
3867     d.v[2] = v[2] * s;\r
3868     d.v[3] = v[3] * s;\r
3869     d.v[4] = v[4] * s;\r
3870     d.v[5] = v[5] * s;\r
3871 \r
3872     return d;\r
3873   }\r
3874 \r
3875   // Multiplies two matrices into destination with minimum copying.\r
3876   static SymMat3& Multiply(SymMat3* d, const SymMat3& a, const SymMat3& b) {\r
3877     // _00 _01 _02 _11 _12 _22\r
3878 \r
3879     d->v[0] = a.v[0] * b.v[0];\r
3880     d->v[1] = a.v[0] * b.v[1] + a.v[1] * b.v[3];\r
3881     d->v[2] = a.v[0] * b.v[2] + a.v[1] * b.v[4];\r
3882 \r
3883     d->v[3] = a.v[3] * b.v[3];\r
3884     d->v[4] = a.v[3] * b.v[4] + a.v[4] * b.v[5];\r
3885 \r
3886     d->v[5] = a.v[5] * b.v[5];\r
3887 \r
3888     return *d;\r
3889   }\r
3890 \r
3891   inline T Determinant() const {\r
3892     const this_type& m = *this;\r
3893     T d;\r
3894 \r
3895     d = m(0, 0) * (m(1, 1) * m(2, 2) - m(1, 2) * m(2, 1));\r
3896     d -= m(0, 1) * (m(1, 0) * m(2, 2) - m(1, 2) * m(2, 0));\r
3897     d += m(0, 2) * (m(1, 0) * m(2, 1) - m(1, 1) * m(2, 0));\r
3898 \r
3899     return d;\r
3900   }\r
3901 \r
3902   inline this_type Inverse() const {\r
3903     this_type a;\r
3904     const this_type& m = *this;\r
3905     T d = Determinant();\r
3906 \r
3907     OVR_MATH_ASSERT(d != 0);\r
3908     T s = T(1) / d;\r
3909 \r
3910     a(0, 0) = s * (m(1, 1) * m(2, 2) - m(1, 2) * m(2, 1));\r
3911 \r
3912     a(0, 1) = s * (m(0, 2) * m(2, 1) - m(0, 1) * m(2, 2));\r
3913     a(1, 1) = s * (m(0, 0) * m(2, 2) - m(0, 2) * m(2, 0));\r
3914 \r
3915     a(0, 2) = s * (m(0, 1) * m(1, 2) - m(0, 2) * m(1, 1));\r
3916     a(1, 2) = s * (m(0, 2) * m(1, 0) - m(0, 0) * m(1, 2));\r
3917     a(2, 2) = s * (m(0, 0) * m(1, 1) - m(0, 1) * m(1, 0));\r
3918 \r
3919     return a;\r
3920   }\r
3921 \r
3922   inline T Trace() const {\r
3923     return v[0] + v[3] + v[5];\r
3924   }\r
3925 \r
3926   // M = a*a.t()\r
3927   inline void Rank1(const Vector3<T>& a) {\r
3928     v[0] = a.x * a.x;\r
3929     v[1] = a.x * a.y;\r
3930     v[2] = a.x * a.z;\r
3931     v[3] = a.y * a.y;\r
3932     v[4] = a.y * a.z;\r
3933     v[5] = a.z * a.z;\r
3934   }\r
3935 \r
3936   // M += a*a.t()\r
3937   inline void Rank1Add(const Vector3<T>& a) {\r
3938     v[0] += a.x * a.x;\r
3939     v[1] += a.x * a.y;\r
3940     v[2] += a.x * a.z;\r
3941     v[3] += a.y * a.y;\r
3942     v[4] += a.y * a.z;\r
3943     v[5] += a.z * a.z;\r
3944   }\r
3945 \r
3946   // M -= a*a.t()\r
3947   inline void Rank1Sub(const Vector3<T>& a) {\r
3948     v[0] -= a.x * a.x;\r
3949     v[1] -= a.x * a.y;\r
3950     v[2] -= a.x * a.z;\r
3951     v[3] -= a.y * a.y;\r
3952     v[4] -= a.y * a.z;\r
3953     v[5] -= a.z * a.z;\r
3954   }\r
3955 };\r
3956 \r
3957 typedef SymMat3<float> SymMat3f;\r
3958 typedef SymMat3<double> SymMat3d;\r
3959 \r
3960 template <class T>\r
3961 inline Matrix3<T> operator*(const SymMat3<T>& a, const SymMat3<T>& b) {\r
3962 #define AJB_ARBC(r, c) (a(r, 0) * b(0, c) + a(r, 1) * b(1, c) + a(r, 2) * b(2, c))\r
3963   return Matrix3<T>(\r
3964       AJB_ARBC(0, 0),\r
3965       AJB_ARBC(0, 1),\r
3966       AJB_ARBC(0, 2),\r
3967       AJB_ARBC(1, 0),\r
3968       AJB_ARBC(1, 1),\r
3969       AJB_ARBC(1, 2),\r
3970       AJB_ARBC(2, 0),\r
3971       AJB_ARBC(2, 1),\r
3972       AJB_ARBC(2, 2));\r
3973 #undef AJB_ARBC\r
3974 }\r
3975 \r
3976 template <class T>\r
3977 inline Matrix3<T> operator*(const Matrix3<T>& a, const SymMat3<T>& b) {\r
3978 #define AJB_ARBC(r, c) (a(r, 0) * b(0, c) + a(r, 1) * b(1, c) + a(r, 2) * b(2, c))\r
3979   return Matrix3<T>(\r
3980       AJB_ARBC(0, 0),\r
3981       AJB_ARBC(0, 1),\r
3982       AJB_ARBC(0, 2),\r
3983       AJB_ARBC(1, 0),\r
3984       AJB_ARBC(1, 1),\r
3985       AJB_ARBC(1, 2),\r
3986       AJB_ARBC(2, 0),\r
3987       AJB_ARBC(2, 1),\r
3988       AJB_ARBC(2, 2));\r
3989 #undef AJB_ARBC\r
3990 }\r
3991 \r
3992 //-------------------------------------------------------------------------------------\r
3993 // ***** Angle\r
3994 \r
3995 // Cleanly representing the algebra of 2D rotations.\r
3996 // The operations maintain the angle between -Pi and Pi, the same range as atan2.\r
3997 \r
3998 template <class T>\r
3999 class Angle {\r
4000  public:\r
4001   enum AngularUnits { Radians = 0, Degrees = 1 };\r
4002 \r
4003   Angle() : a(0) {}\r
4004 \r
4005   // Fix the range to be between -Pi and Pi\r
4006   Angle(T a_, AngularUnits u = Radians)\r
4007       : a((u == Radians) ? a_ : a_ * ((T)MATH_DOUBLE_DEGREETORADFACTOR)) {\r
4008     FixRange();\r
4009   }\r
4010 \r
4011   T Get(AngularUnits u = Radians) const {\r
4012     return (u == Radians) ? a : a * ((T)MATH_DOUBLE_RADTODEGREEFACTOR);\r
4013   }\r
4014   void Set(const T& x, AngularUnits u = Radians) {\r
4015     a = (u == Radians) ? x : x * ((T)MATH_DOUBLE_DEGREETORADFACTOR);\r
4016     FixRange();\r
4017   }\r
4018   int Sign() const {\r
4019     if (a == 0)\r
4020       return 0;\r
4021     else\r
4022       return (a > 0) ? 1 : -1;\r
4023   }\r
4024   T Abs() const {\r
4025     return (a >= 0) ? a : -a;\r
4026   }\r
4027 \r
4028   bool operator==(const Angle& b) const {\r
4029     return a == b.a;\r
4030   }\r
4031   bool operator!=(const Angle& b) const {\r
4032     return a != b.a;\r
4033   }\r
4034   //    bool operator<  (const Angle& b) const    { return a < a.b; }\r
4035   //    bool operator>  (const Angle& b) const    { return a > a.b; }\r
4036   //    bool operator<= (const Angle& b) const    { return a <= a.b; }\r
4037   //    bool operator>= (const Angle& b) const    { return a >= a.b; }\r
4038   //    bool operator= (const T& x)               { a = x; FixRange(); }\r
4039 \r
4040   // These operations assume a is already between -Pi and Pi.\r
4041   Angle& operator+=(const Angle& b) {\r
4042     a = a + b.a;\r
4043     FastFixRange();\r
4044     return *this;\r
4045   }\r
4046   Angle& operator+=(const T& x) {\r
4047     a = a + x;\r
4048     FixRange();\r
4049     return *this;\r
4050   }\r
4051   Angle operator+(const Angle& b) const {\r
4052     Angle res = *this;\r
4053     res += b;\r
4054     return res;\r
4055   }\r
4056   Angle operator+(const T& x) const {\r
4057     Angle res = *this;\r
4058     res += x;\r
4059     return res;\r
4060   }\r
4061   Angle& operator-=(const Angle& b) {\r
4062     a = a - b.a;\r
4063     FastFixRange();\r
4064     return *this;\r
4065   }\r
4066   Angle& operator-=(const T& x) {\r
4067     a = a - x;\r
4068     FixRange();\r
4069     return *this;\r
4070   }\r
4071   Angle operator-(const Angle& b) const {\r
4072     Angle res = *this;\r
4073     res -= b;\r
4074     return res;\r
4075   }\r
4076   Angle operator-(const T& x) const {\r
4077     Angle res = *this;\r
4078     res -= x;\r
4079     return res;\r
4080   }\r
4081 \r
4082   T Distance(const Angle& b) {\r
4083     T c = fabs(a - b.a);\r
4084     return (c <= ((T)MATH_DOUBLE_PI)) ? c : ((T)MATH_DOUBLE_TWOPI) - c;\r
4085   }\r
4086 \r
4087  private:\r
4088   // The stored angle, which should be maintained between -Pi and Pi\r
4089   T a;\r
4090 \r
4091   // Fixes the angle range to [-Pi,Pi], but assumes no more than 2Pi away on either side\r
4092   inline void FastFixRange() {\r
4093     if (a < -((T)MATH_DOUBLE_PI))\r
4094       a += ((T)MATH_DOUBLE_TWOPI);\r
4095     else if (a > ((T)MATH_DOUBLE_PI))\r
4096       a -= ((T)MATH_DOUBLE_TWOPI);\r
4097   }\r
4098 \r
4099   // Fixes the angle range to [-Pi,Pi] for any given range, but slower then the fast method\r
4100   inline void FixRange() {\r
4101     // do nothing if the value is already in the correct range, since fmod call is expensive\r
4102     if (a >= -((T)MATH_DOUBLE_PI) && a <= ((T)MATH_DOUBLE_PI))\r
4103       return;\r
4104     a = fmod(a, ((T)MATH_DOUBLE_TWOPI));\r
4105     if (a < -((T)MATH_DOUBLE_PI))\r
4106       a += ((T)MATH_DOUBLE_TWOPI);\r
4107     else if (a > ((T)MATH_DOUBLE_PI))\r
4108       a -= ((T)MATH_DOUBLE_TWOPI);\r
4109   }\r
4110 };\r
4111 \r
4112 typedef Angle<float> Anglef;\r
4113 typedef Angle<double> Angled;\r
4114 \r
4115 //-------------------------------------------------------------------------------------\r
4116 // ***** Plane\r
4117 \r
4118 // Consists of a normal vector and distance from the origin where the plane is located.\r
4119 \r
4120 template <class T>\r
4121 class Plane {\r
4122  public:\r
4123   Vector3<T> N;\r
4124   T D;\r
4125 \r
4126   Plane() : D(0) {}\r
4127 \r
4128   // Normals must already be normalized\r
4129   Plane(const Vector3<T>& n, T d) : N(n), D(d) {}\r
4130   Plane(T x, T y, T z, T d) : N(x, y, z), D(d) {}\r
4131 \r
4132   // construct from a point on the plane and the normal\r
4133   Plane(const Vector3<T>& p, const Vector3<T>& n) : N(n), D(-(p.Dot(n))) {}\r
4134 \r
4135   // Find the point to plane distance. The sign indicates what side of the plane the point is on (0\r
4136   // = point on plane).\r
4137   T TestSide(const Vector3<T>& p) const {\r
4138     return (N.Dot(p)) + D;\r
4139   }\r
4140 \r
4141   Plane<T> Flipped() const {\r
4142     return Plane(-N, -D);\r
4143   }\r
4144 \r
4145   void Flip() {\r
4146     N = -N;\r
4147     D = -D;\r
4148   }\r
4149 \r
4150   bool operator==(const Plane<T>& rhs) const {\r
4151     return (this->D == rhs.D && this->N == rhs.N);\r
4152   }\r
4153 };\r
4154 \r
4155 typedef Plane<float> Planef;\r
4156 typedef Plane<double> Planed;\r
4157 \r
4158 //-----------------------------------------------------------------------------------\r
4159 // ***** ScaleAndOffset2D\r
4160 \r
4161 struct ScaleAndOffset2D {\r
4162   Vector2f Scale;\r
4163   Vector2f Offset;\r
4164 \r
4165   ScaleAndOffset2D(float sx = 0.0f, float sy = 0.0f, float ox = 0.0f, float oy = 0.0f)\r
4166       : Scale(sx, sy), Offset(ox, oy) {}\r
4167 };\r
4168 \r
4169 //-----------------------------------------------------------------------------------\r
4170 // ***** FovPort\r
4171 \r
4172 // FovPort describes Field Of View (FOV) of a viewport.\r
4173 // This class has values for up, down, left and right, stored in\r
4174 // tangent of the angle units to simplify calculations.\r
4175 //\r
4176 // As an example, for a standard 90 degree vertical FOV, we would\r
4177 // have: { UpTan = tan(90 degrees / 2), DownTan = tan(90 degrees / 2) }.\r
4178 //\r
4179 // CreateFromRadians/Degrees helper functions can be used to\r
4180 // access FOV in different units.\r
4181 \r
4182 // ***** FovPort\r
4183 \r
4184 struct FovPort {\r
4185   float UpTan;\r
4186   float DownTan;\r
4187   float LeftTan;\r
4188   float RightTan;\r
4189 \r
4190   FovPort(float sideTan = 0.0f)\r
4191       : UpTan(sideTan), DownTan(sideTan), LeftTan(sideTan), RightTan(sideTan) {}\r
4192   FovPort(float u, float d, float l, float r) : UpTan(u), DownTan(d), LeftTan(l), RightTan(r) {}\r
4193 \r
4194 #ifndef OVR_EXCLUDE_CAPI_FROM_MATH\r
4195   // C-interop support.\r
4196   typedef CompatibleTypes<FovPort>::Type CompatibleType;\r
4197 \r
4198   FovPort(const CompatibleType& s)\r
4199       : UpTan(s.UpTan), DownTan(s.DownTan), LeftTan(s.LeftTan), RightTan(s.RightTan) {}\r
4200 \r
4201   operator const CompatibleType&() const {\r
4202     OVR_MATH_STATIC_ASSERT(sizeof(FovPort) == sizeof(CompatibleType), "sizeof(FovPort) failure");\r
4203     return reinterpret_cast<const CompatibleType&>(*this);\r
4204   }\r
4205 #endif\r
4206 \r
4207   static FovPort CreateFromRadians(float horizontalFov, float verticalFov) {\r
4208     FovPort result;\r
4209     result.UpTan = tanf(verticalFov * 0.5f);\r
4210     result.DownTan = tanf(verticalFov * 0.5f);\r
4211     result.LeftTan = tanf(horizontalFov * 0.5f);\r
4212     result.RightTan = tanf(horizontalFov * 0.5f);\r
4213     return result;\r
4214   }\r
4215 \r
4216   static FovPort CreateFromDegrees(float horizontalFovDegrees, float verticalFovDegrees) {\r
4217     return CreateFromRadians(DegreeToRad(horizontalFovDegrees), DegreeToRad(verticalFovDegrees));\r
4218   }\r
4219 \r
4220   //  Get Horizontal/Vertical components of Fov in radians.\r
4221   float GetVerticalFovRadians() const {\r
4222     return atanf(UpTan) + atanf(DownTan);\r
4223   }\r
4224   float GetHorizontalFovRadians() const {\r
4225     return atanf(LeftTan) + atanf(RightTan);\r
4226   }\r
4227   //  Get Horizontal/Vertical components of Fov in degrees.\r
4228   float GetVerticalFovDegrees() const {\r
4229     return RadToDegree(GetVerticalFovRadians());\r
4230   }\r
4231   float GetHorizontalFovDegrees() const {\r
4232     return RadToDegree(GetHorizontalFovRadians());\r
4233   }\r
4234 \r
4235   // Compute maximum tangent value among all four sides.\r
4236   float GetMaxSideTan() const {\r
4237     return OVRMath_Max(OVRMath_Max(UpTan, DownTan), OVRMath_Max(LeftTan, RightTan));\r
4238   }\r
4239 \r
4240   static ScaleAndOffset2D CreateNDCScaleAndOffsetFromFov(FovPort tanHalfFov) {\r
4241     float projXScale = 2.0f / (tanHalfFov.LeftTan + tanHalfFov.RightTan);\r
4242     float projXOffset = (tanHalfFov.LeftTan - tanHalfFov.RightTan) * projXScale * 0.5f;\r
4243     float projYScale = 2.0f / (tanHalfFov.UpTan + tanHalfFov.DownTan);\r
4244     float projYOffset = (tanHalfFov.UpTan - tanHalfFov.DownTan) * projYScale * 0.5f;\r
4245 \r
4246     ScaleAndOffset2D result;\r
4247     result.Scale = Vector2f(projXScale, projYScale);\r
4248     result.Offset = Vector2f(projXOffset, projYOffset);\r
4249     // Hey - why is that Y.Offset negated?\r
4250     // It's because a projection matrix transforms from world coords with Y=up,\r
4251     // whereas this is from NDC which is Y=down.\r
4252 \r
4253     return result;\r
4254   }\r
4255 \r
4256   // Converts Fov Tan angle units to [-1,1] render target NDC space\r
4257   Vector2f TanAngleToRendertargetNDC(Vector2f const& tanEyeAngle) {\r
4258     ScaleAndOffset2D eyeToSourceNDC = CreateNDCScaleAndOffsetFromFov(*this);\r
4259     return tanEyeAngle * eyeToSourceNDC.Scale + eyeToSourceNDC.Offset;\r
4260   }\r
4261 \r
4262   // Compute per-channel minimum and maximum of Fov.\r
4263   static FovPort Min(const FovPort& a, const FovPort& b) {\r
4264     FovPort fov(\r
4265         OVRMath_Min(a.UpTan, b.UpTan),\r
4266         OVRMath_Min(a.DownTan, b.DownTan),\r
4267         OVRMath_Min(a.LeftTan, b.LeftTan),\r
4268         OVRMath_Min(a.RightTan, b.RightTan));\r
4269     return fov;\r
4270   }\r
4271 \r
4272   static FovPort Max(const FovPort& a, const FovPort& b) {\r
4273     FovPort fov(\r
4274         OVRMath_Max(a.UpTan, b.UpTan),\r
4275         OVRMath_Max(a.DownTan, b.DownTan),\r
4276         OVRMath_Max(a.LeftTan, b.LeftTan),\r
4277         OVRMath_Max(a.RightTan, b.RightTan));\r
4278     return fov;\r
4279   }\r
4280 \r
4281   static FovPort Uncant(const FovPort& cantedFov, Quatf canting) {\r
4282     FovPort uncantedFov = cantedFov;\r
4283 \r
4284     // make 3D vectors from the FovPorts projected to z=1 plane\r
4285     Vector3f leftUp = Vector3f(cantedFov.LeftTan, cantedFov.UpTan, 1.0f);\r
4286     Vector3f rightUp = Vector3f(-cantedFov.RightTan, cantedFov.UpTan, 1.0f);\r
4287     Vector3f leftDown = Vector3f(cantedFov.LeftTan, -cantedFov.DownTan, 1.0f);\r
4288     Vector3f rightDown = Vector3f(-cantedFov.RightTan, -cantedFov.DownTan, 1.0f);\r
4289 \r
4290     // rotate these vectors using the canting specified\r
4291     leftUp = canting.Rotate(leftUp);\r
4292     rightUp = canting.Rotate(rightUp);\r
4293     leftDown = canting.Rotate(leftDown);\r
4294     rightDown = canting.Rotate(rightDown);\r
4295 \r
4296     // If the z coordinates of any of the corners end up being really small or negative, then\r
4297     // projection will generate extremely large or inverted frustums and we don't really want that\r
4298     const float kMinValidZ = 0.01f;\r
4299 \r
4300     // re-project back to z=1 plane while making sure we don't generate gigantic values (hence max)\r
4301     leftUp /= OVRMath_Max(leftUp.z, kMinValidZ);\r
4302     rightUp /= OVRMath_Max(rightUp.z, kMinValidZ);\r
4303     leftDown /= OVRMath_Max(leftDown.z, kMinValidZ);\r
4304     rightDown /= OVRMath_Max(rightDown.z, kMinValidZ);\r
4305 \r
4306     // generate new FovTans as "bounding box" values\r
4307     uncantedFov.UpTan = OVRMath_Max(leftUp.y, rightUp.y);\r
4308     uncantedFov.DownTan = OVRMath_Max(-leftDown.y, -rightDown.y);\r
4309     uncantedFov.LeftTan = OVRMath_Max(leftUp.x, leftDown.x);\r
4310     uncantedFov.RightTan = OVRMath_Max(-rightDown.x, -rightUp.x);\r
4311 \r
4312     return uncantedFov;\r
4313   }\r
4314 \r
4315   template <class T>\r
4316   static FovPort ScaleFovPort(const FovPort& fov, OVR::Vector2<T> scaleFactors) {\r
4317     FovPort retFov = FovPort(fov);\r
4318     retFov.LeftTan *= ((scaleFactors.x != 0.0) ? scaleFactors.x : 1.0f);\r
4319     retFov.RightTan *= ((scaleFactors.x != 0.0) ? scaleFactors.x : 1.0f);\r
4320     retFov.UpTan *= ((scaleFactors.y != 0.0) ? scaleFactors.y : 1.0f);\r
4321     retFov.DownTan *= ((scaleFactors.y != 0.0) ? scaleFactors.y : 1.0f);\r
4322     return retFov;\r
4323   }\r
4324 };\r
4325 \r
4326 } // Namespace OVR\r
4327 \r
4328 #if defined(_MSC_VER)\r
4329 #pragma warning(pop)\r
4330 #endif\r
4331 \r
4332 #endif\r