oculus1
diff libovr/Src/Kernel/OVR_Math.h @ 3:b069a5c27388
added a couple more stuff, fixed all the LibOVR line endings
author | John Tsiombikas <nuclear@member.fsf.org> |
---|---|
date | Sun, 15 Sep 2013 04:10:05 +0300 |
parents | e2f9e4603129 |
children |
line diff
1.1 --- a/libovr/Src/Kernel/OVR_Math.h Sat Sep 14 17:51:03 2013 +0300 1.2 +++ b/libovr/Src/Kernel/OVR_Math.h Sun Sep 15 04:10:05 2013 +0300 1.3 @@ -1,1 +1,1149 @@ 1.4 -/************************************************************************************ 1.5 1.6 PublicHeader: OVR.h 1.7 Filename : OVR_Math.h 1.8 Content : Implementation of 3D primitives such as vectors, matrices. 1.9 Created : September 4, 2012 1.10 Authors : Andrew Reisse, Michael Antonov, Steve LaValle, Anna Yershova 1.11 1.12 Copyright : Copyright 2012 Oculus VR, Inc. All Rights reserved. 1.13 1.14 Use of this software is subject to the terms of the Oculus license 1.15 agreement provided at the time of installation or download, or which 1.16 otherwise accompanies this software in either electronic or hard copy form. 1.17 1.18 *************************************************************************************/ 1.19 1.20 #ifndef OVR_Math_h 1.21 #define OVR_Math_h 1.22 1.23 #include <assert.h> 1.24 #include <stdlib.h> 1.25 #include <math.h> 1.26 1.27 #include "OVR_Types.h" 1.28 #include "OVR_RefCount.h" 1.29 1.30 namespace OVR { 1.31 1.32 //------------------------------------------------------------------------------------- 1.33 // Constants for 3D world/axis definitions. 1.34 1.35 // Definitions of axes for coordinate and rotation conversions. 1.36 enum Axis 1.37 { 1.38 Axis_X = 0, Axis_Y = 1, Axis_Z = 2 1.39 }; 1.40 1.41 // RotateDirection describes the rotation direction around an axis, interpreted as follows: 1.42 // CW - Clockwise while looking "down" from positive axis towards the origin. 1.43 // CCW - Counter-clockwise while looking from the positive axis towards the origin, 1.44 // which is in the negative axis direction. 1.45 // CCW is the default for the RHS coordinate system. Oculus standard RHS coordinate 1.46 // system defines Y up, X right, and Z back (pointing out from the screen). In this 1.47 // system Rotate_CCW around Z will specifies counter-clockwise rotation in XY plane. 1.48 enum RotateDirection 1.49 { 1.50 Rotate_CCW = 1, 1.51 Rotate_CW = -1 1.52 }; 1.53 1.54 enum HandedSystem 1.55 { 1.56 Handed_R = 1, Handed_L = -1 1.57 }; 1.58 1.59 // AxisDirection describes which way the axis points. Used by WorldAxes. 1.60 enum AxisDirection 1.61 { 1.62 Axis_Up = 2, 1.63 Axis_Down = -2, 1.64 Axis_Right = 1, 1.65 Axis_Left = -1, 1.66 Axis_In = 3, 1.67 Axis_Out = -3 1.68 }; 1.69 1.70 struct WorldAxes 1.71 { 1.72 AxisDirection XAxis, YAxis, ZAxis; 1.73 1.74 WorldAxes(AxisDirection x, AxisDirection y, AxisDirection z) 1.75 : XAxis(x), YAxis(y), ZAxis(z) 1.76 { OVR_ASSERT(abs(x) != abs(y) && abs(y) != abs(z) && abs(z) != abs(x));} 1.77 }; 1.78 1.79 1.80 //------------------------------------------------------------------------------------- 1.81 // ***** Math 1.82 1.83 // Math class contains constants and functions. This class is a template specialized 1.84 // per type, with Math<float> and Math<double> being distinct. 1.85 template<class Type> 1.86 class Math 1.87 { 1.88 }; 1.89 1.90 // Single-precision Math constants class. 1.91 template<> 1.92 class Math<float> 1.93 { 1.94 public: 1.95 static const float Pi; 1.96 static const float TwoPi; 1.97 static const float PiOver2; 1.98 static const float PiOver4; 1.99 static const float E; 1.100 1.101 static const float MaxValue; // Largest positive float Value 1.102 static const float MinPositiveValue; // Smallest possible positive value 1.103 1.104 static const float RadToDegreeFactor; 1.105 static const float DegreeToRadFactor; 1.106 1.107 static const float Tolerance; // 0.00001f; 1.108 static const float SingularityRadius; //0.00000000001f for Gimbal lock numerical problems 1.109 }; 1.110 1.111 // Double-precision Math constants class. 1.112 template<> 1.113 class Math<double> 1.114 { 1.115 public: 1.116 static const double Pi; 1.117 static const double TwoPi; 1.118 static const double PiOver2; 1.119 static const double PiOver4; 1.120 static const double E; 1.121 1.122 static const double MaxValue; // Largest positive double Value 1.123 static const double MinPositiveValue; // Smallest possible positive value 1.124 1.125 static const double RadToDegreeFactor; 1.126 static const double DegreeToRadFactor; 1.127 1.128 static const double Tolerance; // 0.00001f; 1.129 static const double SingularityRadius; //0.00000000001 for Gimbal lock numerical problems 1.130 }; 1.131 1.132 typedef Math<float> Mathf; 1.133 typedef Math<double> Mathd; 1.134 1.135 // Conversion functions between degrees and radians 1.136 template<class FT> 1.137 FT RadToDegree(FT rads) { return rads * Math<FT>::RadToDegreeFactor; } 1.138 template<class FT> 1.139 FT DegreeToRad(FT rads) { return rads * Math<FT>::DegreeToRadFactor; } 1.140 1.141 template<class T> 1.142 class Quat; 1.143 1.144 //------------------------------------------------------------------------------------- 1.145 // ***** Vector2f - 2D Vector2f 1.146 1.147 // Vector2f represents a 2-dimensional vector or point in space, 1.148 // consisting of coordinates x and y, 1.149 1.150 template<class T> 1.151 class Vector2 1.152 { 1.153 public: 1.154 T x, y; 1.155 1.156 Vector2() : x(0), y(0) { } 1.157 Vector2(T x_, T y_) : x(x_), y(y_) { } 1.158 explicit Vector2(T s) : x(s), y(s) { } 1.159 1.160 bool operator== (const Vector2& b) const { return x == b.x && y == b.y; } 1.161 bool operator!= (const Vector2& b) const { return x != b.x || y != b.y; } 1.162 1.163 Vector2 operator+ (const Vector2& b) const { return Vector2(x + b.x, y + b.y); } 1.164 Vector2& operator+= (const Vector2& b) { x += b.x; y += b.y; return *this; } 1.165 Vector2 operator- (const Vector2& b) const { return Vector2(x - b.x, y - b.y); } 1.166 Vector2& operator-= (const Vector2& b) { x -= b.x; y -= b.y; return *this; } 1.167 Vector2 operator- () const { return Vector2(-x, -y); } 1.168 1.169 // Scalar multiplication/division scales vector. 1.170 Vector2 operator* (T s) const { return Vector2(x*s, y*s); } 1.171 Vector2& operator*= (T s) { x *= s; y *= s; return *this; } 1.172 1.173 Vector2 operator/ (T s) const { T rcp = T(1)/s; 1.174 return Vector2(x*rcp, y*rcp); } 1.175 Vector2& operator/= (T s) { T rcp = T(1)/s; 1.176 x *= rcp; y *= rcp; 1.177 return *this; } 1.178 1.179 // Compare two vectors for equality with tolerance. Returns true if vectors match withing tolerance. 1.180 bool Compare(const Vector2&b, T tolerance = Mathf::Tolerance) 1.181 { 1.182 return (fabs(b.x-x) < tolerance) && (fabs(b.y-y) < tolerance); 1.183 } 1.184 1.185 // Dot product overload. 1.186 // Used to calculate angle q between two vectors among other things, 1.187 // as (A dot B) = |a||b|cos(q). 1.188 T operator* (const Vector2& b) const { return x*b.x + y*b.y; } 1.189 1.190 // Returns the angle from this vector to b, in radians. 1.191 T Angle(const Vector2& b) const { return acos((*this * b)/(Length()*b.Length())); } 1.192 1.193 // Return Length of the vector squared. 1.194 T LengthSq() const { return (x * x + y * y); } 1.195 // Return vector length. 1.196 T Length() const { return sqrt(LengthSq()); } 1.197 1.198 // Returns distance between two points represented by vectors. 1.199 T Distance(Vector2& b) const { return (*this - b).Length(); } 1.200 1.201 // Determine if this a unit vector. 1.202 bool IsNormalized() const { return fabs(LengthSq() - T(1)) < Math<T>::Tolerance; } 1.203 // Normalize, convention vector length to 1. 1.204 void Normalize() { *this /= Length(); } 1.205 // Returns normalized (unit) version of the vector without modifying itself. 1.206 Vector2 Normalized() const { return *this / Length(); } 1.207 1.208 // Linearly interpolates from this vector to another. 1.209 // Factor should be between 0.0 and 1.0, with 0 giving full value to this. 1.210 Vector2 Lerp(const Vector2& b, T f) const { return *this*(T(1) - f) + b*f; } 1.211 1.212 // Projects this vector onto the argument; in other words, 1.213 // A.Project(B) returns projection of vector A onto B. 1.214 Vector2 ProjectTo(const Vector2& b) const { return b * ((*this * b) / b.LengthSq()); } 1.215 }; 1.216 1.217 1.218 typedef Vector2<float> Vector2f; 1.219 typedef Vector2<double> Vector2d; 1.220 1.221 //------------------------------------------------------------------------------------- 1.222 // ***** Vector3f - 3D Vector3f 1.223 1.224 // Vector3f represents a 3-dimensional vector or point in space, 1.225 // consisting of coordinates x, y and z. 1.226 1.227 template<class T> 1.228 class Vector3 1.229 { 1.230 public: 1.231 T x, y, z; 1.232 1.233 Vector3() : x(0), y(0), z(0) { } 1.234 Vector3(T x_, T y_, T z_ = 0) : x(x_), y(y_), z(z_) { } 1.235 explicit Vector3(T s) : x(s), y(s), z(s) { } 1.236 1.237 bool operator== (const Vector3& b) const { return x == b.x && y == b.y && z == b.z; } 1.238 bool operator!= (const Vector3& b) const { return x != b.x || y != b.y || z != b.z; } 1.239 1.240 Vector3 operator+ (const Vector3& b) const { return Vector3(x + b.x, y + b.y, z + b.z); } 1.241 Vector3& operator+= (const Vector3& b) { x += b.x; y += b.y; z += b.z; return *this; } 1.242 Vector3 operator- (const Vector3& b) const { return Vector3(x - b.x, y - b.y, z - b.z); } 1.243 Vector3& operator-= (const Vector3& b) { x -= b.x; y -= b.y; z -= b.z; return *this; } 1.244 Vector3 operator- () const { return Vector3(-x, -y, -z); } 1.245 1.246 // Scalar multiplication/division scales vector. 1.247 Vector3 operator* (T s) const { return Vector3(x*s, y*s, z*s); } 1.248 Vector3& operator*= (T s) { x *= s; y *= s; z *= s; return *this; } 1.249 1.250 Vector3 operator/ (T s) const { T rcp = T(1)/s; 1.251 return Vector3(x*rcp, y*rcp, z*rcp); } 1.252 Vector3& operator/= (T s) { T rcp = T(1)/s; 1.253 x *= rcp; y *= rcp; z *= rcp; 1.254 return *this; } 1.255 1.256 // Compare two vectors for equality with tolerance. Returns true if vectors match withing tolerance. 1.257 bool Compare(const Vector3&b, T tolerance = Mathf::Tolerance) 1.258 { 1.259 return (fabs(b.x-x) < tolerance) && (fabs(b.y-y) < tolerance) && (fabs(b.z-z) < tolerance); 1.260 } 1.261 1.262 // Dot product overload. 1.263 // Used to calculate angle q between two vectors among other things, 1.264 // as (A dot B) = |a||b|cos(q). 1.265 T operator* (const Vector3& b) const { return x*b.x + y*b.y + z*b.z; } 1.266 1.267 // Compute cross product, which generates a normal vector. 1.268 // Direction vector can be determined by right-hand rule: Pointing index finder in 1.269 // direction a and middle finger in direction b, thumb will point in a.Cross(b). 1.270 Vector3 Cross(const Vector3& b) const { return Vector3(y*b.z - z*b.y, 1.271 z*b.x - x*b.z, 1.272 x*b.y - y*b.x); } 1.273 1.274 // Returns the angle from this vector to b, in radians. 1.275 T Angle(const Vector3& b) const { return acos((*this * b)/(Length()*b.Length())); } 1.276 1.277 // Return Length of the vector squared. 1.278 T LengthSq() const { return (x * x + y * y + z * z); } 1.279 // Return vector length. 1.280 T Length() const { return sqrt(LengthSq()); } 1.281 1.282 // Returns distance between two points represented by vectors. 1.283 T Distance(Vector3& b) const { return (*this - b).Length(); } 1.284 1.285 // Determine if this a unit vector. 1.286 bool IsNormalized() const { return fabs(LengthSq() - T(1)) < Math<T>::Tolerance; } 1.287 // Normalize, convention vector length to 1. 1.288 void Normalize() { *this /= Length(); } 1.289 // Returns normalized (unit) version of the vector without modifying itself. 1.290 Vector3 Normalized() const { return *this / Length(); } 1.291 1.292 // Linearly interpolates from this vector to another. 1.293 // Factor should be between 0.0 and 1.0, with 0 giving full value to this. 1.294 Vector3 Lerp(const Vector3& b, T f) const { return *this*(T(1) - f) + b*f; } 1.295 1.296 // Projects this vector onto the argument; in other words, 1.297 // A.Project(B) returns projection of vector A onto B. 1.298 Vector3 ProjectTo(const Vector3& b) const { return b * ((*this * b) / b.LengthSq()); } 1.299 }; 1.300 1.301 1.302 typedef Vector3<float> Vector3f; 1.303 typedef Vector3<double> Vector3d; 1.304 1.305 1.306 //------------------------------------------------------------------------------------- 1.307 // ***** Matrix4f 1.308 1.309 // Matrix4f is a 4x4 matrix used for 3d transformations and projections. 1.310 // Translation stored in the last column. 1.311 // The matrix is stored in row-major order in memory, meaning that values 1.312 // of the first row are stored before the next one. 1.313 // 1.314 // The arrangement of the matrix is chosen to be in Right-Handed 1.315 // coordinate system and counterclockwise rotations when looking down 1.316 // the axis 1.317 // 1.318 // Transformation Order: 1.319 // - Transformations are applied from right to left, so the expression 1.320 // M1 * M2 * M3 * V means that the vector V is transformed by M3 first, 1.321 // followed by M2 and M1. 1.322 // 1.323 // Coordinate system: Right Handed 1.324 // 1.325 // Rotations: Counterclockwise when looking down the axis. All angles are in radians. 1.326 // 1.327 // | sx 01 02 tx | // First column (sx, 10, 20): Axis X basis vector. 1.328 // | 10 sy 12 ty | // Second column (01, sy, 21): Axis Y basis vector. 1.329 // | 20 21 sz tz | // Third columnt (02, 12, sz): Axis Z basis vector. 1.330 // | 30 31 32 33 | 1.331 // 1.332 // The basis vectors are first three columns. 1.333 1.334 class Matrix4f 1.335 { 1.336 static Matrix4f IdentityValue; 1.337 1.338 public: 1.339 float M[4][4]; 1.340 1.341 enum NoInitType { NoInit }; 1.342 1.343 // Construct with no memory initialization. 1.344 Matrix4f(NoInitType) { } 1.345 1.346 // By default, we construct identity matrix. 1.347 Matrix4f() 1.348 { 1.349 SetIdentity(); 1.350 } 1.351 1.352 Matrix4f(float m11, float m12, float m13, float m14, 1.353 float m21, float m22, float m23, float m24, 1.354 float m31, float m32, float m33, float m34, 1.355 float m41, float m42, float m43, float m44) 1.356 { 1.357 M[0][0] = m11; M[0][1] = m12; M[0][2] = m13; M[0][3] = m14; 1.358 M[1][0] = m21; M[1][1] = m22; M[1][2] = m23; M[1][3] = m24; 1.359 M[2][0] = m31; M[2][1] = m32; M[2][2] = m33; M[2][3] = m34; 1.360 M[3][0] = m41; M[3][1] = m42; M[3][2] = m43; M[3][3] = m44; 1.361 } 1.362 1.363 Matrix4f(float m11, float m12, float m13, 1.364 float m21, float m22, float m23, 1.365 float m31, float m32, float m33) 1.366 { 1.367 M[0][0] = m11; M[0][1] = m12; M[0][2] = m13; M[0][3] = 0; 1.368 M[1][0] = m21; M[1][1] = m22; M[1][2] = m23; M[1][3] = 0; 1.369 M[2][0] = m31; M[2][1] = m32; M[2][2] = m33; M[2][3] = 0; 1.370 M[3][0] = 0; M[3][1] = 0; M[3][2] = 0; M[3][3] = 1; 1.371 } 1.372 1.373 static const Matrix4f& Identity() { return IdentityValue; } 1.374 1.375 void SetIdentity() 1.376 { 1.377 M[0][0] = M[1][1] = M[2][2] = M[3][3] = 1; 1.378 M[0][1] = M[1][0] = M[2][3] = M[3][1] = 0; 1.379 M[0][2] = M[1][2] = M[2][0] = M[3][2] = 0; 1.380 M[0][3] = M[1][3] = M[2][1] = M[3][0] = 0; 1.381 } 1.382 1.383 // Multiplies two matrices into destination with minimum copying. 1.384 static Matrix4f& Multiply(Matrix4f* d, const Matrix4f& a, const Matrix4f& b) 1.385 { 1.386 OVR_ASSERT((d != &a) && (d != &b)); 1.387 int i = 0; 1.388 do { 1.389 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]; 1.390 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]; 1.391 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]; 1.392 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]; 1.393 } while((++i) < 4); 1.394 1.395 return *d; 1.396 } 1.397 1.398 Matrix4f operator* (const Matrix4f& b) const 1.399 { 1.400 Matrix4f result(Matrix4f::NoInit); 1.401 Multiply(&result, *this, b); 1.402 return result; 1.403 } 1.404 1.405 Matrix4f& operator*= (const Matrix4f& b) 1.406 { 1.407 return Multiply(this, Matrix4f(*this), b); 1.408 } 1.409 1.410 Matrix4f operator* (float s) const 1.411 { 1.412 return Matrix4f(M[0][0] * s, M[0][1] * s, M[0][2] * s, M[0][3] * s, 1.413 M[1][0] * s, M[1][1] * s, M[1][2] * s, M[1][3] * s, 1.414 M[2][0] * s, M[2][1] * s, M[2][2] * s, M[2][3] * s, 1.415 M[3][0] * s, M[3][1] * s, M[3][2] * s, M[3][3] * s); 1.416 } 1.417 1.418 Matrix4f& operator*= (float s) 1.419 { 1.420 M[0][0] *= s; M[0][1] *= s; M[0][2] *= s; M[0][3] *= s; 1.421 M[1][0] *= s; M[1][1] *= s; M[1][2] *= s; M[1][3] *= s; 1.422 M[2][0] *= s; M[2][1] *= s; M[2][2] *= s; M[2][3] *= s; 1.423 M[3][0] *= s; M[3][1] *= s; M[3][2] *= s; M[3][3] *= s; 1.424 return *this; 1.425 } 1.426 1.427 Vector3f Transform(const Vector3f& v) const 1.428 { 1.429 return Vector3f(M[0][0] * v.x + M[0][1] * v.y + M[0][2] * v.z + M[0][3], 1.430 M[1][0] * v.x + M[1][1] * v.y + M[1][2] * v.z + M[1][3], 1.431 M[2][0] * v.x + M[2][1] * v.y + M[2][2] * v.z + M[2][3]); 1.432 } 1.433 1.434 Matrix4f Transposed() const 1.435 { 1.436 return Matrix4f(M[0][0], M[1][0], M[2][0], M[3][0], 1.437 M[0][1], M[1][1], M[2][1], M[3][1], 1.438 M[0][2], M[1][2], M[2][2], M[3][2], 1.439 M[0][3], M[1][3], M[2][3], M[3][3]); 1.440 } 1.441 1.442 void Transpose() 1.443 { 1.444 *this = Transposed(); 1.445 } 1.446 1.447 1.448 float SubDet (const int* rows, const int* cols) const 1.449 { 1.450 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]]) 1.451 - 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]]) 1.452 + 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]]); 1.453 } 1.454 1.455 float Cofactor(int I, int J) const 1.456 { 1.457 const int indices[4][3] = {{1,2,3},{0,2,3},{0,1,3},{0,1,2}}; 1.458 return ((I+J)&1) ? -SubDet(indices[I],indices[J]) : SubDet(indices[I],indices[J]); 1.459 } 1.460 1.461 float Determinant() const 1.462 { 1.463 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); 1.464 } 1.465 1.466 Matrix4f Adjugated() const 1.467 { 1.468 return Matrix4f(Cofactor(0,0), Cofactor(1,0), Cofactor(2,0), Cofactor(3,0), 1.469 Cofactor(0,1), Cofactor(1,1), Cofactor(2,1), Cofactor(3,1), 1.470 Cofactor(0,2), Cofactor(1,2), Cofactor(2,2), Cofactor(3,2), 1.471 Cofactor(0,3), Cofactor(1,3), Cofactor(2,3), Cofactor(3,3)); 1.472 } 1.473 1.474 Matrix4f Inverted() const 1.475 { 1.476 float det = Determinant(); 1.477 assert(det != 0); 1.478 return Adjugated() * (1.0f/det); 1.479 } 1.480 1.481 void Invert() 1.482 { 1.483 *this = Inverted(); 1.484 } 1.485 1.486 //AnnaSteve: 1.487 // a,b,c, are the YawPitchRoll angles to be returned 1.488 // rotation a around axis A1 1.489 // is followed by rotation b around axis A2 1.490 // is followed by rotation c around axis A3 1.491 // rotations are CCW or CW (D) in LH or RH coordinate system (S) 1.492 template <Axis A1, Axis A2, Axis A3, RotateDirection D, HandedSystem S> 1.493 void ToEulerAngles(float *a, float *b, float *c) 1.494 { 1.495 OVR_COMPILER_ASSERT((A1 != A2) && (A2 != A3) && (A1 != A3)); 1.496 1.497 float psign = -1.0f; 1.498 if (((A1 + 1) % 3 == A2) && ((A2 + 1) % 3 == A3)) // Determine whether even permutation 1.499 psign = 1.0f; 1.500 1.501 float pm = psign*M[A1][A3]; 1.502 if (pm < -1.0f + Math<float>::SingularityRadius) 1.503 { // South pole singularity 1.504 *a = 0.0f; 1.505 *b = -S*D*Math<float>::PiOver2; 1.506 *c = S*D*atan2( psign*M[A2][A1], M[A2][A2] ); 1.507 } 1.508 else if (pm > 1.0 - Math<float>::SingularityRadius) 1.509 { // North pole singularity 1.510 *a = 0.0f; 1.511 *b = S*D*Math<float>::PiOver2; 1.512 *c = S*D*atan2( psign*M[A2][A1], M[A2][A2] ); 1.513 } 1.514 else 1.515 { // Normal case (nonsingular) 1.516 *a = S*D*atan2( -psign*M[A2][A3], M[A3][A3] ); 1.517 *b = S*D*asin(pm); 1.518 *c = S*D*atan2( -psign*M[A1][A2], M[A1][A1] ); 1.519 } 1.520 1.521 return; 1.522 } 1.523 1.524 //AnnaSteve: 1.525 // a,b,c, are the YawPitchRoll angles to be returned 1.526 // rotation a around axis A1 1.527 // is followed by rotation b around axis A2 1.528 // is followed by rotation c around axis A1 1.529 // rotations are CCW or CW (D) in LH or RH coordinate system (S) 1.530 template <Axis A1, Axis A2, RotateDirection D, HandedSystem S> 1.531 void ToEulerAnglesABA(float *a, float *b, float *c) 1.532 { 1.533 OVR_COMPILER_ASSERT(A1 != A2); 1.534 1.535 // Determine the axis that was not supplied 1.536 int m = 3 - A1 - A2; 1.537 1.538 float psign = -1.0f; 1.539 if ((A1 + 1) % 3 == A2) // Determine whether even permutation 1.540 psign = 1.0f; 1.541 1.542 float c2 = M[A1][A1]; 1.543 if (c2 < -1.0 + Math<float>::SingularityRadius) 1.544 { // South pole singularity 1.545 *a = 0.0f; 1.546 *b = S*D*Math<float>::Pi; 1.547 *c = S*D*atan2( -psign*M[A2][m],M[A2][A2]); 1.548 } 1.549 else if (c2 > 1.0 - Math<float>::SingularityRadius) 1.550 { // North pole singularity 1.551 *a = 0.0f; 1.552 *b = 0.0f; 1.553 *c = S*D*atan2( -psign*M[A2][m],M[A2][A2]); 1.554 } 1.555 else 1.556 { // Normal case (nonsingular) 1.557 *a = S*D*atan2( M[A2][A1],-psign*M[m][A1]); 1.558 *b = S*D*acos(c2); 1.559 *c = S*D*atan2( M[A1][A2],psign*M[A1][m]); 1.560 } 1.561 return; 1.562 } 1.563 1.564 // Creates a matrix that converts the vertices from one coordinate system 1.565 // to another. 1.566 // 1.567 static Matrix4f AxisConversion(const WorldAxes& to, const WorldAxes& from) 1.568 { 1.569 // Holds axis values from the 'to' structure 1.570 int toArray[3] = { to.XAxis, to.YAxis, to.ZAxis }; 1.571 1.572 // The inverse of the toArray 1.573 int inv[4]; 1.574 inv[0] = inv[abs(to.XAxis)] = 0; 1.575 inv[abs(to.YAxis)] = 1; 1.576 inv[abs(to.ZAxis)] = 2; 1.577 1.578 Matrix4f m(0, 0, 0, 1.579 0, 0, 0, 1.580 0, 0, 0); 1.581 1.582 // Only three values in the matrix need to be changed to 1 or -1. 1.583 m.M[inv[abs(from.XAxis)]][0] = float(from.XAxis/toArray[inv[abs(from.XAxis)]]); 1.584 m.M[inv[abs(from.YAxis)]][1] = float(from.YAxis/toArray[inv[abs(from.YAxis)]]); 1.585 m.M[inv[abs(from.ZAxis)]][2] = float(from.ZAxis/toArray[inv[abs(from.ZAxis)]]); 1.586 return m; 1.587 } 1.588 1.589 1.590 1.591 static Matrix4f Translation(const Vector3f& v) 1.592 { 1.593 Matrix4f t; 1.594 t.M[0][3] = v.x; 1.595 t.M[1][3] = v.y; 1.596 t.M[2][3] = v.z; 1.597 return t; 1.598 } 1.599 1.600 static Matrix4f Translation(float x, float y, float z = 0.0f) 1.601 { 1.602 Matrix4f t; 1.603 t.M[0][3] = x; 1.604 t.M[1][3] = y; 1.605 t.M[2][3] = z; 1.606 return t; 1.607 } 1.608 1.609 static Matrix4f Scaling(const Vector3f& v) 1.610 { 1.611 Matrix4f t; 1.612 t.M[0][0] = v.x; 1.613 t.M[1][1] = v.y; 1.614 t.M[2][2] = v.z; 1.615 return t; 1.616 } 1.617 1.618 static Matrix4f Scaling(float x, float y, float z) 1.619 { 1.620 Matrix4f t; 1.621 t.M[0][0] = x; 1.622 t.M[1][1] = y; 1.623 t.M[2][2] = z; 1.624 return t; 1.625 } 1.626 1.627 static Matrix4f Scaling(float s) 1.628 { 1.629 Matrix4f t; 1.630 t.M[0][0] = s; 1.631 t.M[1][1] = s; 1.632 t.M[2][2] = s; 1.633 return t; 1.634 } 1.635 1.636 1.637 1.638 //AnnaSteve : Just for quick testing. Not for final API. Need to remove case. 1.639 static Matrix4f RotationAxis(Axis A, float angle, RotateDirection d, HandedSystem s) 1.640 { 1.641 float sina = s * d *sin(angle); 1.642 float cosa = cos(angle); 1.643 1.644 switch(A) 1.645 { 1.646 case Axis_X: 1.647 return Matrix4f(1, 0, 0, 1.648 0, cosa, -sina, 1.649 0, sina, cosa); 1.650 case Axis_Y: 1.651 return Matrix4f(cosa, 0, sina, 1.652 0, 1, 0, 1.653 -sina, 0, cosa); 1.654 case Axis_Z: 1.655 return Matrix4f(cosa, -sina, 0, 1.656 sina, cosa, 0, 1.657 0, 0, 1); 1.658 } 1.659 } 1.660 1.661 1.662 // Creates a rotation matrix rotating around the X axis by 'angle' radians. 1.663 // Rotation direction is depends on the coordinate system: 1.664 // RHS (Oculus default): Positive angle values rotate Counter-clockwise (CCW), 1.665 // while looking in the negative axis direction. This is the 1.666 // same as looking down from positive axis values towards origin. 1.667 // LHS: Positive angle values rotate clock-wise (CW), while looking in the 1.668 // negative axis direction. 1.669 static Matrix4f RotationX(float angle) 1.670 { 1.671 float sina = sin(angle); 1.672 float cosa = cos(angle); 1.673 return Matrix4f(1, 0, 0, 1.674 0, cosa, -sina, 1.675 0, sina, cosa); 1.676 } 1.677 1.678 // Creates a rotation matrix rotating around the Y axis by 'angle' radians. 1.679 // Rotation direction is depends on the coordinate system: 1.680 // RHS (Oculus default): Positive angle values rotate Counter-clockwise (CCW), 1.681 // while looking in the negative axis direction. This is the 1.682 // same as looking down from positive axis values towards origin. 1.683 // LHS: Positive angle values rotate clock-wise (CW), while looking in the 1.684 // negative axis direction. 1.685 static Matrix4f RotationY(float angle) 1.686 { 1.687 float sina = sin(angle); 1.688 float cosa = cos(angle); 1.689 return Matrix4f(cosa, 0, sina, 1.690 0, 1, 0, 1.691 -sina, 0, cosa); 1.692 } 1.693 1.694 // Creates a rotation matrix rotating around the Z axis by 'angle' radians. 1.695 // Rotation direction is depends on the coordinate system: 1.696 // RHS (Oculus default): Positive angle values rotate Counter-clockwise (CCW), 1.697 // while looking in the negative axis direction. This is the 1.698 // same as looking down from positive axis values towards origin. 1.699 // LHS: Positive angle values rotate clock-wise (CW), while looking in the 1.700 // negative axis direction. 1.701 static Matrix4f RotationZ(float angle) 1.702 { 1.703 float sina = sin(angle); 1.704 float cosa = cos(angle); 1.705 return Matrix4f(cosa, -sina, 0, 1.706 sina, cosa, 0, 1.707 0, 0, 1); 1.708 } 1.709 1.710 1.711 // LookAtRH creates a View transformation matrix for right-handed coordinate system. 1.712 // The resulting matrix points camera from 'eye' towards 'at' direction, with 'up' 1.713 // specifying the up vector. The resulting matrix should be used with PerspectiveRH 1.714 // projection. 1.715 static Matrix4f LookAtRH(const Vector3f& eye, const Vector3f& at, const Vector3f& up); 1.716 1.717 // LookAtLH creates a View transformation matrix for left-handed coordinate system. 1.718 // The resulting matrix points camera from 'eye' towards 'at' direction, with 'up' 1.719 // specifying the up vector. 1.720 static Matrix4f LookAtLH(const Vector3f& eye, const Vector3f& at, const Vector3f& up); 1.721 1.722 1.723 // PerspectiveRH creates a right-handed perspective projection matrix that can be 1.724 // used with the Oculus sample renderer. 1.725 // yfov - Specifies vertical field of view in radians. 1.726 // aspect - Screen aspect ration, which is usually width/height for square pixels. 1.727 // Note that xfov = yfov * aspect. 1.728 // znear - Absolute value of near Z clipping clipping range. 1.729 // zfar - Absolute value of far Z clipping clipping range (larger then near). 1.730 // Even though RHS usually looks in the direction of negative Z, positive values 1.731 // are expected for znear and zfar. 1.732 static Matrix4f PerspectiveRH(float yfov, float aspect, float znear, float zfar); 1.733 1.734 1.735 // PerspectiveRH creates a left-handed perspective projection matrix that can be 1.736 // used with the Oculus sample renderer. 1.737 // yfov - Specifies vertical field of view in radians. 1.738 // aspect - Screen aspect ration, which is usually width/height for square pixels. 1.739 // Note that xfov = yfov * aspect. 1.740 // znear - Absolute value of near Z clipping clipping range. 1.741 // zfar - Absolute value of far Z clipping clipping range (larger then near). 1.742 static Matrix4f PerspectiveLH(float yfov, float aspect, float znear, float zfar); 1.743 1.744 1.745 static Matrix4f Ortho2D(float w, float h); 1.746 }; 1.747 1.748 1.749 //------------------------------------------------------------------------------------- 1.750 // ***** Quat 1.751 1.752 // Quatf represents a quaternion class used for rotations. 1.753 // 1.754 // Quaternion multiplications are done in right-to-left order, to match the 1.755 // behavior of matrices. 1.756 1.757 1.758 template<class T> 1.759 class Quat 1.760 { 1.761 public: 1.762 // w + Xi + Yj + Zk 1.763 T x, y, z, w; 1.764 1.765 Quat() : x(0), y(0), z(0), w(1) {} 1.766 Quat(T x_, T y_, T z_, T w_) : x(x_), y(y_), z(z_), w(w_) {} 1.767 1.768 1.769 // Constructs rotation quaternion around the axis. 1.770 Quat(const Vector3<T>& axis, T angle) 1.771 { 1.772 Vector3<T> unitAxis = axis.Normalized(); 1.773 T sinHalfAngle = sin(angle * T(0.5)); 1.774 1.775 w = cos(angle * T(0.5)); 1.776 x = unitAxis.x * sinHalfAngle; 1.777 y = unitAxis.y * sinHalfAngle; 1.778 z = unitAxis.z * sinHalfAngle; 1.779 } 1.780 1.781 //AnnaSteve: 1.782 void AxisAngle(Axis A, T angle, RotateDirection d, HandedSystem s) 1.783 { 1.784 T sinHalfAngle = s * d *sin(angle * (T)0.5); 1.785 T v[3]; 1.786 v[0] = v[1] = v[2] = (T)0; 1.787 v[A] = sinHalfAngle; 1.788 //return Quat(v[0], v[1], v[2], cos(angle * (T)0.5)); 1.789 w = cos(angle * (T)0.5); 1.790 x = v[0]; 1.791 y = v[1]; 1.792 z = v[2]; 1.793 } 1.794 1.795 1.796 void GetAxisAngle(Vector3<T>* axis, T* angle) const 1.797 { 1.798 if (LengthSq() > Math<T>::Tolerance * Math<T>::Tolerance) 1.799 { 1.800 *axis = Vector3<T>(x, y, z).Normalized(); 1.801 *angle = 2 * acos(w); 1.802 } 1.803 else 1.804 { 1.805 *axis = Vector3<T>(1, 0, 0); 1.806 *angle= 0; 1.807 } 1.808 } 1.809 1.810 bool operator== (const Quat& b) const { return x == b.x && y == b.y && z == b.z && w == b.w; } 1.811 bool operator!= (const Quat& b) const { return x != b.x || y != b.y || z != b.z || w != b.w; } 1.812 1.813 Quat operator+ (const Quat& b) const { return Quat(x + b.x, y + b.y, z + b.z, w + b.w); } 1.814 Quat& operator+= (const Quat& b) { w += b.w; x += b.x; y += b.y; z += b.z; return *this; } 1.815 Quat operator- (const Quat& b) const { return Quat(x - b.x, y - b.y, z - b.z, w - b.w); } 1.816 Quat& operator-= (const Quat& b) { w -= b.w; x -= b.x; y -= b.y; z -= b.z; return *this; } 1.817 1.818 Quat operator* (T s) const { return Quat(x * s, y * s, z * s, w * s); } 1.819 Quat& operator*= (T s) { w *= s; x *= s; y *= s; z *= s; return *this; } 1.820 Quat operator/ (T s) const { T rcp = T(1)/s; return Quat(x * rcp, y * rcp, z * rcp, w *rcp); } 1.821 Quat& operator/= (T s) { T rcp = T(1)/s; w *= rcp; x *= rcp; y *= rcp; z *= rcp; return *this; } 1.822 1.823 // Get Imaginary part vector 1.824 Vector3<T> Imag() const { return Vector3<T>(x,y,z); } 1.825 1.826 // Get quaternion length. 1.827 T Length() const { return sqrt(x * x + y * y + z * z + w * w); } 1.828 // Get quaternion length squared. 1.829 T LengthSq() const { return (x * x + y * y + z * z + w * w); } 1.830 // Simple Eulidean distance in R^4 (not SLERP distance, but at least respects Haar measure) 1.831 T Distance(const Quat& q) const 1.832 { 1.833 T d1 = (*this - q).Length(); 1.834 T d2 = (*this + q).Length(); // Antipoldal point check 1.835 return (d1 < d2) ? d1 : d2; 1.836 } 1.837 T DistanceSq(const Quat& q) const 1.838 { 1.839 T d1 = (*this - q).LengthSq(); 1.840 T d2 = (*this + q).LengthSq(); // Antipoldal point check 1.841 return (d1 < d2) ? d1 : d2; 1.842 } 1.843 1.844 // Normalize 1.845 bool IsNormalized() const { return fabs(LengthSq() - 1) < Math<T>::Tolerance; } 1.846 void Normalize() { *this /= Length(); } 1.847 Quat Normalized() const { return *this / Length(); } 1.848 1.849 // Returns conjugate of the quaternion. Produces inverse rotation if quaternion is normalized. 1.850 Quat Conj() const { return Quat(-x, -y, -z, w); } 1.851 1.852 // AnnaSteve fixed: order of quaternion multiplication 1.853 // Quaternion multiplication. Combines quaternion rotations, performing the one on the 1.854 // right hand side first. 1.855 Quat operator* (const Quat& b) const { return Quat(w * b.x + x * b.w + y * b.z - z * b.y, 1.856 w * b.y - x * b.z + y * b.w + z * b.x, 1.857 w * b.z + x * b.y - y * b.x + z * b.w, 1.858 w * b.w - x * b.x - y * b.y - z * b.z); } 1.859 1.860 // 1.861 // this^p normalized; same as rotating by this p times. 1.862 Quat PowNormalized(T p) const 1.863 { 1.864 Vector3<T> v; 1.865 T a; 1.866 GetAxisAngle(&v, &a); 1.867 return Quat(v, a * p); 1.868 } 1.869 1.870 // Rotate transforms vector in a manner that matches Matrix rotations (counter-clockwise, 1.871 // assuming negative direction of the axis). Standard formula: q(t) * V * q(t)^-1. 1.872 Vector3<T> Rotate(const Vector3<T>& v) const 1.873 { 1.874 return ((*this * Quat<T>(v.x, v.y, v.z, 0)) * Inverted()).Imag(); 1.875 } 1.876 1.877 1.878 // Inversed quaternion rotates in the opposite direction. 1.879 Quat Inverted() const 1.880 { 1.881 return Quat(-x, -y, -z, w); 1.882 } 1.883 1.884 // Sets this quaternion to the one rotates in the opposite direction. 1.885 void Invert() 1.886 { 1.887 *this = Quat(-x, -y, -z, w); 1.888 } 1.889 1.890 // Converting quaternion to matrix. 1.891 operator Matrix4f() const 1.892 { 1.893 T ww = w*w; 1.894 T xx = x*x; 1.895 T yy = y*y; 1.896 T zz = z*z; 1.897 1.898 return Matrix4f(float(ww + xx - yy - zz), float(T(2) * (x*y - w*z)), float(T(2) * (x*z + w*y)), 1.899 float(T(2) * (x*y + w*z)), float(ww - xx + yy - zz), float(T(2) * (y*z - w*x)), 1.900 float(T(2) * (x*z - w*y)), float(T(2) * (y*z + w*x)), float(ww - xx - yy + zz) ); 1.901 } 1.902 1.903 1.904 // GetEulerAngles extracts Euler angles from the quaternion, in the specified order of 1.905 // axis rotations and the specified coordinate system. Right-handed coordinate system 1.906 // is the default, with CCW rotations while looking in the negative axis direction. 1.907 // Here a,b,c, are the Yaw/Pitch/Roll angles to be returned. 1.908 // rotation a around axis A1 1.909 // is followed by rotation b around axis A2 1.910 // is followed by rotation c around axis A3 1.911 // rotations are CCW or CW (D) in LH or RH coordinate system (S) 1.912 template <Axis A1, Axis A2, Axis A3, RotateDirection D, HandedSystem S> 1.913 void GetEulerAngles(T *a, T *b, T *c) 1.914 { 1.915 OVR_COMPILER_ASSERT((A1 != A2) && (A2 != A3) && (A1 != A3)); 1.916 1.917 T Q[3] = { x, y, z }; //Quaternion components x,y,z 1.918 1.919 T ww = w*w; 1.920 T Q11 = Q[A1]*Q[A1]; 1.921 T Q22 = Q[A2]*Q[A2]; 1.922 T Q33 = Q[A3]*Q[A3]; 1.923 1.924 T psign = T(-1.0); 1.925 // Determine whether even permutation 1.926 if (((A1 + 1) % 3 == A2) && ((A2 + 1) % 3 == A3)) 1.927 psign = T(1.0); 1.928 1.929 T s2 = psign * T(2.0) * (psign*w*Q[A2] + Q[A1]*Q[A3]); 1.930 1.931 if (s2 < (T)-1.0 + Math<T>::SingularityRadius) 1.932 { // South pole singularity 1.933 *a = T(0.0); 1.934 *b = -S*D*Math<T>::PiOver2; 1.935 *c = S*D*atan2((T)2.0*(psign*Q[A1]*Q[A2] + w*Q[A3]), 1.936 ww + Q22 - Q11 - Q33 ); 1.937 } 1.938 else if (s2 > (T)1.0 - Math<T>::SingularityRadius) 1.939 { // North pole singularity 1.940 *a = (T)0.0; 1.941 *b = S*D*Math<T>::PiOver2; 1.942 *c = S*D*atan2((T)2.0*(psign*Q[A1]*Q[A2] + w*Q[A3]), 1.943 ww + Q22 - Q11 - Q33); 1.944 } 1.945 else 1.946 { 1.947 *a = -S*D*atan2((T)-2.0*(w*Q[A1] - psign*Q[A2]*Q[A3]), 1.948 ww + Q33 - Q11 - Q22); 1.949 *b = S*D*asin(s2); 1.950 *c = S*D*atan2((T)2.0*(w*Q[A3] - psign*Q[A1]*Q[A2]), 1.951 ww + Q11 - Q22 - Q33); 1.952 } 1.953 return; 1.954 } 1.955 1.956 template <Axis A1, Axis A2, Axis A3, RotateDirection D> 1.957 void GetEulerAngles(T *a, T *b, T *c) 1.958 { GetEulerAngles<A1, A2, A3, D, Handed_R>(a, b, c); } 1.959 1.960 template <Axis A1, Axis A2, Axis A3> 1.961 void GetEulerAngles(T *a, T *b, T *c) 1.962 { GetEulerAngles<A1, A2, A3, Rotate_CCW, Handed_R>(a, b, c); } 1.963 1.964 1.965 // GetEulerAnglesABA extracts Euler angles from the quaternion, in the specified order of 1.966 // axis rotations and the specified coordinate system. Right-handed coordinate system 1.967 // is the default, with CCW rotations while looking in the negative axis direction. 1.968 // Here a,b,c, are the Yaw/Pitch/Roll angles to be returned. 1.969 // rotation a around axis A1 1.970 // is followed by rotation b around axis A2 1.971 // is followed by rotation c around axis A1 1.972 // Rotations are CCW or CW (D) in LH or RH coordinate system (S) 1.973 template <Axis A1, Axis A2, RotateDirection D, HandedSystem S> 1.974 void GetEulerAnglesABA(T *a, T *b, T *c) 1.975 { 1.976 OVR_COMPILER_ASSERT(A1 != A2); 1.977 1.978 T Q[3] = {x, y, z}; // Quaternion components 1.979 1.980 // Determine the missing axis that was not supplied 1.981 int m = 3 - A1 - A2; 1.982 1.983 T ww = w*w; 1.984 T Q11 = Q[A1]*Q[A1]; 1.985 T Q22 = Q[A2]*Q[A2]; 1.986 T Qmm = Q[m]*Q[m]; 1.987 1.988 T psign = T(-1.0); 1.989 if ((A1 + 1) % 3 == A2) // Determine whether even permutation 1.990 { 1.991 psign = (T)1.0; 1.992 } 1.993 1.994 T c2 = ww + Q11 - Q22 - Qmm; 1.995 if (c2 < (T)-1.0 + Math<T>::SingularityRadius) 1.996 { // South pole singularity 1.997 *a = (T)0.0; 1.998 *b = S*D*Math<T>::Pi; 1.999 *c = S*D*atan2( (T)2.0*(w*Q[A1] - psign*Q[A2]*Q[m]), 1.1000 ww + Q22 - Q11 - Qmm); 1.1001 } 1.1002 else if (c2 > (T)1.0 - Math<T>::SingularityRadius) 1.1003 { // North pole singularity 1.1004 *a = (T)0.0; 1.1005 *b = (T)0.0; 1.1006 *c = S*D*atan2( (T)2.0*(w*Q[A1] - psign*Q[A2]*Q[m]), 1.1007 ww + Q22 - Q11 - Qmm); 1.1008 } 1.1009 else 1.1010 { 1.1011 *a = S*D*atan2( psign*w*Q[m] + Q[A1]*Q[A2], 1.1012 w*Q[A2] -psign*Q[A1]*Q[m]); 1.1013 *b = S*D*acos(c2); 1.1014 *c = S*D*atan2( -psign*w*Q[m] + Q[A1]*Q[A2], 1.1015 w*Q[A2] + psign*Q[A1]*Q[m]); 1.1016 } 1.1017 return; 1.1018 } 1.1019 }; 1.1020 1.1021 1.1022 typedef Quat<float> Quatf; 1.1023 typedef Quat<double> Quatd; 1.1024 1.1025 1.1026 1.1027 //------------------------------------------------------------------------------------- 1.1028 // ***** Angle 1.1029 1.1030 // Cleanly representing the algebra of 2D rotations. 1.1031 // The operations maintain the angle between -Pi and Pi, the same range as atan2. 1.1032 // 1.1033 1.1034 template<class T> 1.1035 class Angle 1.1036 { 1.1037 public: 1.1038 enum AngularUnits 1.1039 { 1.1040 Radians = 0, 1.1041 Degrees = 1 1.1042 }; 1.1043 1.1044 Angle() : a(0) {} 1.1045 1.1046 // Fix the range to be between -Pi and Pi 1.1047 Angle(T a_, AngularUnits u = Radians) : a((u == Radians) ? a_ : a_*Math<T>::DegreeToRadFactor) { FixRange(); } 1.1048 1.1049 T Get(AngularUnits u = Radians) const { return (u == Radians) ? a : a*Math<T>::RadToDegreeFactor; } 1.1050 void Set(const T& x, AngularUnits u = Radians) { a = (u == Radians) ? x : x*Math<T>::DegreeToRadFactor; FixRange(); } 1.1051 int Sign() const { if (a == 0) return 0; else return (a > 0) ? 1 : -1; } 1.1052 T Abs() const { return (a > 0) ? a : -a; } 1.1053 1.1054 bool operator== (const Angle& b) const { return a == b.a; } 1.1055 bool operator!= (const Angle& b) const { return a != b.a; } 1.1056 // bool operator< (const Angle& b) const { return a < a.b; } 1.1057 // bool operator> (const Angle& b) const { return a > a.b; } 1.1058 // bool operator<= (const Angle& b) const { return a <= a.b; } 1.1059 // bool operator>= (const Angle& b) const { return a >= a.b; } 1.1060 // bool operator= (const T& x) { a = x; FixRange(); } 1.1061 1.1062 // These operations assume a is already between -Pi and Pi. 1.1063 Angle operator+ (const Angle& b) const { return Angle(a + b.a); } 1.1064 Angle operator+ (const T& x) const { return Angle(a + x); } 1.1065 Angle& operator+= (const Angle& b) { a = a + b.a; FastFixRange(); return *this; } 1.1066 Angle& operator+= (const T& x) { a = a + x; FixRange(); return *this; } 1.1067 Angle operator- (const Angle& b) const { return Angle(a - b.a); } 1.1068 Angle operator- (const T& x) const { return Angle(a - x); } 1.1069 Angle& operator-= (const Angle& b) { a = a - b.a; FastFixRange(); return *this; } 1.1070 Angle& operator-= (const T& x) { a = a - x; FixRange(); return *this; } 1.1071 1.1072 T Distance(const Angle& b) { T c = fabs(a - b.a); return (c <= Math<T>::Pi) ? c : Math<T>::TwoPi - c; } 1.1073 1.1074 private: 1.1075 1.1076 // The stored angle, which should be maintained between -Pi and Pi 1.1077 T a; 1.1078 1.1079 // Fixes the angle range to [-Pi,Pi], but assumes no more than 2Pi away on either side 1.1080 inline void FastFixRange() 1.1081 { 1.1082 if (a < -Math<T>::Pi) 1.1083 a += Math<T>::TwoPi; 1.1084 else if (a > Math<T>::Pi) 1.1085 a -= Math<T>::TwoPi; 1.1086 } 1.1087 1.1088 // Fixes the angle range to [-Pi,Pi] for any given range, but slower then the fast method 1.1089 inline void FixRange() 1.1090 { 1.1091 a = fmod(a,Math<T>::TwoPi); 1.1092 if (a < -Math<T>::Pi) 1.1093 a += Math<T>::TwoPi; 1.1094 else if (a > Math<T>::Pi) 1.1095 a -= Math<T>::TwoPi; 1.1096 } 1.1097 }; 1.1098 1.1099 1.1100 typedef Angle<float> Anglef; 1.1101 typedef Angle<double> Angled; 1.1102 1.1103 1.1104 //------------------------------------------------------------------------------------- 1.1105 // ***** Plane 1.1106 1.1107 // Consists of a normal vector and distance from the origin where the plane is located. 1.1108 1.1109 template<class T> 1.1110 class Plane : public RefCountBase<Plane<T> > 1.1111 { 1.1112 public: 1.1113 Vector3<T> N; 1.1114 T D; 1.1115 1.1116 Plane() : D(0) {} 1.1117 1.1118 // Normals must already be normalized 1.1119 Plane(const Vector3<T>& n, T d) : N(n), D(d) {} 1.1120 Plane(T x, T y, T z, T d) : N(x,y,z), D(d) {} 1.1121 1.1122 // construct from a point on the plane and the normal 1.1123 Plane(const Vector3<T>& p, const Vector3<T>& n) : N(n), D(-(p * n)) {} 1.1124 1.1125 // Find the point to plane distance. The sign indicates what side of the plane the point is on (0 = point on plane). 1.1126 T TestSide(const Vector3<T>& p) const 1.1127 { 1.1128 return (N * p) + D; 1.1129 } 1.1130 1.1131 Plane<T> Flipped() const 1.1132 { 1.1133 return Plane(-N, -D); 1.1134 } 1.1135 1.1136 void Flip() 1.1137 { 1.1138 N = -N; 1.1139 D = -D; 1.1140 } 1.1141 1.1142 bool operator==(const Plane<T>& rhs) const 1.1143 { 1.1144 return (this->D == rhs.D && this->N == rhs.N); 1.1145 } 1.1146 }; 1.1147 1.1148 typedef Plane<float> Planef; 1.1149 1.1150 } 1.1151 1.1152 #endif 1.1153 \ No newline at end of file 1.1154 +/************************************************************************************ 1.1155 + 1.1156 +PublicHeader: OVR.h 1.1157 +Filename : OVR_Math.h 1.1158 +Content : Implementation of 3D primitives such as vectors, matrices. 1.1159 +Created : September 4, 2012 1.1160 +Authors : Andrew Reisse, Michael Antonov, Steve LaValle, Anna Yershova 1.1161 + 1.1162 +Copyright : Copyright 2012 Oculus VR, Inc. All Rights reserved. 1.1163 + 1.1164 +Use of this software is subject to the terms of the Oculus license 1.1165 +agreement provided at the time of installation or download, or which 1.1166 +otherwise accompanies this software in either electronic or hard copy form. 1.1167 + 1.1168 +*************************************************************************************/ 1.1169 + 1.1170 +#ifndef OVR_Math_h 1.1171 +#define OVR_Math_h 1.1172 + 1.1173 +#include <assert.h> 1.1174 +#include <stdlib.h> 1.1175 +#include <math.h> 1.1176 + 1.1177 +#include "OVR_Types.h" 1.1178 +#include "OVR_RefCount.h" 1.1179 + 1.1180 +namespace OVR { 1.1181 + 1.1182 +//------------------------------------------------------------------------------------- 1.1183 +// Constants for 3D world/axis definitions. 1.1184 + 1.1185 +// Definitions of axes for coordinate and rotation conversions. 1.1186 +enum Axis 1.1187 +{ 1.1188 + Axis_X = 0, Axis_Y = 1, Axis_Z = 2 1.1189 +}; 1.1190 + 1.1191 +// RotateDirection describes the rotation direction around an axis, interpreted as follows: 1.1192 +// CW - Clockwise while looking "down" from positive axis towards the origin. 1.1193 +// CCW - Counter-clockwise while looking from the positive axis towards the origin, 1.1194 +// which is in the negative axis direction. 1.1195 +// CCW is the default for the RHS coordinate system. Oculus standard RHS coordinate 1.1196 +// system defines Y up, X right, and Z back (pointing out from the screen). In this 1.1197 +// system Rotate_CCW around Z will specifies counter-clockwise rotation in XY plane. 1.1198 +enum RotateDirection 1.1199 +{ 1.1200 + Rotate_CCW = 1, 1.1201 + Rotate_CW = -1 1.1202 +}; 1.1203 + 1.1204 +enum HandedSystem 1.1205 +{ 1.1206 + Handed_R = 1, Handed_L = -1 1.1207 +}; 1.1208 + 1.1209 +// AxisDirection describes which way the axis points. Used by WorldAxes. 1.1210 +enum AxisDirection 1.1211 +{ 1.1212 + Axis_Up = 2, 1.1213 + Axis_Down = -2, 1.1214 + Axis_Right = 1, 1.1215 + Axis_Left = -1, 1.1216 + Axis_In = 3, 1.1217 + Axis_Out = -3 1.1218 +}; 1.1219 + 1.1220 +struct WorldAxes 1.1221 +{ 1.1222 + AxisDirection XAxis, YAxis, ZAxis; 1.1223 + 1.1224 + WorldAxes(AxisDirection x, AxisDirection y, AxisDirection z) 1.1225 + : XAxis(x), YAxis(y), ZAxis(z) 1.1226 + { OVR_ASSERT(abs(x) != abs(y) && abs(y) != abs(z) && abs(z) != abs(x));} 1.1227 +}; 1.1228 + 1.1229 + 1.1230 +//------------------------------------------------------------------------------------- 1.1231 +// ***** Math 1.1232 + 1.1233 +// Math class contains constants and functions. This class is a template specialized 1.1234 +// per type, with Math<float> and Math<double> being distinct. 1.1235 +template<class Type> 1.1236 +class Math 1.1237 +{ 1.1238 +}; 1.1239 + 1.1240 +// Single-precision Math constants class. 1.1241 +template<> 1.1242 +class Math<float> 1.1243 +{ 1.1244 +public: 1.1245 + static const float Pi; 1.1246 + static const float TwoPi; 1.1247 + static const float PiOver2; 1.1248 + static const float PiOver4; 1.1249 + static const float E; 1.1250 + 1.1251 + static const float MaxValue; // Largest positive float Value 1.1252 + static const float MinPositiveValue; // Smallest possible positive value 1.1253 + 1.1254 + static const float RadToDegreeFactor; 1.1255 + static const float DegreeToRadFactor; 1.1256 + 1.1257 + static const float Tolerance; // 0.00001f; 1.1258 + static const float SingularityRadius; //0.00000000001f for Gimbal lock numerical problems 1.1259 +}; 1.1260 + 1.1261 +// Double-precision Math constants class. 1.1262 +template<> 1.1263 +class Math<double> 1.1264 +{ 1.1265 +public: 1.1266 + static const double Pi; 1.1267 + static const double TwoPi; 1.1268 + static const double PiOver2; 1.1269 + static const double PiOver4; 1.1270 + static const double E; 1.1271 + 1.1272 + static const double MaxValue; // Largest positive double Value 1.1273 + static const double MinPositiveValue; // Smallest possible positive value 1.1274 + 1.1275 + static const double RadToDegreeFactor; 1.1276 + static const double DegreeToRadFactor; 1.1277 + 1.1278 + static const double Tolerance; // 0.00001f; 1.1279 + static const double SingularityRadius; //0.00000000001 for Gimbal lock numerical problems 1.1280 +}; 1.1281 + 1.1282 +typedef Math<float> Mathf; 1.1283 +typedef Math<double> Mathd; 1.1284 + 1.1285 +// Conversion functions between degrees and radians 1.1286 +template<class FT> 1.1287 +FT RadToDegree(FT rads) { return rads * Math<FT>::RadToDegreeFactor; } 1.1288 +template<class FT> 1.1289 +FT DegreeToRad(FT rads) { return rads * Math<FT>::DegreeToRadFactor; } 1.1290 + 1.1291 +template<class T> 1.1292 +class Quat; 1.1293 + 1.1294 +//------------------------------------------------------------------------------------- 1.1295 +// ***** Vector2f - 2D Vector2f 1.1296 + 1.1297 +// Vector2f represents a 2-dimensional vector or point in space, 1.1298 +// consisting of coordinates x and y, 1.1299 + 1.1300 +template<class T> 1.1301 +class Vector2 1.1302 +{ 1.1303 +public: 1.1304 + T x, y; 1.1305 + 1.1306 + Vector2() : x(0), y(0) { } 1.1307 + Vector2(T x_, T y_) : x(x_), y(y_) { } 1.1308 + explicit Vector2(T s) : x(s), y(s) { } 1.1309 + 1.1310 + bool operator== (const Vector2& b) const { return x == b.x && y == b.y; } 1.1311 + bool operator!= (const Vector2& b) const { return x != b.x || y != b.y; } 1.1312 + 1.1313 + Vector2 operator+ (const Vector2& b) const { return Vector2(x + b.x, y + b.y); } 1.1314 + Vector2& operator+= (const Vector2& b) { x += b.x; y += b.y; return *this; } 1.1315 + Vector2 operator- (const Vector2& b) const { return Vector2(x - b.x, y - b.y); } 1.1316 + Vector2& operator-= (const Vector2& b) { x -= b.x; y -= b.y; return *this; } 1.1317 + Vector2 operator- () const { return Vector2(-x, -y); } 1.1318 + 1.1319 + // Scalar multiplication/division scales vector. 1.1320 + Vector2 operator* (T s) const { return Vector2(x*s, y*s); } 1.1321 + Vector2& operator*= (T s) { x *= s; y *= s; return *this; } 1.1322 + 1.1323 + Vector2 operator/ (T s) const { T rcp = T(1)/s; 1.1324 + return Vector2(x*rcp, y*rcp); } 1.1325 + Vector2& operator/= (T s) { T rcp = T(1)/s; 1.1326 + x *= rcp; y *= rcp; 1.1327 + return *this; } 1.1328 + 1.1329 + // Compare two vectors for equality with tolerance. Returns true if vectors match withing tolerance. 1.1330 + bool Compare(const Vector2&b, T tolerance = Mathf::Tolerance) 1.1331 + { 1.1332 + return (fabs(b.x-x) < tolerance) && (fabs(b.y-y) < tolerance); 1.1333 + } 1.1334 + 1.1335 + // Dot product overload. 1.1336 + // Used to calculate angle q between two vectors among other things, 1.1337 + // as (A dot B) = |a||b|cos(q). 1.1338 + T operator* (const Vector2& b) const { return x*b.x + y*b.y; } 1.1339 + 1.1340 + // Returns the angle from this vector to b, in radians. 1.1341 + T Angle(const Vector2& b) const { return acos((*this * b)/(Length()*b.Length())); } 1.1342 + 1.1343 + // Return Length of the vector squared. 1.1344 + T LengthSq() const { return (x * x + y * y); } 1.1345 + // Return vector length. 1.1346 + T Length() const { return sqrt(LengthSq()); } 1.1347 + 1.1348 + // Returns distance between two points represented by vectors. 1.1349 + T Distance(Vector2& b) const { return (*this - b).Length(); } 1.1350 + 1.1351 + // Determine if this a unit vector. 1.1352 + bool IsNormalized() const { return fabs(LengthSq() - T(1)) < Math<T>::Tolerance; } 1.1353 + // Normalize, convention vector length to 1. 1.1354 + void Normalize() { *this /= Length(); } 1.1355 + // Returns normalized (unit) version of the vector without modifying itself. 1.1356 + Vector2 Normalized() const { return *this / Length(); } 1.1357 + 1.1358 + // Linearly interpolates from this vector to another. 1.1359 + // Factor should be between 0.0 and 1.0, with 0 giving full value to this. 1.1360 + Vector2 Lerp(const Vector2& b, T f) const { return *this*(T(1) - f) + b*f; } 1.1361 + 1.1362 + // Projects this vector onto the argument; in other words, 1.1363 + // A.Project(B) returns projection of vector A onto B. 1.1364 + Vector2 ProjectTo(const Vector2& b) const { return b * ((*this * b) / b.LengthSq()); } 1.1365 +}; 1.1366 + 1.1367 + 1.1368 +typedef Vector2<float> Vector2f; 1.1369 +typedef Vector2<double> Vector2d; 1.1370 + 1.1371 +//------------------------------------------------------------------------------------- 1.1372 +// ***** Vector3f - 3D Vector3f 1.1373 + 1.1374 +// Vector3f represents a 3-dimensional vector or point in space, 1.1375 +// consisting of coordinates x, y and z. 1.1376 + 1.1377 +template<class T> 1.1378 +class Vector3 1.1379 +{ 1.1380 +public: 1.1381 + T x, y, z; 1.1382 + 1.1383 + Vector3() : x(0), y(0), z(0) { } 1.1384 + Vector3(T x_, T y_, T z_ = 0) : x(x_), y(y_), z(z_) { } 1.1385 + explicit Vector3(T s) : x(s), y(s), z(s) { } 1.1386 + 1.1387 + bool operator== (const Vector3& b) const { return x == b.x && y == b.y && z == b.z; } 1.1388 + bool operator!= (const Vector3& b) const { return x != b.x || y != b.y || z != b.z; } 1.1389 + 1.1390 + Vector3 operator+ (const Vector3& b) const { return Vector3(x + b.x, y + b.y, z + b.z); } 1.1391 + Vector3& operator+= (const Vector3& b) { x += b.x; y += b.y; z += b.z; return *this; } 1.1392 + Vector3 operator- (const Vector3& b) const { return Vector3(x - b.x, y - b.y, z - b.z); } 1.1393 + Vector3& operator-= (const Vector3& b) { x -= b.x; y -= b.y; z -= b.z; return *this; } 1.1394 + Vector3 operator- () const { return Vector3(-x, -y, -z); } 1.1395 + 1.1396 + // Scalar multiplication/division scales vector. 1.1397 + Vector3 operator* (T s) const { return Vector3(x*s, y*s, z*s); } 1.1398 + Vector3& operator*= (T s) { x *= s; y *= s; z *= s; return *this; } 1.1399 + 1.1400 + Vector3 operator/ (T s) const { T rcp = T(1)/s; 1.1401 + return Vector3(x*rcp, y*rcp, z*rcp); } 1.1402 + Vector3& operator/= (T s) { T rcp = T(1)/s; 1.1403 + x *= rcp; y *= rcp; z *= rcp; 1.1404 + return *this; } 1.1405 + 1.1406 + // Compare two vectors for equality with tolerance. Returns true if vectors match withing tolerance. 1.1407 + bool Compare(const Vector3&b, T tolerance = Mathf::Tolerance) 1.1408 + { 1.1409 + return (fabs(b.x-x) < tolerance) && (fabs(b.y-y) < tolerance) && (fabs(b.z-z) < tolerance); 1.1410 + } 1.1411 + 1.1412 + // Dot product overload. 1.1413 + // Used to calculate angle q between two vectors among other things, 1.1414 + // as (A dot B) = |a||b|cos(q). 1.1415 + T operator* (const Vector3& b) const { return x*b.x + y*b.y + z*b.z; } 1.1416 + 1.1417 + // Compute cross product, which generates a normal vector. 1.1418 + // Direction vector can be determined by right-hand rule: Pointing index finder in 1.1419 + // direction a and middle finger in direction b, thumb will point in a.Cross(b). 1.1420 + Vector3 Cross(const Vector3& b) const { return Vector3(y*b.z - z*b.y, 1.1421 + z*b.x - x*b.z, 1.1422 + x*b.y - y*b.x); } 1.1423 + 1.1424 + // Returns the angle from this vector to b, in radians. 1.1425 + T Angle(const Vector3& b) const { return acos((*this * b)/(Length()*b.Length())); } 1.1426 + 1.1427 + // Return Length of the vector squared. 1.1428 + T LengthSq() const { return (x * x + y * y + z * z); } 1.1429 + // Return vector length. 1.1430 + T Length() const { return sqrt(LengthSq()); } 1.1431 + 1.1432 + // Returns distance between two points represented by vectors. 1.1433 + T Distance(Vector3& b) const { return (*this - b).Length(); } 1.1434 + 1.1435 + // Determine if this a unit vector. 1.1436 + bool IsNormalized() const { return fabs(LengthSq() - T(1)) < Math<T>::Tolerance; } 1.1437 + // Normalize, convention vector length to 1. 1.1438 + void Normalize() { *this /= Length(); } 1.1439 + // Returns normalized (unit) version of the vector without modifying itself. 1.1440 + Vector3 Normalized() const { return *this / Length(); } 1.1441 + 1.1442 + // Linearly interpolates from this vector to another. 1.1443 + // Factor should be between 0.0 and 1.0, with 0 giving full value to this. 1.1444 + Vector3 Lerp(const Vector3& b, T f) const { return *this*(T(1) - f) + b*f; } 1.1445 + 1.1446 + // Projects this vector onto the argument; in other words, 1.1447 + // A.Project(B) returns projection of vector A onto B. 1.1448 + Vector3 ProjectTo(const Vector3& b) const { return b * ((*this * b) / b.LengthSq()); } 1.1449 +}; 1.1450 + 1.1451 + 1.1452 +typedef Vector3<float> Vector3f; 1.1453 +typedef Vector3<double> Vector3d; 1.1454 + 1.1455 + 1.1456 +//------------------------------------------------------------------------------------- 1.1457 +// ***** Matrix4f 1.1458 + 1.1459 +// Matrix4f is a 4x4 matrix used for 3d transformations and projections. 1.1460 +// Translation stored in the last column. 1.1461 +// The matrix is stored in row-major order in memory, meaning that values 1.1462 +// of the first row are stored before the next one. 1.1463 +// 1.1464 +// The arrangement of the matrix is chosen to be in Right-Handed 1.1465 +// coordinate system and counterclockwise rotations when looking down 1.1466 +// the axis 1.1467 +// 1.1468 +// Transformation Order: 1.1469 +// - Transformations are applied from right to left, so the expression 1.1470 +// M1 * M2 * M3 * V means that the vector V is transformed by M3 first, 1.1471 +// followed by M2 and M1. 1.1472 +// 1.1473 +// Coordinate system: Right Handed 1.1474 +// 1.1475 +// Rotations: Counterclockwise when looking down the axis. All angles are in radians. 1.1476 +// 1.1477 +// | sx 01 02 tx | // First column (sx, 10, 20): Axis X basis vector. 1.1478 +// | 10 sy 12 ty | // Second column (01, sy, 21): Axis Y basis vector. 1.1479 +// | 20 21 sz tz | // Third columnt (02, 12, sz): Axis Z basis vector. 1.1480 +// | 30 31 32 33 | 1.1481 +// 1.1482 +// The basis vectors are first three columns. 1.1483 + 1.1484 +class Matrix4f 1.1485 +{ 1.1486 + static Matrix4f IdentityValue; 1.1487 + 1.1488 +public: 1.1489 + float M[4][4]; 1.1490 + 1.1491 + enum NoInitType { NoInit }; 1.1492 + 1.1493 + // Construct with no memory initialization. 1.1494 + Matrix4f(NoInitType) { } 1.1495 + 1.1496 + // By default, we construct identity matrix. 1.1497 + Matrix4f() 1.1498 + { 1.1499 + SetIdentity(); 1.1500 + } 1.1501 + 1.1502 + Matrix4f(float m11, float m12, float m13, float m14, 1.1503 + float m21, float m22, float m23, float m24, 1.1504 + float m31, float m32, float m33, float m34, 1.1505 + float m41, float m42, float m43, float m44) 1.1506 + { 1.1507 + M[0][0] = m11; M[0][1] = m12; M[0][2] = m13; M[0][3] = m14; 1.1508 + M[1][0] = m21; M[1][1] = m22; M[1][2] = m23; M[1][3] = m24; 1.1509 + M[2][0] = m31; M[2][1] = m32; M[2][2] = m33; M[2][3] = m34; 1.1510 + M[3][0] = m41; M[3][1] = m42; M[3][2] = m43; M[3][3] = m44; 1.1511 + } 1.1512 + 1.1513 + Matrix4f(float m11, float m12, float m13, 1.1514 + float m21, float m22, float m23, 1.1515 + float m31, float m32, float m33) 1.1516 + { 1.1517 + M[0][0] = m11; M[0][1] = m12; M[0][2] = m13; M[0][3] = 0; 1.1518 + M[1][0] = m21; M[1][1] = m22; M[1][2] = m23; M[1][3] = 0; 1.1519 + M[2][0] = m31; M[2][1] = m32; M[2][2] = m33; M[2][3] = 0; 1.1520 + M[3][0] = 0; M[3][1] = 0; M[3][2] = 0; M[3][3] = 1; 1.1521 + } 1.1522 + 1.1523 + static const Matrix4f& Identity() { return IdentityValue; } 1.1524 + 1.1525 + void SetIdentity() 1.1526 + { 1.1527 + M[0][0] = M[1][1] = M[2][2] = M[3][3] = 1; 1.1528 + M[0][1] = M[1][0] = M[2][3] = M[3][1] = 0; 1.1529 + M[0][2] = M[1][2] = M[2][0] = M[3][2] = 0; 1.1530 + M[0][3] = M[1][3] = M[2][1] = M[3][0] = 0; 1.1531 + } 1.1532 + 1.1533 + // Multiplies two matrices into destination with minimum copying. 1.1534 + static Matrix4f& Multiply(Matrix4f* d, const Matrix4f& a, const Matrix4f& b) 1.1535 + { 1.1536 + OVR_ASSERT((d != &a) && (d != &b)); 1.1537 + int i = 0; 1.1538 + do { 1.1539 + 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]; 1.1540 + 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]; 1.1541 + 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]; 1.1542 + 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]; 1.1543 + } while((++i) < 4); 1.1544 + 1.1545 + return *d; 1.1546 + } 1.1547 + 1.1548 + Matrix4f operator* (const Matrix4f& b) const 1.1549 + { 1.1550 + Matrix4f result(Matrix4f::NoInit); 1.1551 + Multiply(&result, *this, b); 1.1552 + return result; 1.1553 + } 1.1554 + 1.1555 + Matrix4f& operator*= (const Matrix4f& b) 1.1556 + { 1.1557 + return Multiply(this, Matrix4f(*this), b); 1.1558 + } 1.1559 + 1.1560 + Matrix4f operator* (float s) const 1.1561 + { 1.1562 + return Matrix4f(M[0][0] * s, M[0][1] * s, M[0][2] * s, M[0][3] * s, 1.1563 + M[1][0] * s, M[1][1] * s, M[1][2] * s, M[1][3] * s, 1.1564 + M[2][0] * s, M[2][1] * s, M[2][2] * s, M[2][3] * s, 1.1565 + M[3][0] * s, M[3][1] * s, M[3][2] * s, M[3][3] * s); 1.1566 + } 1.1567 + 1.1568 + Matrix4f& operator*= (float s) 1.1569 + { 1.1570 + M[0][0] *= s; M[0][1] *= s; M[0][2] *= s; M[0][3] *= s; 1.1571 + M[1][0] *= s; M[1][1] *= s; M[1][2] *= s; M[1][3] *= s; 1.1572 + M[2][0] *= s; M[2][1] *= s; M[2][2] *= s; M[2][3] *= s; 1.1573 + M[3][0] *= s; M[3][1] *= s; M[3][2] *= s; M[3][3] *= s; 1.1574 + return *this; 1.1575 + } 1.1576 + 1.1577 + Vector3f Transform(const Vector3f& v) const 1.1578 + { 1.1579 + return Vector3f(M[0][0] * v.x + M[0][1] * v.y + M[0][2] * v.z + M[0][3], 1.1580 + M[1][0] * v.x + M[1][1] * v.y + M[1][2] * v.z + M[1][3], 1.1581 + M[2][0] * v.x + M[2][1] * v.y + M[2][2] * v.z + M[2][3]); 1.1582 + } 1.1583 + 1.1584 + Matrix4f Transposed() const 1.1585 + { 1.1586 + return Matrix4f(M[0][0], M[1][0], M[2][0], M[3][0], 1.1587 + M[0][1], M[1][1], M[2][1], M[3][1], 1.1588 + M[0][2], M[1][2], M[2][2], M[3][2], 1.1589 + M[0][3], M[1][3], M[2][3], M[3][3]); 1.1590 + } 1.1591 + 1.1592 + void Transpose() 1.1593 + { 1.1594 + *this = Transposed(); 1.1595 + } 1.1596 + 1.1597 + 1.1598 + float SubDet (const int* rows, const int* cols) const 1.1599 + { 1.1600 + 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]]) 1.1601 + - 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]]) 1.1602 + + 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]]); 1.1603 + } 1.1604 + 1.1605 + float Cofactor(int I, int J) const 1.1606 + { 1.1607 + const int indices[4][3] = {{1,2,3},{0,2,3},{0,1,3},{0,1,2}}; 1.1608 + return ((I+J)&1) ? -SubDet(indices[I],indices[J]) : SubDet(indices[I],indices[J]); 1.1609 + } 1.1610 + 1.1611 + float Determinant() const 1.1612 + { 1.1613 + 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); 1.1614 + } 1.1615 + 1.1616 + Matrix4f Adjugated() const 1.1617 + { 1.1618 + return Matrix4f(Cofactor(0,0), Cofactor(1,0), Cofactor(2,0), Cofactor(3,0), 1.1619 + Cofactor(0,1), Cofactor(1,1), Cofactor(2,1), Cofactor(3,1), 1.1620 + Cofactor(0,2), Cofactor(1,2), Cofactor(2,2), Cofactor(3,2), 1.1621 + Cofactor(0,3), Cofactor(1,3), Cofactor(2,3), Cofactor(3,3)); 1.1622 + } 1.1623 + 1.1624 + Matrix4f Inverted() const 1.1625 + { 1.1626 + float det = Determinant(); 1.1627 + assert(det != 0); 1.1628 + return Adjugated() * (1.0f/det); 1.1629 + } 1.1630 + 1.1631 + void Invert() 1.1632 + { 1.1633 + *this = Inverted(); 1.1634 + } 1.1635 + 1.1636 + //AnnaSteve: 1.1637 + // a,b,c, are the YawPitchRoll angles to be returned 1.1638 + // rotation a around axis A1 1.1639 + // is followed by rotation b around axis A2 1.1640 + // is followed by rotation c around axis A3 1.1641 + // rotations are CCW or CW (D) in LH or RH coordinate system (S) 1.1642 + template <Axis A1, Axis A2, Axis A3, RotateDirection D, HandedSystem S> 1.1643 + void ToEulerAngles(float *a, float *b, float *c) 1.1644 + { 1.1645 + OVR_COMPILER_ASSERT((A1 != A2) && (A2 != A3) && (A1 != A3)); 1.1646 + 1.1647 + float psign = -1.0f; 1.1648 + if (((A1 + 1) % 3 == A2) && ((A2 + 1) % 3 == A3)) // Determine whether even permutation 1.1649 + psign = 1.0f; 1.1650 + 1.1651 + float pm = psign*M[A1][A3]; 1.1652 + if (pm < -1.0f + Math<float>::SingularityRadius) 1.1653 + { // South pole singularity 1.1654 + *a = 0.0f; 1.1655 + *b = -S*D*Math<float>::PiOver2; 1.1656 + *c = S*D*atan2( psign*M[A2][A1], M[A2][A2] ); 1.1657 + } 1.1658 + else if (pm > 1.0 - Math<float>::SingularityRadius) 1.1659 + { // North pole singularity 1.1660 + *a = 0.0f; 1.1661 + *b = S*D*Math<float>::PiOver2; 1.1662 + *c = S*D*atan2( psign*M[A2][A1], M[A2][A2] ); 1.1663 + } 1.1664 + else 1.1665 + { // Normal case (nonsingular) 1.1666 + *a = S*D*atan2( -psign*M[A2][A3], M[A3][A3] ); 1.1667 + *b = S*D*asin(pm); 1.1668 + *c = S*D*atan2( -psign*M[A1][A2], M[A1][A1] ); 1.1669 + } 1.1670 + 1.1671 + return; 1.1672 + } 1.1673 + 1.1674 + //AnnaSteve: 1.1675 + // a,b,c, are the YawPitchRoll angles to be returned 1.1676 + // rotation a around axis A1 1.1677 + // is followed by rotation b around axis A2 1.1678 + // is followed by rotation c around axis A1 1.1679 + // rotations are CCW or CW (D) in LH or RH coordinate system (S) 1.1680 + template <Axis A1, Axis A2, RotateDirection D, HandedSystem S> 1.1681 + void ToEulerAnglesABA(float *a, float *b, float *c) 1.1682 + { 1.1683 + OVR_COMPILER_ASSERT(A1 != A2); 1.1684 + 1.1685 + // Determine the axis that was not supplied 1.1686 + int m = 3 - A1 - A2; 1.1687 + 1.1688 + float psign = -1.0f; 1.1689 + if ((A1 + 1) % 3 == A2) // Determine whether even permutation 1.1690 + psign = 1.0f; 1.1691 + 1.1692 + float c2 = M[A1][A1]; 1.1693 + if (c2 < -1.0 + Math<float>::SingularityRadius) 1.1694 + { // South pole singularity 1.1695 + *a = 0.0f; 1.1696 + *b = S*D*Math<float>::Pi; 1.1697 + *c = S*D*atan2( -psign*M[A2][m],M[A2][A2]); 1.1698 + } 1.1699 + else if (c2 > 1.0 - Math<float>::SingularityRadius) 1.1700 + { // North pole singularity 1.1701 + *a = 0.0f; 1.1702 + *b = 0.0f; 1.1703 + *c = S*D*atan2( -psign*M[A2][m],M[A2][A2]); 1.1704 + } 1.1705 + else 1.1706 + { // Normal case (nonsingular) 1.1707 + *a = S*D*atan2( M[A2][A1],-psign*M[m][A1]); 1.1708 + *b = S*D*acos(c2); 1.1709 + *c = S*D*atan2( M[A1][A2],psign*M[A1][m]); 1.1710 + } 1.1711 + return; 1.1712 + } 1.1713 + 1.1714 + // Creates a matrix that converts the vertices from one coordinate system 1.1715 + // to another. 1.1716 + // 1.1717 + static Matrix4f AxisConversion(const WorldAxes& to, const WorldAxes& from) 1.1718 + { 1.1719 + // Holds axis values from the 'to' structure 1.1720 + int toArray[3] = { to.XAxis, to.YAxis, to.ZAxis }; 1.1721 + 1.1722 + // The inverse of the toArray 1.1723 + int inv[4]; 1.1724 + inv[0] = inv[abs(to.XAxis)] = 0; 1.1725 + inv[abs(to.YAxis)] = 1; 1.1726 + inv[abs(to.ZAxis)] = 2; 1.1727 + 1.1728 + Matrix4f m(0, 0, 0, 1.1729 + 0, 0, 0, 1.1730 + 0, 0, 0); 1.1731 + 1.1732 + // Only three values in the matrix need to be changed to 1 or -1. 1.1733 + m.M[inv[abs(from.XAxis)]][0] = float(from.XAxis/toArray[inv[abs(from.XAxis)]]); 1.1734 + m.M[inv[abs(from.YAxis)]][1] = float(from.YAxis/toArray[inv[abs(from.YAxis)]]); 1.1735 + m.M[inv[abs(from.ZAxis)]][2] = float(from.ZAxis/toArray[inv[abs(from.ZAxis)]]); 1.1736 + return m; 1.1737 + } 1.1738 + 1.1739 + 1.1740 + 1.1741 + static Matrix4f Translation(const Vector3f& v) 1.1742 + { 1.1743 + Matrix4f t; 1.1744 + t.M[0][3] = v.x; 1.1745 + t.M[1][3] = v.y; 1.1746 + t.M[2][3] = v.z; 1.1747 + return t; 1.1748 + } 1.1749 + 1.1750 + static Matrix4f Translation(float x, float y, float z = 0.0f) 1.1751 + { 1.1752 + Matrix4f t; 1.1753 + t.M[0][3] = x; 1.1754 + t.M[1][3] = y; 1.1755 + t.M[2][3] = z; 1.1756 + return t; 1.1757 + } 1.1758 + 1.1759 + static Matrix4f Scaling(const Vector3f& v) 1.1760 + { 1.1761 + Matrix4f t; 1.1762 + t.M[0][0] = v.x; 1.1763 + t.M[1][1] = v.y; 1.1764 + t.M[2][2] = v.z; 1.1765 + return t; 1.1766 + } 1.1767 + 1.1768 + static Matrix4f Scaling(float x, float y, float z) 1.1769 + { 1.1770 + Matrix4f t; 1.1771 + t.M[0][0] = x; 1.1772 + t.M[1][1] = y; 1.1773 + t.M[2][2] = z; 1.1774 + return t; 1.1775 + } 1.1776 + 1.1777 + static Matrix4f Scaling(float s) 1.1778 + { 1.1779 + Matrix4f t; 1.1780 + t.M[0][0] = s; 1.1781 + t.M[1][1] = s; 1.1782 + t.M[2][2] = s; 1.1783 + return t; 1.1784 + } 1.1785 + 1.1786 + 1.1787 + 1.1788 + //AnnaSteve : Just for quick testing. Not for final API. Need to remove case. 1.1789 + static Matrix4f RotationAxis(Axis A, float angle, RotateDirection d, HandedSystem s) 1.1790 + { 1.1791 + float sina = s * d *sin(angle); 1.1792 + float cosa = cos(angle); 1.1793 + 1.1794 + switch(A) 1.1795 + { 1.1796 + case Axis_X: 1.1797 + return Matrix4f(1, 0, 0, 1.1798 + 0, cosa, -sina, 1.1799 + 0, sina, cosa); 1.1800 + case Axis_Y: 1.1801 + return Matrix4f(cosa, 0, sina, 1.1802 + 0, 1, 0, 1.1803 + -sina, 0, cosa); 1.1804 + case Axis_Z: 1.1805 + return Matrix4f(cosa, -sina, 0, 1.1806 + sina, cosa, 0, 1.1807 + 0, 0, 1); 1.1808 + } 1.1809 + } 1.1810 + 1.1811 + 1.1812 + // Creates a rotation matrix rotating around the X axis by 'angle' radians. 1.1813 + // Rotation direction is depends on the coordinate system: 1.1814 + // RHS (Oculus default): Positive angle values rotate Counter-clockwise (CCW), 1.1815 + // while looking in the negative axis direction. This is the 1.1816 + // same as looking down from positive axis values towards origin. 1.1817 + // LHS: Positive angle values rotate clock-wise (CW), while looking in the 1.1818 + // negative axis direction. 1.1819 + static Matrix4f RotationX(float angle) 1.1820 + { 1.1821 + float sina = sin(angle); 1.1822 + float cosa = cos(angle); 1.1823 + return Matrix4f(1, 0, 0, 1.1824 + 0, cosa, -sina, 1.1825 + 0, sina, cosa); 1.1826 + } 1.1827 + 1.1828 + // Creates a rotation matrix rotating around the Y axis by 'angle' radians. 1.1829 + // Rotation direction is depends on the coordinate system: 1.1830 + // RHS (Oculus default): Positive angle values rotate Counter-clockwise (CCW), 1.1831 + // while looking in the negative axis direction. This is the 1.1832 + // same as looking down from positive axis values towards origin. 1.1833 + // LHS: Positive angle values rotate clock-wise (CW), while looking in the 1.1834 + // negative axis direction. 1.1835 + static Matrix4f RotationY(float angle) 1.1836 + { 1.1837 + float sina = sin(angle); 1.1838 + float cosa = cos(angle); 1.1839 + return Matrix4f(cosa, 0, sina, 1.1840 + 0, 1, 0, 1.1841 + -sina, 0, cosa); 1.1842 + } 1.1843 + 1.1844 + // Creates a rotation matrix rotating around the Z axis by 'angle' radians. 1.1845 + // Rotation direction is depends on the coordinate system: 1.1846 + // RHS (Oculus default): Positive angle values rotate Counter-clockwise (CCW), 1.1847 + // while looking in the negative axis direction. This is the 1.1848 + // same as looking down from positive axis values towards origin. 1.1849 + // LHS: Positive angle values rotate clock-wise (CW), while looking in the 1.1850 + // negative axis direction. 1.1851 + static Matrix4f RotationZ(float angle) 1.1852 + { 1.1853 + float sina = sin(angle); 1.1854 + float cosa = cos(angle); 1.1855 + return Matrix4f(cosa, -sina, 0, 1.1856 + sina, cosa, 0, 1.1857 + 0, 0, 1); 1.1858 + } 1.1859 + 1.1860 + 1.1861 + // LookAtRH creates a View transformation matrix for right-handed coordinate system. 1.1862 + // The resulting matrix points camera from 'eye' towards 'at' direction, with 'up' 1.1863 + // specifying the up vector. The resulting matrix should be used with PerspectiveRH 1.1864 + // projection. 1.1865 + static Matrix4f LookAtRH(const Vector3f& eye, const Vector3f& at, const Vector3f& up); 1.1866 + 1.1867 + // LookAtLH creates a View transformation matrix for left-handed coordinate system. 1.1868 + // The resulting matrix points camera from 'eye' towards 'at' direction, with 'up' 1.1869 + // specifying the up vector. 1.1870 + static Matrix4f LookAtLH(const Vector3f& eye, const Vector3f& at, const Vector3f& up); 1.1871 + 1.1872 + 1.1873 + // PerspectiveRH creates a right-handed perspective projection matrix that can be 1.1874 + // used with the Oculus sample renderer. 1.1875 + // yfov - Specifies vertical field of view in radians. 1.1876 + // aspect - Screen aspect ration, which is usually width/height for square pixels. 1.1877 + // Note that xfov = yfov * aspect. 1.1878 + // znear - Absolute value of near Z clipping clipping range. 1.1879 + // zfar - Absolute value of far Z clipping clipping range (larger then near). 1.1880 + // Even though RHS usually looks in the direction of negative Z, positive values 1.1881 + // are expected for znear and zfar. 1.1882 + static Matrix4f PerspectiveRH(float yfov, float aspect, float znear, float zfar); 1.1883 + 1.1884 + 1.1885 + // PerspectiveRH creates a left-handed perspective projection matrix that can be 1.1886 + // used with the Oculus sample renderer. 1.1887 + // yfov - Specifies vertical field of view in radians. 1.1888 + // aspect - Screen aspect ration, which is usually width/height for square pixels. 1.1889 + // Note that xfov = yfov * aspect. 1.1890 + // znear - Absolute value of near Z clipping clipping range. 1.1891 + // zfar - Absolute value of far Z clipping clipping range (larger then near). 1.1892 + static Matrix4f PerspectiveLH(float yfov, float aspect, float znear, float zfar); 1.1893 + 1.1894 + 1.1895 + static Matrix4f Ortho2D(float w, float h); 1.1896 +}; 1.1897 + 1.1898 + 1.1899 +//------------------------------------------------------------------------------------- 1.1900 +// ***** Quat 1.1901 + 1.1902 +// Quatf represents a quaternion class used for rotations. 1.1903 +// 1.1904 +// Quaternion multiplications are done in right-to-left order, to match the 1.1905 +// behavior of matrices. 1.1906 + 1.1907 + 1.1908 +template<class T> 1.1909 +class Quat 1.1910 +{ 1.1911 +public: 1.1912 + // w + Xi + Yj + Zk 1.1913 + T x, y, z, w; 1.1914 + 1.1915 + Quat() : x(0), y(0), z(0), w(1) {} 1.1916 + Quat(T x_, T y_, T z_, T w_) : x(x_), y(y_), z(z_), w(w_) {} 1.1917 + 1.1918 + 1.1919 + // Constructs rotation quaternion around the axis. 1.1920 + Quat(const Vector3<T>& axis, T angle) 1.1921 + { 1.1922 + Vector3<T> unitAxis = axis.Normalized(); 1.1923 + T sinHalfAngle = sin(angle * T(0.5)); 1.1924 + 1.1925 + w = cos(angle * T(0.5)); 1.1926 + x = unitAxis.x * sinHalfAngle; 1.1927 + y = unitAxis.y * sinHalfAngle; 1.1928 + z = unitAxis.z * sinHalfAngle; 1.1929 + } 1.1930 + 1.1931 + //AnnaSteve: 1.1932 + void AxisAngle(Axis A, T angle, RotateDirection d, HandedSystem s) 1.1933 + { 1.1934 + T sinHalfAngle = s * d *sin(angle * (T)0.5); 1.1935 + T v[3]; 1.1936 + v[0] = v[1] = v[2] = (T)0; 1.1937 + v[A] = sinHalfAngle; 1.1938 + //return Quat(v[0], v[1], v[2], cos(angle * (T)0.5)); 1.1939 + w = cos(angle * (T)0.5); 1.1940 + x = v[0]; 1.1941 + y = v[1]; 1.1942 + z = v[2]; 1.1943 + } 1.1944 + 1.1945 + 1.1946 + void GetAxisAngle(Vector3<T>* axis, T* angle) const 1.1947 + { 1.1948 + if (LengthSq() > Math<T>::Tolerance * Math<T>::Tolerance) 1.1949 + { 1.1950 + *axis = Vector3<T>(x, y, z).Normalized(); 1.1951 + *angle = 2 * acos(w); 1.1952 + } 1.1953 + else 1.1954 + { 1.1955 + *axis = Vector3<T>(1, 0, 0); 1.1956 + *angle= 0; 1.1957 + } 1.1958 + } 1.1959 + 1.1960 + bool operator== (const Quat& b) const { return x == b.x && y == b.y && z == b.z && w == b.w; } 1.1961 + bool operator!= (const Quat& b) const { return x != b.x || y != b.y || z != b.z || w != b.w; } 1.1962 + 1.1963 + Quat operator+ (const Quat& b) const { return Quat(x + b.x, y + b.y, z + b.z, w + b.w); } 1.1964 + Quat& operator+= (const Quat& b) { w += b.w; x += b.x; y += b.y; z += b.z; return *this; } 1.1965 + Quat operator- (const Quat& b) const { return Quat(x - b.x, y - b.y, z - b.z, w - b.w); } 1.1966 + Quat& operator-= (const Quat& b) { w -= b.w; x -= b.x; y -= b.y; z -= b.z; return *this; } 1.1967 + 1.1968 + Quat operator* (T s) const { return Quat(x * s, y * s, z * s, w * s); } 1.1969 + Quat& operator*= (T s) { w *= s; x *= s; y *= s; z *= s; return *this; } 1.1970 + Quat operator/ (T s) const { T rcp = T(1)/s; return Quat(x * rcp, y * rcp, z * rcp, w *rcp); } 1.1971 + Quat& operator/= (T s) { T rcp = T(1)/s; w *= rcp; x *= rcp; y *= rcp; z *= rcp; return *this; } 1.1972 + 1.1973 + // Get Imaginary part vector 1.1974 + Vector3<T> Imag() const { return Vector3<T>(x,y,z); } 1.1975 + 1.1976 + // Get quaternion length. 1.1977 + T Length() const { return sqrt(x * x + y * y + z * z + w * w); } 1.1978 + // Get quaternion length squared. 1.1979 + T LengthSq() const { return (x * x + y * y + z * z + w * w); } 1.1980 + // Simple Eulidean distance in R^4 (not SLERP distance, but at least respects Haar measure) 1.1981 + T Distance(const Quat& q) const 1.1982 + { 1.1983 + T d1 = (*this - q).Length(); 1.1984 + T d2 = (*this + q).Length(); // Antipoldal point check 1.1985 + return (d1 < d2) ? d1 : d2; 1.1986 + } 1.1987 + T DistanceSq(const Quat& q) const 1.1988 + { 1.1989 + T d1 = (*this - q).LengthSq(); 1.1990 + T d2 = (*this + q).LengthSq(); // Antipoldal point check 1.1991 + return (d1 < d2) ? d1 : d2; 1.1992 + } 1.1993 + 1.1994 + // Normalize 1.1995 + bool IsNormalized() const { return fabs(LengthSq() - 1) < Math<T>::Tolerance; } 1.1996 + void Normalize() { *this /= Length(); } 1.1997 + Quat Normalized() const { return *this / Length(); } 1.1998 + 1.1999 + // Returns conjugate of the quaternion. Produces inverse rotation if quaternion is normalized. 1.2000 + Quat Conj() const { return Quat(-x, -y, -z, w); } 1.2001 + 1.2002 + // AnnaSteve fixed: order of quaternion multiplication 1.2003 + // Quaternion multiplication. Combines quaternion rotations, performing the one on the 1.2004 + // right hand side first. 1.2005 + Quat operator* (const Quat& b) const { return Quat(w * b.x + x * b.w + y * b.z - z * b.y, 1.2006 + w * b.y - x * b.z + y * b.w + z * b.x, 1.2007 + w * b.z + x * b.y - y * b.x + z * b.w, 1.2008 + w * b.w - x * b.x - y * b.y - z * b.z); } 1.2009 + 1.2010 + // 1.2011 + // this^p normalized; same as rotating by this p times. 1.2012 + Quat PowNormalized(T p) const 1.2013 + { 1.2014 + Vector3<T> v; 1.2015 + T a; 1.2016 + GetAxisAngle(&v, &a); 1.2017 + return Quat(v, a * p); 1.2018 + } 1.2019 + 1.2020 + // Rotate transforms vector in a manner that matches Matrix rotations (counter-clockwise, 1.2021 + // assuming negative direction of the axis). Standard formula: q(t) * V * q(t)^-1. 1.2022 + Vector3<T> Rotate(const Vector3<T>& v) const 1.2023 + { 1.2024 + return ((*this * Quat<T>(v.x, v.y, v.z, 0)) * Inverted()).Imag(); 1.2025 + } 1.2026 + 1.2027 + 1.2028 + // Inversed quaternion rotates in the opposite direction. 1.2029 + Quat Inverted() const 1.2030 + { 1.2031 + return Quat(-x, -y, -z, w); 1.2032 + } 1.2033 + 1.2034 + // Sets this quaternion to the one rotates in the opposite direction. 1.2035 + void Invert() 1.2036 + { 1.2037 + *this = Quat(-x, -y, -z, w); 1.2038 + } 1.2039 + 1.2040 + // Converting quaternion to matrix. 1.2041 + operator Matrix4f() const 1.2042 + { 1.2043 + T ww = w*w; 1.2044 + T xx = x*x; 1.2045 + T yy = y*y; 1.2046 + T zz = z*z; 1.2047 + 1.2048 + return Matrix4f(float(ww + xx - yy - zz), float(T(2) * (x*y - w*z)), float(T(2) * (x*z + w*y)), 1.2049 + float(T(2) * (x*y + w*z)), float(ww - xx + yy - zz), float(T(2) * (y*z - w*x)), 1.2050 + float(T(2) * (x*z - w*y)), float(T(2) * (y*z + w*x)), float(ww - xx - yy + zz) ); 1.2051 + } 1.2052 + 1.2053 + 1.2054 + // GetEulerAngles extracts Euler angles from the quaternion, in the specified order of 1.2055 + // axis rotations and the specified coordinate system. Right-handed coordinate system 1.2056 + // is the default, with CCW rotations while looking in the negative axis direction. 1.2057 + // Here a,b,c, are the Yaw/Pitch/Roll angles to be returned. 1.2058 + // rotation a around axis A1 1.2059 + // is followed by rotation b around axis A2 1.2060 + // is followed by rotation c around axis A3 1.2061 + // rotations are CCW or CW (D) in LH or RH coordinate system (S) 1.2062 + template <Axis A1, Axis A2, Axis A3, RotateDirection D, HandedSystem S> 1.2063 + void GetEulerAngles(T *a, T *b, T *c) 1.2064 + { 1.2065 + OVR_COMPILER_ASSERT((A1 != A2) && (A2 != A3) && (A1 != A3)); 1.2066 + 1.2067 + T Q[3] = { x, y, z }; //Quaternion components x,y,z 1.2068 + 1.2069 + T ww = w*w; 1.2070 + T Q11 = Q[A1]*Q[A1]; 1.2071 + T Q22 = Q[A2]*Q[A2]; 1.2072 + T Q33 = Q[A3]*Q[A3]; 1.2073 + 1.2074 + T psign = T(-1.0); 1.2075 + // Determine whether even permutation 1.2076 + if (((A1 + 1) % 3 == A2) && ((A2 + 1) % 3 == A3)) 1.2077 + psign = T(1.0); 1.2078 + 1.2079 + T s2 = psign * T(2.0) * (psign*w*Q[A2] + Q[A1]*Q[A3]); 1.2080 + 1.2081 + if (s2 < (T)-1.0 + Math<T>::SingularityRadius) 1.2082 + { // South pole singularity 1.2083 + *a = T(0.0); 1.2084 + *b = -S*D*Math<T>::PiOver2; 1.2085 + *c = S*D*atan2((T)2.0*(psign*Q[A1]*Q[A2] + w*Q[A3]), 1.2086 + ww + Q22 - Q11 - Q33 ); 1.2087 + } 1.2088 + else if (s2 > (T)1.0 - Math<T>::SingularityRadius) 1.2089 + { // North pole singularity 1.2090 + *a = (T)0.0; 1.2091 + *b = S*D*Math<T>::PiOver2; 1.2092 + *c = S*D*atan2((T)2.0*(psign*Q[A1]*Q[A2] + w*Q[A3]), 1.2093 + ww + Q22 - Q11 - Q33); 1.2094 + } 1.2095 + else 1.2096 + { 1.2097 + *a = -S*D*atan2((T)-2.0*(w*Q[A1] - psign*Q[A2]*Q[A3]), 1.2098 + ww + Q33 - Q11 - Q22); 1.2099 + *b = S*D*asin(s2); 1.2100 + *c = S*D*atan2((T)2.0*(w*Q[A3] - psign*Q[A1]*Q[A2]), 1.2101 + ww + Q11 - Q22 - Q33); 1.2102 + } 1.2103 + return; 1.2104 + } 1.2105 + 1.2106 + template <Axis A1, Axis A2, Axis A3, RotateDirection D> 1.2107 + void GetEulerAngles(T *a, T *b, T *c) 1.2108 + { GetEulerAngles<A1, A2, A3, D, Handed_R>(a, b, c); } 1.2109 + 1.2110 + template <Axis A1, Axis A2, Axis A3> 1.2111 + void GetEulerAngles(T *a, T *b, T *c) 1.2112 + { GetEulerAngles<A1, A2, A3, Rotate_CCW, Handed_R>(a, b, c); } 1.2113 + 1.2114 + 1.2115 + // GetEulerAnglesABA extracts Euler angles from the quaternion, in the specified order of 1.2116 + // axis rotations and the specified coordinate system. Right-handed coordinate system 1.2117 + // is the default, with CCW rotations while looking in the negative axis direction. 1.2118 + // Here a,b,c, are the Yaw/Pitch/Roll angles to be returned. 1.2119 + // rotation a around axis A1 1.2120 + // is followed by rotation b around axis A2 1.2121 + // is followed by rotation c around axis A1 1.2122 + // Rotations are CCW or CW (D) in LH or RH coordinate system (S) 1.2123 + template <Axis A1, Axis A2, RotateDirection D, HandedSystem S> 1.2124 + void GetEulerAnglesABA(T *a, T *b, T *c) 1.2125 + { 1.2126 + OVR_COMPILER_ASSERT(A1 != A2); 1.2127 + 1.2128 + T Q[3] = {x, y, z}; // Quaternion components 1.2129 + 1.2130 + // Determine the missing axis that was not supplied 1.2131 + int m = 3 - A1 - A2; 1.2132 + 1.2133 + T ww = w*w; 1.2134 + T Q11 = Q[A1]*Q[A1]; 1.2135 + T Q22 = Q[A2]*Q[A2]; 1.2136 + T Qmm = Q[m]*Q[m]; 1.2137 + 1.2138 + T psign = T(-1.0); 1.2139 + if ((A1 + 1) % 3 == A2) // Determine whether even permutation 1.2140 + { 1.2141 + psign = (T)1.0; 1.2142 + } 1.2143 + 1.2144 + T c2 = ww + Q11 - Q22 - Qmm; 1.2145 + if (c2 < (T)-1.0 + Math<T>::SingularityRadius) 1.2146 + { // South pole singularity 1.2147 + *a = (T)0.0; 1.2148 + *b = S*D*Math<T>::Pi; 1.2149 + *c = S*D*atan2( (T)2.0*(w*Q[A1] - psign*Q[A2]*Q[m]), 1.2150 + ww + Q22 - Q11 - Qmm); 1.2151 + } 1.2152 + else if (c2 > (T)1.0 - Math<T>::SingularityRadius) 1.2153 + { // North pole singularity 1.2154 + *a = (T)0.0; 1.2155 + *b = (T)0.0; 1.2156 + *c = S*D*atan2( (T)2.0*(w*Q[A1] - psign*Q[A2]*Q[m]), 1.2157 + ww + Q22 - Q11 - Qmm); 1.2158 + } 1.2159 + else 1.2160 + { 1.2161 + *a = S*D*atan2( psign*w*Q[m] + Q[A1]*Q[A2], 1.2162 + w*Q[A2] -psign*Q[A1]*Q[m]); 1.2163 + *b = S*D*acos(c2); 1.2164 + *c = S*D*atan2( -psign*w*Q[m] + Q[A1]*Q[A2], 1.2165 + w*Q[A2] + psign*Q[A1]*Q[m]); 1.2166 + } 1.2167 + return; 1.2168 + } 1.2169 +}; 1.2170 + 1.2171 + 1.2172 +typedef Quat<float> Quatf; 1.2173 +typedef Quat<double> Quatd; 1.2174 + 1.2175 + 1.2176 + 1.2177 +//------------------------------------------------------------------------------------- 1.2178 +// ***** Angle 1.2179 + 1.2180 +// Cleanly representing the algebra of 2D rotations. 1.2181 +// The operations maintain the angle between -Pi and Pi, the same range as atan2. 1.2182 +// 1.2183 + 1.2184 +template<class T> 1.2185 +class Angle 1.2186 +{ 1.2187 +public: 1.2188 + enum AngularUnits 1.2189 + { 1.2190 + Radians = 0, 1.2191 + Degrees = 1 1.2192 + }; 1.2193 + 1.2194 + Angle() : a(0) {} 1.2195 + 1.2196 + // Fix the range to be between -Pi and Pi 1.2197 + Angle(T a_, AngularUnits u = Radians) : a((u == Radians) ? a_ : a_*Math<T>::DegreeToRadFactor) { FixRange(); } 1.2198 + 1.2199 + T Get(AngularUnits u = Radians) const { return (u == Radians) ? a : a*Math<T>::RadToDegreeFactor; } 1.2200 + void Set(const T& x, AngularUnits u = Radians) { a = (u == Radians) ? x : x*Math<T>::DegreeToRadFactor; FixRange(); } 1.2201 + int Sign() const { if (a == 0) return 0; else return (a > 0) ? 1 : -1; } 1.2202 + T Abs() const { return (a > 0) ? a : -a; } 1.2203 + 1.2204 + bool operator== (const Angle& b) const { return a == b.a; } 1.2205 + bool operator!= (const Angle& b) const { return a != b.a; } 1.2206 +// bool operator< (const Angle& b) const { return a < a.b; } 1.2207 +// bool operator> (const Angle& b) const { return a > a.b; } 1.2208 +// bool operator<= (const Angle& b) const { return a <= a.b; } 1.2209 +// bool operator>= (const Angle& b) const { return a >= a.b; } 1.2210 +// bool operator= (const T& x) { a = x; FixRange(); } 1.2211 + 1.2212 + // These operations assume a is already between -Pi and Pi. 1.2213 + Angle operator+ (const Angle& b) const { return Angle(a + b.a); } 1.2214 + Angle operator+ (const T& x) const { return Angle(a + x); } 1.2215 + Angle& operator+= (const Angle& b) { a = a + b.a; FastFixRange(); return *this; } 1.2216 + Angle& operator+= (const T& x) { a = a + x; FixRange(); return *this; } 1.2217 + Angle operator- (const Angle& b) const { return Angle(a - b.a); } 1.2218 + Angle operator- (const T& x) const { return Angle(a - x); } 1.2219 + Angle& operator-= (const Angle& b) { a = a - b.a; FastFixRange(); return *this; } 1.2220 + Angle& operator-= (const T& x) { a = a - x; FixRange(); return *this; } 1.2221 + 1.2222 + T Distance(const Angle& b) { T c = fabs(a - b.a); return (c <= Math<T>::Pi) ? c : Math<T>::TwoPi - c; } 1.2223 + 1.2224 +private: 1.2225 + 1.2226 + // The stored angle, which should be maintained between -Pi and Pi 1.2227 + T a; 1.2228 + 1.2229 + // Fixes the angle range to [-Pi,Pi], but assumes no more than 2Pi away on either side 1.2230 + inline void FastFixRange() 1.2231 + { 1.2232 + if (a < -Math<T>::Pi) 1.2233 + a += Math<T>::TwoPi; 1.2234 + else if (a > Math<T>::Pi) 1.2235 + a -= Math<T>::TwoPi; 1.2236 + } 1.2237 + 1.2238 + // Fixes the angle range to [-Pi,Pi] for any given range, but slower then the fast method 1.2239 + inline void FixRange() 1.2240 + { 1.2241 + a = fmod(a,Math<T>::TwoPi); 1.2242 + if (a < -Math<T>::Pi) 1.2243 + a += Math<T>::TwoPi; 1.2244 + else if (a > Math<T>::Pi) 1.2245 + a -= Math<T>::TwoPi; 1.2246 + } 1.2247 +}; 1.2248 + 1.2249 + 1.2250 +typedef Angle<float> Anglef; 1.2251 +typedef Angle<double> Angled; 1.2252 + 1.2253 + 1.2254 +//------------------------------------------------------------------------------------- 1.2255 +// ***** Plane 1.2256 + 1.2257 +// Consists of a normal vector and distance from the origin where the plane is located. 1.2258 + 1.2259 +template<class T> 1.2260 +class Plane : public RefCountBase<Plane<T> > 1.2261 +{ 1.2262 +public: 1.2263 + Vector3<T> N; 1.2264 + T D; 1.2265 + 1.2266 + Plane() : D(0) {} 1.2267 + 1.2268 + // Normals must already be normalized 1.2269 + Plane(const Vector3<T>& n, T d) : N(n), D(d) {} 1.2270 + Plane(T x, T y, T z, T d) : N(x,y,z), D(d) {} 1.2271 + 1.2272 + // construct from a point on the plane and the normal 1.2273 + Plane(const Vector3<T>& p, const Vector3<T>& n) : N(n), D(-(p * n)) {} 1.2274 + 1.2275 + // Find the point to plane distance. The sign indicates what side of the plane the point is on (0 = point on plane). 1.2276 + T TestSide(const Vector3<T>& p) const 1.2277 + { 1.2278 + return (N * p) + D; 1.2279 + } 1.2280 + 1.2281 + Plane<T> Flipped() const 1.2282 + { 1.2283 + return Plane(-N, -D); 1.2284 + } 1.2285 + 1.2286 + void Flip() 1.2287 + { 1.2288 + N = -N; 1.2289 + D = -D; 1.2290 + } 1.2291 + 1.2292 + bool operator==(const Plane<T>& rhs) const 1.2293 + { 1.2294 + return (this->D == rhs.D && this->N == rhs.N); 1.2295 + } 1.2296 +}; 1.2297 + 1.2298 +typedef Plane<float> Planef; 1.2299 + 1.2300 +} 1.2301 + 1.2302 +#endif