oculus1

annotate libovr/Src/Kernel/OVR_Math.h @ 29:9a973ef0e2a3

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