00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017 #ifndef OVR_Math_h
00018 #define OVR_Math_h
00019
00020 #include <assert.h>
00021 #include <stdlib.h>
00022 #include <math.h>
00023
00024 #include "OVR_Types.h"
00025 #include "OVR_RefCount.h"
00026
00027 namespace OVR {
00028
00029
00030
00031
00032
00033 enum Axis
00034 {
00035 Axis_X = 0, Axis_Y = 1, Axis_Z = 2
00036 };
00037
00038
00039
00040
00041
00042
00043
00044
00045 enum RotateDirection
00046 {
00047 Rotate_CCW = 1,
00048 Rotate_CW = -1
00049 };
00050
00051 enum HandedSystem
00052 {
00053 Handed_R = 1, Handed_L = -1
00054 };
00055
00056
00057 enum AxisDirection
00058 {
00059 Axis_Up = 2,
00060 Axis_Down = -2,
00061 Axis_Right = 1,
00062 Axis_Left = -1,
00063 Axis_In = 3,
00064 Axis_Out = -3
00065 };
00066
00067 struct WorldAxes
00068 {
00069 AxisDirection XAxis, YAxis, ZAxis;
00070
00071 WorldAxes(AxisDirection x, AxisDirection y, AxisDirection z)
00072 : XAxis(x), YAxis(y), ZAxis(z)
00073 { OVR_ASSERT(abs(x) != abs(y) && abs(y) != abs(z) && abs(z) != abs(x));}
00074 };
00075
00076
00077
00078
00079
00080
00081
00082 template<class Type>
00083 class Math
00084 {
00085 };
00086
00087
00088 template<>
00089 class Math<float>
00090 {
00091 public:
00092 static const float Pi;
00093 static const float TwoPi;
00094 static const float PiOver2;
00095 static const float PiOver4;
00096 static const float E;
00097
00098 static const float MaxValue;
00099 static const float MinPositiveValue;
00100
00101 static const float RadToDegreeFactor;
00102 static const float DegreeToRadFactor;
00103
00104 static const float Tolerance;
00105 static const float SingularityRadius;
00106 };
00107
00108
00109 template<>
00110 class Math<double>
00111 {
00112 public:
00113 static const double Pi;
00114 static const double TwoPi;
00115 static const double PiOver2;
00116 static const double PiOver4;
00117 static const double E;
00118
00119 static const double MaxValue;
00120 static const double MinPositiveValue;
00121
00122 static const double RadToDegreeFactor;
00123 static const double DegreeToRadFactor;
00124
00125 static const double Tolerance;
00126 static const double SingularityRadius;
00127 };
00128
00129 typedef Math<float> Mathf;
00130 typedef Math<double> Mathd;
00131
00132
00133 template<class FT>
00134 FT RadToDegree(FT rads) { return rads * Math<FT>::RadToDegreeFactor; }
00135 template<class FT>
00136 FT DegreeToRad(FT rads) { return rads * Math<FT>::DegreeToRadFactor; }
00137
00138 template<class T>
00139 class Quat;
00140
00141
00142
00143
00144
00145
00146
00147 template<class T>
00148 class Vector2
00149 {
00150 public:
00151 T x, y;
00152
00153 Vector2() : x(0), y(0) { }
00154 Vector2(T x_, T y_) : x(x_), y(y_) { }
00155 explicit Vector2(T s) : x(s), y(s) { }
00156
00157 bool operator== (const Vector2& b) const { return x == b.x && y == b.y; }
00158 bool operator!= (const Vector2& b) const { return x != b.x || y != b.y; }
00159
00160 Vector2 operator+ (const Vector2& b) const { return Vector2(x + b.x, y + b.y); }
00161 Vector2& operator+= (const Vector2& b) { x += b.x; y += b.y; return *this; }
00162 Vector2 operator- (const Vector2& b) const { return Vector2(x - b.x, y - b.y); }
00163 Vector2& operator-= (const Vector2& b) { x -= b.x; y -= b.y; return *this; }
00164 Vector2 operator- () const { return Vector2(-x, -y); }
00165
00166
00167 Vector2 operator* (T s) const { return Vector2(x*s, y*s); }
00168 Vector2& operator*= (T s) { x *= s; y *= s; return *this; }
00169
00170 Vector2 operator/ (T s) const { T rcp = T(1)/s;
00171 return Vector2(x*rcp, y*rcp); }
00172 Vector2& operator/= (T s) { T rcp = T(1)/s;
00173 x *= rcp; y *= rcp;
00174 return *this; }
00175
00176
00177 bool Compare(const Vector2&b, T tolerance = Mathf::Tolerance)
00178 {
00179 return (fabs(b.x-x) < tolerance) && (fabs(b.y-y) < tolerance);
00180 }
00181
00182
00183
00184
00185 T operator* (const Vector2& b) const { return x*b.x + y*b.y; }
00186
00187
00188 T Angle(const Vector2& b) const { return acos((*this * b)/(Length()*b.Length())); }
00189
00190
00191 T LengthSq() const { return (x * x + y * y); }
00192
00193 T Length() const { return sqrt(LengthSq()); }
00194
00195
00196 T Distance(Vector2& b) const { return (*this - b).Length(); }
00197
00198
00199 bool IsNormalized() const { return fabs(LengthSq() - T(1)) < Math<T>::Tolerance; }
00200
00201 void Normalize() { *this /= Length(); }
00202
00203 Vector2 Normalized() const { return *this / Length(); }
00204
00205
00206
00207 Vector2 Lerp(const Vector2& b, T f) const { return *this*(T(1) - f) + b*f; }
00208
00209
00210
00211 Vector2 ProjectTo(const Vector2& b) const { return b * ((*this * b) / b.LengthSq()); }
00212 };
00213
00214
00215 typedef Vector2<float> Vector2f;
00216 typedef Vector2<double> Vector2d;
00217
00218
00219
00220
00221
00222
00223
00224 template<class T>
00225 class Vector3
00226 {
00227 public:
00228 T x, y, z;
00229
00230 Vector3() : x(0), y(0), z(0) { }
00231 Vector3(T x_, T y_, T z_ = 0) : x(x_), y(y_), z(z_) { }
00232 explicit Vector3(T s) : x(s), y(s), z(s) { }
00233
00234 bool operator== (const Vector3& b) const { return x == b.x && y == b.y && z == b.z; }
00235 bool operator!= (const Vector3& b) const { return x != b.x || y != b.y || z != b.z; }
00236
00237 Vector3 operator+ (const Vector3& b) const { return Vector3(x + b.x, y + b.y, z + b.z); }
00238 Vector3& operator+= (const Vector3& b) { x += b.x; y += b.y; z += b.z; return *this; }
00239 Vector3 operator- (const Vector3& b) const { return Vector3(x - b.x, y - b.y, z - b.z); }
00240 Vector3& operator-= (const Vector3& b) { x -= b.x; y -= b.y; z -= b.z; return *this; }
00241 Vector3 operator- () const { return Vector3(-x, -y, -z); }
00242
00243
00244 Vector3 operator* (T s) const { return Vector3(x*s, y*s, z*s); }
00245 Vector3& operator*= (T s) { x *= s; y *= s; z *= s; return *this; }
00246
00247 Vector3 operator/ (T s) const { T rcp = T(1)/s;
00248 return Vector3(x*rcp, y*rcp, z*rcp); }
00249 Vector3& operator/= (T s) { T rcp = T(1)/s;
00250 x *= rcp; y *= rcp; z *= rcp;
00251 return *this; }
00252
00253
00254 bool Compare(const Vector3&b, T tolerance = Mathf::Tolerance)
00255 {
00256 return (fabs(b.x-x) < tolerance) && (fabs(b.y-y) < tolerance) && (fabs(b.z-z) < tolerance);
00257 }
00258
00259
00260
00261
00262 T operator* (const Vector3& b) const { return x*b.x + y*b.y + z*b.z; }
00263
00264
00265
00266
00267 Vector3 Cross(const Vector3& b) const { return Vector3(y*b.z - z*b.y,
00268 z*b.x - x*b.z,
00269 x*b.y - y*b.x); }
00270
00271
00272 T Angle(const Vector3& b) const { return acos((*this * b)/(Length()*b.Length())); }
00273
00274
00275 T LengthSq() const { return (x * x + y * y + z * z); }
00276
00277 T Length() const { return sqrt(LengthSq()); }
00278
00279
00280 T Distance(Vector3& b) const { return (*this - b).Length(); }
00281
00282
00283 bool IsNormalized() const { return fabs(LengthSq() - T(1)) < Math<T>::Tolerance; }
00284
00285 void Normalize() { *this /= Length(); }
00286
00287 Vector3 Normalized() const { return *this / Length(); }
00288
00289
00290
00291 Vector3 Lerp(const Vector3& b, T f) const { return *this*(T(1) - f) + b*f; }
00292
00293
00294
00295 Vector3 ProjectTo(const Vector3& b) const { return b * ((*this * b) / b.LengthSq()); }
00296 };
00297
00298
00299 typedef Vector3<float> Vector3f;
00300 typedef Vector3<double> Vector3d;
00301
00302
00303
00304
00305
00306
00307
00308
00309
00310
00311
00312
00313
00314
00315
00316
00317
00318
00319
00320
00321
00322
00323
00324
00325
00326
00327
00328
00329
00330
00331 class Matrix4f
00332 {
00333 static Matrix4f IdentityValue;
00334
00335 public:
00336 float M[4][4];
00337
00338 enum NoInitType { NoInit };
00339
00340
00341 Matrix4f(NoInitType) { }
00342
00343
00344 Matrix4f()
00345 {
00346 SetIdentity();
00347 }
00348
00349 Matrix4f(float m11, float m12, float m13, float m14,
00350 float m21, float m22, float m23, float m24,
00351 float m31, float m32, float m33, float m34,
00352 float m41, float m42, float m43, float m44)
00353 {
00354 M[0][0] = m11; M[0][1] = m12; M[0][2] = m13; M[0][3] = m14;
00355 M[1][0] = m21; M[1][1] = m22; M[1][2] = m23; M[1][3] = m24;
00356 M[2][0] = m31; M[2][1] = m32; M[2][2] = m33; M[2][3] = m34;
00357 M[3][0] = m41; M[3][1] = m42; M[3][2] = m43; M[3][3] = m44;
00358 }
00359
00360 Matrix4f(float m11, float m12, float m13,
00361 float m21, float m22, float m23,
00362 float m31, float m32, float m33)
00363 {
00364 M[0][0] = m11; M[0][1] = m12; M[0][2] = m13; M[0][3] = 0;
00365 M[1][0] = m21; M[1][1] = m22; M[1][2] = m23; M[1][3] = 0;
00366 M[2][0] = m31; M[2][1] = m32; M[2][2] = m33; M[2][3] = 0;
00367 M[3][0] = 0; M[3][1] = 0; M[3][2] = 0; M[3][3] = 1;
00368 }
00369
00370 static const Matrix4f& Identity() { return IdentityValue; }
00371
00372 void SetIdentity()
00373 {
00374 M[0][0] = M[1][1] = M[2][2] = M[3][3] = 1;
00375 M[0][1] = M[1][0] = M[2][3] = M[3][1] = 0;
00376 M[0][2] = M[1][2] = M[2][0] = M[3][2] = 0;
00377 M[0][3] = M[1][3] = M[2][1] = M[3][0] = 0;
00378 }
00379
00380
00381 static Matrix4f& Multiply(Matrix4f* d, const Matrix4f& a, const Matrix4f& b)
00382 {
00383 OVR_ASSERT((d != &a) && (d != &b));
00384 int i = 0;
00385 do {
00386 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];
00387 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];
00388 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];
00389 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];
00390 } while((++i) < 4);
00391
00392 return *d;
00393 }
00394
00395 Matrix4f operator* (const Matrix4f& b) const
00396 {
00397 Matrix4f result(Matrix4f::NoInit);
00398 Multiply(&result, *this, b);
00399 return result;
00400 }
00401
00402 Matrix4f& operator*= (const Matrix4f& b)
00403 {
00404 return Multiply(this, Matrix4f(*this), b);
00405 }
00406
00407 Matrix4f operator* (float s) const
00408 {
00409 return Matrix4f(M[0][0] * s, M[0][1] * s, M[0][2] * s, M[0][3] * s,
00410 M[1][0] * s, M[1][1] * s, M[1][2] * s, M[1][3] * s,
00411 M[2][0] * s, M[2][1] * s, M[2][2] * s, M[2][3] * s,
00412 M[3][0] * s, M[3][1] * s, M[3][2] * s, M[3][3] * s);
00413 }
00414
00415 Matrix4f& operator*= (float s)
00416 {
00417 M[0][0] *= s; M[0][1] *= s; M[0][2] *= s; M[0][3] *= s;
00418 M[1][0] *= s; M[1][1] *= s; M[1][2] *= s; M[1][3] *= s;
00419 M[2][0] *= s; M[2][1] *= s; M[2][2] *= s; M[2][3] *= s;
00420 M[3][0] *= s; M[3][1] *= s; M[3][2] *= s; M[3][3] *= s;
00421 return *this;
00422 }
00423
00424 Vector3f Transform(const Vector3f& v) const
00425 {
00426 return Vector3f(M[0][0] * v.x + M[0][1] * v.y + M[0][2] * v.z + M[0][3],
00427 M[1][0] * v.x + M[1][1] * v.y + M[1][2] * v.z + M[1][3],
00428 M[2][0] * v.x + M[2][1] * v.y + M[2][2] * v.z + M[2][3]);
00429 }
00430
00431 Matrix4f Transposed() const
00432 {
00433 return Matrix4f(M[0][0], M[1][0], M[2][0], M[3][0],
00434 M[0][1], M[1][1], M[2][1], M[3][1],
00435 M[0][2], M[1][2], M[2][2], M[3][2],
00436 M[0][3], M[1][3], M[2][3], M[3][3]);
00437 }
00438
00439 void Transpose()
00440 {
00441 *this = Transposed();
00442 }
00443
00444
00445 float SubDet (const int* rows, const int* cols) const
00446 {
00447 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]])
00448 - 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]])
00449 + 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]]);
00450 }
00451
00452 float Cofactor(int I, int J) const
00453 {
00454 const int indices[4][3] = {{1,2,3},{0,2,3},{0,1,3},{0,1,2}};
00455 return ((I+J)&1) ? -SubDet(indices[I],indices[J]) : SubDet(indices[I],indices[J]);
00456 }
00457
00458 float Determinant() const
00459 {
00460 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);
00461 }
00462
00463 Matrix4f Adjugated() const
00464 {
00465 return Matrix4f(Cofactor(0,0), Cofactor(1,0), Cofactor(2,0), Cofactor(3,0),
00466 Cofactor(0,1), Cofactor(1,1), Cofactor(2,1), Cofactor(3,1),
00467 Cofactor(0,2), Cofactor(1,2), Cofactor(2,2), Cofactor(3,2),
00468 Cofactor(0,3), Cofactor(1,3), Cofactor(2,3), Cofactor(3,3));
00469 }
00470
00471 Matrix4f Inverted() const
00472 {
00473 float det = Determinant();
00474 assert(det != 0);
00475 return Adjugated() * (1.0f/det);
00476 }
00477
00478 void Invert()
00479 {
00480 *this = Inverted();
00481 }
00482
00483
00484
00485
00486
00487
00488
00489 template <Axis A1, Axis A2, Axis A3, RotateDirection D, HandedSystem S>
00490 void ToEulerAngles(float *a, float *b, float *c)
00491 {
00492 OVR_COMPILER_ASSERT((A1 != A2) && (A2 != A3) && (A1 != A3));
00493
00494 float psign = -1.0f;
00495 if (((A1 + 1) % 3 == A2) && ((A2 + 1) % 3 == A3))
00496 psign = 1.0f;
00497
00498 float pm = psign*M[A1][A3];
00499 if (pm < -1.0f + Math<float>::SingularityRadius)
00500 {
00501 *a = 0.0f;
00502 *b = -S*D*Math<float>::PiOver2;
00503 *c = S*D*atan2( psign*M[A2][A1], M[A2][A2] );
00504 }
00505 else if (pm > 1.0 - Math<float>::SingularityRadius)
00506 {
00507 *a = 0.0f;
00508 *b = S*D*Math<float>::PiOver2;
00509 *c = S*D*atan2( psign*M[A2][A1], M[A2][A2] );
00510 }
00511 else
00512 {
00513 *a = S*D*atan2( -psign*M[A2][A3], M[A3][A3] );
00514 *b = S*D*asin(pm);
00515 *c = S*D*atan2( -psign*M[A1][A2], M[A1][A1] );
00516 }
00517
00518 return;
00519 }
00520
00521
00522
00523
00524
00525
00526
00527 template <Axis A1, Axis A2, RotateDirection D, HandedSystem S>
00528 void ToEulerAnglesABA(float *a, float *b, float *c)
00529 {
00530 OVR_COMPILER_ASSERT(A1 != A2);
00531
00532
00533 int m = 3 - A1 - A2;
00534
00535 float psign = -1.0f;
00536 if ((A1 + 1) % 3 == A2)
00537 psign = 1.0f;
00538
00539 float c2 = M[A1][A1];
00540 if (c2 < -1.0 + Math<float>::SingularityRadius)
00541 {
00542 *a = 0.0f;
00543 *b = S*D*Math<float>::Pi;
00544 *c = S*D*atan2( -psign*M[A2][m],M[A2][A2]);
00545 }
00546 else if (c2 > 1.0 - Math<float>::SingularityRadius)
00547 {
00548 *a = 0.0f;
00549 *b = 0.0f;
00550 *c = S*D*atan2( -psign*M[A2][m],M[A2][A2]);
00551 }
00552 else
00553 {
00554 *a = S*D*atan2( M[A2][A1],-psign*M[m][A1]);
00555 *b = S*D*acos(c2);
00556 *c = S*D*atan2( M[A1][A2],psign*M[A1][m]);
00557 }
00558 return;
00559 }
00560
00561
00562
00563
00564 static Matrix4f AxisConversion(const WorldAxes& to, const WorldAxes& from)
00565 {
00566
00567 int toArray[3] = { to.XAxis, to.YAxis, to.ZAxis };
00568
00569
00570 int inv[4];
00571 inv[0] = inv[abs(to.XAxis)] = 0;
00572 inv[abs(to.YAxis)] = 1;
00573 inv[abs(to.ZAxis)] = 2;
00574
00575 Matrix4f m(0, 0, 0,
00576 0, 0, 0,
00577 0, 0, 0);
00578
00579
00580 m.M[inv[abs(from.XAxis)]][0] = float(from.XAxis/toArray[inv[abs(from.XAxis)]]);
00581 m.M[inv[abs(from.YAxis)]][1] = float(from.YAxis/toArray[inv[abs(from.YAxis)]]);
00582 m.M[inv[abs(from.ZAxis)]][2] = float(from.ZAxis/toArray[inv[abs(from.ZAxis)]]);
00583 return m;
00584 }
00585
00586
00587
00588 static Matrix4f Translation(const Vector3f& v)
00589 {
00590 Matrix4f t;
00591 t.M[0][3] = v.x;
00592 t.M[1][3] = v.y;
00593 t.M[2][3] = v.z;
00594 return t;
00595 }
00596
00597 static Matrix4f Translation(float x, float y, float z = 0.0f)
00598 {
00599 Matrix4f t;
00600 t.M[0][3] = x;
00601 t.M[1][3] = y;
00602 t.M[2][3] = z;
00603 return t;
00604 }
00605
00606 static Matrix4f Scaling(const Vector3f& v)
00607 {
00608 Matrix4f t;
00609 t.M[0][0] = v.x;
00610 t.M[1][1] = v.y;
00611 t.M[2][2] = v.z;
00612 return t;
00613 }
00614
00615 static Matrix4f Scaling(float x, float y, float z)
00616 {
00617 Matrix4f t;
00618 t.M[0][0] = x;
00619 t.M[1][1] = y;
00620 t.M[2][2] = z;
00621 return t;
00622 }
00623
00624 static Matrix4f Scaling(float s)
00625 {
00626 Matrix4f t;
00627 t.M[0][0] = s;
00628 t.M[1][1] = s;
00629 t.M[2][2] = s;
00630 return t;
00631 }
00632
00633
00634
00635
00636 static Matrix4f RotationAxis(Axis A, float angle, RotateDirection d, HandedSystem s)
00637 {
00638 float sina = s * d *sin(angle);
00639 float cosa = cos(angle);
00640
00641 switch(A)
00642 {
00643 case Axis_X:
00644 return Matrix4f(1, 0, 0,
00645 0, cosa, -sina,
00646 0, sina, cosa);
00647 case Axis_Y:
00648 return Matrix4f(cosa, 0, sina,
00649 0, 1, 0,
00650 -sina, 0, cosa);
00651 case Axis_Z:
00652 return Matrix4f(cosa, -sina, 0,
00653 sina, cosa, 0,
00654 0, 0, 1);
00655 }
00656 }
00657
00658
00659
00660
00661
00662
00663
00664
00665
00666 static Matrix4f RotationX(float angle)
00667 {
00668 float sina = sin(angle);
00669 float cosa = cos(angle);
00670 return Matrix4f(1, 0, 0,
00671 0, cosa, -sina,
00672 0, sina, cosa);
00673 }
00674
00675
00676
00677
00678
00679
00680
00681
00682 static Matrix4f RotationY(float angle)
00683 {
00684 float sina = sin(angle);
00685 float cosa = cos(angle);
00686 return Matrix4f(cosa, 0, sina,
00687 0, 1, 0,
00688 -sina, 0, cosa);
00689 }
00690
00691
00692
00693
00694
00695
00696
00697
00698 static Matrix4f RotationZ(float angle)
00699 {
00700 float sina = sin(angle);
00701 float cosa = cos(angle);
00702 return Matrix4f(cosa, -sina, 0,
00703 sina, cosa, 0,
00704 0, 0, 1);
00705 }
00706
00707
00708
00709
00710
00711
00712 static Matrix4f LookAtRH(const Vector3f& eye, const Vector3f& at, const Vector3f& up);
00713
00714
00715
00716
00717 static Matrix4f LookAtLH(const Vector3f& eye, const Vector3f& at, const Vector3f& up);
00718
00719
00720
00721
00722
00723
00724
00725
00726
00727
00728
00729 static Matrix4f PerspectiveRH(float yfov, float aspect, float znear, float zfar);
00730
00731
00732
00733
00734
00735
00736
00737
00738
00739 static Matrix4f PerspectiveLH(float yfov, float aspect, float znear, float zfar);
00740
00741
00742 static Matrix4f Ortho2D(float w, float h);
00743 };
00744
00745
00746
00747
00748
00749
00750
00751
00752
00753
00754
00755 template<class T>
00756 class Quat
00757 {
00758 public:
00759
00760 T x, y, z, w;
00761
00762 Quat() : x(0), y(0), z(0), w(1) {}
00763 Quat(T x_, T y_, T z_, T w_) : x(x_), y(y_), z(z_), w(w_) {}
00764
00765
00766
00767 Quat(const Vector3<T>& axis, T angle)
00768 {
00769 Vector3<T> unitAxis = axis.Normalized();
00770 T sinHalfAngle = sin(angle * T(0.5));
00771
00772 w = cos(angle * T(0.5));
00773 x = unitAxis.x * sinHalfAngle;
00774 y = unitAxis.y * sinHalfAngle;
00775 z = unitAxis.z * sinHalfAngle;
00776 }
00777
00778
00779 void AxisAngle(Axis A, T angle, RotateDirection d, HandedSystem s)
00780 {
00781 T sinHalfAngle = s * d *sin(angle * (T)0.5);
00782 T v[3];
00783 v[0] = v[1] = v[2] = (T)0;
00784 v[A] = sinHalfAngle;
00785
00786 w = cos(angle * (T)0.5);
00787 x = v[0];
00788 y = v[1];
00789 z = v[2];
00790 }
00791
00792
00793 void GetAxisAngle(Vector3<T>* axis, T* angle) const
00794 {
00795 if (LengthSq() > Math<T>::Tolerance * Math<T>::Tolerance)
00796 {
00797 *axis = Vector3<T>(x, y, z).Normalized();
00798 *angle = 2 * acos(w);
00799 }
00800 else
00801 {
00802 *axis = Vector3<T>(1, 0, 0);
00803 *angle= 0;
00804 }
00805 }
00806
00807 bool operator== (const Quat& b) const { return x == b.x && y == b.y && z == b.z && w == b.w; }
00808 bool operator!= (const Quat& b) const { return x != b.x || y != b.y || z != b.z || w != b.w; }
00809
00810 Quat operator+ (const Quat& b) const { return Quat(x + b.x, y + b.y, z + b.z, w + b.w); }
00811 Quat& operator+= (const Quat& b) { w += b.w; x += b.x; y += b.y; z += b.z; return *this; }
00812 Quat operator- (const Quat& b) const { return Quat(x - b.x, y - b.y, z - b.z, w - b.w); }
00813 Quat& operator-= (const Quat& b) { w -= b.w; x -= b.x; y -= b.y; z -= b.z; return *this; }
00814
00815 Quat operator* (T s) const { return Quat(x * s, y * s, z * s, w * s); }
00816 Quat& operator*= (T s) { w *= s; x *= s; y *= s; z *= s; return *this; }
00817 Quat operator/ (T s) const { T rcp = T(1)/s; return Quat(x * rcp, y * rcp, z * rcp, w *rcp); }
00818 Quat& operator/= (T s) { T rcp = T(1)/s; w *= rcp; x *= rcp; y *= rcp; z *= rcp; return *this; }
00819
00820
00821 Vector3<T> Imag() const { return Vector3<T>(x,y,z); }
00822
00823
00824 T Length() const { return sqrt(x * x + y * y + z * z + w * w); }
00825
00826 T LengthSq() const { return (x * x + y * y + z * z + w * w); }
00827
00828 T Distance(const Quat& q) const
00829 {
00830 T d1 = (*this - q).Length();
00831 T d2 = (*this + q).Length();
00832 return (d1 < d2) ? d1 : d2;
00833 }
00834 T DistanceSq(const Quat& q) const
00835 {
00836 T d1 = (*this - q).LengthSq();
00837 T d2 = (*this + q).LengthSq();
00838 return (d1 < d2) ? d1 : d2;
00839 }
00840
00841
00842 bool IsNormalized() const { return fabs(LengthSq() - 1) < Math<T>::Tolerance; }
00843 void Normalize() { *this /= Length(); }
00844 Quat Normalized() const { return *this / Length(); }
00845
00846
00847 Quat Conj() const { return Quat(-x, -y, -z, w); }
00848
00849
00850
00851
00852 Quat operator* (const Quat& b) const { return Quat(w * b.x + x * b.w + y * b.z - z * b.y,
00853 w * b.y - x * b.z + y * b.w + z * b.x,
00854 w * b.z + x * b.y - y * b.x + z * b.w,
00855 w * b.w - x * b.x - y * b.y - z * b.z); }
00856
00857
00858
00859 Quat PowNormalized(T p) const
00860 {
00861 Vector3<T> v;
00862 T a;
00863 GetAxisAngle(&v, &a);
00864 return Quat(v, a * p);
00865 }
00866
00867
00868
00869 Vector3<T> Rotate(const Vector3<T>& v) const
00870 {
00871 return ((*this * Quat<T>(v.x, v.y, v.z, 0)) * Inverted()).Imag();
00872 }
00873
00874
00875
00876 Quat Inverted() const
00877 {
00878 return Quat(-x, -y, -z, w);
00879 }
00880
00881
00882 void Invert() const
00883 {
00884 *this = Quat(-x, -y, -z, w);
00885 }
00886
00887
00888 operator Matrix4f() const
00889 {
00890 T ww = w*w;
00891 T xx = x*x;
00892 T yy = y*y;
00893 T zz = z*z;
00894
00895 return Matrix4f(float(ww + xx - yy - zz), float(T(2) * (x*y - w*z)), float(T(2) * (x*z + w*y)),
00896 float(T(2) * (x*y + w*z)), float(ww - xx + yy - zz), float(T(2) * (y*z - w*x)),
00897 float(T(2) * (x*z - w*y)), float(T(2) * (y*z + w*x)), float(ww - xx - yy + zz) );
00898 }
00899
00900
00901
00902
00903
00904
00905
00906
00907
00908
00909 template <Axis A1, Axis A2, Axis A3, RotateDirection D, HandedSystem S>
00910 void GetEulerAngles(T *a, T *b, T *c)
00911 {
00912 OVR_COMPILER_ASSERT((A1 != A2) && (A2 != A3) && (A1 != A3));
00913
00914 T Q[3] = { x, y, z };
00915
00916 T ww = w*w;
00917 T Q11 = Q[A1]*Q[A1];
00918 T Q22 = Q[A2]*Q[A2];
00919 T Q33 = Q[A3]*Q[A3];
00920
00921 T psign = T(-1.0);
00922
00923 if (((A1 + 1) % 3 == A2) && ((A2 + 1) % 3 == A3))
00924 psign = T(1.0);
00925
00926 T s2 = psign * T(2.0) * (psign*w*Q[A2] + Q[A1]*Q[A3]);
00927
00928 if (s2 < (T)-1.0 + Math<T>::SingularityRadius)
00929 {
00930 *a = T(0.0);
00931 *b = -S*D*Math<T>::PiOver2;
00932 *c = S*D*atan2((T)2.0*(psign*Q[A1]*Q[A2] + w*Q[A3]),
00933 ww + Q22 - Q11 - Q33 );
00934 }
00935 else if (s2 > (T)1.0 - Math<T>::SingularityRadius)
00936 {
00937 *a = (T)0.0;
00938 *b = S*D*Math<T>::PiOver2;
00939 *c = S*D*atan2((T)2.0*(psign*Q[A1]*Q[A2] + w*Q[A3]),
00940 ww + Q22 - Q11 - Q33);
00941 }
00942 else
00943 {
00944 *a = -S*D*atan2((T)-2.0*(w*Q[A1] - psign*Q[A2]*Q[A3]),
00945 ww + Q33 - Q11 - Q22);
00946 *b = S*D*asin(s2);
00947 *c = S*D*atan2((T)2.0*(w*Q[A3] - psign*Q[A1]*Q[A2]),
00948 ww + Q11 - Q22 - Q33);
00949 }
00950 return;
00951 }
00952
00953 template <Axis A1, Axis A2, Axis A3, RotateDirection D>
00954 void GetEulerAngles(T *a, T *b, T *c)
00955 { GetEulerAngles<A1, A2, A3, D, Handed_R>(a, b, c); }
00956
00957 template <Axis A1, Axis A2, Axis A3>
00958 void GetEulerAngles(T *a, T *b, T *c)
00959 { GetEulerAngles<A1, A2, A3, Rotate_CCW, Handed_R>(a, b, c); }
00960
00961
00962
00963
00964
00965
00966
00967
00968
00969
00970 template <Axis A1, Axis A2, RotateDirection D, HandedSystem S>
00971 void GetEulerAnglesABA(T *a, T *b, T *c)
00972 {
00973 OVR_COMPILER_ASSERT(A1 != A2);
00974
00975 T Q[3] = {x, y, z};
00976
00977
00978 int m = 3 - A1 - A2;
00979
00980 T ww = w*w;
00981 T Q11 = Q[A1]*Q[A1];
00982 T Q22 = Q[A2]*Q[A2];
00983 T Qmm = Q[m]*Q[m];
00984
00985 T psign = T(-1.0);
00986 if ((A1 + 1) % 3 == A2)
00987 {
00988 psign = (T)1.0;
00989 }
00990
00991 T c2 = ww + Q11 - Q22 - Qmm;
00992 if (c2 < (T)-1.0 + Math<T>::SingularityRadius)
00993 {
00994 *a = (T)0.0;
00995 *b = S*D*Math<T>::Pi;
00996 *c = S*D*atan2( (T)2.0*(w*Q[A1] - psign*Q[A2]*Q[m]),
00997 ww + Q22 - Q11 - Qmm);
00998 }
00999 else if (c2 > (T)1.0 - Math<T>::SingularityRadius)
01000 {
01001 *a = (T)0.0;
01002 *b = (T)0.0;
01003 *c = S*D*atan2( (T)2.0*(w*Q[A1] - psign*Q[A2]*Q[m]),
01004 ww + Q22 - Q11 - Qmm);
01005 }
01006 else
01007 {
01008 *a = S*D*atan2( psign*w*Q[m] + Q[A1]*Q[A2],
01009 w*Q[A2] -psign*Q[A1]*Q[m]);
01010 *b = S*D*acos(c2);
01011 *c = S*D*atan2( -psign*w*Q[m] + Q[A1]*Q[A2],
01012 w*Q[A2] + psign*Q[A1]*Q[m]);
01013 }
01014 return;
01015 }
01016 };
01017
01018
01019 typedef Quat<float> Quatf;
01020 typedef Quat<double> Quatd;
01021
01022
01023
01024
01025
01026
01027
01028
01029
01030
01031 template<class T>
01032 class Angle
01033 {
01034 public:
01035 enum AngularUnits
01036 {
01037 Radians = 0,
01038 Degrees = 1
01039 };
01040
01041 Angle() : a(0) {}
01042
01043
01044 Angle(T a_, AngularUnits u = Radians) : a((u == Radians) ? a_ : a_*Math<T>::DegreeToRadFactor) { FixRange(); }
01045
01046 T Get(AngularUnits u = Radians) const { return (u == Radians) ? a : a*Math<T>::RadToDegreeFactor; }
01047 void Set(const T& x, AngularUnits u = Radians) { a = (u == Radians) ? x : x*Math<T>::DegreeToRadFactor; FixRange(); }
01048 int Sign() const { if (a == 0) return 0; else return (a > 0) ? 1 : -1; }
01049 T Abs() const { return (a > 0) ? a : -a; }
01050
01051 bool operator== (const Angle& b) const { return a == b.a; }
01052 bool operator!= (const Angle& b) const { return a != b.a; }
01053
01054
01055
01056
01057
01058
01059
01060 Angle operator+ (const Angle& b) const { return Angle(a + b.a); }
01061 Angle operator+ (const T& x) const { return Angle(a + x); }
01062 Angle& operator+= (const Angle& b) { a = a + b.a; FastFixRange(); return *this; }
01063 Angle& operator+= (const T& x) { a = a + x; FixRange(); return *this; }
01064 Angle operator- (const Angle& b) const { return Angle(a - b.a); }
01065 Angle operator- (const T& x) const { return Angle(a - x); }
01066 Angle& operator-= (const Angle& b) { a = a - b.a; FastFixRange(); return *this; }
01067 Angle& operator-= (const T& x) { a = a - x; FixRange(); return *this; }
01068
01069 T Distance(const Angle& b) { T c = fabs(a - b.a); return (c <= Math<T>::Pi) ? c : Math<T>::TwoPi - c; }
01070
01071 private:
01072
01073
01074 T a;
01075
01076
01077 inline void FastFixRange()
01078 {
01079 if (a < -Math<T>::Pi)
01080 a += Math<T>::TwoPi;
01081 else if (a > Math<T>::Pi)
01082 a -= Math<T>::TwoPi;
01083 }
01084
01085
01086 inline void FixRange()
01087 {
01088 a = fmod(a,Math<T>::TwoPi);
01089 if (a < -Math<T>::Pi)
01090 a += Math<T>::TwoPi;
01091 else if (a > Math<T>::Pi)
01092 a -= Math<T>::TwoPi;
01093 }
01094 };
01095
01096
01097 typedef Angle<float> Anglef;
01098 typedef Angle<double> Angled;
01099
01100
01101
01102
01103
01104
01105
01106 template<class T>
01107 class Plane : public RefCountBase<Plane<T> >
01108 {
01109 public:
01110 Vector3<T> N;
01111 T D;
01112
01113 Plane() : D(0) {}
01114
01115
01116 Plane(const Vector3<T>& n, T d) : N(n), D(d) {}
01117 Plane(T x, T y, T z, T d) : N(x,y,z), D(d) {}
01118
01119
01120 Plane(const Vector3<T>& p, const Vector3<T>& n) : N(n), D(-(p * n)) {}
01121
01122
01123 T TestSide(const Vector3<T>& p) const
01124 {
01125 return (N * p) + D;
01126 }
01127
01128 Plane<T> Flipped() const
01129 {
01130 return Plane(-N, -D);
01131 }
01132
01133 void Flip()
01134 {
01135 N = -N;
01136 D = -D;
01137 }
01138
01139 bool operator==(const Plane<T>& rhs) const
01140 {
01141 return (this->D == rhs.D && this->N == rhs.N);
01142 }
01143 };
01144
01145 typedef Plane<float> Planef;
01146
01147 }
01148
01149 #endif