oculus1

annotate libovr/Src/Kernel/OVR_Math.h @ 1:e2f9e4603129

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