ovr_sdk
view LibOVR/Src/Kernel/OVR_Math.h @ 0:1b39a1b46319
initial 0.4.4
| author | John Tsiombikas <nuclear@member.fsf.org> |
|---|---|
| date | Wed, 14 Jan 2015 06:51:16 +0200 |
| parents | |
| children |
line source
1 /************************************************************************************
3 PublicHeader: OVR_Kernel.h
4 Filename : OVR_Math.h
5 Content : Implementation of 3D primitives such as vectors, matrices.
6 Created : September 4, 2012
7 Authors : Andrew Reisse, Michael Antonov, Steve LaValle,
8 Anna Yershova, Max Katsev, Dov Katz
10 Copyright : Copyright 2014 Oculus VR, LLC All Rights reserved.
12 Licensed under the Oculus VR Rift SDK License Version 3.2 (the "License");
13 you may not use the Oculus VR Rift SDK except in compliance with the License,
14 which is provided at the time of installation or download, or which
15 otherwise accompanies this software in either electronic or hard copy form.
17 You may obtain a copy of the License at
19 http://www.oculusvr.com/licenses/LICENSE-3.2
21 Unless required by applicable law or agreed to in writing, the Oculus VR SDK
22 distributed under the License is distributed on an "AS IS" BASIS,
23 WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
24 See the License for the specific language governing permissions and
25 limitations under the License.
27 *************************************************************************************/
29 #ifndef OVR_Math_h
30 #define OVR_Math_h
32 #include <assert.h>
33 #include <stdlib.h>
34 #include <math.h>
36 #include "OVR_Types.h"
37 #include "OVR_RefCount.h"
38 #include "OVR_Std.h"
39 #include "OVR_Alg.h"
42 namespace OVR {
44 //-------------------------------------------------------------------------------------
45 // ***** Constants for 3D world/axis definitions.
47 // Definitions of axes for coordinate and rotation conversions.
48 enum Axis
49 {
50 Axis_X = 0, Axis_Y = 1, Axis_Z = 2
51 };
53 // RotateDirection describes the rotation direction around an axis, interpreted as follows:
54 // CW - Clockwise while looking "down" from positive axis towards the origin.
55 // CCW - Counter-clockwise while looking from the positive axis towards the origin,
56 // which is in the negative axis direction.
57 // CCW is the default for the RHS coordinate system. Oculus standard RHS coordinate
58 // system defines Y up, X right, and Z back (pointing out from the screen). In this
59 // system Rotate_CCW around Z will specifies counter-clockwise rotation in XY plane.
60 enum RotateDirection
61 {
62 Rotate_CCW = 1,
63 Rotate_CW = -1
64 };
66 // Constants for right handed and left handed coordinate systems
67 enum HandedSystem
68 {
69 Handed_R = 1, Handed_L = -1
70 };
72 // AxisDirection describes which way the coordinate axis points. Used by WorldAxes.
73 enum AxisDirection
74 {
75 Axis_Up = 2,
76 Axis_Down = -2,
77 Axis_Right = 1,
78 Axis_Left = -1,
79 Axis_In = 3,
80 Axis_Out = -3
81 };
83 struct WorldAxes
84 {
85 AxisDirection XAxis, YAxis, ZAxis;
87 WorldAxes(AxisDirection x, AxisDirection y, AxisDirection z)
88 : XAxis(x), YAxis(y), ZAxis(z)
89 { OVR_ASSERT(abs(x) != abs(y) && abs(y) != abs(z) && abs(z) != abs(x));}
90 };
92 } // namespace OVR
95 //------------------------------------------------------------------------------------//
96 // ***** C Compatibility Types
98 // These declarations are used to support conversion between C types used in
99 // LibOVR C interfaces and their C++ versions. As an example, they allow passing
100 // Vector3f into a function that expects ovrVector3f.
102 typedef struct ovrQuatf_ ovrQuatf;
103 typedef struct ovrQuatd_ ovrQuatd;
104 typedef struct ovrSizei_ ovrSizei;
105 typedef struct ovrSizef_ ovrSizef;
106 typedef struct ovrRecti_ ovrRecti;
107 typedef struct ovrVector2i_ ovrVector2i;
108 typedef struct ovrVector2f_ ovrVector2f;
109 typedef struct ovrVector3f_ ovrVector3f;
110 typedef struct ovrVector3d_ ovrVector3d;
111 typedef struct ovrMatrix3d_ ovrMatrix3d;
112 typedef struct ovrMatrix4f_ ovrMatrix4f;
113 typedef struct ovrPosef_ ovrPosef;
114 typedef struct ovrPosed_ ovrPosed;
115 typedef struct ovrPoseStatef_ ovrPoseStatef;
116 typedef struct ovrPoseStated_ ovrPoseStated;
118 namespace OVR {
120 // Forward-declare our templates.
121 template<class T> class Quat;
122 template<class T> class Size;
123 template<class T> class Rect;
124 template<class T> class Vector2;
125 template<class T> class Vector3;
126 template<class T> class Matrix3;
127 template<class T> class Matrix4;
128 template<class T> class Pose;
129 template<class T> class PoseState;
131 // CompatibleTypes::Type is used to lookup a compatible C-version of a C++ class.
132 template<class C>
133 struct CompatibleTypes
134 {
135 // Declaration here seems necessary for MSVC; specializations are
136 // used instead.
137 typedef struct {} Type;
138 };
140 // Specializations providing CompatibleTypes::Type value.
141 template<> struct CompatibleTypes<Quat<float> > { typedef ovrQuatf Type; };
142 template<> struct CompatibleTypes<Quat<double> > { typedef ovrQuatd Type; };
143 template<> struct CompatibleTypes<Matrix3<double> > { typedef ovrMatrix3d Type; };
144 template<> struct CompatibleTypes<Matrix4<float> > { typedef ovrMatrix4f Type; };
145 template<> struct CompatibleTypes<Size<int> > { typedef ovrSizei Type; };
146 template<> struct CompatibleTypes<Size<float> > { typedef ovrSizef Type; };
147 template<> struct CompatibleTypes<Rect<int> > { typedef ovrRecti Type; };
148 template<> struct CompatibleTypes<Vector2<int> > { typedef ovrVector2i Type; };
149 template<> struct CompatibleTypes<Vector2<float> > { typedef ovrVector2f Type; };
150 template<> struct CompatibleTypes<Vector3<float> > { typedef ovrVector3f Type; };
151 template<> struct CompatibleTypes<Vector3<double> > { typedef ovrVector3d Type; };
153 template<> struct CompatibleTypes<Pose<float> > { typedef ovrPosef Type; };
154 template<> struct CompatibleTypes<Pose<double> > { typedef ovrPosed Type; };
156 //------------------------------------------------------------------------------------//
157 // ***** Math
158 //
159 // Math class contains constants and functions. This class is a template specialized
160 // per type, with Math<float> and Math<double> being distinct.
161 template<class Type>
162 class Math
163 {
164 public:
165 // By default, support explicit conversion to float. This allows Vector2<int> to
166 // compile, for example.
167 typedef float OtherFloatType;
168 };
171 #define MATH_FLOAT_PI (3.1415926f)
172 #define MATH_FLOAT_TWOPI (2.0f *MATH_FLOAT_PI)
173 #define MATH_FLOAT_PIOVER2 (0.5f *MATH_FLOAT_PI)
174 #define MATH_FLOAT_PIOVER4 (0.25f*MATH_FLOAT_PI)
175 #define MATH_FLOAT_E (2.7182818f)
176 #define MATH_FLOAT_MAXVALUE (FLT_MAX)
177 #define MATH_FLOAT MINPOSITIVEVALUE (FLT_MIN)
178 #define MATH_FLOAT_RADTODEGREEFACTOR (360.0f / MATH_FLOAT_TWOPI)
179 #define MATH_FLOAT_DEGREETORADFACTOR (MATH_FLOAT_TWOPI / 360.0f)
180 #define MATH_FLOAT_TOLERANCE (0.00001f)
181 #define MATH_FLOAT_SINGULARITYRADIUS (0.0000001f) // Use for Gimbal lock numerical problems
183 #define MATH_DOUBLE_PI (3.14159265358979)
184 #define MATH_DOUBLE_TWOPI (2.0f *MATH_DOUBLE_PI)
185 #define MATH_DOUBLE_PIOVER2 (0.5f *MATH_DOUBLE_PI)
186 #define MATH_DOUBLE_PIOVER4 (0.25f*MATH_DOUBLE_PI)
187 #define MATH_DOUBLE_E (2.71828182845905)
188 #define MATH_DOUBLE_MAXVALUE (DBL_MAX)
189 #define MATH_DOUBLE MINPOSITIVEVALUE (DBL_MIN)
190 #define MATH_DOUBLE_RADTODEGREEFACTOR (360.0f / MATH_DOUBLE_TWOPI)
191 #define MATH_DOUBLE_DEGREETORADFACTOR (MATH_DOUBLE_TWOPI / 360.0f)
192 #define MATH_DOUBLE_TOLERANCE (0.00001)
193 #define MATH_DOUBLE_SINGULARITYRADIUS (0.000000000001) // Use for Gimbal lock numerical problems
198 // Single-precision Math constants class.
199 template<>
200 class Math<float>
201 {
202 public:
203 typedef double OtherFloatType;
204 };
206 // Double-precision Math constants class.
207 template<>
208 class Math<double>
209 {
210 public:
211 typedef float OtherFloatType;
212 };
215 typedef Math<float> Mathf;
216 typedef Math<double> Mathd;
218 // Conversion functions between degrees and radians
219 template<class T>
220 T RadToDegree(T rads) { return rads * ((T)MATH_DOUBLE_RADTODEGREEFACTOR); }
221 template<class T>
222 T DegreeToRad(T rads) { return rads * ((T)MATH_DOUBLE_DEGREETORADFACTOR); }
224 // Numerically stable acos function
225 template<class T>
226 T Acos(T val) {
227 if (val > T(1)) return T(0);
228 else if (val < T(-1)) return ((T)MATH_DOUBLE_PI);
229 else return acos(val);
230 };
232 // Numerically stable asin function
233 template<class T>
234 T Asin(T val) {
235 if (val > T(1)) return ((T)MATH_DOUBLE_PIOVER2);
236 else if (val < T(-1)) return ((T)MATH_DOUBLE_PIOVER2) * T(3);
237 else return asin(val);
238 };
240 #ifdef OVR_CC_MSVC
241 inline int isnan(double x) { return _isnan(x); };
242 #endif
244 template<class T>
245 class Quat;
248 //-------------------------------------------------------------------------------------
249 // ***** Vector2<>
251 // Vector2f (Vector2d) represents a 2-dimensional vector or point in space,
252 // consisting of coordinates x and y
254 template<class T>
255 class Vector2
256 {
257 public:
258 T x, y;
260 Vector2() : x(0), y(0) { }
261 Vector2(T x_, T y_) : x(x_), y(y_) { }
262 explicit Vector2(T s) : x(s), y(s) { }
263 explicit Vector2(const Vector2<typename Math<T>::OtherFloatType> &src)
264 : x((T)src.x), y((T)src.y) { }
267 // C-interop support.
268 typedef typename CompatibleTypes<Vector2<T> >::Type CompatibleType;
270 Vector2(const CompatibleType& s) : x(s.x), y(s.y) { }
272 operator const CompatibleType& () const
273 {
274 static_assert(sizeof(Vector2<T>) == sizeof(CompatibleType), "sizeof(Vector2<T>) failure");
275 return reinterpret_cast<const CompatibleType&>(*this);
276 }
279 bool operator== (const Vector2& b) const { return x == b.x && y == b.y; }
280 bool operator!= (const Vector2& b) const { return x != b.x || y != b.y; }
282 Vector2 operator+ (const Vector2& b) const { return Vector2(x + b.x, y + b.y); }
283 Vector2& operator+= (const Vector2& b) { x += b.x; y += b.y; return *this; }
284 Vector2 operator- (const Vector2& b) const { return Vector2(x - b.x, y - b.y); }
285 Vector2& operator-= (const Vector2& b) { x -= b.x; y -= b.y; return *this; }
286 Vector2 operator- () const { return Vector2(-x, -y); }
288 // Scalar multiplication/division scales vector.
289 Vector2 operator* (T s) const { return Vector2(x*s, y*s); }
290 Vector2& operator*= (T s) { x *= s; y *= s; return *this; }
292 Vector2 operator/ (T s) const { T rcp = T(1)/s;
293 return Vector2(x*rcp, y*rcp); }
294 Vector2& operator/= (T s) { T rcp = T(1)/s;
295 x *= rcp; y *= rcp;
296 return *this; }
298 static Vector2 Min(const Vector2& a, const Vector2& b) { return Vector2((a.x < b.x) ? a.x : b.x,
299 (a.y < b.y) ? a.y : b.y); }
300 static Vector2 Max(const Vector2& a, const Vector2& b) { return Vector2((a.x > b.x) ? a.x : b.x,
301 (a.y > b.y) ? a.y : b.y); }
303 // Compare two vectors for equality with tolerance. Returns true if vectors match withing tolerance.
304 bool Compare(const Vector2&b, T tolerance = ((T)MATH_DOUBLE_TOLERANCE))
305 {
306 return (fabs(b.x-x) < tolerance) && (fabs(b.y-y) < tolerance);
307 }
309 // Access element by index
310 T& operator[] (int idx)
311 {
312 OVR_ASSERT(0 <= idx && idx < 2);
313 return *(&x + idx);
314 }
315 const T& operator[] (int idx) const
316 {
317 OVR_ASSERT(0 <= idx && idx < 2);
318 return *(&x + idx);
319 }
321 // Entry-wise product of two vectors
322 Vector2 EntrywiseMultiply(const Vector2& b) const { return Vector2(x * b.x, y * b.y);}
325 // Multiply and divide operators do entry-wise math. Used Dot() for dot product.
326 Vector2 operator* (const Vector2& b) const { return Vector2(x * b.x, y * b.y); }
327 Vector2 operator/ (const Vector2& b) const { return Vector2(x / b.x, y / b.y); }
329 // Dot product
330 // Used to calculate angle q between two vectors among other things,
331 // as (A dot B) = |a||b|cos(q).
332 T Dot(const Vector2& b) const { return x*b.x + y*b.y; }
334 // Returns the angle from this vector to b, in radians.
335 T Angle(const Vector2& b) const
336 {
337 T div = LengthSq()*b.LengthSq();
338 OVR_ASSERT(div != T(0));
339 T result = Acos((this->Dot(b))/sqrt(div));
340 return result;
341 }
343 // Return Length of the vector squared.
344 T LengthSq() const { return (x * x + y * y); }
346 // Return vector length.
347 T Length() const { return sqrt(LengthSq()); }
349 // Returns squared distance between two points represented by vectors.
350 T DistanceSq(const Vector2& b) const { return (*this - b).LengthSq(); }
352 // Returns distance between two points represented by vectors.
353 T Distance(const Vector2& b) const { return (*this - b).Length(); }
355 // Determine if this a unit vector.
356 bool IsNormalized() const { return fabs(LengthSq() - T(1)) < ((T)MATH_DOUBLE_TOLERANCE); }
358 // Normalize, convention vector length to 1.
359 void Normalize()
360 {
361 T l = Length();
362 OVR_ASSERT(l != T(0));
363 *this /= l;
364 }
365 // Returns normalized (unit) version of the vector without modifying itself.
366 Vector2 Normalized() const
367 {
368 T l = Length();
369 OVR_ASSERT(l != T(0));
370 return *this / l;
371 }
373 // Linearly interpolates from this vector to another.
374 // Factor should be between 0.0 and 1.0, with 0 giving full value to this.
375 Vector2 Lerp(const Vector2& b, T f) const { return *this*(T(1) - f) + b*f; }
377 // Projects this vector onto the argument; in other words,
378 // A.Project(B) returns projection of vector A onto B.
379 Vector2 ProjectTo(const Vector2& b) const
380 {
381 T l2 = b.LengthSq();
382 OVR_ASSERT(l2 != T(0));
383 return b * ( Dot(b) / l2 );
384 }
385 };
388 typedef Vector2<float> Vector2f;
389 typedef Vector2<double> Vector2d;
390 typedef Vector2<int> Vector2i;
392 typedef Vector2<float> Point2f;
393 typedef Vector2<double> Point2d;
394 typedef Vector2<int> Point2i;
396 //-------------------------------------------------------------------------------------
397 // ***** Vector3<> - 3D vector of {x, y, z}
399 //
400 // Vector3f (Vector3d) represents a 3-dimensional vector or point in space,
401 // consisting of coordinates x, y and z.
403 template<class T>
404 class Vector3
405 {
406 public:
407 T x, y, z;
409 // FIXME: default initialization of a vector class can be very expensive in a full-blown
410 // application. A few hundred thousand vector constructions is not unlikely and can add
411 // up to milliseconds of time on processors like the PS3 PPU.
412 Vector3() : x(0), y(0), z(0) { }
413 Vector3(T x_, T y_, T z_ = 0) : x(x_), y(y_), z(z_) { }
414 explicit Vector3(T s) : x(s), y(s), z(s) { }
415 explicit Vector3(const Vector3<typename Math<T>::OtherFloatType> &src)
416 : x((T)src.x), y((T)src.y), z((T)src.z) { }
418 static const Vector3 ZERO;
420 // C-interop support.
421 typedef typename CompatibleTypes<Vector3<T> >::Type CompatibleType;
423 Vector3(const CompatibleType& s) : x(s.x), y(s.y), z(s.z) { }
425 operator const CompatibleType& () const
426 {
427 static_assert(sizeof(Vector3<T>) == sizeof(CompatibleType), "sizeof(Vector3<T>) failure");
428 return reinterpret_cast<const CompatibleType&>(*this);
429 }
431 bool operator== (const Vector3& b) const { return x == b.x && y == b.y && z == b.z; }
432 bool operator!= (const Vector3& b) const { return x != b.x || y != b.y || z != b.z; }
434 Vector3 operator+ (const Vector3& b) const { return Vector3(x + b.x, y + b.y, z + b.z); }
435 Vector3& operator+= (const Vector3& b) { x += b.x; y += b.y; z += b.z; return *this; }
436 Vector3 operator- (const Vector3& b) const { return Vector3(x - b.x, y - b.y, z - b.z); }
437 Vector3& operator-= (const Vector3& b) { x -= b.x; y -= b.y; z -= b.z; return *this; }
438 Vector3 operator- () const { return Vector3(-x, -y, -z); }
440 // Scalar multiplication/division scales vector.
441 Vector3 operator* (T s) const { return Vector3(x*s, y*s, z*s); }
442 Vector3& operator*= (T s) { x *= s; y *= s; z *= s; return *this; }
444 Vector3 operator/ (T s) const { T rcp = T(1)/s;
445 return Vector3(x*rcp, y*rcp, z*rcp); }
446 Vector3& operator/= (T s) { T rcp = T(1)/s;
447 x *= rcp; y *= rcp; z *= rcp;
448 return *this; }
450 static Vector3 Min(const Vector3& a, const Vector3& b)
451 {
452 return Vector3((a.x < b.x) ? a.x : b.x,
453 (a.y < b.y) ? a.y : b.y,
454 (a.z < b.z) ? a.z : b.z);
455 }
456 static Vector3 Max(const Vector3& a, const Vector3& b)
457 {
458 return Vector3((a.x > b.x) ? a.x : b.x,
459 (a.y > b.y) ? a.y : b.y,
460 (a.z > b.z) ? a.z : b.z);
461 }
463 // Compare two vectors for equality with tolerance. Returns true if vectors match withing tolerance.
464 bool Compare(const Vector3&b, T tolerance = ((T)MATH_DOUBLE_TOLERANCE))
465 {
466 return (fabs(b.x-x) < tolerance) &&
467 (fabs(b.y-y) < tolerance) &&
468 (fabs(b.z-z) < tolerance);
469 }
471 T& operator[] (int idx)
472 {
473 OVR_ASSERT(0 <= idx && idx < 3);
474 return *(&x + idx);
475 }
477 const T& operator[] (int idx) const
478 {
479 OVR_ASSERT(0 <= idx && idx < 3);
480 return *(&x + idx);
481 }
483 // Entrywise product of two vectors
484 Vector3 EntrywiseMultiply(const Vector3& b) const { return Vector3(x * b.x,
485 y * b.y,
486 z * b.z);}
488 // Multiply and divide operators do entry-wise math
489 Vector3 operator* (const Vector3& b) const { return Vector3(x * b.x,
490 y * b.y,
491 z * b.z); }
493 Vector3 operator/ (const Vector3& b) const { return Vector3(x / b.x,
494 y / b.y,
495 z / b.z); }
498 // Dot product
499 // Used to calculate angle q between two vectors among other things,
500 // as (A dot B) = |a||b|cos(q).
501 T Dot(const Vector3& b) const { return x*b.x + y*b.y + z*b.z; }
503 // Compute cross product, which generates a normal vector.
504 // Direction vector can be determined by right-hand rule: Pointing index finder in
505 // direction a and middle finger in direction b, thumb will point in a.Cross(b).
506 Vector3 Cross(const Vector3& b) const { return Vector3(y*b.z - z*b.y,
507 z*b.x - x*b.z,
508 x*b.y - y*b.x); }
510 // Returns the angle from this vector to b, in radians.
511 T Angle(const Vector3& b) const
512 {
513 T div = LengthSq()*b.LengthSq();
514 OVR_ASSERT(div != T(0));
515 T result = Acos((this->Dot(b))/sqrt(div));
516 return result;
517 }
519 // Return Length of the vector squared.
520 T LengthSq() const { return (x * x + y * y + z * z); }
522 // Return vector length.
523 T Length() const { return sqrt(LengthSq()); }
525 // Returns squared distance between two points represented by vectors.
526 T DistanceSq(Vector3 const& b) const { return (*this - b).LengthSq(); }
528 // Returns distance between two points represented by vectors.
529 T Distance(Vector3 const& b) const { return (*this - b).Length(); }
531 // Determine if this a unit vector.
532 bool IsNormalized() const { return fabs(LengthSq() - T(1)) < ((T)MATH_DOUBLE_TOLERANCE); }
534 // Normalize, convention vector length to 1.
535 void Normalize()
536 {
537 T l = Length();
538 OVR_ASSERT(l != T(0));
539 *this /= l;
540 }
542 // Returns normalized (unit) version of the vector without modifying itself.
543 Vector3 Normalized() const
544 {
545 T l = Length();
546 OVR_ASSERT(l != T(0));
547 return *this / l;
548 }
550 // Linearly interpolates from this vector to another.
551 // Factor should be between 0.0 and 1.0, with 0 giving full value to this.
552 Vector3 Lerp(const Vector3& b, T f) const { return *this*(T(1) - f) + b*f; }
554 // Projects this vector onto the argument; in other words,
555 // A.Project(B) returns projection of vector A onto B.
556 Vector3 ProjectTo(const Vector3& b) const
557 {
558 T l2 = b.LengthSq();
559 OVR_ASSERT(l2 != T(0));
560 return b * ( Dot(b) / l2 );
561 }
563 // Projects this vector onto a plane defined by a normal vector
564 Vector3 ProjectToPlane(const Vector3& normal) const { return *this - this->ProjectTo(normal); }
565 };
567 typedef Vector3<float> Vector3f;
568 typedef Vector3<double> Vector3d;
569 typedef Vector3<int32_t> Vector3i;
571 static_assert((sizeof(Vector3f) == 3*sizeof(float)), "sizeof(Vector3f) failure");
572 static_assert((sizeof(Vector3d) == 3*sizeof(double)), "sizeof(Vector3d) failure");
573 static_assert((sizeof(Vector3i) == 3*sizeof(int32_t)), "sizeof(Vector3i) failure");
575 typedef Vector3<float> Point3f;
576 typedef Vector3<double> Point3d;
577 typedef Vector3<int32_t> Point3i;
580 //-------------------------------------------------------------------------------------
581 // ***** Vector4<> - 4D vector of {x, y, z, w}
583 //
584 // Vector4f (Vector4d) represents a 3-dimensional vector or point in space,
585 // consisting of coordinates x, y, z and w.
587 template<class T>
588 class Vector4
589 {
590 public:
591 T x, y, z, w;
593 // FIXME: default initialization of a vector class can be very expensive in a full-blown
594 // application. A few hundred thousand vector constructions is not unlikely and can add
595 // up to milliseconds of time on processors like the PS3 PPU.
596 Vector4() : x(0), y(0), z(0), w(0) { }
597 Vector4(T x_, T y_, T z_, T w_) : x(x_), y(y_), z(z_), w(w_) { }
598 explicit Vector4(T s) : x(s), y(s), z(s), w(s) { }
599 explicit Vector4(const Vector3<T>& v, const float w_=1) : x(v.x), y(v.y), z(v.z), w(w_) { }
600 explicit Vector4(const Vector4<typename Math<T>::OtherFloatType> &src)
601 : x((T)src.x), y((T)src.y), z((T)src.z), w((T)src.w) { }
603 static const Vector4 ZERO;
605 // C-interop support.
606 typedef typename CompatibleTypes< Vector4<T> >::Type CompatibleType;
608 Vector4(const CompatibleType& s) : x(s.x), y(s.y), z(s.z), w(s.w) { }
610 operator const CompatibleType& () const
611 {
612 static_assert(sizeof(Vector4<T>) == sizeof(CompatibleType), "sizeof(Vector4<T>) failure");
613 return reinterpret_cast<const CompatibleType&>(*this);
614 }
616 Vector4& operator= (const Vector3<T>& other) { x=other.x; y=other.y; z=other.z; w=1; return *this; }
617 bool operator== (const Vector4& b) const { return x == b.x && y == b.y && z == b.z && w == b.w; }
618 bool operator!= (const Vector4& b) const { return x != b.x || y != b.y || z != b.z || w != b.w; }
620 Vector4 operator+ (const Vector4& b) const { return Vector4(x + b.x, y + b.y, z + b.z, w + b.w); }
621 Vector4& operator+= (const Vector4& b) { x += b.x; y += b.y; z += b.z; w += b.w; return *this; }
622 Vector4 operator- (const Vector4& b) const { return Vector4(x - b.x, y - b.y, z - b.z, w - b.w); }
623 Vector4& operator-= (const Vector4& b) { x -= b.x; y -= b.y; z -= b.z; w -= b.w; return *this; }
624 Vector4 operator- () const { return Vector4(-x, -y, -z, -w); }
626 // Scalar multiplication/division scales vector.
627 Vector4 operator* (T s) const { return Vector4(x*s, y*s, z*s, w*s); }
628 Vector4& operator*= (T s) { x *= s; y *= s; z *= s; w *= s;return *this; }
630 Vector4 operator/ (T s) const { T rcp = T(1)/s;
631 return Vector4(x*rcp, y*rcp, z*rcp, w*rcp); }
632 Vector4& operator/= (T s) { T rcp = T(1)/s;
633 x *= rcp; y *= rcp; z *= rcp; w *= rcp;
634 return *this; }
636 static Vector4 Min(const Vector4& a, const Vector4& b)
637 {
638 return Vector4((a.x < b.x) ? a.x : b.x,
639 (a.y < b.y) ? a.y : b.y,
640 (a.z < b.z) ? a.z : b.z,
641 (a.w < b.w) ? a.w : b.w);
642 }
643 static Vector4 Max(const Vector4& a, const Vector4& b)
644 {
645 return Vector4((a.x > b.x) ? a.x : b.x,
646 (a.y > b.y) ? a.y : b.y,
647 (a.z > b.z) ? a.z : b.z,
648 (a.w > b.w) ? a.w : b.w);
649 }
651 // Compare two vectors for equality with tolerance. Returns true if vectors match withing tolerance.
652 bool Compare(const Vector4&b, T tolerance = ((T)MATH_DOUBLE_TOLERANCE))
653 {
654 return (fabs(b.x-x) < tolerance) &&
655 (fabs(b.y-y) < tolerance) &&
656 (fabs(b.z-z) < tolerance) &&
657 (fabs(b.w-w) < tolerance);
658 }
660 T& operator[] (int idx)
661 {
662 OVR_ASSERT(0 <= idx && idx < 4);
663 return *(&x + idx);
664 }
666 const T& operator[] (int idx) const
667 {
668 OVR_ASSERT(0 <= idx && idx < 4);
669 return *(&x + idx);
670 }
672 // Entry wise product of two vectors
673 Vector4 EntrywiseMultiply(const Vector4& b) const { return Vector4(x * b.x,
674 y * b.y,
675 z * b.z);}
677 // Multiply and divide operators do entry-wise math
678 Vector4 operator* (const Vector4& b) const { return Vector4(x * b.x,
679 y * b.y,
680 z * b.z,
681 w * b.w); }
683 Vector4 operator/ (const Vector4& b) const { return Vector4(x / b.x,
684 y / b.y,
685 z / b.z,
686 w / b.w); }
689 // Dot product
690 T Dot(const Vector4& b) const { return x*b.x + y*b.y + z*b.z + w*b.w; }
692 // Return Length of the vector squared.
693 T LengthSq() const { return (x * x + y * y + z * z + w * w); }
695 // Return vector length.
696 T Length() const { return sqrt(LengthSq()); }
698 // Determine if this a unit vector.
699 bool IsNormalized() const { return fabs(LengthSq() - T(1)) < Math<T>::Tolerance; }
701 // Normalize, convention vector length to 1.
702 void Normalize()
703 {
704 T l = Length();
705 OVR_ASSERT(l != T(0));
706 *this /= l;
707 }
709 // Returns normalized (unit) version of the vector without modifying itself.
710 Vector4 Normalized() const
711 {
712 T l = Length();
713 OVR_ASSERT(l != T(0));
714 return *this / l;
715 }
716 };
718 typedef Vector4<float> Vector4f;
719 typedef Vector4<double> Vector4d;
720 typedef Vector4<int> Vector4i;
723 //-------------------------------------------------------------------------------------
724 // ***** Bounds3
726 // Bounds class used to describe a 3D axis aligned bounding box.
728 template<class T>
729 class Bounds3
730 {
731 public:
732 Vector3<T> b[2];
734 Bounds3()
735 {
736 }
738 Bounds3( const Vector3<T> & mins, const Vector3<T> & maxs )
739 {
740 b[0] = mins;
741 b[1] = maxs;
742 }
744 void Clear()
745 {
746 b[0].x = b[0].y = b[0].z = Math<T>::MaxValue;
747 b[1].x = b[1].y = b[1].z = -Math<T>::MaxValue;
748 }
750 void AddPoint( const Vector3<T> & v )
751 {
752 b[0].x = Alg::Min( b[0].x, v.x );
753 b[0].y = Alg::Min( b[0].y, v.y );
754 b[0].z = Alg::Min( b[0].z, v.z );
755 b[1].x = Alg::Max( b[1].x, v.x );
756 b[1].y = Alg::Max( b[1].y, v.y );
757 b[1].z = Alg::Max( b[1].z, v.z );
758 }
760 const Vector3<T> & GetMins() const { return b[0]; }
761 const Vector3<T> & GetMaxs() const { return b[1]; }
763 Vector3<T> & GetMins() { return b[0]; }
764 Vector3<T> & GetMaxs() { return b[1]; }
765 };
767 typedef Bounds3<float> Bounds3f;
768 typedef Bounds3<double> Bounds3d;
771 //-------------------------------------------------------------------------------------
772 // ***** Size
774 // Size class represents 2D size with Width, Height components.
775 // Used to describe distentions of render targets, etc.
777 template<class T>
778 class Size
779 {
780 public:
781 T w, h;
783 Size() : w(0), h(0) { }
784 Size(T w_, T h_) : w(w_), h(h_) { }
785 explicit Size(T s) : w(s), h(s) { }
786 explicit Size(const Size<typename Math<T>::OtherFloatType> &src)
787 : w((T)src.w), h((T)src.h) { }
789 // C-interop support.
790 typedef typename CompatibleTypes<Size<T> >::Type CompatibleType;
792 Size(const CompatibleType& s) : w(s.w), h(s.h) { }
794 operator const CompatibleType& () const
795 {
796 static_assert(sizeof(Size<T>) == sizeof(CompatibleType), "sizeof(Size<T>) failure");
797 return reinterpret_cast<const CompatibleType&>(*this);
798 }
800 bool operator== (const Size& b) const { return w == b.w && h == b.h; }
801 bool operator!= (const Size& b) const { return w != b.w || h != b.h; }
803 Size operator+ (const Size& b) const { return Size(w + b.w, h + b.h); }
804 Size& operator+= (const Size& b) { w += b.w; h += b.h; return *this; }
805 Size operator- (const Size& b) const { return Size(w - b.w, h - b.h); }
806 Size& operator-= (const Size& b) { w -= b.w; h -= b.h; return *this; }
807 Size operator- () const { return Size(-w, -h); }
808 Size operator* (const Size& b) const { return Size(w * b.w, h * b.h); }
809 Size& operator*= (const Size& b) { w *= b.w; h *= b.h; return *this; }
810 Size operator/ (const Size& b) const { return Size(w / b.w, h / b.h); }
811 Size& operator/= (const Size& b) { w /= b.w; h /= b.h; return *this; }
813 // Scalar multiplication/division scales both components.
814 Size operator* (T s) const { return Size(w*s, h*s); }
815 Size& operator*= (T s) { w *= s; h *= s; return *this; }
816 Size operator/ (T s) const { return Size(w/s, h/s); }
817 Size& operator/= (T s) { w /= s; h /= s; return *this; }
819 static Size Min(const Size& a, const Size& b) { return Size((a.w < b.w) ? a.w : b.w,
820 (a.h < b.h) ? a.h : b.h); }
821 static Size Max(const Size& a, const Size& b) { return Size((a.w > b.w) ? a.w : b.w,
822 (a.h > b.h) ? a.h : b.h); }
824 T Area() const { return w * h; }
826 inline Vector2<T> ToVector() const { return Vector2<T>(w, h); }
827 };
830 typedef Size<int> Sizei;
831 typedef Size<unsigned> Sizeu;
832 typedef Size<float> Sizef;
833 typedef Size<double> Sized;
837 //-----------------------------------------------------------------------------------
838 // ***** Rect
840 // Rect describes a rectangular area for rendering, that includes position and size.
841 template<class T>
842 class Rect
843 {
844 public:
845 T x, y;
846 T w, h;
848 Rect() { }
849 Rect(T x1, T y1, T w1, T h1) : x(x1), y(y1), w(w1), h(h1) { }
850 Rect(const Vector2<T>& pos, const Size<T>& sz) : x(pos.x), y(pos.y), w(sz.w), h(sz.h) { }
851 Rect(const Size<T>& sz) : x(0), y(0), w(sz.w), h(sz.h) { }
853 // C-interop support.
854 typedef typename CompatibleTypes<Rect<T> >::Type CompatibleType;
856 Rect(const CompatibleType& s) : x(s.Pos.x), y(s.Pos.y), w(s.Size.w), h(s.Size.h) { }
858 operator const CompatibleType& () const
859 {
860 static_assert(sizeof(Rect<T>) == sizeof(CompatibleType), "sizeof(Rect<T>) failure");
861 return reinterpret_cast<const CompatibleType&>(*this);
862 }
864 Vector2<T> GetPos() const { return Vector2<T>(x, y); }
865 Size<T> GetSize() const { return Size<T>(w, h); }
866 void SetPos(const Vector2<T>& pos) { x = pos.x; y = pos.y; }
867 void SetSize(const Size<T>& sz) { w = sz.w; h = sz.h; }
869 bool operator == (const Rect& vp) const
870 { return (x == vp.x) && (y == vp.y) && (w == vp.w) && (h == vp.h); }
871 bool operator != (const Rect& vp) const
872 { return !operator == (vp); }
873 };
875 typedef Rect<int> Recti;
878 //-------------------------------------------------------------------------------------//
879 // ***** Quat
880 //
881 // Quatf represents a quaternion class used for rotations.
882 //
883 // Quaternion multiplications are done in right-to-left order, to match the
884 // behavior of matrices.
887 template<class T>
888 class Quat
889 {
890 public:
891 // w + Xi + Yj + Zk
892 T x, y, z, w;
894 Quat() : x(0), y(0), z(0), w(1) { }
895 Quat(T x_, T y_, T z_, T w_) : x(x_), y(y_), z(z_), w(w_) { }
896 explicit Quat(const Quat<typename Math<T>::OtherFloatType> &src)
897 : x((T)src.x), y((T)src.y), z((T)src.z), w((T)src.w) { }
899 typedef typename CompatibleTypes<Quat<T> >::Type CompatibleType;
901 // C-interop support.
902 Quat(const CompatibleType& s) : x(s.x), y(s.y), z(s.z), w(s.w) { }
904 operator CompatibleType () const
905 {
906 CompatibleType result;
907 result.x = x;
908 result.y = y;
909 result.z = z;
910 result.w = w;
911 return result;
912 }
914 // Constructs quaternion for rotation around the axis by an angle.
915 Quat(const Vector3<T>& axis, T angle)
916 {
917 // Make sure we don't divide by zero.
918 if (axis.LengthSq() == 0)
919 {
920 // Assert if the axis is zero, but the angle isn't
921 OVR_ASSERT(angle == 0);
922 x = 0; y = 0; z = 0; w = 1;
923 return;
924 }
926 Vector3<T> unitAxis = axis.Normalized();
927 T sinHalfAngle = sin(angle * T(0.5));
929 w = cos(angle * T(0.5));
930 x = unitAxis.x * sinHalfAngle;
931 y = unitAxis.y * sinHalfAngle;
932 z = unitAxis.z * sinHalfAngle;
933 }
935 // Constructs quaternion for rotation around one of the coordinate axis by an angle.
936 Quat(Axis A, T angle, RotateDirection d = Rotate_CCW, HandedSystem s = Handed_R)
937 {
938 T sinHalfAngle = s * d *sin(angle * T(0.5));
939 T v[3];
940 v[0] = v[1] = v[2] = T(0);
941 v[A] = sinHalfAngle;
943 w = cos(angle * T(0.5));
944 x = v[0];
945 y = v[1];
946 z = v[2];
947 }
949 // Compute axis and angle from quaternion
950 void GetAxisAngle(Vector3<T>* axis, T* angle) const
951 {
952 if ( x*x + y*y + z*z > ((T)MATH_DOUBLE_TOLERANCE) * ((T)MATH_DOUBLE_TOLERANCE) ) {
953 *axis = Vector3<T>(x, y, z).Normalized();
954 *angle = 2 * Acos(w);
955 if (*angle > ((T)MATH_DOUBLE_PI)) // Reduce the magnitude of the angle, if necessary
956 {
957 *angle = ((T)MATH_DOUBLE_TWOPI) - *angle;
958 *axis = *axis * (-1);
959 }
960 }
961 else
962 {
963 *axis = Vector3<T>(1, 0, 0);
964 *angle= 0;
965 }
966 }
968 // Constructs the quaternion from a rotation matrix
969 explicit Quat(const Matrix4<T>& m)
970 {
971 T trace = m.M[0][0] + m.M[1][1] + m.M[2][2];
973 // In almost all cases, the first part is executed.
974 // However, if the trace is not positive, the other
975 // cases arise.
976 if (trace > T(0))
977 {
978 T s = sqrt(trace + T(1)) * T(2); // s=4*qw
979 w = T(0.25) * s;
980 x = (m.M[2][1] - m.M[1][2]) / s;
981 y = (m.M[0][2] - m.M[2][0]) / s;
982 z = (m.M[1][0] - m.M[0][1]) / s;
983 }
984 else if ((m.M[0][0] > m.M[1][1])&&(m.M[0][0] > m.M[2][2]))
985 {
986 T s = sqrt(T(1) + m.M[0][0] - m.M[1][1] - m.M[2][2]) * T(2);
987 w = (m.M[2][1] - m.M[1][2]) / s;
988 x = T(0.25) * s;
989 y = (m.M[0][1] + m.M[1][0]) / s;
990 z = (m.M[2][0] + m.M[0][2]) / s;
991 }
992 else if (m.M[1][1] > m.M[2][2])
993 {
994 T s = sqrt(T(1) + m.M[1][1] - m.M[0][0] - m.M[2][2]) * T(2); // S=4*qy
995 w = (m.M[0][2] - m.M[2][0]) / s;
996 x = (m.M[0][1] + m.M[1][0]) / s;
997 y = T(0.25) * s;
998 z = (m.M[1][2] + m.M[2][1]) / s;
999 }
1000 else
1001 {
1002 T s = sqrt(T(1) + m.M[2][2] - m.M[0][0] - m.M[1][1]) * T(2); // S=4*qz
1003 w = (m.M[1][0] - m.M[0][1]) / s;
1004 x = (m.M[0][2] + m.M[2][0]) / s;
1005 y = (m.M[1][2] + m.M[2][1]) / s;
1006 z = T(0.25) * s;
1007 }
1008 }
1010 // Constructs the quaternion from a rotation matrix
1011 explicit Quat(const Matrix3<T>& m)
1012 {
1013 T trace = m.M[0][0] + m.M[1][1] + m.M[2][2];
1015 // In almost all cases, the first part is executed.
1016 // However, if the trace is not positive, the other
1017 // cases arise.
1018 if (trace > T(0))
1019 {
1020 T s = sqrt(trace + T(1)) * T(2); // s=4*qw
1021 w = T(0.25) * s;
1022 x = (m.M[2][1] - m.M[1][2]) / s;
1023 y = (m.M[0][2] - m.M[2][0]) / s;
1024 z = (m.M[1][0] - m.M[0][1]) / s;
1025 }
1026 else if ((m.M[0][0] > m.M[1][1])&&(m.M[0][0] > m.M[2][2]))
1027 {
1028 T s = sqrt(T(1) + m.M[0][0] - m.M[1][1] - m.M[2][2]) * T(2);
1029 w = (m.M[2][1] - m.M[1][2]) / s;
1030 x = T(0.25) * s;
1031 y = (m.M[0][1] + m.M[1][0]) / s;
1032 z = (m.M[2][0] + m.M[0][2]) / s;
1033 }
1034 else if (m.M[1][1] > m.M[2][2])
1035 {
1036 T s = sqrt(T(1) + m.M[1][1] - m.M[0][0] - m.M[2][2]) * T(2); // S=4*qy
1037 w = (m.M[0][2] - m.M[2][0]) / s;
1038 x = (m.M[0][1] + m.M[1][0]) / s;
1039 y = T(0.25) * s;
1040 z = (m.M[1][2] + m.M[2][1]) / s;
1041 }
1042 else
1043 {
1044 T s = sqrt(T(1) + m.M[2][2] - m.M[0][0] - m.M[1][1]) * T(2); // S=4*qz
1045 w = (m.M[1][0] - m.M[0][1]) / s;
1046 x = (m.M[0][2] + m.M[2][0]) / s;
1047 y = (m.M[1][2] + m.M[2][1]) / s;
1048 z = T(0.25) * s;
1049 }
1050 }
1052 bool operator== (const Quat& b) const { return x == b.x && y == b.y && z == b.z && w == b.w; }
1053 bool operator!= (const Quat& b) const { return x != b.x || y != b.y || z != b.z || w != b.w; }
1055 Quat operator+ (const Quat& b) const { return Quat(x + b.x, y + b.y, z + b.z, w + b.w); }
1056 Quat& operator+= (const Quat& b) { w += b.w; x += b.x; y += b.y; z += b.z; return *this; }
1057 Quat operator- (const Quat& b) const { return Quat(x - b.x, y - b.y, z - b.z, w - b.w); }
1058 Quat& operator-= (const Quat& b) { w -= b.w; x -= b.x; y -= b.y; z -= b.z; return *this; }
1060 Quat operator* (T s) const { return Quat(x * s, y * s, z * s, w * s); }
1061 Quat& operator*= (T s) { w *= s; x *= s; y *= s; z *= s; return *this; }
1062 Quat operator/ (T s) const { T rcp = T(1)/s; return Quat(x * rcp, y * rcp, z * rcp, w *rcp); }
1063 Quat& operator/= (T s) { T rcp = T(1)/s; w *= rcp; x *= rcp; y *= rcp; z *= rcp; return *this; }
1066 // Get Imaginary part vector
1067 Vector3<T> Imag() const { return Vector3<T>(x,y,z); }
1069 // Get quaternion length.
1070 T Length() const { return sqrt(LengthSq()); }
1072 // Get quaternion length squared.
1073 T LengthSq() const { return (x * x + y * y + z * z + w * w); }
1075 // Simple Euclidean distance in R^4 (not SLERP distance, but at least respects Haar measure)
1076 T Distance(const Quat& q) const
1077 {
1078 T d1 = (*this - q).Length();
1079 T d2 = (*this + q).Length(); // Antipodal point check
1080 return (d1 < d2) ? d1 : d2;
1081 }
1083 T DistanceSq(const Quat& q) const
1084 {
1085 T d1 = (*this - q).LengthSq();
1086 T d2 = (*this + q).LengthSq(); // Antipodal point check
1087 return (d1 < d2) ? d1 : d2;
1088 }
1090 T Dot(const Quat& q) const
1091 {
1092 return x * q.x + y * q.y + z * q.z + w * q.w;
1093 }
1095 // Angle between two quaternions in radians
1096 T Angle(const Quat& q) const
1097 {
1098 return 2 * Acos(Alg::Abs(Dot(q)));
1099 }
1101 // Normalize
1102 bool IsNormalized() const { return fabs(LengthSq() - T(1)) < ((T)MATH_DOUBLE_TOLERANCE); }
1104 void Normalize()
1105 {
1106 T l = Length();
1107 OVR_ASSERT(l != T(0));
1108 *this /= l;
1109 }
1111 Quat Normalized() const
1112 {
1113 T l = Length();
1114 OVR_ASSERT(l != T(0));
1115 return *this / l;
1116 }
1118 // Returns conjugate of the quaternion. Produces inverse rotation if quaternion is normalized.
1119 Quat Conj() const { return Quat(-x, -y, -z, w); }
1121 // Quaternion multiplication. Combines quaternion rotations, performing the one on the
1122 // right hand side first.
1123 Quat operator* (const Quat& b) const { return Quat(w * b.x + x * b.w + y * b.z - z * b.y,
1124 w * b.y - x * b.z + y * b.w + z * b.x,
1125 w * b.z + x * b.y - y * b.x + z * b.w,
1126 w * b.w - x * b.x - y * b.y - z * b.z); }
1128 //
1129 // this^p normalized; same as rotating by this p times.
1130 Quat PowNormalized(T p) const
1131 {
1132 Vector3<T> v;
1133 T a;
1134 GetAxisAngle(&v, &a);
1135 return Quat(v, a * p);
1136 }
1138 // Normalized linear interpolation of quaternions
1139 Quat Nlerp(const Quat& other, T a)
1140 {
1141 T sign = (Dot(other) >= 0) ? 1 : -1;
1142 return (*this * sign * a + other * (1-a)).Normalized();
1143 }
1145 // Rotate transforms vector in a manner that matches Matrix rotations (counter-clockwise,
1146 // assuming negative direction of the axis). Standard formula: q(t) * V * q(t)^-1.
1147 Vector3<T> Rotate(const Vector3<T>& v) const
1148 {
1149 return ((*this * Quat<T>(v.x, v.y, v.z, T(0))) * Inverted()).Imag();
1150 }
1152 // Inversed quaternion rotates in the opposite direction.
1153 Quat Inverted() const
1154 {
1155 return Quat(-x, -y, -z, w);
1156 }
1158 // Sets this quaternion to the one rotates in the opposite direction.
1159 void Invert()
1160 {
1161 *this = Quat(-x, -y, -z, w);
1162 }
1164 // GetEulerAngles extracts Euler angles from the quaternion, in the specified order of
1165 // axis rotations and the specified coordinate system. Right-handed coordinate system
1166 // is the default, with CCW rotations while looking in the negative axis direction.
1167 // Here a,b,c, are the Yaw/Pitch/Roll angles to be returned.
1168 // rotation a around axis A1
1169 // is followed by rotation b around axis A2
1170 // is followed by rotation c around axis A3
1171 // rotations are CCW or CW (D) in LH or RH coordinate system (S)
1172 template <Axis A1, Axis A2, Axis A3, RotateDirection D, HandedSystem S>
1173 void GetEulerAngles(T *a, T *b, T *c) const
1174 {
1175 static_assert((A1 != A2) && (A2 != A3) && (A1 != A3), "(A1 != A2) && (A2 != A3) && (A1 != A3)");
1177 T Q[3] = { x, y, z }; //Quaternion components x,y,z
1179 T ww = w*w;
1180 T Q11 = Q[A1]*Q[A1];
1181 T Q22 = Q[A2]*Q[A2];
1182 T Q33 = Q[A3]*Q[A3];
1184 T psign = T(-1);
1185 // Determine whether even permutation
1186 if (((A1 + 1) % 3 == A2) && ((A2 + 1) % 3 == A3))
1187 psign = T(1);
1189 T s2 = psign * T(2) * (psign*w*Q[A2] + Q[A1]*Q[A3]);
1191 if (s2 < T(-1) + ((T)MATH_DOUBLE_SINGULARITYRADIUS))
1192 { // South pole singularity
1193 *a = T(0);
1194 *b = -S*D*((T)MATH_DOUBLE_PIOVER2);
1195 *c = S*D*atan2(T(2)*(psign*Q[A1]*Q[A2] + w*Q[A3]),
1196 ww + Q22 - Q11 - Q33 );
1197 }
1198 else if (s2 > T(1) - ((T)MATH_DOUBLE_SINGULARITYRADIUS))
1199 { // North pole singularity
1200 *a = T(0);
1201 *b = S*D*((T)MATH_DOUBLE_PIOVER2);
1202 *c = S*D*atan2(T(2)*(psign*Q[A1]*Q[A2] + w*Q[A3]),
1203 ww + Q22 - Q11 - Q33);
1204 }
1205 else
1206 {
1207 *a = -S*D*atan2(T(-2)*(w*Q[A1] - psign*Q[A2]*Q[A3]),
1208 ww + Q33 - Q11 - Q22);
1209 *b = S*D*asin(s2);
1210 *c = S*D*atan2(T(2)*(w*Q[A3] - psign*Q[A1]*Q[A2]),
1211 ww + Q11 - Q22 - Q33);
1212 }
1213 return;
1214 }
1216 template <Axis A1, Axis A2, Axis A3, RotateDirection D>
1217 void GetEulerAngles(T *a, T *b, T *c) const
1218 { GetEulerAngles<A1, A2, A3, D, Handed_R>(a, b, c); }
1220 template <Axis A1, Axis A2, Axis A3>
1221 void GetEulerAngles(T *a, T *b, T *c) const
1222 { GetEulerAngles<A1, A2, A3, Rotate_CCW, Handed_R>(a, b, c); }
1224 // GetEulerAnglesABA extracts Euler angles from the quaternion, in the specified order of
1225 // axis rotations and the specified coordinate system. Right-handed coordinate system
1226 // is the default, with CCW rotations while looking in the negative axis direction.
1227 // Here a,b,c, are the Yaw/Pitch/Roll angles to be returned.
1228 // rotation a around axis A1
1229 // is followed by rotation b around axis A2
1230 // is followed by rotation c around axis A1
1231 // Rotations are CCW or CW (D) in LH or RH coordinate system (S)
1232 template <Axis A1, Axis A2, RotateDirection D, HandedSystem S>
1233 void GetEulerAnglesABA(T *a, T *b, T *c) const
1234 {
1235 static_assert(A1 != A2, "A1 != A2");
1237 T Q[3] = {x, y, z}; // Quaternion components
1239 // Determine the missing axis that was not supplied
1240 int m = 3 - A1 - A2;
1242 T ww = w*w;
1243 T Q11 = Q[A1]*Q[A1];
1244 T Q22 = Q[A2]*Q[A2];
1245 T Qmm = Q[m]*Q[m];
1247 T psign = T(-1);
1248 if ((A1 + 1) % 3 == A2) // Determine whether even permutation
1249 {
1250 psign = T(1);
1251 }
1253 T c2 = ww + Q11 - Q22 - Qmm;
1254 if (c2 < T(-1) + Math<T>::SingularityRadius)
1255 { // South pole singularity
1256 *a = T(0);
1257 *b = S*D*((T)MATH_DOUBLE_PI);
1258 *c = S*D*atan2( T(2)*(w*Q[A1] - psign*Q[A2]*Q[m]),
1259 ww + Q22 - Q11 - Qmm);
1260 }
1261 else if (c2 > T(1) - Math<T>::SingularityRadius)
1262 { // North pole singularity
1263 *a = T(0);
1264 *b = T(0);
1265 *c = S*D*atan2( T(2)*(w*Q[A1] - psign*Q[A2]*Q[m]),
1266 ww + Q22 - Q11 - Qmm);
1267 }
1268 else
1269 {
1270 *a = S*D*atan2( psign*w*Q[m] + Q[A1]*Q[A2],
1271 w*Q[A2] -psign*Q[A1]*Q[m]);
1272 *b = S*D*acos(c2);
1273 *c = S*D*atan2( -psign*w*Q[m] + Q[A1]*Q[A2],
1274 w*Q[A2] + psign*Q[A1]*Q[m]);
1275 }
1276 return;
1277 }
1278 };
1280 typedef Quat<float> Quatf;
1281 typedef Quat<double> Quatd;
1283 static_assert((sizeof(Quatf) == 4*sizeof(float)), "sizeof(Quatf) failure");
1284 static_assert((sizeof(Quatd) == 4*sizeof(double)), "sizeof(Quatd) failure");
1286 //-------------------------------------------------------------------------------------
1287 // ***** Pose
1289 // Position and orientation combined.
1291 template<class T>
1292 class Pose
1293 {
1294 public:
1295 typedef typename CompatibleTypes<Pose<T> >::Type CompatibleType;
1297 Pose() { }
1298 Pose(const Quat<T>& orientation, const Vector3<T>& pos)
1299 : Rotation(orientation), Translation(pos) { }
1300 Pose(const Pose& s)
1301 : Rotation(s.Rotation), Translation(s.Translation) { }
1302 Pose(const CompatibleType& s)
1303 : Rotation(s.Orientation), Translation(s.Position) { }
1304 explicit Pose(const Pose<typename Math<T>::OtherFloatType> &s)
1305 : Rotation(s.Rotation), Translation(s.Translation) { }
1307 operator typename CompatibleTypes<Pose<T> >::Type () const
1308 {
1309 typename CompatibleTypes<Pose<T> >::Type result;
1310 result.Orientation = Rotation;
1311 result.Position = Translation;
1312 return result;
1313 }
1315 Quat<T> Rotation;
1316 Vector3<T> Translation;
1318 static_assert((sizeof(T) == sizeof(double) || sizeof(T) == sizeof(float)), "(sizeof(T) == sizeof(double) || sizeof(T) == sizeof(float))");
1320 void ToArray(T* arr) const
1321 {
1322 T temp[7] = { Rotation.x, Rotation.y, Rotation.z, Rotation.w, Translation.x, Translation.y, Translation.z };
1323 for (int i = 0; i < 7; i++) arr[i] = temp[i];
1324 }
1326 static Pose<T> FromArray(const T* v)
1327 {
1328 Quat<T> rotation(v[0], v[1], v[2], v[3]);
1329 Vector3<T> translation(v[4], v[5], v[6]);
1330 return Pose<T>(rotation, translation);
1331 }
1333 Vector3<T> Rotate(const Vector3<T>& v) const
1334 {
1335 return Rotation.Rotate(v);
1336 }
1338 Vector3<T> Translate(const Vector3<T>& v) const
1339 {
1340 return v + Translation;
1341 }
1343 Vector3<T> Apply(const Vector3<T>& v) const
1344 {
1345 return Translate(Rotate(v));
1346 }
1348 Pose operator*(const Pose& other) const
1349 {
1350 return Pose(Rotation * other.Rotation, Apply(other.Translation));
1351 }
1353 Pose Inverted() const
1354 {
1355 Quat<T> inv = Rotation.Inverted();
1356 return Pose(inv, inv.Rotate(-Translation));
1357 }
1358 };
1360 typedef Pose<float> Posef;
1361 typedef Pose<double> Posed;
1363 static_assert((sizeof(Posed) == sizeof(Quatd) + sizeof(Vector3d)), "sizeof(Posed) failure");
1364 static_assert((sizeof(Posef) == sizeof(Quatf) + sizeof(Vector3f)), "sizeof(Posef) failure");
1367 //-------------------------------------------------------------------------------------
1368 // ***** Matrix4
1369 //
1370 // Matrix4 is a 4x4 matrix used for 3d transformations and projections.
1371 // Translation stored in the last column.
1372 // The matrix is stored in row-major order in memory, meaning that values
1373 // of the first row are stored before the next one.
1374 //
1375 // The arrangement of the matrix is chosen to be in Right-Handed
1376 // coordinate system and counterclockwise rotations when looking down
1377 // the axis
1378 //
1379 // Transformation Order:
1380 // - Transformations are applied from right to left, so the expression
1381 // M1 * M2 * M3 * V means that the vector V is transformed by M3 first,
1382 // followed by M2 and M1.
1383 //
1384 // Coordinate system: Right Handed
1385 //
1386 // Rotations: Counterclockwise when looking down the axis. All angles are in radians.
1387 //
1388 // | sx 01 02 tx | // First column (sx, 10, 20): Axis X basis vector.
1389 // | 10 sy 12 ty | // Second column (01, sy, 21): Axis Y basis vector.
1390 // | 20 21 sz tz | // Third columnt (02, 12, sz): Axis Z basis vector.
1391 // | 30 31 32 33 |
1392 //
1393 // The basis vectors are first three columns.
1395 template<class T>
1396 class Matrix4
1397 {
1398 static const Matrix4 IdentityValue;
1400 public:
1401 T M[4][4];
1403 enum NoInitType { NoInit };
1405 // Construct with no memory initialization.
1406 Matrix4(NoInitType) { }
1408 // By default, we construct identity matrix.
1409 Matrix4()
1410 {
1411 SetIdentity();
1412 }
1414 Matrix4(T m11, T m12, T m13, T m14,
1415 T m21, T m22, T m23, T m24,
1416 T m31, T m32, T m33, T m34,
1417 T m41, T m42, T m43, T m44)
1418 {
1419 M[0][0] = m11; M[0][1] = m12; M[0][2] = m13; M[0][3] = m14;
1420 M[1][0] = m21; M[1][1] = m22; M[1][2] = m23; M[1][3] = m24;
1421 M[2][0] = m31; M[2][1] = m32; M[2][2] = m33; M[2][3] = m34;
1422 M[3][0] = m41; M[3][1] = m42; M[3][2] = m43; M[3][3] = m44;
1423 }
1425 Matrix4(T m11, T m12, T m13,
1426 T m21, T m22, T m23,
1427 T m31, T m32, T m33)
1428 {
1429 M[0][0] = m11; M[0][1] = m12; M[0][2] = m13; M[0][3] = 0;
1430 M[1][0] = m21; M[1][1] = m22; M[1][2] = m23; M[1][3] = 0;
1431 M[2][0] = m31; M[2][1] = m32; M[2][2] = m33; M[2][3] = 0;
1432 M[3][0] = 0; M[3][1] = 0; M[3][2] = 0; M[3][3] = 1;
1433 }
1435 explicit Matrix4(const Quat<T>& q)
1436 {
1437 T ww = q.w*q.w;
1438 T xx = q.x*q.x;
1439 T yy = q.y*q.y;
1440 T zz = q.z*q.z;
1442 M[0][0] = ww + xx - yy - zz; M[0][1] = 2 * (q.x*q.y - q.w*q.z); M[0][2] = 2 * (q.x*q.z + q.w*q.y); M[0][3] = 0;
1443 M[1][0] = 2 * (q.x*q.y + q.w*q.z); M[1][1] = ww - xx + yy - zz; M[1][2] = 2 * (q.y*q.z - q.w*q.x); M[1][3] = 0;
1444 M[2][0] = 2 * (q.x*q.z - q.w*q.y); M[2][1] = 2 * (q.y*q.z + q.w*q.x); M[2][2] = ww - xx - yy + zz; M[2][3] = 0;
1445 M[3][0] = 0; M[3][1] = 0; M[3][2] = 0; M[3][3] = 1;
1446 }
1448 explicit Matrix4(const Pose<T>& p)
1449 {
1450 Matrix4 result(p.Rotation);
1451 result.SetTranslation(p.Translation);
1452 *this = result;
1453 }
1455 // C-interop support
1456 explicit Matrix4(const Matrix4<typename Math<T>::OtherFloatType> &src)
1457 {
1458 for (int i = 0; i < 4; i++)
1459 for (int j = 0; j < 4; j++)
1460 M[i][j] = (T)src.M[i][j];
1461 }
1463 // C-interop support.
1464 Matrix4(const typename CompatibleTypes<Matrix4<T> >::Type& s)
1465 {
1466 static_assert(sizeof(s) == sizeof(Matrix4), "sizeof(s) == sizeof(Matrix4)");
1467 memcpy(M, s.M, sizeof(M));
1468 }
1470 operator typename CompatibleTypes<Matrix4<T> >::Type () const
1471 {
1472 typename CompatibleTypes<Matrix4<T> >::Type result;
1473 static_assert(sizeof(result) == sizeof(Matrix4), "sizeof(result) == sizeof(Matrix4)");
1474 memcpy(result.M, M, sizeof(M));
1475 return result;
1476 }
1478 void ToString(char* dest, size_t destsize) const
1479 {
1480 size_t pos = 0;
1481 for (int r=0; r<4; r++)
1482 for (int c=0; c<4; c++)
1483 pos += OVR_sprintf(dest+pos, destsize-pos, "%g ", M[r][c]);
1484 }
1486 static Matrix4 FromString(const char* src)
1487 {
1488 Matrix4 result;
1489 if (src)
1490 {
1491 for (int r=0; r<4; r++)
1492 {
1493 for (int c=0; c<4; c++)
1494 {
1495 result.M[r][c] = (T)atof(src);
1496 while (src && *src != ' ')
1497 {
1498 src++;
1499 }
1500 while (src && *src == ' ')
1501 {
1502 src++;
1503 }
1504 }
1505 }
1506 }
1507 return result;
1508 }
1510 static const Matrix4& Identity() { return IdentityValue; }
1512 void SetIdentity()
1513 {
1514 M[0][0] = M[1][1] = M[2][2] = M[3][3] = 1;
1515 M[0][1] = M[1][0] = M[2][3] = M[3][1] = 0;
1516 M[0][2] = M[1][2] = M[2][0] = M[3][2] = 0;
1517 M[0][3] = M[1][3] = M[2][1] = M[3][0] = 0;
1518 }
1520 void SetXBasis(const Vector3f & v)
1521 {
1522 M[0][0] = v.x;
1523 M[1][0] = v.y;
1524 M[2][0] = v.z;
1525 }
1526 Vector3f GetXBasis() const
1527 {
1528 return Vector3f(M[0][0], M[1][0], M[2][0]);
1529 }
1531 void SetYBasis(const Vector3f & v)
1532 {
1533 M[0][1] = v.x;
1534 M[1][1] = v.y;
1535 M[2][1] = v.z;
1536 }
1537 Vector3f GetYBasis() const
1538 {
1539 return Vector3f(M[0][1], M[1][1], M[2][1]);
1540 }
1542 void SetZBasis(const Vector3f & v)
1543 {
1544 M[0][2] = v.x;
1545 M[1][2] = v.y;
1546 M[2][2] = v.z;
1547 }
1548 Vector3f GetZBasis() const
1549 {
1550 return Vector3f(M[0][2], M[1][2], M[2][2]);
1551 }
1553 bool operator== (const Matrix4& b) const
1554 {
1555 bool isEqual = true;
1556 for (int i = 0; i < 4; i++)
1557 for (int j = 0; j < 4; j++)
1558 isEqual &= (M[i][j] == b.M[i][j]);
1560 return isEqual;
1561 }
1563 Matrix4 operator+ (const Matrix4& b) const
1564 {
1565 Matrix4 result(*this);
1566 result += b;
1567 return result;
1568 }
1570 Matrix4& operator+= (const Matrix4& b)
1571 {
1572 for (int i = 0; i < 4; i++)
1573 for (int j = 0; j < 4; j++)
1574 M[i][j] += b.M[i][j];
1575 return *this;
1576 }
1578 Matrix4 operator- (const Matrix4& b) const
1579 {
1580 Matrix4 result(*this);
1581 result -= b;
1582 return result;
1583 }
1585 Matrix4& operator-= (const Matrix4& b)
1586 {
1587 for (int i = 0; i < 4; i++)
1588 for (int j = 0; j < 4; j++)
1589 M[i][j] -= b.M[i][j];
1590 return *this;
1591 }
1593 // Multiplies two matrices into destination with minimum copying.
1594 static Matrix4& Multiply(Matrix4* d, const Matrix4& a, const Matrix4& b)
1595 {
1596 OVR_ASSERT((d != &a) && (d != &b));
1597 int i = 0;
1598 do {
1599 d->M[i][0] = a.M[i][0] * b.M[0][0] + a.M[i][1] * b.M[1][0] + a.M[i][2] * b.M[2][0] + a.M[i][3] * b.M[3][0];
1600 d->M[i][1] = a.M[i][0] * b.M[0][1] + a.M[i][1] * b.M[1][1] + a.M[i][2] * b.M[2][1] + a.M[i][3] * b.M[3][1];
1601 d->M[i][2] = a.M[i][0] * b.M[0][2] + a.M[i][1] * b.M[1][2] + a.M[i][2] * b.M[2][2] + a.M[i][3] * b.M[3][2];
1602 d->M[i][3] = a.M[i][0] * b.M[0][3] + a.M[i][1] * b.M[1][3] + a.M[i][2] * b.M[2][3] + a.M[i][3] * b.M[3][3];
1603 } while((++i) < 4);
1605 return *d;
1606 }
1608 Matrix4 operator* (const Matrix4& b) const
1609 {
1610 Matrix4 result(Matrix4::NoInit);
1611 Multiply(&result, *this, b);
1612 return result;
1613 }
1615 Matrix4& operator*= (const Matrix4& b)
1616 {
1617 return Multiply(this, Matrix4(*this), b);
1618 }
1620 Matrix4 operator* (T s) const
1621 {
1622 Matrix4 result(*this);
1623 result *= s;
1624 return result;
1625 }
1627 Matrix4& operator*= (T s)
1628 {
1629 for (int i = 0; i < 4; i++)
1630 for (int j = 0; j < 4; j++)
1631 M[i][j] *= s;
1632 return *this;
1633 }
1636 Matrix4 operator/ (T s) const
1637 {
1638 Matrix4 result(*this);
1639 result /= s;
1640 return result;
1641 }
1643 Matrix4& operator/= (T s)
1644 {
1645 for (int i = 0; i < 4; i++)
1646 for (int j = 0; j < 4; j++)
1647 M[i][j] /= s;
1648 return *this;
1649 }
1651 Vector3<T> Transform(const Vector3<T>& v) const
1652 {
1653 const T rcpW = 1.0f / (M[3][0] * v.x + M[3][1] * v.y + M[3][2] * v.z + M[3][3]);
1654 return Vector3<T>((M[0][0] * v.x + M[0][1] * v.y + M[0][2] * v.z + M[0][3]) * rcpW,
1655 (M[1][0] * v.x + M[1][1] * v.y + M[1][2] * v.z + M[1][3]) * rcpW,
1656 (M[2][0] * v.x + M[2][1] * v.y + M[2][2] * v.z + M[2][3]) * rcpW);
1657 }
1659 Vector4<T> Transform(const Vector4<T>& v) const
1660 {
1661 return Vector4<T>(M[0][0] * v.x + M[0][1] * v.y + M[0][2] * v.z + M[0][3] * v.w,
1662 M[1][0] * v.x + M[1][1] * v.y + M[1][2] * v.z + M[1][3] * v.w,
1663 M[2][0] * v.x + M[2][1] * v.y + M[2][2] * v.z + M[2][3] * v.w,
1664 M[3][0] * v.x + M[3][1] * v.y + M[3][2] * v.z + M[3][3] * v.w);
1665 }
1667 Matrix4 Transposed() const
1668 {
1669 return Matrix4(M[0][0], M[1][0], M[2][0], M[3][0],
1670 M[0][1], M[1][1], M[2][1], M[3][1],
1671 M[0][2], M[1][2], M[2][2], M[3][2],
1672 M[0][3], M[1][3], M[2][3], M[3][3]);
1673 }
1675 void Transpose()
1676 {
1677 *this = Transposed();
1678 }
1681 T SubDet (const size_t* rows, const size_t* cols) const
1682 {
1683 return M[rows[0]][cols[0]] * (M[rows[1]][cols[1]] * M[rows[2]][cols[2]] - M[rows[1]][cols[2]] * M[rows[2]][cols[1]])
1684 - M[rows[0]][cols[1]] * (M[rows[1]][cols[0]] * M[rows[2]][cols[2]] - M[rows[1]][cols[2]] * M[rows[2]][cols[0]])
1685 + M[rows[0]][cols[2]] * (M[rows[1]][cols[0]] * M[rows[2]][cols[1]] - M[rows[1]][cols[1]] * M[rows[2]][cols[0]]);
1686 }
1688 T Cofactor(size_t I, size_t J) const
1689 {
1690 const size_t indices[4][3] = {{1,2,3},{0,2,3},{0,1,3},{0,1,2}};
1691 return ((I+J)&1) ? -SubDet(indices[I],indices[J]) : SubDet(indices[I],indices[J]);
1692 }
1694 T Determinant() const
1695 {
1696 return M[0][0] * Cofactor(0,0) + M[0][1] * Cofactor(0,1) + M[0][2] * Cofactor(0,2) + M[0][3] * Cofactor(0,3);
1697 }
1699 Matrix4 Adjugated() const
1700 {
1701 return Matrix4(Cofactor(0,0), Cofactor(1,0), Cofactor(2,0), Cofactor(3,0),
1702 Cofactor(0,1), Cofactor(1,1), Cofactor(2,1), Cofactor(3,1),
1703 Cofactor(0,2), Cofactor(1,2), Cofactor(2,2), Cofactor(3,2),
1704 Cofactor(0,3), Cofactor(1,3), Cofactor(2,3), Cofactor(3,3));
1705 }
1707 Matrix4 Inverted() const
1708 {
1709 T det = Determinant();
1710 assert(det != 0);
1711 return Adjugated() * (1.0f/det);
1712 }
1714 void Invert()
1715 {
1716 *this = Inverted();
1717 }
1719 // This is more efficient than general inverse, but ONLY works
1720 // correctly if it is a homogeneous transform matrix (rot + trans)
1721 Matrix4 InvertedHomogeneousTransform() const
1722 {
1723 // Make the inverse rotation matrix
1724 Matrix4 rinv = this->Transposed();
1725 rinv.M[3][0] = rinv.M[3][1] = rinv.M[3][2] = 0.0f;
1726 // Make the inverse translation matrix
1727 Vector3<T> tvinv(-M[0][3],-M[1][3],-M[2][3]);
1728 Matrix4 tinv = Matrix4::Translation(tvinv);
1729 return rinv * tinv; // "untranslate", then "unrotate"
1730 }
1732 // This is more efficient than general inverse, but ONLY works
1733 // correctly if it is a homogeneous transform matrix (rot + trans)
1734 void InvertHomogeneousTransform()
1735 {
1736 *this = InvertedHomogeneousTransform();
1737 }
1739 // Matrix to Euler Angles conversion
1740 // a,b,c, are the YawPitchRoll angles to be returned
1741 // rotation a around axis A1
1742 // is followed by rotation b around axis A2
1743 // is followed by rotation c around axis A3
1744 // rotations are CCW or CW (D) in LH or RH coordinate system (S)
1745 template <Axis A1, Axis A2, Axis A3, RotateDirection D, HandedSystem S>
1746 void ToEulerAngles(T *a, T *b, T *c) const
1747 {
1748 static_assert((A1 != A2) && (A2 != A3) && (A1 != A3), "(A1 != A2) && (A2 != A3) && (A1 != A3)");
1750 T psign = -1;
1751 if (((A1 + 1) % 3 == A2) && ((A2 + 1) % 3 == A3)) // Determine whether even permutation
1752 psign = 1;
1754 T pm = psign*M[A1][A3];
1755 if (pm < -1.0f + Math<T>::SingularityRadius)
1756 { // South pole singularity
1757 *a = 0;
1758 *b = -S*D*((T)MATH_DOUBLE_PIOVER2);
1759 *c = S*D*atan2( psign*M[A2][A1], M[A2][A2] );
1760 }
1761 else if (pm > 1.0f - Math<T>::SingularityRadius)
1762 { // North pole singularity
1763 *a = 0;
1764 *b = S*D*((T)MATH_DOUBLE_PIOVER2);
1765 *c = S*D*atan2( psign*M[A2][A1], M[A2][A2] );
1766 }
1767 else
1768 { // Normal case (nonsingular)
1769 *a = S*D*atan2( -psign*M[A2][A3], M[A3][A3] );
1770 *b = S*D*asin(pm);
1771 *c = S*D*atan2( -psign*M[A1][A2], M[A1][A1] );
1772 }
1774 return;
1775 }
1777 // Matrix to Euler Angles conversion
1778 // a,b,c, are the YawPitchRoll angles to be returned
1779 // rotation a around axis A1
1780 // is followed by rotation b around axis A2
1781 // is followed by rotation c around axis A1
1782 // rotations are CCW or CW (D) in LH or RH coordinate system (S)
1783 template <Axis A1, Axis A2, RotateDirection D, HandedSystem S>
1784 void ToEulerAnglesABA(T *a, T *b, T *c) const
1785 {
1786 static_assert(A1 != A2, "A1 != A2");
1788 // Determine the axis that was not supplied
1789 int m = 3 - A1 - A2;
1791 T psign = -1;
1792 if ((A1 + 1) % 3 == A2) // Determine whether even permutation
1793 psign = 1.0f;
1795 T c2 = M[A1][A1];
1796 if (c2 < -1 + Math<T>::SingularityRadius)
1797 { // South pole singularity
1798 *a = 0;
1799 *b = S*D*((T)MATH_DOUBLE_PI);
1800 *c = S*D*atan2( -psign*M[A2][m],M[A2][A2]);
1801 }
1802 else if (c2 > 1.0f - Math<T>::SingularityRadius)
1803 { // North pole singularity
1804 *a = 0;
1805 *b = 0;
1806 *c = S*D*atan2( -psign*M[A2][m],M[A2][A2]);
1807 }
1808 else
1809 { // Normal case (nonsingular)
1810 *a = S*D*atan2( M[A2][A1],-psign*M[m][A1]);
1811 *b = S*D*acos(c2);
1812 *c = S*D*atan2( M[A1][A2],psign*M[A1][m]);
1813 }
1814 return;
1815 }
1817 // Creates a matrix that converts the vertices from one coordinate system
1818 // to another.
1819 static Matrix4 AxisConversion(const WorldAxes& to, const WorldAxes& from)
1820 {
1821 // Holds axis values from the 'to' structure
1822 int toArray[3] = { to.XAxis, to.YAxis, to.ZAxis };
1824 // The inverse of the toArray
1825 int inv[4];
1826 inv[0] = inv[abs(to.XAxis)] = 0;
1827 inv[abs(to.YAxis)] = 1;
1828 inv[abs(to.ZAxis)] = 2;
1830 Matrix4 m(0, 0, 0,
1831 0, 0, 0,
1832 0, 0, 0);
1834 // Only three values in the matrix need to be changed to 1 or -1.
1835 m.M[inv[abs(from.XAxis)]][0] = T(from.XAxis/toArray[inv[abs(from.XAxis)]]);
1836 m.M[inv[abs(from.YAxis)]][1] = T(from.YAxis/toArray[inv[abs(from.YAxis)]]);
1837 m.M[inv[abs(from.ZAxis)]][2] = T(from.ZAxis/toArray[inv[abs(from.ZAxis)]]);
1838 return m;
1839 }
1842 // Creates a matrix for translation by vector
1843 static Matrix4 Translation(const Vector3<T>& v)
1844 {
1845 Matrix4 t;
1846 t.M[0][3] = v.x;
1847 t.M[1][3] = v.y;
1848 t.M[2][3] = v.z;
1849 return t;
1850 }
1852 // Creates a matrix for translation by vector
1853 static Matrix4 Translation(T x, T y, T z = 0.0f)
1854 {
1855 Matrix4 t;
1856 t.M[0][3] = x;
1857 t.M[1][3] = y;
1858 t.M[2][3] = z;
1859 return t;
1860 }
1862 // Sets the translation part
1863 void SetTranslation(const Vector3<T>& v)
1864 {
1865 M[0][3] = v.x;
1866 M[1][3] = v.y;
1867 M[2][3] = v.z;
1868 }
1870 Vector3<T> GetTranslation() const
1871 {
1872 return Vector3<T>( M[0][3], M[1][3], M[2][3] );
1873 }
1875 // Creates a matrix for scaling by vector
1876 static Matrix4 Scaling(const Vector3<T>& v)
1877 {
1878 Matrix4 t;
1879 t.M[0][0] = v.x;
1880 t.M[1][1] = v.y;
1881 t.M[2][2] = v.z;
1882 return t;
1883 }
1885 // Creates a matrix for scaling by vector
1886 static Matrix4 Scaling(T x, T y, T z)
1887 {
1888 Matrix4 t;
1889 t.M[0][0] = x;
1890 t.M[1][1] = y;
1891 t.M[2][2] = z;
1892 return t;
1893 }
1895 // Creates a matrix for scaling by constant
1896 static Matrix4 Scaling(T s)
1897 {
1898 Matrix4 t;
1899 t.M[0][0] = s;
1900 t.M[1][1] = s;
1901 t.M[2][2] = s;
1902 return t;
1903 }
1905 // Simple L1 distance in R^12
1906 T Distance(const Matrix4& m2) const
1907 {
1908 T d = fabs(M[0][0] - m2.M[0][0]) + fabs(M[0][1] - m2.M[0][1]);
1909 d += fabs(M[0][2] - m2.M[0][2]) + fabs(M[0][3] - m2.M[0][3]);
1910 d += fabs(M[1][0] - m2.M[1][0]) + fabs(M[1][1] - m2.M[1][1]);
1911 d += fabs(M[1][2] - m2.M[1][2]) + fabs(M[1][3] - m2.M[1][3]);
1912 d += fabs(M[2][0] - m2.M[2][0]) + fabs(M[2][1] - m2.M[2][1]);
1913 d += fabs(M[2][2] - m2.M[2][2]) + fabs(M[2][3] - m2.M[2][3]);
1914 d += fabs(M[3][0] - m2.M[3][0]) + fabs(M[3][1] - m2.M[3][1]);
1915 d += fabs(M[3][2] - m2.M[3][2]) + fabs(M[3][3] - m2.M[3][3]);
1916 return d;
1917 }
1919 // Creates a rotation matrix rotating around the X axis by 'angle' radians.
1920 // Just for quick testing. Not for final API. Need to remove case.
1921 static Matrix4 RotationAxis(Axis A, T angle, RotateDirection d, HandedSystem s)
1922 {
1923 T sina = s * d *sin(angle);
1924 T cosa = cos(angle);
1926 switch(A)
1927 {
1928 case Axis_X:
1929 return Matrix4(1, 0, 0,
1930 0, cosa, -sina,
1931 0, sina, cosa);
1932 case Axis_Y:
1933 return Matrix4(cosa, 0, sina,
1934 0, 1, 0,
1935 -sina, 0, cosa);
1936 case Axis_Z:
1937 return Matrix4(cosa, -sina, 0,
1938 sina, cosa, 0,
1939 0, 0, 1);
1940 }
1941 }
1944 // Creates a rotation matrix rotating around the X axis by 'angle' radians.
1945 // Rotation direction is depends on the coordinate system:
1946 // RHS (Oculus default): Positive angle values rotate Counter-clockwise (CCW),
1947 // while looking in the negative axis direction. This is the
1948 // same as looking down from positive axis values towards origin.
1949 // LHS: Positive angle values rotate clock-wise (CW), while looking in the
1950 // negative axis direction.
1951 static Matrix4 RotationX(T angle)
1952 {
1953 T sina = sin(angle);
1954 T cosa = cos(angle);
1955 return Matrix4(1, 0, 0,
1956 0, cosa, -sina,
1957 0, sina, cosa);
1958 }
1960 // Creates a rotation matrix rotating around the Y axis by 'angle' radians.
1961 // Rotation direction is depends on the coordinate system:
1962 // RHS (Oculus default): Positive angle values rotate Counter-clockwise (CCW),
1963 // while looking in the negative axis direction. This is the
1964 // same as looking down from positive axis values towards origin.
1965 // LHS: Positive angle values rotate clock-wise (CW), while looking in the
1966 // negative axis direction.
1967 static Matrix4 RotationY(T angle)
1968 {
1969 T sina = sin(angle);
1970 T cosa = cos(angle);
1971 return Matrix4(cosa, 0, sina,
1972 0, 1, 0,
1973 -sina, 0, cosa);
1974 }
1976 // Creates a rotation matrix rotating around the Z axis by 'angle' radians.
1977 // Rotation direction is depends on the coordinate system:
1978 // RHS (Oculus default): Positive angle values rotate Counter-clockwise (CCW),
1979 // while looking in the negative axis direction. This is the
1980 // same as looking down from positive axis values towards origin.
1981 // LHS: Positive angle values rotate clock-wise (CW), while looking in the
1982 // negative axis direction.
1983 static Matrix4 RotationZ(T angle)
1984 {
1985 T sina = sin(angle);
1986 T cosa = cos(angle);
1987 return Matrix4(cosa, -sina, 0,
1988 sina, cosa, 0,
1989 0, 0, 1);
1990 }
1992 // LookAtRH creates a View transformation matrix for right-handed coordinate system.
1993 // The resulting matrix points camera from 'eye' towards 'at' direction, with 'up'
1994 // specifying the up vector. The resulting matrix should be used with PerspectiveRH
1995 // projection.
1996 static Matrix4 LookAtRH(const Vector3<T>& eye, const Vector3<T>& at, const Vector3<T>& up)
1997 {
1998 Vector3<T> z = (eye - at).Normalized(); // Forward
1999 Vector3<T> x = up.Cross(z).Normalized(); // Right
2000 Vector3<T> y = z.Cross(x);
2002 Matrix4 m(x.x, x.y, x.z, -(x.Dot(eye)),
2003 y.x, y.y, y.z, -(y.Dot(eye)),
2004 z.x, z.y, z.z, -(z.Dot(eye)),
2005 0, 0, 0, 1 );
2006 return m;
2007 }
2009 // LookAtLH creates a View transformation matrix for left-handed coordinate system.
2010 // The resulting matrix points camera from 'eye' towards 'at' direction, with 'up'
2011 // specifying the up vector.
2012 static Matrix4 LookAtLH(const Vector3<T>& eye, const Vector3<T>& at, const Vector3<T>& up)
2013 {
2014 Vector3<T> z = (at - eye).Normalized(); // Forward
2015 Vector3<T> x = up.Cross(z).Normalized(); // Right
2016 Vector3<T> y = z.Cross(x);
2018 Matrix4 m(x.x, x.y, x.z, -(x.Dot(eye)),
2019 y.x, y.y, y.z, -(y.Dot(eye)),
2020 z.x, z.y, z.z, -(z.Dot(eye)),
2021 0, 0, 0, 1 );
2022 return m;
2023 }
2025 // PerspectiveRH creates a right-handed perspective projection matrix that can be
2026 // used with the Oculus sample renderer.
2027 // yfov - Specifies vertical field of view in radians.
2028 // aspect - Screen aspect ration, which is usually width/height for square pixels.
2029 // Note that xfov = yfov * aspect.
2030 // znear - Absolute value of near Z clipping clipping range.
2031 // zfar - Absolute value of far Z clipping clipping range (larger then near).
2032 // Even though RHS usually looks in the direction of negative Z, positive values
2033 // are expected for znear and zfar.
2034 static Matrix4 PerspectiveRH(T yfov, T aspect, T znear, T zfar)
2035 {
2036 Matrix4 m;
2037 T tanHalfFov = tan(yfov * 0.5f);
2039 m.M[0][0] = 1. / (aspect * tanHalfFov);
2040 m.M[1][1] = 1. / tanHalfFov;
2041 m.M[2][2] = zfar / (znear - zfar);
2042 m.M[3][2] = -1.;
2043 m.M[2][3] = (zfar * znear) / (znear - zfar);
2044 m.M[3][3] = 0.;
2046 // Note: Post-projection matrix result assumes Left-Handed coordinate system,
2047 // with Y up, X right and Z forward. This supports positive z-buffer values.
2048 // This is the case even for RHS coordinate input.
2049 return m;
2050 }
2052 // PerspectiveLH creates a left-handed perspective projection matrix that can be
2053 // used with the Oculus sample renderer.
2054 // yfov - Specifies vertical field of view in radians.
2055 // aspect - Screen aspect ration, which is usually width/height for square pixels.
2056 // Note that xfov = yfov * aspect.
2057 // znear - Absolute value of near Z clipping clipping range.
2058 // zfar - Absolute value of far Z clipping clipping range (larger then near).
2059 static Matrix4 PerspectiveLH(T yfov, T aspect, T znear, T zfar)
2060 {
2061 Matrix4 m;
2062 T tanHalfFov = tan(yfov * 0.5f);
2064 m.M[0][0] = 1. / (aspect * tanHalfFov);
2065 m.M[1][1] = 1. / tanHalfFov;
2066 //m.M[2][2] = zfar / (znear - zfar);
2067 m.M[2][2] = zfar / (zfar - znear);
2068 m.M[3][2] = -1.;
2069 m.M[2][3] = (zfar * znear) / (znear - zfar);
2070 m.M[3][3] = 0.;
2072 // Note: Post-projection matrix result assumes Left-Handed coordinate system,
2073 // with Y up, X right and Z forward. This supports positive z-buffer values.
2074 // This is the case even for RHS coordinate input.
2075 return m;
2076 }
2078 static Matrix4 Ortho2D(T w, T h)
2079 {
2080 Matrix4 m;
2081 m.M[0][0] = 2.0/w;
2082 m.M[1][1] = -2.0/h;
2083 m.M[0][3] = -1.0;
2084 m.M[1][3] = 1.0;
2085 m.M[2][2] = 0;
2086 return m;
2087 }
2088 };
2090 typedef Matrix4<float> Matrix4f;
2091 typedef Matrix4<double> Matrix4d;
2093 //-------------------------------------------------------------------------------------
2094 // ***** Matrix3
2095 //
2096 // Matrix3 is a 3x3 matrix used for representing a rotation matrix.
2097 // The matrix is stored in row-major order in memory, meaning that values
2098 // of the first row are stored before the next one.
2099 //
2100 // The arrangement of the matrix is chosen to be in Right-Handed
2101 // coordinate system and counterclockwise rotations when looking down
2102 // the axis
2103 //
2104 // Transformation Order:
2105 // - Transformations are applied from right to left, so the expression
2106 // M1 * M2 * M3 * V means that the vector V is transformed by M3 first,
2107 // followed by M2 and M1.
2108 //
2109 // Coordinate system: Right Handed
2110 //
2111 // Rotations: Counterclockwise when looking down the axis. All angles are in radians.
2113 template<typename T>
2114 class SymMat3;
2116 template<class T>
2117 class Matrix3
2118 {
2119 static const Matrix3 IdentityValue;
2121 public:
2122 T M[3][3];
2124 enum NoInitType { NoInit };
2126 // Construct with no memory initialization.
2127 Matrix3(NoInitType) { }
2129 // By default, we construct identity matrix.
2130 Matrix3()
2131 {
2132 SetIdentity();
2133 }
2135 Matrix3(T m11, T m12, T m13,
2136 T m21, T m22, T m23,
2137 T m31, T m32, T m33)
2138 {
2139 M[0][0] = m11; M[0][1] = m12; M[0][2] = m13;
2140 M[1][0] = m21; M[1][1] = m22; M[1][2] = m23;
2141 M[2][0] = m31; M[2][1] = m32; M[2][2] = m33;
2142 }
2144 /*
2145 explicit Matrix3(const Quat<T>& q)
2146 {
2147 T ww = q.w*q.w;
2148 T xx = q.x*q.x;
2149 T yy = q.y*q.y;
2150 T zz = q.z*q.z;
2152 M[0][0] = ww + xx - yy - zz; M[0][1] = 2 * (q.x*q.y - q.w*q.z); M[0][2] = 2 * (q.x*q.z + q.w*q.y);
2153 M[1][0] = 2 * (q.x*q.y + q.w*q.z); M[1][1] = ww - xx + yy - zz; M[1][2] = 2 * (q.y*q.z - q.w*q.x);
2154 M[2][0] = 2 * (q.x*q.z - q.w*q.y); M[2][1] = 2 * (q.y*q.z + q.w*q.x); M[2][2] = ww - xx - yy + zz;
2155 }
2156 */
2158 explicit Matrix3(const Quat<T>& q)
2159 {
2160 const T tx = q.x+q.x, ty = q.y+q.y, tz = q.z+q.z;
2161 const T twx = q.w*tx, twy = q.w*ty, twz = q.w*tz;
2162 const T txx = q.x*tx, txy = q.x*ty, txz = q.x*tz;
2163 const T tyy = q.y*ty, tyz = q.y*tz, tzz = q.z*tz;
2164 M[0][0] = T(1) - (tyy + tzz); M[0][1] = txy - twz; M[0][2] = txz + twy;
2165 M[1][0] = txy + twz; M[1][1] = T(1) - (txx + tzz); M[1][2] = tyz - twx;
2166 M[2][0] = txz - twy; M[2][1] = tyz + twx; M[2][2] = T(1) - (txx + tyy);
2167 }
2169 inline explicit Matrix3(T s)
2170 {
2171 M[0][0] = M[1][1] = M[2][2] = s;
2172 M[0][1] = M[0][2] = M[1][0] = M[1][2] = M[2][0] = M[2][1] = 0;
2173 }
2175 explicit Matrix3(const Pose<T>& p)
2176 {
2177 Matrix3 result(p.Rotation);
2178 result.SetTranslation(p.Translation);
2179 *this = result;
2180 }
2182 // C-interop support
2183 explicit Matrix3(const Matrix4<typename Math<T>::OtherFloatType> &src)
2184 {
2185 for (int i = 0; i < 3; i++)
2186 for (int j = 0; j < 3; j++)
2187 M[i][j] = (T)src.M[i][j];
2188 }
2190 // C-interop support.
2191 Matrix3(const typename CompatibleTypes<Matrix3<T> >::Type& s)
2192 {
2193 static_assert(sizeof(s) == sizeof(Matrix3), "sizeof(s) == sizeof(Matrix3)");
2194 memcpy(M, s.M, sizeof(M));
2195 }
2197 operator const typename CompatibleTypes<Matrix3<T> >::Type () const
2198 {
2199 typename CompatibleTypes<Matrix3<T> >::Type result;
2200 static_assert(sizeof(result) == sizeof(Matrix3), "sizeof(result) == sizeof(Matrix3)");
2201 memcpy(result.M, M, sizeof(M));
2202 return result;
2203 }
2205 void ToString(char* dest, size_t destsize) const
2206 {
2207 size_t pos = 0;
2208 for (int r=0; r<3; r++)
2209 for (int c=0; c<3; c++)
2210 pos += OVR_sprintf(dest+pos, destsize-pos, "%g ", M[r][c]);
2211 }
2213 static Matrix3 FromString(const char* src)
2214 {
2215 Matrix3 result;
2216 for (int r=0; r<3; r++)
2217 for (int c=0; c<3; c++)
2218 {
2219 result.M[r][c] = (T)atof(src);
2220 while (src && *src != ' ')
2221 src++;
2222 while (src && *src == ' ')
2223 src++;
2224 }
2225 return result;
2226 }
2228 static const Matrix3& Identity() { return IdentityValue; }
2230 void SetIdentity()
2231 {
2232 M[0][0] = M[1][1] = M[2][2] = 1;
2233 M[0][1] = M[1][0] = M[2][0] = 0;
2234 M[0][2] = M[1][2] = M[2][1] = 0;
2235 }
2237 bool operator== (const Matrix3& b) const
2238 {
2239 bool isEqual = true;
2240 for (int i = 0; i < 3; i++)
2241 for (int j = 0; j < 3; j++)
2242 isEqual &= (M[i][j] == b.M[i][j]);
2244 return isEqual;
2245 }
2247 Matrix3 operator+ (const Matrix3& b) const
2248 {
2249 Matrix4<T> result(*this);
2250 result += b;
2251 return result;
2252 }
2254 Matrix3& operator+= (const Matrix3& b)
2255 {
2256 for (int i = 0; i < 3; i++)
2257 for (int j = 0; j < 3; j++)
2258 M[i][j] += b.M[i][j];
2259 return *this;
2260 }
2262 void operator= (const Matrix3& b)
2263 {
2264 for (int i = 0; i < 3; i++)
2265 for (int j = 0; j < 3; j++)
2266 M[i][j] = b.M[i][j];
2267 return;
2268 }
2270 void operator= (const SymMat3<T>& b)
2271 {
2272 for (int i = 0; i < 3; i++)
2273 for (int j = 0; j < 3; j++)
2274 M[i][j] = 0;
2276 M[0][0] = b.v[0];
2277 M[0][1] = b.v[1];
2278 M[0][2] = b.v[2];
2279 M[1][1] = b.v[3];
2280 M[1][2] = b.v[4];
2281 M[2][2] = b.v[5];
2283 return;
2284 }
2286 Matrix3 operator- (const Matrix3& b) const
2287 {
2288 Matrix3 result(*this);
2289 result -= b;
2290 return result;
2291 }
2293 Matrix3& operator-= (const Matrix3& b)
2294 {
2295 for (int i = 0; i < 3; i++)
2296 for (int j = 0; j < 3; j++)
2297 M[i][j] -= b.M[i][j];
2298 return *this;
2299 }
2301 // Multiplies two matrices into destination with minimum copying.
2302 static Matrix3& Multiply(Matrix3* d, const Matrix3& a, const Matrix3& b)
2303 {
2304 OVR_ASSERT((d != &a) && (d != &b));
2305 int i = 0;
2306 do {
2307 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];
2308 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];
2309 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];
2310 } while((++i) < 3);
2312 return *d;
2313 }
2315 Matrix3 operator* (const Matrix3& b) const
2316 {
2317 Matrix3 result(Matrix3::NoInit);
2318 Multiply(&result, *this, b);
2319 return result;
2320 }
2322 Matrix3& operator*= (const Matrix3& b)
2323 {
2324 return Multiply(this, Matrix3(*this), b);
2325 }
2327 Matrix3 operator* (T s) const
2328 {
2329 Matrix3 result(*this);
2330 result *= s;
2331 return result;
2332 }
2334 Matrix3& operator*= (T s)
2335 {
2336 for (int i = 0; i < 3; i++)
2337 for (int j = 0; j < 3; j++)
2338 M[i][j] *= s;
2339 return *this;
2340 }
2342 Vector3<T> operator* (const Vector3<T> &b) const
2343 {
2344 Vector3<T> result;
2345 result.x = M[0][0]*b.x + M[0][1]*b.y + M[0][2]*b.z;
2346 result.y = M[1][0]*b.x + M[1][1]*b.y + M[1][2]*b.z;
2347 result.z = M[2][0]*b.x + M[2][1]*b.y + M[2][2]*b.z;
2349 return result;
2350 }
2352 Matrix3 operator/ (T s) const
2353 {
2354 Matrix3 result(*this);
2355 result /= s;
2356 return result;
2357 }
2359 Matrix3& operator/= (T s)
2360 {
2361 for (int i = 0; i < 3; i++)
2362 for (int j = 0; j < 3; j++)
2363 M[i][j] /= s;
2364 return *this;
2365 }
2367 Vector2<T> Transform(const Vector2<T>& v) const
2368 {
2369 const float rcpZ = 1.0f / (M[2][0] * v.x + M[2][1] * v.y + M[2][2]);
2370 return Vector2<T>((M[0][0] * v.x + M[0][1] * v.y + M[0][2]) * rcpZ,
2371 (M[1][0] * v.x + M[1][1] * v.y + M[1][2]) * rcpZ);
2372 }
2374 Vector3<T> Transform(const Vector3<T>& v) const
2375 {
2376 return Vector3<T>(M[0][0] * v.x + M[0][1] * v.y + M[0][2] * v.z,
2377 M[1][0] * v.x + M[1][1] * v.y + M[1][2] * v.z,
2378 M[2][0] * v.x + M[2][1] * v.y + M[2][2] * v.z);
2379 }
2381 Matrix3 Transposed() const
2382 {
2383 return Matrix3(M[0][0], M[1][0], M[2][0],
2384 M[0][1], M[1][1], M[2][1],
2385 M[0][2], M[1][2], M[2][2]);
2386 }
2388 void Transpose()
2389 {
2390 *this = Transposed();
2391 }
2394 T SubDet (const size_t* rows, const size_t* cols) const
2395 {
2396 return M[rows[0]][cols[0]] * (M[rows[1]][cols[1]] * M[rows[2]][cols[2]] - M[rows[1]][cols[2]] * M[rows[2]][cols[1]])
2397 - M[rows[0]][cols[1]] * (M[rows[1]][cols[0]] * M[rows[2]][cols[2]] - M[rows[1]][cols[2]] * M[rows[2]][cols[0]])
2398 + M[rows[0]][cols[2]] * (M[rows[1]][cols[0]] * M[rows[2]][cols[1]] - M[rows[1]][cols[1]] * M[rows[2]][cols[0]]);
2399 }
2401 // M += a*b.t()
2402 inline void Rank1Add(const Vector3<T> &a, const Vector3<T> &b)
2403 {
2404 M[0][0] += a.x*b.x; M[0][1] += a.x*b.y; M[0][2] += a.x*b.z;
2405 M[1][0] += a.y*b.x; M[1][1] += a.y*b.y; M[1][2] += a.y*b.z;
2406 M[2][0] += a.z*b.x; M[2][1] += a.z*b.y; M[2][2] += a.z*b.z;
2407 }
2409 // M -= a*b.t()
2410 inline void Rank1Sub(const Vector3<T> &a, const Vector3<T> &b)
2411 {
2412 M[0][0] -= a.x*b.x; M[0][1] -= a.x*b.y; M[0][2] -= a.x*b.z;
2413 M[1][0] -= a.y*b.x; M[1][1] -= a.y*b.y; M[1][2] -= a.y*b.z;
2414 M[2][0] -= a.z*b.x; M[2][1] -= a.z*b.y; M[2][2] -= a.z*b.z;
2415 }
2417 inline Vector3<T> Col(int c) const
2418 {
2419 return Vector3<T>(M[0][c], M[1][c], M[2][c]);
2420 }
2422 inline Vector3<T> Row(int r) const
2423 {
2424 return Vector3<T>(M[r][0], M[r][1], M[r][2]);
2425 }
2427 inline T Determinant() const
2428 {
2429 const Matrix3<T>& m = *this;
2430 T d;
2432 d = m.M[0][0] * (m.M[1][1]*m.M[2][2] - m.M[1][2] * m.M[2][1]);
2433 d -= m.M[0][1] * (m.M[1][0]*m.M[2][2] - m.M[1][2] * m.M[2][0]);
2434 d += m.M[0][2] * (m.M[1][0]*m.M[2][1] - m.M[1][1] * m.M[2][0]);
2436 return d;
2437 }
2439 inline Matrix3<T> Inverse() const
2440 {
2441 Matrix3<T> a;
2442 const Matrix3<T>& m = *this;
2443 T d = Determinant();
2445 assert(d != 0);
2446 T s = T(1)/d;
2448 a.M[0][0] = s * (m.M[1][1] * m.M[2][2] - m.M[1][2] * m.M[2][1]);
2449 a.M[1][0] = s * (m.M[1][2] * m.M[2][0] - m.M[1][0] * m.M[2][2]);
2450 a.M[2][0] = s * (m.M[1][0] * m.M[2][1] - m.M[1][1] * m.M[2][0]);
2452 a.M[0][1] = s * (m.M[0][2] * m.M[2][1] - m.M[0][1] * m.M[2][2]);
2453 a.M[1][1] = s * (m.M[0][0] * m.M[2][2] - m.M[0][2] * m.M[2][0]);
2454 a.M[2][1] = s * (m.M[0][1] * m.M[2][0] - m.M[0][0] * m.M[2][1]);
2456 a.M[0][2] = s * (m.M[0][1] * m.M[1][2] - m.M[0][2] * m.M[1][1]);
2457 a.M[1][2] = s * (m.M[0][2] * m.M[1][0] - m.M[0][0] * m.M[1][2]);
2458 a.M[2][2] = s * (m.M[0][0] * m.M[1][1] - m.M[0][1] * m.M[1][0]);
2460 return a;
2461 }
2463 };
2465 typedef Matrix3<float> Matrix3f;
2466 typedef Matrix3<double> Matrix3d;
2468 //-------------------------------------------------------------------------------------
2470 template<typename T>
2471 class SymMat3
2472 {
2473 private:
2474 typedef SymMat3<T> this_type;
2476 public:
2477 typedef T Value_t;
2478 // Upper symmetric
2479 T v[6]; // _00 _01 _02 _11 _12 _22
2481 inline SymMat3() {}
2483 inline explicit SymMat3(T s)
2484 {
2485 v[0] = v[3] = v[5] = s;
2486 v[1] = v[2] = v[4] = 0;
2487 }
2489 inline explicit SymMat3(T a00, T a01, T a02, T a11, T a12, T a22)
2490 {
2491 v[0] = a00; v[1] = a01; v[2] = a02;
2492 v[3] = a11; v[4] = a12;
2493 v[5] = a22;
2494 }
2496 static inline int Index(unsigned int i, unsigned int j)
2497 {
2498 return (i <= j) ? (3*i - i*(i+1)/2 + j) : (3*j - j*(j+1)/2 + i);
2499 }
2501 inline T operator()(int i, int j) const { return v[Index(i,j)]; }
2503 inline T &operator()(int i, int j) { return v[Index(i,j)]; }
2505 template<typename U>
2506 inline SymMat3<U> CastTo() const
2507 {
2508 return SymMat3<U>(static_cast<U>(v[0]), static_cast<U>(v[1]), static_cast<U>(v[2]),
2509 static_cast<U>(v[3]), static_cast<U>(v[4]), static_cast<U>(v[5]));
2510 }
2512 inline this_type& operator+=(const this_type& b)
2513 {
2514 v[0]+=b.v[0];
2515 v[1]+=b.v[1];
2516 v[2]+=b.v[2];
2517 v[3]+=b.v[3];
2518 v[4]+=b.v[4];
2519 v[5]+=b.v[5];
2520 return *this;
2521 }
2523 inline this_type& operator-=(const this_type& b)
2524 {
2525 v[0]-=b.v[0];
2526 v[1]-=b.v[1];
2527 v[2]-=b.v[2];
2528 v[3]-=b.v[3];
2529 v[4]-=b.v[4];
2530 v[5]-=b.v[5];
2532 return *this;
2533 }
2535 inline this_type& operator*=(T s)
2536 {
2537 v[0]*=s;
2538 v[1]*=s;
2539 v[2]*=s;
2540 v[3]*=s;
2541 v[4]*=s;
2542 v[5]*=s;
2544 return *this;
2545 }
2547 inline SymMat3 operator*(T s) const
2548 {
2549 SymMat3 d;
2550 d.v[0] = v[0]*s;
2551 d.v[1] = v[1]*s;
2552 d.v[2] = v[2]*s;
2553 d.v[3] = v[3]*s;
2554 d.v[4] = v[4]*s;
2555 d.v[5] = v[5]*s;
2557 return d;
2558 }
2560 // Multiplies two matrices into destination with minimum copying.
2561 static SymMat3& Multiply(SymMat3* d, const SymMat3& a, const SymMat3& b)
2562 {
2563 // _00 _01 _02 _11 _12 _22
2565 d->v[0] = a.v[0] * b.v[0];
2566 d->v[1] = a.v[0] * b.v[1] + a.v[1] * b.v[3];
2567 d->v[2] = a.v[0] * b.v[2] + a.v[1] * b.v[4];
2569 d->v[3] = a.v[3] * b.v[3];
2570 d->v[4] = a.v[3] * b.v[4] + a.v[4] * b.v[5];
2572 d->v[5] = a.v[5] * b.v[5];
2574 return *d;
2575 }
2577 inline T Determinant() const
2578 {
2579 const this_type& m = *this;
2580 T d;
2582 d = m(0,0) * (m(1,1)*m(2,2) - m(1,2) * m(2,1));
2583 d -= m(0,1) * (m(1,0)*m(2,2) - m(1,2) * m(2,0));
2584 d += m(0,2) * (m(1,0)*m(2,1) - m(1,1) * m(2,0));
2586 return d;
2587 }
2589 inline this_type Inverse() const
2590 {
2591 this_type a;
2592 const this_type& m = *this;
2593 T d = Determinant();
2595 assert(d != 0);
2596 T s = T(1)/d;
2598 a(0,0) = s * (m(1,1) * m(2,2) - m(1,2) * m(2,1));
2600 a(0,1) = s * (m(0,2) * m(2,1) - m(0,1) * m(2,2));
2601 a(1,1) = s * (m(0,0) * m(2,2) - m(0,2) * m(2,0));
2603 a(0,2) = s * (m(0,1) * m(1,2) - m(0,2) * m(1,1));
2604 a(1,2) = s * (m(0,2) * m(1,0) - m(0,0) * m(1,2));
2605 a(2,2) = s * (m(0,0) * m(1,1) - m(0,1) * m(1,0));
2607 return a;
2608 }
2610 inline T Trace() const { return v[0] + v[3] + v[5]; }
2612 // M = a*a.t()
2613 inline void Rank1(const Vector3<T> &a)
2614 {
2615 v[0] = a.x*a.x; v[1] = a.x*a.y; v[2] = a.x*a.z;
2616 v[3] = a.y*a.y; v[4] = a.y*a.z;
2617 v[5] = a.z*a.z;
2618 }
2620 // M += a*a.t()
2621 inline void Rank1Add(const Vector3<T> &a)
2622 {
2623 v[0] += a.x*a.x; v[1] += a.x*a.y; v[2] += a.x*a.z;
2624 v[3] += a.y*a.y; v[4] += a.y*a.z;
2625 v[5] += a.z*a.z;
2626 }
2628 // M -= a*a.t()
2629 inline void Rank1Sub(const Vector3<T> &a)
2630 {
2631 v[0] -= a.x*a.x; v[1] -= a.x*a.y; v[2] -= a.x*a.z;
2632 v[3] -= a.y*a.y; v[4] -= a.y*a.z;
2633 v[5] -= a.z*a.z;
2634 }
2635 };
2637 typedef SymMat3<float> SymMat3f;
2638 typedef SymMat3<double> SymMat3d;
2640 template<typename T>
2641 inline Matrix3<T> operator*(const SymMat3<T>& a, const SymMat3<T>& b)
2642 {
2643 #define AJB_ARBC(r,c) (a(r,0)*b(0,c)+a(r,1)*b(1,c)+a(r,2)*b(2,c))
2644 return Matrix3<T>(
2645 AJB_ARBC(0,0), AJB_ARBC(0,1), AJB_ARBC(0,2),
2646 AJB_ARBC(1,0), AJB_ARBC(1,1), AJB_ARBC(1,2),
2647 AJB_ARBC(2,0), AJB_ARBC(2,1), AJB_ARBC(2,2));
2648 #undef AJB_ARBC
2649 }
2651 template<typename T>
2652 inline Matrix3<T> operator*(const Matrix3<T>& a, const SymMat3<T>& b)
2653 {
2654 #define AJB_ARBC(r,c) (a(r,0)*b(0,c)+a(r,1)*b(1,c)+a(r,2)*b(2,c))
2655 return Matrix3<T>(
2656 AJB_ARBC(0,0), AJB_ARBC(0,1), AJB_ARBC(0,2),
2657 AJB_ARBC(1,0), AJB_ARBC(1,1), AJB_ARBC(1,2),
2658 AJB_ARBC(2,0), AJB_ARBC(2,1), AJB_ARBC(2,2));
2659 #undef AJB_ARBC
2660 }
2662 //-------------------------------------------------------------------------------------
2663 // ***** Angle
2665 // Cleanly representing the algebra of 2D rotations.
2666 // The operations maintain the angle between -Pi and Pi, the same range as atan2.
2668 template<class T>
2669 class Angle
2670 {
2671 public:
2672 enum AngularUnits
2673 {
2674 Radians = 0,
2675 Degrees = 1
2676 };
2678 Angle() : a(0) {}
2680 // Fix the range to be between -Pi and Pi
2681 Angle(T a_, AngularUnits u = Radians) : a((u == Radians) ? a_ : a_*((T)MATH_DOUBLE_DEGREETORADFACTOR)) { FixRange(); }
2683 T Get(AngularUnits u = Radians) const { return (u == Radians) ? a : a*((T)MATH_DOUBLE_RADTODEGREEFACTOR); }
2684 void Set(const T& x, AngularUnits u = Radians) { a = (u == Radians) ? x : x*((T)MATH_DOUBLE_DEGREETORADFACTOR); FixRange(); }
2685 int Sign() const { if (a == 0) return 0; else return (a > 0) ? 1 : -1; }
2686 T Abs() const { return (a > 0) ? a : -a; }
2688 bool operator== (const Angle& b) const { return a == b.a; }
2689 bool operator!= (const Angle& b) const { return a != b.a; }
2690 // bool operator< (const Angle& b) const { return a < a.b; }
2691 // bool operator> (const Angle& b) const { return a > a.b; }
2692 // bool operator<= (const Angle& b) const { return a <= a.b; }
2693 // bool operator>= (const Angle& b) const { return a >= a.b; }
2694 // bool operator= (const T& x) { a = x; FixRange(); }
2696 // These operations assume a is already between -Pi and Pi.
2697 Angle& operator+= (const Angle& b) { a = a + b.a; FastFixRange(); return *this; }
2698 Angle& operator+= (const T& x) { a = a + x; FixRange(); return *this; }
2699 Angle operator+ (const Angle& b) const { Angle res = *this; res += b; return res; }
2700 Angle operator+ (const T& x) const { Angle res = *this; res += x; return res; }
2701 Angle& operator-= (const Angle& b) { a = a - b.a; FastFixRange(); return *this; }
2702 Angle& operator-= (const T& x) { a = a - x; FixRange(); return *this; }
2703 Angle operator- (const Angle& b) const { Angle res = *this; res -= b; return res; }
2704 Angle operator- (const T& x) const { Angle res = *this; res -= x; return res; }
2706 T Distance(const Angle& b) { T c = fabs(a - b.a); return (c <= ((T)MATH_DOUBLE_PI)) ? c : ((T)MATH_DOUBLE_TWOPI) - c; }
2708 private:
2710 // The stored angle, which should be maintained between -Pi and Pi
2711 T a;
2713 // Fixes the angle range to [-Pi,Pi], but assumes no more than 2Pi away on either side
2714 inline void FastFixRange()
2715 {
2716 if (a < -((T)MATH_DOUBLE_PI))
2717 a += ((T)MATH_DOUBLE_TWOPI);
2718 else if (a > ((T)MATH_DOUBLE_PI))
2719 a -= ((T)MATH_DOUBLE_TWOPI);
2720 }
2722 // Fixes the angle range to [-Pi,Pi] for any given range, but slower then the fast method
2723 inline void FixRange()
2724 {
2725 // do nothing if the value is already in the correct range, since fmod call is expensive
2726 if (a >= -((T)MATH_DOUBLE_PI) && a <= ((T)MATH_DOUBLE_PI))
2727 return;
2728 a = fmod(a,((T)MATH_DOUBLE_TWOPI));
2729 if (a < -((T)MATH_DOUBLE_PI))
2730 a += ((T)MATH_DOUBLE_TWOPI);
2731 else if (a > ((T)MATH_DOUBLE_PI))
2732 a -= ((T)MATH_DOUBLE_TWOPI);
2733 }
2734 };
2737 typedef Angle<float> Anglef;
2738 typedef Angle<double> Angled;
2741 //-------------------------------------------------------------------------------------
2742 // ***** Plane
2744 // Consists of a normal vector and distance from the origin where the plane is located.
2746 template<class T>
2747 class Plane
2748 {
2749 public:
2750 Vector3<T> N;
2751 T D;
2753 Plane() : D(0) {}
2755 // Normals must already be normalized
2756 Plane(const Vector3<T>& n, T d) : N(n), D(d) {}
2757 Plane(T x, T y, T z, T d) : N(x,y,z), D(d) {}
2759 // construct from a point on the plane and the normal
2760 Plane(const Vector3<T>& p, const Vector3<T>& n) : N(n), D(-(p * n)) {}
2762 // Find the point to plane distance. The sign indicates what side of the plane the point is on (0 = point on plane).
2763 T TestSide(const Vector3<T>& p) const
2764 {
2765 return (N.Dot(p)) + D;
2766 }
2768 Plane<T> Flipped() const
2769 {
2770 return Plane(-N, -D);
2771 }
2773 void Flip()
2774 {
2775 N = -N;
2776 D = -D;
2777 }
2779 bool operator==(const Plane<T>& rhs) const
2780 {
2781 return (this->D == rhs.D && this->N == rhs.N);
2782 }
2783 };
2785 typedef Plane<float> Planef;
2786 typedef Plane<double> Planed;
2789 } // Namespace OVR
2791 #endif