oculus1
view libovr/Src/Kernel/OVR_Math.h @ 29:9a973ef0e2a3
fixed the performance issue under MacOSX by replacing glutSolidTeapot (which
uses glEvalMesh) with my own teapot generator.
| author | John Tsiombikas <nuclear@member.fsf.org> |
|---|---|
| date | Sun, 27 Oct 2013 06:31:18 +0200 |
| parents | e2f9e4603129 |
| children |
line source
1 /************************************************************************************
3 PublicHeader: OVR.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, Anna Yershova
9 Copyright : Copyright 2012 Oculus VR, Inc. All Rights reserved.
11 Use of this software is subject to the terms of the Oculus license
12 agreement provided at the time of installation or download, or which
13 otherwise accompanies this software in either electronic or hard copy form.
15 *************************************************************************************/
17 #ifndef OVR_Math_h
18 #define OVR_Math_h
20 #include <assert.h>
21 #include <stdlib.h>
22 #include <math.h>
24 #include "OVR_Types.h"
25 #include "OVR_RefCount.h"
27 namespace OVR {
29 //-------------------------------------------------------------------------------------
30 // Constants for 3D world/axis definitions.
32 // Definitions of axes for coordinate and rotation conversions.
33 enum Axis
34 {
35 Axis_X = 0, Axis_Y = 1, Axis_Z = 2
36 };
38 // RotateDirection describes the rotation direction around an axis, interpreted as follows:
39 // CW - Clockwise while looking "down" from positive axis towards the origin.
40 // CCW - Counter-clockwise while looking from the positive axis towards the origin,
41 // which is in the negative axis direction.
42 // CCW is the default for the RHS coordinate system. Oculus standard RHS coordinate
43 // system defines Y up, X right, and Z back (pointing out from the screen). In this
44 // system Rotate_CCW around Z will specifies counter-clockwise rotation in XY plane.
45 enum RotateDirection
46 {
47 Rotate_CCW = 1,
48 Rotate_CW = -1
49 };
51 enum HandedSystem
52 {
53 Handed_R = 1, Handed_L = -1
54 };
56 // AxisDirection describes which way the axis points. Used by WorldAxes.
57 enum AxisDirection
58 {
59 Axis_Up = 2,
60 Axis_Down = -2,
61 Axis_Right = 1,
62 Axis_Left = -1,
63 Axis_In = 3,
64 Axis_Out = -3
65 };
67 struct WorldAxes
68 {
69 AxisDirection XAxis, YAxis, ZAxis;
71 WorldAxes(AxisDirection x, AxisDirection y, AxisDirection z)
72 : XAxis(x), YAxis(y), ZAxis(z)
73 { OVR_ASSERT(abs(x) != abs(y) && abs(y) != abs(z) && abs(z) != abs(x));}
74 };
77 //-------------------------------------------------------------------------------------
78 // ***** Math
80 // Math class contains constants and functions. This class is a template specialized
81 // per type, with Math<float> and Math<double> being distinct.
82 template<class Type>
83 class Math
84 {
85 };
87 // Single-precision Math constants class.
88 template<>
89 class Math<float>
90 {
91 public:
92 static const float Pi;
93 static const float TwoPi;
94 static const float PiOver2;
95 static const float PiOver4;
96 static const float E;
98 static const float MaxValue; // Largest positive float Value
99 static const float MinPositiveValue; // Smallest possible positive value
101 static const float RadToDegreeFactor;
102 static const float DegreeToRadFactor;
104 static const float Tolerance; // 0.00001f;
105 static const float SingularityRadius; //0.00000000001f for Gimbal lock numerical problems
106 };
108 // Double-precision Math constants class.
109 template<>
110 class Math<double>
111 {
112 public:
113 static const double Pi;
114 static const double TwoPi;
115 static const double PiOver2;
116 static const double PiOver4;
117 static const double E;
119 static const double MaxValue; // Largest positive double Value
120 static const double MinPositiveValue; // Smallest possible positive value
122 static const double RadToDegreeFactor;
123 static const double DegreeToRadFactor;
125 static const double Tolerance; // 0.00001f;
126 static const double SingularityRadius; //0.00000000001 for Gimbal lock numerical problems
127 };
129 typedef Math<float> Mathf;
130 typedef Math<double> Mathd;
132 // Conversion functions between degrees and radians
133 template<class FT>
134 FT RadToDegree(FT rads) { return rads * Math<FT>::RadToDegreeFactor; }
135 template<class FT>
136 FT DegreeToRad(FT rads) { return rads * Math<FT>::DegreeToRadFactor; }
138 template<class T>
139 class Quat;
141 //-------------------------------------------------------------------------------------
142 // ***** Vector2f - 2D Vector2f
144 // Vector2f represents a 2-dimensional vector or point in space,
145 // consisting of coordinates x and y,
147 template<class T>
148 class Vector2
149 {
150 public:
151 T x, y;
153 Vector2() : x(0), y(0) { }
154 Vector2(T x_, T y_) : x(x_), y(y_) { }
155 explicit Vector2(T s) : x(s), y(s) { }
157 bool operator== (const Vector2& b) const { return x == b.x && y == b.y; }
158 bool operator!= (const Vector2& b) const { return x != b.x || y != b.y; }
160 Vector2 operator+ (const Vector2& b) const { return Vector2(x + b.x, y + b.y); }
161 Vector2& operator+= (const Vector2& b) { x += b.x; y += b.y; return *this; }
162 Vector2 operator- (const Vector2& b) const { return Vector2(x - b.x, y - b.y); }
163 Vector2& operator-= (const Vector2& b) { x -= b.x; y -= b.y; return *this; }
164 Vector2 operator- () const { return Vector2(-x, -y); }
166 // Scalar multiplication/division scales vector.
167 Vector2 operator* (T s) const { return Vector2(x*s, y*s); }
168 Vector2& operator*= (T s) { x *= s; y *= s; return *this; }
170 Vector2 operator/ (T s) const { T rcp = T(1)/s;
171 return Vector2(x*rcp, y*rcp); }
172 Vector2& operator/= (T s) { T rcp = T(1)/s;
173 x *= rcp; y *= rcp;
174 return *this; }
176 // Compare two vectors for equality with tolerance. Returns true if vectors match withing tolerance.
177 bool Compare(const Vector2&b, T tolerance = Mathf::Tolerance)
178 {
179 return (fabs(b.x-x) < tolerance) && (fabs(b.y-y) < tolerance);
180 }
182 // Dot product overload.
183 // Used to calculate angle q between two vectors among other things,
184 // as (A dot B) = |a||b|cos(q).
185 T operator* (const Vector2& b) const { return x*b.x + y*b.y; }
187 // Returns the angle from this vector to b, in radians.
188 T Angle(const Vector2& b) const { return acos((*this * b)/(Length()*b.Length())); }
190 // Return Length of the vector squared.
191 T LengthSq() const { return (x * x + y * y); }
192 // Return vector length.
193 T Length() const { return sqrt(LengthSq()); }
195 // Returns distance between two points represented by vectors.
196 T Distance(Vector2& b) const { return (*this - b).Length(); }
198 // Determine if this a unit vector.
199 bool IsNormalized() const { return fabs(LengthSq() - T(1)) < Math<T>::Tolerance; }
200 // Normalize, convention vector length to 1.
201 void Normalize() { *this /= Length(); }
202 // Returns normalized (unit) version of the vector without modifying itself.
203 Vector2 Normalized() const { return *this / Length(); }
205 // Linearly interpolates from this vector to another.
206 // Factor should be between 0.0 and 1.0, with 0 giving full value to this.
207 Vector2 Lerp(const Vector2& b, T f) const { return *this*(T(1) - f) + b*f; }
209 // Projects this vector onto the argument; in other words,
210 // A.Project(B) returns projection of vector A onto B.
211 Vector2 ProjectTo(const Vector2& b) const { return b * ((*this * b) / b.LengthSq()); }
212 };
215 typedef Vector2<float> Vector2f;
216 typedef Vector2<double> Vector2d;
218 //-------------------------------------------------------------------------------------
219 // ***** Vector3f - 3D Vector3f
221 // Vector3f represents a 3-dimensional vector or point in space,
222 // consisting of coordinates x, y and z.
224 template<class T>
225 class Vector3
226 {
227 public:
228 T x, y, z;
230 Vector3() : x(0), y(0), z(0) { }
231 Vector3(T x_, T y_, T z_ = 0) : x(x_), y(y_), z(z_) { }
232 explicit Vector3(T s) : x(s), y(s), z(s) { }
234 bool operator== (const Vector3& b) const { return x == b.x && y == b.y && z == b.z; }
235 bool operator!= (const Vector3& b) const { return x != b.x || y != b.y || z != b.z; }
237 Vector3 operator+ (const Vector3& b) const { return Vector3(x + b.x, y + b.y, z + b.z); }
238 Vector3& operator+= (const Vector3& b) { x += b.x; y += b.y; z += b.z; return *this; }
239 Vector3 operator- (const Vector3& b) const { return Vector3(x - b.x, y - b.y, z - b.z); }
240 Vector3& operator-= (const Vector3& b) { x -= b.x; y -= b.y; z -= b.z; return *this; }
241 Vector3 operator- () const { return Vector3(-x, -y, -z); }
243 // Scalar multiplication/division scales vector.
244 Vector3 operator* (T s) const { return Vector3(x*s, y*s, z*s); }
245 Vector3& operator*= (T s) { x *= s; y *= s; z *= s; return *this; }
247 Vector3 operator/ (T s) const { T rcp = T(1)/s;
248 return Vector3(x*rcp, y*rcp, z*rcp); }
249 Vector3& operator/= (T s) { T rcp = T(1)/s;
250 x *= rcp; y *= rcp; z *= rcp;
251 return *this; }
253 // Compare two vectors for equality with tolerance. Returns true if vectors match withing tolerance.
254 bool Compare(const Vector3&b, T tolerance = Mathf::Tolerance)
255 {
256 return (fabs(b.x-x) < tolerance) && (fabs(b.y-y) < tolerance) && (fabs(b.z-z) < tolerance);
257 }
259 // Dot product overload.
260 // Used to calculate angle q between two vectors among other things,
261 // as (A dot B) = |a||b|cos(q).
262 T operator* (const Vector3& b) const { return x*b.x + y*b.y + z*b.z; }
264 // Compute cross product, which generates a normal vector.
265 // Direction vector can be determined by right-hand rule: Pointing index finder in
266 // direction a and middle finger in direction b, thumb will point in a.Cross(b).
267 Vector3 Cross(const Vector3& b) const { return Vector3(y*b.z - z*b.y,
268 z*b.x - x*b.z,
269 x*b.y - y*b.x); }
271 // Returns the angle from this vector to b, in radians.
272 T Angle(const Vector3& b) const { return acos((*this * b)/(Length()*b.Length())); }
274 // Return Length of the vector squared.
275 T LengthSq() const { return (x * x + y * y + z * z); }
276 // Return vector length.
277 T Length() const { return sqrt(LengthSq()); }
279 // Returns distance between two points represented by vectors.
280 T Distance(Vector3& b) const { return (*this - b).Length(); }
282 // Determine if this a unit vector.
283 bool IsNormalized() const { return fabs(LengthSq() - T(1)) < Math<T>::Tolerance; }
284 // Normalize, convention vector length to 1.
285 void Normalize() { *this /= Length(); }
286 // Returns normalized (unit) version of the vector without modifying itself.
287 Vector3 Normalized() const { return *this / Length(); }
289 // Linearly interpolates from this vector to another.
290 // Factor should be between 0.0 and 1.0, with 0 giving full value to this.
291 Vector3 Lerp(const Vector3& b, T f) const { return *this*(T(1) - f) + b*f; }
293 // Projects this vector onto the argument; in other words,
294 // A.Project(B) returns projection of vector A onto B.
295 Vector3 ProjectTo(const Vector3& b) const { return b * ((*this * b) / b.LengthSq()); }
296 };
299 typedef Vector3<float> Vector3f;
300 typedef Vector3<double> Vector3d;
303 //-------------------------------------------------------------------------------------
304 // ***** Matrix4f
306 // Matrix4f is a 4x4 matrix used for 3d transformations and projections.
307 // Translation stored in the last column.
308 // The matrix is stored in row-major order in memory, meaning that values
309 // of the first row are stored before the next one.
310 //
311 // The arrangement of the matrix is chosen to be in Right-Handed
312 // coordinate system and counterclockwise rotations when looking down
313 // the axis
314 //
315 // Transformation Order:
316 // - Transformations are applied from right to left, so the expression
317 // M1 * M2 * M3 * V means that the vector V is transformed by M3 first,
318 // followed by M2 and M1.
319 //
320 // Coordinate system: Right Handed
321 //
322 // Rotations: Counterclockwise when looking down the axis. All angles are in radians.
323 //
324 // | sx 01 02 tx | // First column (sx, 10, 20): Axis X basis vector.
325 // | 10 sy 12 ty | // Second column (01, sy, 21): Axis Y basis vector.
326 // | 20 21 sz tz | // Third columnt (02, 12, sz): Axis Z basis vector.
327 // | 30 31 32 33 |
328 //
329 // The basis vectors are first three columns.
331 class Matrix4f
332 {
333 static Matrix4f IdentityValue;
335 public:
336 float M[4][4];
338 enum NoInitType { NoInit };
340 // Construct with no memory initialization.
341 Matrix4f(NoInitType) { }
343 // By default, we construct identity matrix.
344 Matrix4f()
345 {
346 SetIdentity();
347 }
349 Matrix4f(float m11, float m12, float m13, float m14,
350 float m21, float m22, float m23, float m24,
351 float m31, float m32, float m33, float m34,
352 float m41, float m42, float m43, float m44)
353 {
354 M[0][0] = m11; M[0][1] = m12; M[0][2] = m13; M[0][3] = m14;
355 M[1][0] = m21; M[1][1] = m22; M[1][2] = m23; M[1][3] = m24;
356 M[2][0] = m31; M[2][1] = m32; M[2][2] = m33; M[2][3] = m34;
357 M[3][0] = m41; M[3][1] = m42; M[3][2] = m43; M[3][3] = m44;
358 }
360 Matrix4f(float m11, float m12, float m13,
361 float m21, float m22, float m23,
362 float m31, float m32, float m33)
363 {
364 M[0][0] = m11; M[0][1] = m12; M[0][2] = m13; M[0][3] = 0;
365 M[1][0] = m21; M[1][1] = m22; M[1][2] = m23; M[1][3] = 0;
366 M[2][0] = m31; M[2][1] = m32; M[2][2] = m33; M[2][3] = 0;
367 M[3][0] = 0; M[3][1] = 0; M[3][2] = 0; M[3][3] = 1;
368 }
370 static const Matrix4f& Identity() { return IdentityValue; }
372 void SetIdentity()
373 {
374 M[0][0] = M[1][1] = M[2][2] = M[3][3] = 1;
375 M[0][1] = M[1][0] = M[2][3] = M[3][1] = 0;
376 M[0][2] = M[1][2] = M[2][0] = M[3][2] = 0;
377 M[0][3] = M[1][3] = M[2][1] = M[3][0] = 0;
378 }
380 // Multiplies two matrices into destination with minimum copying.
381 static Matrix4f& Multiply(Matrix4f* d, const Matrix4f& a, const Matrix4f& b)
382 {
383 OVR_ASSERT((d != &a) && (d != &b));
384 int i = 0;
385 do {
386 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];
387 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];
388 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];
389 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];
390 } while((++i) < 4);
392 return *d;
393 }
395 Matrix4f operator* (const Matrix4f& b) const
396 {
397 Matrix4f result(Matrix4f::NoInit);
398 Multiply(&result, *this, b);
399 return result;
400 }
402 Matrix4f& operator*= (const Matrix4f& b)
403 {
404 return Multiply(this, Matrix4f(*this), b);
405 }
407 Matrix4f operator* (float s) const
408 {
409 return Matrix4f(M[0][0] * s, M[0][1] * s, M[0][2] * s, M[0][3] * s,
410 M[1][0] * s, M[1][1] * s, M[1][2] * s, M[1][3] * s,
411 M[2][0] * s, M[2][1] * s, M[2][2] * s, M[2][3] * s,
412 M[3][0] * s, M[3][1] * s, M[3][2] * s, M[3][3] * s);
413 }
415 Matrix4f& operator*= (float s)
416 {
417 M[0][0] *= s; M[0][1] *= s; M[0][2] *= s; M[0][3] *= s;
418 M[1][0] *= s; M[1][1] *= s; M[1][2] *= s; M[1][3] *= s;
419 M[2][0] *= s; M[2][1] *= s; M[2][2] *= s; M[2][3] *= s;
420 M[3][0] *= s; M[3][1] *= s; M[3][2] *= s; M[3][3] *= s;
421 return *this;
422 }
424 Vector3f Transform(const Vector3f& v) const
425 {
426 return Vector3f(M[0][0] * v.x + M[0][1] * v.y + M[0][2] * v.z + M[0][3],
427 M[1][0] * v.x + M[1][1] * v.y + M[1][2] * v.z + M[1][3],
428 M[2][0] * v.x + M[2][1] * v.y + M[2][2] * v.z + M[2][3]);
429 }
431 Matrix4f Transposed() const
432 {
433 return Matrix4f(M[0][0], M[1][0], M[2][0], M[3][0],
434 M[0][1], M[1][1], M[2][1], M[3][1],
435 M[0][2], M[1][2], M[2][2], M[3][2],
436 M[0][3], M[1][3], M[2][3], M[3][3]);
437 }
439 void Transpose()
440 {
441 *this = Transposed();
442 }
445 float SubDet (const int* rows, const int* cols) const
446 {
447 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]])
448 - 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]])
449 + 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]]);
450 }
452 float Cofactor(int I, int J) const
453 {
454 const int indices[4][3] = {{1,2,3},{0,2,3},{0,1,3},{0,1,2}};
455 return ((I+J)&1) ? -SubDet(indices[I],indices[J]) : SubDet(indices[I],indices[J]);
456 }
458 float Determinant() const
459 {
460 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);
461 }
463 Matrix4f Adjugated() const
464 {
465 return Matrix4f(Cofactor(0,0), Cofactor(1,0), Cofactor(2,0), Cofactor(3,0),
466 Cofactor(0,1), Cofactor(1,1), Cofactor(2,1), Cofactor(3,1),
467 Cofactor(0,2), Cofactor(1,2), Cofactor(2,2), Cofactor(3,2),
468 Cofactor(0,3), Cofactor(1,3), Cofactor(2,3), Cofactor(3,3));
469 }
471 Matrix4f Inverted() const
472 {
473 float det = Determinant();
474 assert(det != 0);
475 return Adjugated() * (1.0f/det);
476 }
478 void Invert()
479 {
480 *this = Inverted();
481 }
483 //AnnaSteve:
484 // a,b,c, are the YawPitchRoll angles to be returned
485 // rotation a around axis A1
486 // is followed by rotation b around axis A2
487 // is followed by rotation c around axis A3
488 // rotations are CCW or CW (D) in LH or RH coordinate system (S)
489 template <Axis A1, Axis A2, Axis A3, RotateDirection D, HandedSystem S>
490 void ToEulerAngles(float *a, float *b, float *c)
491 {
492 OVR_COMPILER_ASSERT((A1 != A2) && (A2 != A3) && (A1 != A3));
494 float psign = -1.0f;
495 if (((A1 + 1) % 3 == A2) && ((A2 + 1) % 3 == A3)) // Determine whether even permutation
496 psign = 1.0f;
498 float pm = psign*M[A1][A3];
499 if (pm < -1.0f + Math<float>::SingularityRadius)
500 { // South pole singularity
501 *a = 0.0f;
502 *b = -S*D*Math<float>::PiOver2;
503 *c = S*D*atan2( psign*M[A2][A1], M[A2][A2] );
504 }
505 else if (pm > 1.0 - Math<float>::SingularityRadius)
506 { // North pole singularity
507 *a = 0.0f;
508 *b = S*D*Math<float>::PiOver2;
509 *c = S*D*atan2( psign*M[A2][A1], M[A2][A2] );
510 }
511 else
512 { // Normal case (nonsingular)
513 *a = S*D*atan2( -psign*M[A2][A3], M[A3][A3] );
514 *b = S*D*asin(pm);
515 *c = S*D*atan2( -psign*M[A1][A2], M[A1][A1] );
516 }
518 return;
519 }
521 //AnnaSteve:
522 // a,b,c, are the YawPitchRoll angles to be returned
523 // rotation a around axis A1
524 // is followed by rotation b around axis A2
525 // is followed by rotation c around axis A1
526 // rotations are CCW or CW (D) in LH or RH coordinate system (S)
527 template <Axis A1, Axis A2, RotateDirection D, HandedSystem S>
528 void ToEulerAnglesABA(float *a, float *b, float *c)
529 {
530 OVR_COMPILER_ASSERT(A1 != A2);
532 // Determine the axis that was not supplied
533 int m = 3 - A1 - A2;
535 float psign = -1.0f;
536 if ((A1 + 1) % 3 == A2) // Determine whether even permutation
537 psign = 1.0f;
539 float c2 = M[A1][A1];
540 if (c2 < -1.0 + Math<float>::SingularityRadius)
541 { // South pole singularity
542 *a = 0.0f;
543 *b = S*D*Math<float>::Pi;
544 *c = S*D*atan2( -psign*M[A2][m],M[A2][A2]);
545 }
546 else if (c2 > 1.0 - Math<float>::SingularityRadius)
547 { // North pole singularity
548 *a = 0.0f;
549 *b = 0.0f;
550 *c = S*D*atan2( -psign*M[A2][m],M[A2][A2]);
551 }
552 else
553 { // Normal case (nonsingular)
554 *a = S*D*atan2( M[A2][A1],-psign*M[m][A1]);
555 *b = S*D*acos(c2);
556 *c = S*D*atan2( M[A1][A2],psign*M[A1][m]);
557 }
558 return;
559 }
561 // Creates a matrix that converts the vertices from one coordinate system
562 // to another.
563 //
564 static Matrix4f AxisConversion(const WorldAxes& to, const WorldAxes& from)
565 {
566 // Holds axis values from the 'to' structure
567 int toArray[3] = { to.XAxis, to.YAxis, to.ZAxis };
569 // The inverse of the toArray
570 int inv[4];
571 inv[0] = inv[abs(to.XAxis)] = 0;
572 inv[abs(to.YAxis)] = 1;
573 inv[abs(to.ZAxis)] = 2;
575 Matrix4f m(0, 0, 0,
576 0, 0, 0,
577 0, 0, 0);
579 // Only three values in the matrix need to be changed to 1 or -1.
580 m.M[inv[abs(from.XAxis)]][0] = float(from.XAxis/toArray[inv[abs(from.XAxis)]]);
581 m.M[inv[abs(from.YAxis)]][1] = float(from.YAxis/toArray[inv[abs(from.YAxis)]]);
582 m.M[inv[abs(from.ZAxis)]][2] = float(from.ZAxis/toArray[inv[abs(from.ZAxis)]]);
583 return m;
584 }
588 static Matrix4f Translation(const Vector3f& v)
589 {
590 Matrix4f t;
591 t.M[0][3] = v.x;
592 t.M[1][3] = v.y;
593 t.M[2][3] = v.z;
594 return t;
595 }
597 static Matrix4f Translation(float x, float y, float z = 0.0f)
598 {
599 Matrix4f t;
600 t.M[0][3] = x;
601 t.M[1][3] = y;
602 t.M[2][3] = z;
603 return t;
604 }
606 static Matrix4f Scaling(const Vector3f& v)
607 {
608 Matrix4f t;
609 t.M[0][0] = v.x;
610 t.M[1][1] = v.y;
611 t.M[2][2] = v.z;
612 return t;
613 }
615 static Matrix4f Scaling(float x, float y, float z)
616 {
617 Matrix4f t;
618 t.M[0][0] = x;
619 t.M[1][1] = y;
620 t.M[2][2] = z;
621 return t;
622 }
624 static Matrix4f Scaling(float s)
625 {
626 Matrix4f t;
627 t.M[0][0] = s;
628 t.M[1][1] = s;
629 t.M[2][2] = s;
630 return t;
631 }
635 //AnnaSteve : Just for quick testing. Not for final API. Need to remove case.
636 static Matrix4f RotationAxis(Axis A, float angle, RotateDirection d, HandedSystem s)
637 {
638 float sina = s * d *sin(angle);
639 float cosa = cos(angle);
641 switch(A)
642 {
643 case Axis_X:
644 return Matrix4f(1, 0, 0,
645 0, cosa, -sina,
646 0, sina, cosa);
647 case Axis_Y:
648 return Matrix4f(cosa, 0, sina,
649 0, 1, 0,
650 -sina, 0, cosa);
651 case Axis_Z:
652 return Matrix4f(cosa, -sina, 0,
653 sina, cosa, 0,
654 0, 0, 1);
655 }
656 }
659 // Creates a rotation matrix rotating around the X axis by 'angle' radians.
660 // Rotation direction is depends on the coordinate system:
661 // RHS (Oculus default): Positive angle values rotate Counter-clockwise (CCW),
662 // while looking in the negative axis direction. This is the
663 // same as looking down from positive axis values towards origin.
664 // LHS: Positive angle values rotate clock-wise (CW), while looking in the
665 // negative axis direction.
666 static Matrix4f RotationX(float angle)
667 {
668 float sina = sin(angle);
669 float cosa = cos(angle);
670 return Matrix4f(1, 0, 0,
671 0, cosa, -sina,
672 0, sina, cosa);
673 }
675 // Creates a rotation matrix rotating around the Y axis by 'angle' radians.
676 // Rotation direction is depends on the coordinate system:
677 // RHS (Oculus default): Positive angle values rotate Counter-clockwise (CCW),
678 // while looking in the negative axis direction. This is the
679 // same as looking down from positive axis values towards origin.
680 // LHS: Positive angle values rotate clock-wise (CW), while looking in the
681 // negative axis direction.
682 static Matrix4f RotationY(float angle)
683 {
684 float sina = sin(angle);
685 float cosa = cos(angle);
686 return Matrix4f(cosa, 0, sina,
687 0, 1, 0,
688 -sina, 0, cosa);
689 }
691 // Creates a rotation matrix rotating around the Z axis by 'angle' radians.
692 // Rotation direction is depends on the coordinate system:
693 // RHS (Oculus default): Positive angle values rotate Counter-clockwise (CCW),
694 // while looking in the negative axis direction. This is the
695 // same as looking down from positive axis values towards origin.
696 // LHS: Positive angle values rotate clock-wise (CW), while looking in the
697 // negative axis direction.
698 static Matrix4f RotationZ(float angle)
699 {
700 float sina = sin(angle);
701 float cosa = cos(angle);
702 return Matrix4f(cosa, -sina, 0,
703 sina, cosa, 0,
704 0, 0, 1);
705 }
708 // LookAtRH creates a View transformation matrix for right-handed coordinate system.
709 // The resulting matrix points camera from 'eye' towards 'at' direction, with 'up'
710 // specifying the up vector. The resulting matrix should be used with PerspectiveRH
711 // projection.
712 static Matrix4f LookAtRH(const Vector3f& eye, const Vector3f& at, const Vector3f& up);
714 // LookAtLH creates a View transformation matrix for left-handed coordinate system.
715 // The resulting matrix points camera from 'eye' towards 'at' direction, with 'up'
716 // specifying the up vector.
717 static Matrix4f LookAtLH(const Vector3f& eye, const Vector3f& at, const Vector3f& up);
720 // PerspectiveRH creates a right-handed perspective projection matrix that can be
721 // used with the Oculus sample renderer.
722 // yfov - Specifies vertical field of view in radians.
723 // aspect - Screen aspect ration, which is usually width/height for square pixels.
724 // Note that xfov = yfov * aspect.
725 // znear - Absolute value of near Z clipping clipping range.
726 // zfar - Absolute value of far Z clipping clipping range (larger then near).
727 // Even though RHS usually looks in the direction of negative Z, positive values
728 // are expected for znear and zfar.
729 static Matrix4f PerspectiveRH(float yfov, float aspect, float znear, float zfar);
732 // PerspectiveRH creates a left-handed perspective projection matrix that can be
733 // used with the Oculus sample renderer.
734 // yfov - Specifies vertical field of view in radians.
735 // aspect - Screen aspect ration, which is usually width/height for square pixels.
736 // Note that xfov = yfov * aspect.
737 // znear - Absolute value of near Z clipping clipping range.
738 // zfar - Absolute value of far Z clipping clipping range (larger then near).
739 static Matrix4f PerspectiveLH(float yfov, float aspect, float znear, float zfar);
742 static Matrix4f Ortho2D(float w, float h);
743 };
746 //-------------------------------------------------------------------------------------
747 // ***** Quat
749 // Quatf represents a quaternion class used for rotations.
750 //
751 // Quaternion multiplications are done in right-to-left order, to match the
752 // behavior of matrices.
755 template<class T>
756 class Quat
757 {
758 public:
759 // w + Xi + Yj + Zk
760 T x, y, z, w;
762 Quat() : x(0), y(0), z(0), w(1) {}
763 Quat(T x_, T y_, T z_, T w_) : x(x_), y(y_), z(z_), w(w_) {}
766 // Constructs rotation quaternion around the axis.
767 Quat(const Vector3<T>& axis, T angle)
768 {
769 Vector3<T> unitAxis = axis.Normalized();
770 T sinHalfAngle = sin(angle * T(0.5));
772 w = cos(angle * T(0.5));
773 x = unitAxis.x * sinHalfAngle;
774 y = unitAxis.y * sinHalfAngle;
775 z = unitAxis.z * sinHalfAngle;
776 }
778 //AnnaSteve:
779 void AxisAngle(Axis A, T angle, RotateDirection d, HandedSystem s)
780 {
781 T sinHalfAngle = s * d *sin(angle * (T)0.5);
782 T v[3];
783 v[0] = v[1] = v[2] = (T)0;
784 v[A] = sinHalfAngle;
785 //return Quat(v[0], v[1], v[2], cos(angle * (T)0.5));
786 w = cos(angle * (T)0.5);
787 x = v[0];
788 y = v[1];
789 z = v[2];
790 }
793 void GetAxisAngle(Vector3<T>* axis, T* angle) const
794 {
795 if (LengthSq() > Math<T>::Tolerance * Math<T>::Tolerance)
796 {
797 *axis = Vector3<T>(x, y, z).Normalized();
798 *angle = 2 * acos(w);
799 }
800 else
801 {
802 *axis = Vector3<T>(1, 0, 0);
803 *angle= 0;
804 }
805 }
807 bool operator== (const Quat& b) const { return x == b.x && y == b.y && z == b.z && w == b.w; }
808 bool operator!= (const Quat& b) const { return x != b.x || y != b.y || z != b.z || w != b.w; }
810 Quat operator+ (const Quat& b) const { return Quat(x + b.x, y + b.y, z + b.z, w + b.w); }
811 Quat& operator+= (const Quat& b) { w += b.w; x += b.x; y += b.y; z += b.z; return *this; }
812 Quat operator- (const Quat& b) const { return Quat(x - b.x, y - b.y, z - b.z, w - b.w); }
813 Quat& operator-= (const Quat& b) { w -= b.w; x -= b.x; y -= b.y; z -= b.z; return *this; }
815 Quat operator* (T s) const { return Quat(x * s, y * s, z * s, w * s); }
816 Quat& operator*= (T s) { w *= s; x *= s; y *= s; z *= s; return *this; }
817 Quat operator/ (T s) const { T rcp = T(1)/s; return Quat(x * rcp, y * rcp, z * rcp, w *rcp); }
818 Quat& operator/= (T s) { T rcp = T(1)/s; w *= rcp; x *= rcp; y *= rcp; z *= rcp; return *this; }
820 // Get Imaginary part vector
821 Vector3<T> Imag() const { return Vector3<T>(x,y,z); }
823 // Get quaternion length.
824 T Length() const { return sqrt(x * x + y * y + z * z + w * w); }
825 // Get quaternion length squared.
826 T LengthSq() const { return (x * x + y * y + z * z + w * w); }
827 // Simple Eulidean distance in R^4 (not SLERP distance, but at least respects Haar measure)
828 T Distance(const Quat& q) const
829 {
830 T d1 = (*this - q).Length();
831 T d2 = (*this + q).Length(); // Antipoldal point check
832 return (d1 < d2) ? d1 : d2;
833 }
834 T DistanceSq(const Quat& q) const
835 {
836 T d1 = (*this - q).LengthSq();
837 T d2 = (*this + q).LengthSq(); // Antipoldal point check
838 return (d1 < d2) ? d1 : d2;
839 }
841 // Normalize
842 bool IsNormalized() const { return fabs(LengthSq() - 1) < Math<T>::Tolerance; }
843 void Normalize() { *this /= Length(); }
844 Quat Normalized() const { return *this / Length(); }
846 // Returns conjugate of the quaternion. Produces inverse rotation if quaternion is normalized.
847 Quat Conj() const { return Quat(-x, -y, -z, w); }
849 // AnnaSteve fixed: order of quaternion multiplication
850 // Quaternion multiplication. Combines quaternion rotations, performing the one on the
851 // right hand side first.
852 Quat operator* (const Quat& b) const { return Quat(w * b.x + x * b.w + y * b.z - z * b.y,
853 w * b.y - x * b.z + y * b.w + z * b.x,
854 w * b.z + x * b.y - y * b.x + z * b.w,
855 w * b.w - x * b.x - y * b.y - z * b.z); }
857 //
858 // this^p normalized; same as rotating by this p times.
859 Quat PowNormalized(T p) const
860 {
861 Vector3<T> v;
862 T a;
863 GetAxisAngle(&v, &a);
864 return Quat(v, a * p);
865 }
867 // Rotate transforms vector in a manner that matches Matrix rotations (counter-clockwise,
868 // assuming negative direction of the axis). Standard formula: q(t) * V * q(t)^-1.
869 Vector3<T> Rotate(const Vector3<T>& v) const
870 {
871 return ((*this * Quat<T>(v.x, v.y, v.z, 0)) * Inverted()).Imag();
872 }
875 // Inversed quaternion rotates in the opposite direction.
876 Quat Inverted() const
877 {
878 return Quat(-x, -y, -z, w);
879 }
881 // Sets this quaternion to the one rotates in the opposite direction.
882 void Invert()
883 {
884 *this = Quat(-x, -y, -z, w);
885 }
887 // Converting quaternion to matrix.
888 operator Matrix4f() const
889 {
890 T ww = w*w;
891 T xx = x*x;
892 T yy = y*y;
893 T zz = z*z;
895 return Matrix4f(float(ww + xx - yy - zz), float(T(2) * (x*y - w*z)), float(T(2) * (x*z + w*y)),
896 float(T(2) * (x*y + w*z)), float(ww - xx + yy - zz), float(T(2) * (y*z - w*x)),
897 float(T(2) * (x*z - w*y)), float(T(2) * (y*z + w*x)), float(ww - xx - yy + zz) );
898 }
901 // GetEulerAngles extracts Euler angles from the quaternion, in the specified order of
902 // axis rotations and the specified coordinate system. Right-handed coordinate system
903 // is the default, with CCW rotations while looking in the negative axis direction.
904 // Here a,b,c, are the Yaw/Pitch/Roll angles to be returned.
905 // rotation a around axis A1
906 // is followed by rotation b around axis A2
907 // is followed by rotation c around axis A3
908 // rotations are CCW or CW (D) in LH or RH coordinate system (S)
909 template <Axis A1, Axis A2, Axis A3, RotateDirection D, HandedSystem S>
910 void GetEulerAngles(T *a, T *b, T *c)
911 {
912 OVR_COMPILER_ASSERT((A1 != A2) && (A2 != A3) && (A1 != A3));
914 T Q[3] = { x, y, z }; //Quaternion components x,y,z
916 T ww = w*w;
917 T Q11 = Q[A1]*Q[A1];
918 T Q22 = Q[A2]*Q[A2];
919 T Q33 = Q[A3]*Q[A3];
921 T psign = T(-1.0);
922 // Determine whether even permutation
923 if (((A1 + 1) % 3 == A2) && ((A2 + 1) % 3 == A3))
924 psign = T(1.0);
926 T s2 = psign * T(2.0) * (psign*w*Q[A2] + Q[A1]*Q[A3]);
928 if (s2 < (T)-1.0 + Math<T>::SingularityRadius)
929 { // South pole singularity
930 *a = T(0.0);
931 *b = -S*D*Math<T>::PiOver2;
932 *c = S*D*atan2((T)2.0*(psign*Q[A1]*Q[A2] + w*Q[A3]),
933 ww + Q22 - Q11 - Q33 );
934 }
935 else if (s2 > (T)1.0 - Math<T>::SingularityRadius)
936 { // North pole singularity
937 *a = (T)0.0;
938 *b = S*D*Math<T>::PiOver2;
939 *c = S*D*atan2((T)2.0*(psign*Q[A1]*Q[A2] + w*Q[A3]),
940 ww + Q22 - Q11 - Q33);
941 }
942 else
943 {
944 *a = -S*D*atan2((T)-2.0*(w*Q[A1] - psign*Q[A2]*Q[A3]),
945 ww + Q33 - Q11 - Q22);
946 *b = S*D*asin(s2);
947 *c = S*D*atan2((T)2.0*(w*Q[A3] - psign*Q[A1]*Q[A2]),
948 ww + Q11 - Q22 - Q33);
949 }
950 return;
951 }
953 template <Axis A1, Axis A2, Axis A3, RotateDirection D>
954 void GetEulerAngles(T *a, T *b, T *c)
955 { GetEulerAngles<A1, A2, A3, D, Handed_R>(a, b, c); }
957 template <Axis A1, Axis A2, Axis A3>
958 void GetEulerAngles(T *a, T *b, T *c)
959 { GetEulerAngles<A1, A2, A3, Rotate_CCW, Handed_R>(a, b, c); }
962 // GetEulerAnglesABA extracts Euler angles from the quaternion, in the specified order of
963 // axis rotations and the specified coordinate system. Right-handed coordinate system
964 // is the default, with CCW rotations while looking in the negative axis direction.
965 // Here a,b,c, are the Yaw/Pitch/Roll angles to be returned.
966 // rotation a around axis A1
967 // is followed by rotation b around axis A2
968 // is followed by rotation c around axis A1
969 // Rotations are CCW or CW (D) in LH or RH coordinate system (S)
970 template <Axis A1, Axis A2, RotateDirection D, HandedSystem S>
971 void GetEulerAnglesABA(T *a, T *b, T *c)
972 {
973 OVR_COMPILER_ASSERT(A1 != A2);
975 T Q[3] = {x, y, z}; // Quaternion components
977 // Determine the missing axis that was not supplied
978 int m = 3 - A1 - A2;
980 T ww = w*w;
981 T Q11 = Q[A1]*Q[A1];
982 T Q22 = Q[A2]*Q[A2];
983 T Qmm = Q[m]*Q[m];
985 T psign = T(-1.0);
986 if ((A1 + 1) % 3 == A2) // Determine whether even permutation
987 {
988 psign = (T)1.0;
989 }
991 T c2 = ww + Q11 - Q22 - Qmm;
992 if (c2 < (T)-1.0 + Math<T>::SingularityRadius)
993 { // South pole singularity
994 *a = (T)0.0;
995 *b = S*D*Math<T>::Pi;
996 *c = S*D*atan2( (T)2.0*(w*Q[A1] - psign*Q[A2]*Q[m]),
997 ww + Q22 - Q11 - Qmm);
998 }
999 else if (c2 > (T)1.0 - Math<T>::SingularityRadius)
1000 { // North pole singularity
1001 *a = (T)0.0;
1002 *b = (T)0.0;
1003 *c = S*D*atan2( (T)2.0*(w*Q[A1] - psign*Q[A2]*Q[m]),
1004 ww + Q22 - Q11 - Qmm);
1005 }
1006 else
1007 {
1008 *a = S*D*atan2( psign*w*Q[m] + Q[A1]*Q[A2],
1009 w*Q[A2] -psign*Q[A1]*Q[m]);
1010 *b = S*D*acos(c2);
1011 *c = S*D*atan2( -psign*w*Q[m] + Q[A1]*Q[A2],
1012 w*Q[A2] + psign*Q[A1]*Q[m]);
1013 }
1014 return;
1015 }
1016 };
1019 typedef Quat<float> Quatf;
1020 typedef Quat<double> Quatd;
1024 //-------------------------------------------------------------------------------------
1025 // ***** Angle
1027 // Cleanly representing the algebra of 2D rotations.
1028 // The operations maintain the angle between -Pi and Pi, the same range as atan2.
1029 //
1031 template<class T>
1032 class Angle
1033 {
1034 public:
1035 enum AngularUnits
1036 {
1037 Radians = 0,
1038 Degrees = 1
1039 };
1041 Angle() : a(0) {}
1043 // Fix the range to be between -Pi and Pi
1044 Angle(T a_, AngularUnits u = Radians) : a((u == Radians) ? a_ : a_*Math<T>::DegreeToRadFactor) { FixRange(); }
1046 T Get(AngularUnits u = Radians) const { return (u == Radians) ? a : a*Math<T>::RadToDegreeFactor; }
1047 void Set(const T& x, AngularUnits u = Radians) { a = (u == Radians) ? x : x*Math<T>::DegreeToRadFactor; FixRange(); }
1048 int Sign() const { if (a == 0) return 0; else return (a > 0) ? 1 : -1; }
1049 T Abs() const { return (a > 0) ? a : -a; }
1051 bool operator== (const Angle& b) const { return a == b.a; }
1052 bool operator!= (const Angle& b) const { return a != b.a; }
1053 // bool operator< (const Angle& b) const { return a < a.b; }
1054 // bool operator> (const Angle& b) const { return a > a.b; }
1055 // bool operator<= (const Angle& b) const { return a <= a.b; }
1056 // bool operator>= (const Angle& b) const { return a >= a.b; }
1057 // bool operator= (const T& x) { a = x; FixRange(); }
1059 // These operations assume a is already between -Pi and Pi.
1060 Angle operator+ (const Angle& b) const { return Angle(a + b.a); }
1061 Angle operator+ (const T& x) const { return Angle(a + x); }
1062 Angle& operator+= (const Angle& b) { a = a + b.a; FastFixRange(); return *this; }
1063 Angle& operator+= (const T& x) { a = a + x; FixRange(); return *this; }
1064 Angle operator- (const Angle& b) const { return Angle(a - b.a); }
1065 Angle operator- (const T& x) const { return Angle(a - x); }
1066 Angle& operator-= (const Angle& b) { a = a - b.a; FastFixRange(); return *this; }
1067 Angle& operator-= (const T& x) { a = a - x; FixRange(); return *this; }
1069 T Distance(const Angle& b) { T c = fabs(a - b.a); return (c <= Math<T>::Pi) ? c : Math<T>::TwoPi - c; }
1071 private:
1073 // The stored angle, which should be maintained between -Pi and Pi
1074 T a;
1076 // Fixes the angle range to [-Pi,Pi], but assumes no more than 2Pi away on either side
1077 inline void FastFixRange()
1078 {
1079 if (a < -Math<T>::Pi)
1080 a += Math<T>::TwoPi;
1081 else if (a > Math<T>::Pi)
1082 a -= Math<T>::TwoPi;
1083 }
1085 // Fixes the angle range to [-Pi,Pi] for any given range, but slower then the fast method
1086 inline void FixRange()
1087 {
1088 a = fmod(a,Math<T>::TwoPi);
1089 if (a < -Math<T>::Pi)
1090 a += Math<T>::TwoPi;
1091 else if (a > Math<T>::Pi)
1092 a -= Math<T>::TwoPi;
1093 }
1094 };
1097 typedef Angle<float> Anglef;
1098 typedef Angle<double> Angled;
1101 //-------------------------------------------------------------------------------------
1102 // ***** Plane
1104 // Consists of a normal vector and distance from the origin where the plane is located.
1106 template<class T>
1107 class Plane : public RefCountBase<Plane<T> >
1108 {
1109 public:
1110 Vector3<T> N;
1111 T D;
1113 Plane() : D(0) {}
1115 // Normals must already be normalized
1116 Plane(const Vector3<T>& n, T d) : N(n), D(d) {}
1117 Plane(T x, T y, T z, T d) : N(x,y,z), D(d) {}
1119 // construct from a point on the plane and the normal
1120 Plane(const Vector3<T>& p, const Vector3<T>& n) : N(n), D(-(p * n)) {}
1122 // Find the point to plane distance. The sign indicates what side of the plane the point is on (0 = point on plane).
1123 T TestSide(const Vector3<T>& p) const
1124 {
1125 return (N * p) + D;
1126 }
1128 Plane<T> Flipped() const
1129 {
1130 return Plane(-N, -D);
1131 }
1133 void Flip()
1134 {
1135 N = -N;
1136 D = -D;
1137 }
1139 bool operator==(const Plane<T>& rhs) const
1140 {
1141 return (this->D == rhs.D && this->N == rhs.N);
1142 }
1143 };
1145 typedef Plane<float> Planef;
1147 }
1149 #endif