ovr_sdk: LibOVR/Src/Kernel/OVR

ovr_sdk

annotate LibOVR/Src/Kernel/OVR_Math.h @ 0:1b39a1b46319

initial 0.4.4

author	John Tsiombikas <nuclear@member.fsf.org>
date	Wed, 14 Jan 2015 06:51:16 +0200
parents
children

rev	line source
nuclear@0	1 /************************************************************************************
nuclear@0	2
nuclear@0	3 PublicHeader: OVR_Kernel.h
nuclear@0	4 Filename : OVR_Math.h
nuclear@0	5 Content : Implementation of 3D primitives such as vectors, matrices.
nuclear@0	6 Created : September 4, 2012
nuclear@0	7 Authors : Andrew Reisse, Michael Antonov, Steve LaValle,
nuclear@0	8 Anna Yershova, Max Katsev, Dov Katz
nuclear@0	9
nuclear@0	10 Copyright : Copyright 2014 Oculus VR, LLC All Rights reserved.
nuclear@0	11
nuclear@0	12 Licensed under the Oculus VR Rift SDK License Version 3.2 (the "License");
nuclear@0	13 you may not use the Oculus VR Rift SDK except in compliance with the License,
nuclear@0	14 which is provided at the time of installation or download, or which
nuclear@0	15 otherwise accompanies this software in either electronic or hard copy form.
nuclear@0	16
nuclear@0	17 You may obtain a copy of the License at
nuclear@0	18
nuclear@0	19 http://www.oculusvr.com/licenses/LICENSE-3.2
nuclear@0	20
nuclear@0	21 Unless required by applicable law or agreed to in writing, the Oculus VR SDK
nuclear@0	22 distributed under the License is distributed on an "AS IS" BASIS,
nuclear@0	23 WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
nuclear@0	24 See the License for the specific language governing permissions and
nuclear@0	25 limitations under the License.
nuclear@0	26
nuclear@0	27 *************************************************************************************/
nuclear@0	28
nuclear@0	29 #ifndef OVR_Math_h
nuclear@0	30 #define OVR_Math_h
nuclear@0	31
nuclear@0	32 #include <assert.h>
nuclear@0	33 #include <stdlib.h>
nuclear@0	34 #include <math.h>
nuclear@0	35
nuclear@0	36 #include "OVR_Types.h"
nuclear@0	37 #include "OVR_RefCount.h"
nuclear@0	38 #include "OVR_Std.h"
nuclear@0	39 #include "OVR_Alg.h"
nuclear@0	40
nuclear@0	41
nuclear@0	42 namespace OVR {
nuclear@0	43
nuclear@0	44 //-------------------------------------------------------------------------------------
nuclear@0	45 // ***** Constants for 3D world/axis definitions.
nuclear@0	46
nuclear@0	47 // Definitions of axes for coordinate and rotation conversions.
nuclear@0	48 enum Axis
nuclear@0	49 {
nuclear@0	50 Axis_X = 0, Axis_Y = 1, Axis_Z = 2
nuclear@0	51 };
nuclear@0	52
nuclear@0	53 // RotateDirection describes the rotation direction around an axis, interpreted as follows:
nuclear@0	54 // CW - Clockwise while looking "down" from positive axis towards the origin.
nuclear@0	55 // CCW - Counter-clockwise while looking from the positive axis towards the origin,
nuclear@0	56 // which is in the negative axis direction.
nuclear@0	57 // CCW is the default for the RHS coordinate system. Oculus standard RHS coordinate
nuclear@0	58 // system defines Y up, X right, and Z back (pointing out from the screen). In this
nuclear@0	59 // system Rotate_CCW around Z will specifies counter-clockwise rotation in XY plane.
nuclear@0	60 enum RotateDirection
nuclear@0	61 {
nuclear@0	62 Rotate_CCW = 1,
nuclear@0	63 Rotate_CW = -1
nuclear@0	64 };
nuclear@0	65
nuclear@0	66 // Constants for right handed and left handed coordinate systems
nuclear@0	67 enum HandedSystem
nuclear@0	68 {
nuclear@0	69 Handed_R = 1, Handed_L = -1
nuclear@0	70 };
nuclear@0	71
nuclear@0	72 // AxisDirection describes which way the coordinate axis points. Used by WorldAxes.
nuclear@0	73 enum AxisDirection
nuclear@0	74 {
nuclear@0	75 Axis_Up = 2,
nuclear@0	76 Axis_Down = -2,
nuclear@0	77 Axis_Right = 1,
nuclear@0	78 Axis_Left = -1,
nuclear@0	79 Axis_In = 3,
nuclear@0	80 Axis_Out = -3
nuclear@0	81 };
nuclear@0	82
nuclear@0	83 struct WorldAxes
nuclear@0	84 {
nuclear@0	85 AxisDirection XAxis, YAxis, ZAxis;
nuclear@0	86
nuclear@0	87 WorldAxes(AxisDirection x, AxisDirection y, AxisDirection z)
nuclear@0	88 : XAxis(x), YAxis(y), ZAxis(z)
nuclear@0	89 { OVR_ASSERT(abs(x) != abs(y) && abs(y) != abs(z) && abs(z) != abs(x));}
nuclear@0	90 };
nuclear@0	91
nuclear@0	92 } // namespace OVR
nuclear@0	93
nuclear@0	94
nuclear@0	95 //------------------------------------------------------------------------------------//
nuclear@0	96 // ***** C Compatibility Types
nuclear@0	97
nuclear@0	98 // These declarations are used to support conversion between C types used in
nuclear@0	99 // LibOVR C interfaces and their C++ versions. As an example, they allow passing
nuclear@0	100 // Vector3f into a function that expects ovrVector3f.
nuclear@0	101
nuclear@0	102 typedef struct ovrQuatf_ ovrQuatf;
nuclear@0	103 typedef struct ovrQuatd_ ovrQuatd;
nuclear@0	104 typedef struct ovrSizei_ ovrSizei;
nuclear@0	105 typedef struct ovrSizef_ ovrSizef;
nuclear@0	106 typedef struct ovrRecti_ ovrRecti;
nuclear@0	107 typedef struct ovrVector2i_ ovrVector2i;
nuclear@0	108 typedef struct ovrVector2f_ ovrVector2f;
nuclear@0	109 typedef struct ovrVector3f_ ovrVector3f;
nuclear@0	110 typedef struct ovrVector3d_ ovrVector3d;
nuclear@0	111 typedef struct ovrMatrix3d_ ovrMatrix3d;
nuclear@0	112 typedef struct ovrMatrix4f_ ovrMatrix4f;
nuclear@0	113 typedef struct ovrPosef_ ovrPosef;
nuclear@0	114 typedef struct ovrPosed_ ovrPosed;
nuclear@0	115 typedef struct ovrPoseStatef_ ovrPoseStatef;
nuclear@0	116 typedef struct ovrPoseStated_ ovrPoseStated;
nuclear@0	117
nuclear@0	118 namespace OVR {
nuclear@0	119
nuclear@0	120 // Forward-declare our templates.
nuclear@0	121 template<class T> class Quat;
nuclear@0	122 template<class T> class Size;
nuclear@0	123 template<class T> class Rect;
nuclear@0	124 template<class T> class Vector2;
nuclear@0	125 template<class T> class Vector3;
nuclear@0	126 template<class T> class Matrix3;
nuclear@0	127 template<class T> class Matrix4;
nuclear@0	128 template<class T> class Pose;
nuclear@0	129 template<class T> class PoseState;
nuclear@0	130
nuclear@0	131 // CompatibleTypes::Type is used to lookup a compatible C-version of a C++ class.
nuclear@0	132 template<class C>
nuclear@0	133 struct CompatibleTypes
nuclear@0	134 {
nuclear@0	135 // Declaration here seems necessary for MSVC; specializations are
nuclear@0	136 // used instead.
nuclear@0	137 typedef struct {} Type;
nuclear@0	138 };
nuclear@0	139
nuclear@0	140 // Specializations providing CompatibleTypes::Type value.
nuclear@0	141 template<> struct CompatibleTypes<Quat<float> > { typedef ovrQuatf Type; };
nuclear@0	142 template<> struct CompatibleTypes<Quat<double> > { typedef ovrQuatd Type; };
nuclear@0	143 template<> struct CompatibleTypes<Matrix3<double> > { typedef ovrMatrix3d Type; };
nuclear@0	144 template<> struct CompatibleTypes<Matrix4<float> > { typedef ovrMatrix4f Type; };
nuclear@0	145 template<> struct CompatibleTypes<Size<int> > { typedef ovrSizei Type; };
nuclear@0	146 template<> struct CompatibleTypes<Size<float> > { typedef ovrSizef Type; };
nuclear@0	147 template<> struct CompatibleTypes<Rect<int> > { typedef ovrRecti Type; };
nuclear@0	148 template<> struct CompatibleTypes<Vector2<int> > { typedef ovrVector2i Type; };
nuclear@0	149 template<> struct CompatibleTypes<Vector2<float> > { typedef ovrVector2f Type; };
nuclear@0	150 template<> struct CompatibleTypes<Vector3<float> > { typedef ovrVector3f Type; };
nuclear@0	151 template<> struct CompatibleTypes<Vector3<double> > { typedef ovrVector3d Type; };
nuclear@0	152
nuclear@0	153 template<> struct CompatibleTypes<Pose<float> > { typedef ovrPosef Type; };
nuclear@0	154 template<> struct CompatibleTypes<Pose<double> > { typedef ovrPosed Type; };
nuclear@0	155
nuclear@0	156 //------------------------------------------------------------------------------------//
nuclear@0	157 // ***** Math
nuclear@0	158 //
nuclear@0	159 // Math class contains constants and functions. This class is a template specialized
nuclear@0	160 // per type, with Math<float> and Math<double> being distinct.
nuclear@0	161 template<class Type>
nuclear@0	162 class Math
nuclear@0	163 {
nuclear@0	164 public:
nuclear@0	165 // By default, support explicit conversion to float. This allows Vector2<int> to
nuclear@0	166 // compile, for example.
nuclear@0	167 typedef float OtherFloatType;
nuclear@0	168 };
nuclear@0	169
nuclear@0	170
nuclear@0	171 #define MATH_FLOAT_PI (3.1415926f)
nuclear@0	172 #define MATH_FLOAT_TWOPI (2.0f *MATH_FLOAT_PI)
nuclear@0	173 #define MATH_FLOAT_PIOVER2 (0.5f *MATH_FLOAT_PI)
nuclear@0	174 #define MATH_FLOAT_PIOVER4 (0.25f*MATH_FLOAT_PI)
nuclear@0	175 #define MATH_FLOAT_E (2.7182818f)
nuclear@0	176 #define MATH_FLOAT_MAXVALUE (FLT_MAX)
nuclear@0	177 #define MATH_FLOAT MINPOSITIVEVALUE (FLT_MIN)
nuclear@0	178 #define MATH_FLOAT_RADTODEGREEFACTOR (360.0f / MATH_FLOAT_TWOPI)
nuclear@0	179 #define MATH_FLOAT_DEGREETORADFACTOR (MATH_FLOAT_TWOPI / 360.0f)
nuclear@0	180 #define MATH_FLOAT_TOLERANCE (0.00001f)
nuclear@0	181 #define MATH_FLOAT_SINGULARITYRADIUS (0.0000001f) // Use for Gimbal lock numerical problems
nuclear@0	182
nuclear@0	183 #define MATH_DOUBLE_PI (3.14159265358979)
nuclear@0	184 #define MATH_DOUBLE_TWOPI (2.0f *MATH_DOUBLE_PI)
nuclear@0	185 #define MATH_DOUBLE_PIOVER2 (0.5f *MATH_DOUBLE_PI)
nuclear@0	186 #define MATH_DOUBLE_PIOVER4 (0.25f*MATH_DOUBLE_PI)
nuclear@0	187 #define MATH_DOUBLE_E (2.71828182845905)
nuclear@0	188 #define MATH_DOUBLE_MAXVALUE (DBL_MAX)
nuclear@0	189 #define MATH_DOUBLE MINPOSITIVEVALUE (DBL_MIN)
nuclear@0	190 #define MATH_DOUBLE_RADTODEGREEFACTOR (360.0f / MATH_DOUBLE_TWOPI)
nuclear@0	191 #define MATH_DOUBLE_DEGREETORADFACTOR (MATH_DOUBLE_TWOPI / 360.0f)
nuclear@0	192 #define MATH_DOUBLE_TOLERANCE (0.00001)
nuclear@0	193 #define MATH_DOUBLE_SINGULARITYRADIUS (0.000000000001) // Use for Gimbal lock numerical problems
nuclear@0	194
nuclear@0	195
nuclear@0	196
nuclear@0	197
nuclear@0	198 // Single-precision Math constants class.
nuclear@0	199 template<>
nuclear@0	200 class Math<float>
nuclear@0	201 {
nuclear@0	202 public:
nuclear@0	203 typedef double OtherFloatType;
nuclear@0	204 };
nuclear@0	205
nuclear@0	206 // Double-precision Math constants class.
nuclear@0	207 template<>
nuclear@0	208 class Math<double>
nuclear@0	209 {
nuclear@0	210 public:
nuclear@0	211 typedef float OtherFloatType;
nuclear@0	212 };
nuclear@0	213
nuclear@0	214
nuclear@0	215 typedef Math<float> Mathf;
nuclear@0	216 typedef Math<double> Mathd;
nuclear@0	217
nuclear@0	218 // Conversion functions between degrees and radians
nuclear@0	219 template<class T>
nuclear@0	220 T RadToDegree(T rads) { return rads * ((T)MATH_DOUBLE_RADTODEGREEFACTOR); }
nuclear@0	221 template<class T>
nuclear@0	222 T DegreeToRad(T rads) { return rads * ((T)MATH_DOUBLE_DEGREETORADFACTOR); }
nuclear@0	223
nuclear@0	224 // Numerically stable acos function
nuclear@0	225 template<class T>
nuclear@0	226 T Acos(T val) {
nuclear@0	227 if (val > T(1)) return T(0);
nuclear@0	228 else if (val < T(-1)) return ((T)MATH_DOUBLE_PI);
nuclear@0	229 else return acos(val);
nuclear@0	230 };
nuclear@0	231
nuclear@0	232 // Numerically stable asin function
nuclear@0	233 template<class T>
nuclear@0	234 T Asin(T val) {
nuclear@0	235 if (val > T(1)) return ((T)MATH_DOUBLE_PIOVER2);
nuclear@0	236 else if (val < T(-1)) return ((T)MATH_DOUBLE_PIOVER2) * T(3);
nuclear@0	237 else return asin(val);
nuclear@0	238 };
nuclear@0	239
nuclear@0	240 #ifdef OVR_CC_MSVC
nuclear@0	241 inline int isnan(double x) { return _isnan(x); };
nuclear@0	242 #endif
nuclear@0	243
nuclear@0	244 template<class T>
nuclear@0	245 class Quat;
nuclear@0	246
nuclear@0	247
nuclear@0	248 //-------------------------------------------------------------------------------------
nuclear@0	249 // ***** Vector2<>
nuclear@0	250
nuclear@0	251 // Vector2f (Vector2d) represents a 2-dimensional vector or point in space,
nuclear@0	252 // consisting of coordinates x and y
nuclear@0	253
nuclear@0	254 template<class T>
nuclear@0	255 class Vector2
nuclear@0	256 {
nuclear@0	257 public:
nuclear@0	258 T x, y;
nuclear@0	259
nuclear@0	260 Vector2() : x(0), y(0) { }
nuclear@0	261 Vector2(T x_, T y_) : x(x_), y(y_) { }
nuclear@0	262 explicit Vector2(T s) : x(s), y(s) { }
nuclear@0	263 explicit Vector2(const Vector2<typename Math<T>::OtherFloatType> &src)
nuclear@0	264 : x((T)src.x), y((T)src.y) { }
nuclear@0	265
nuclear@0	266
nuclear@0	267 // C-interop support.
nuclear@0	268 typedef typename CompatibleTypes<Vector2<T> >::Type CompatibleType;
nuclear@0	269
nuclear@0	270 Vector2(const CompatibleType& s) : x(s.x), y(s.y) { }
nuclear@0	271
nuclear@0	272 operator const CompatibleType& () const
nuclear@0	273 {
nuclear@0	274 static_assert(sizeof(Vector2<T>) == sizeof(CompatibleType), "sizeof(Vector2<T>) failure");
nuclear@0	275 return reinterpret_cast<const CompatibleType&>(*this);
nuclear@0	276 }
nuclear@0	277
nuclear@0	278
nuclear@0	279 bool operator== (const Vector2& b) const { return x == b.x && y == b.y; }
nuclear@0	280 bool operator!= (const Vector2& b) const { return x != b.x \|\| y != b.y; }
nuclear@0	281
nuclear@0	282 Vector2 operator+ (const Vector2& b) const { return Vector2(x + b.x, y + b.y); }
nuclear@0	283 Vector2& operator+= (const Vector2& b) { x += b.x; y += b.y; return *this; }
nuclear@0	284 Vector2 operator- (const Vector2& b) const { return Vector2(x - b.x, y - b.y); }
nuclear@0	285 Vector2& operator-= (const Vector2& b) { x -= b.x; y -= b.y; return *this; }
nuclear@0	286 Vector2 operator- () const { return Vector2(-x, -y); }
nuclear@0	287
nuclear@0	288 // Scalar multiplication/division scales vector.
nuclear@0	289 Vector2 operator* (T s) const { return Vector2(xs, ys); }
nuclear@0	290 Vector2& operator= (T s) { x = s; y = s; return this; }
nuclear@0	291
nuclear@0	292 Vector2 operator/ (T s) const { T rcp = T(1)/s;
nuclear@0	293 return Vector2(xrcp, yrcp); }
nuclear@0	294 Vector2& operator/= (T s) { T rcp = T(1)/s;
nuclear@0	295 x = rcp; y = rcp;
nuclear@0	296 return *this; }
nuclear@0	297
nuclear@0	298 static Vector2 Min(const Vector2& a, const Vector2& b) { return Vector2((a.x < b.x) ? a.x : b.x,
nuclear@0	299 (a.y < b.y) ? a.y : b.y); }
nuclear@0	300 static Vector2 Max(const Vector2& a, const Vector2& b) { return Vector2((a.x > b.x) ? a.x : b.x,
nuclear@0	301 (a.y > b.y) ? a.y : b.y); }
nuclear@0	302
nuclear@0	303 // Compare two vectors for equality with tolerance. Returns true if vectors match withing tolerance.
nuclear@0	304 bool Compare(const Vector2&b, T tolerance = ((T)MATH_DOUBLE_TOLERANCE))
nuclear@0	305 {
nuclear@0	306 return (fabs(b.x-x) < tolerance) && (fabs(b.y-y) < tolerance);
nuclear@0	307 }
nuclear@0	308
nuclear@0	309 // Access element by index
nuclear@0	310 T& operator[] (int idx)
nuclear@0	311 {
nuclear@0	312 OVR_ASSERT(0 <= idx && idx < 2);
nuclear@0	313 return *(&x + idx);
nuclear@0	314 }
nuclear@0	315 const T& operator[] (int idx) const
nuclear@0	316 {
nuclear@0	317 OVR_ASSERT(0 <= idx && idx < 2);
nuclear@0	318 return *(&x + idx);
nuclear@0	319 }
nuclear@0	320
nuclear@0	321 // Entry-wise product of two vectors
nuclear@0	322 Vector2 EntrywiseMultiply(const Vector2& b) const { return Vector2(x * b.x, y * b.y);}
nuclear@0	323
nuclear@0	324
nuclear@0	325 // Multiply and divide operators do entry-wise math. Used Dot() for dot product.
nuclear@0	326 Vector2 operator* (const Vector2& b) const { return Vector2(x * b.x, y * b.y); }
nuclear@0	327 Vector2 operator/ (const Vector2& b) const { return Vector2(x / b.x, y / b.y); }
nuclear@0	328
nuclear@0	329 // Dot product
nuclear@0	330 // Used to calculate angle q between two vectors among other things,
nuclear@0	331 // as (A dot B) = \|a\|\|b\|cos(q).
nuclear@0	332 T Dot(const Vector2& b) const { return xb.x + yb.y; }
nuclear@0	333
nuclear@0	334 // Returns the angle from this vector to b, in radians.
nuclear@0	335 T Angle(const Vector2& b) const
nuclear@0	336 {
nuclear@0	337 T div = LengthSq()*b.LengthSq();
nuclear@0	338 OVR_ASSERT(div != T(0));
nuclear@0	339 T result = Acos((this->Dot(b))/sqrt(div));
nuclear@0	340 return result;
nuclear@0	341 }
nuclear@0	342
nuclear@0	343 // Return Length of the vector squared.
nuclear@0	344 T LengthSq() const { return (x * x + y * y); }
nuclear@0	345
nuclear@0	346 // Return vector length.
nuclear@0	347 T Length() const { return sqrt(LengthSq()); }
nuclear@0	348
nuclear@0	349 // Returns squared distance between two points represented by vectors.
nuclear@0	350 T DistanceSq(const Vector2& b) const { return (*this - b).LengthSq(); }
nuclear@0	351
nuclear@0	352 // Returns distance between two points represented by vectors.
nuclear@0	353 T Distance(const Vector2& b) const { return (*this - b).Length(); }
nuclear@0	354
nuclear@0	355 // Determine if this a unit vector.
nuclear@0	356 bool IsNormalized() const { return fabs(LengthSq() - T(1)) < ((T)MATH_DOUBLE_TOLERANCE); }
nuclear@0	357
nuclear@0	358 // Normalize, convention vector length to 1.
nuclear@0	359 void Normalize()
nuclear@0	360 {
nuclear@0	361 T l = Length();
nuclear@0	362 OVR_ASSERT(l != T(0));
nuclear@0	363 *this /= l;
nuclear@0	364 }
nuclear@0	365 // Returns normalized (unit) version of the vector without modifying itself.
nuclear@0	366 Vector2 Normalized() const
nuclear@0	367 {
nuclear@0	368 T l = Length();
nuclear@0	369 OVR_ASSERT(l != T(0));
nuclear@0	370 return *this / l;
nuclear@0	371 }
nuclear@0	372
nuclear@0	373 // Linearly interpolates from this vector to another.
nuclear@0	374 // Factor should be between 0.0 and 1.0, with 0 giving full value to this.
nuclear@0	375 Vector2 Lerp(const Vector2& b, T f) const { return this(T(1) - f) + b*f; }
nuclear@0	376
nuclear@0	377 // Projects this vector onto the argument; in other words,
nuclear@0	378 // A.Project(B) returns projection of vector A onto B.
nuclear@0	379 Vector2 ProjectTo(const Vector2& b) const
nuclear@0	380 {
nuclear@0	381 T l2 = b.LengthSq();
nuclear@0	382 OVR_ASSERT(l2 != T(0));
nuclear@0	383 return b * ( Dot(b) / l2 );
nuclear@0	384 }
nuclear@0	385 };
nuclear@0	386
nuclear@0	387
nuclear@0	388 typedef Vector2<float> Vector2f;
nuclear@0	389 typedef Vector2<double> Vector2d;
nuclear@0	390 typedef Vector2<int> Vector2i;
nuclear@0	391
nuclear@0	392 typedef Vector2<float> Point2f;
nuclear@0	393 typedef Vector2<double> Point2d;
nuclear@0	394 typedef Vector2<int> Point2i;
nuclear@0	395
nuclear@0	396 //-------------------------------------------------------------------------------------
nuclear@0	397 // ***** Vector3<> - 3D vector of {x, y, z}
nuclear@0	398
nuclear@0	399 //
nuclear@0	400 // Vector3f (Vector3d) represents a 3-dimensional vector or point in space,
nuclear@0	401 // consisting of coordinates x, y and z.
nuclear@0	402
nuclear@0	403 template<class T>
nuclear@0	404 class Vector3
nuclear@0	405 {
nuclear@0	406 public:
nuclear@0	407 T x, y, z;
nuclear@0	408
nuclear@0	409 // FIXME: default initialization of a vector class can be very expensive in a full-blown
nuclear@0	410 // application. A few hundred thousand vector constructions is not unlikely and can add
nuclear@0	411 // up to milliseconds of time on processors like the PS3 PPU.
nuclear@0	412 Vector3() : x(0), y(0), z(0) { }
nuclear@0	413 Vector3(T x_, T y_, T z_ = 0) : x(x_), y(y_), z(z_) { }
nuclear@0	414 explicit Vector3(T s) : x(s), y(s), z(s) { }
nuclear@0	415 explicit Vector3(const Vector3<typename Math<T>::OtherFloatType> &src)
nuclear@0	416 : x((T)src.x), y((T)src.y), z((T)src.z) { }
nuclear@0	417
nuclear@0	418 static const Vector3 ZERO;
nuclear@0	419
nuclear@0	420 // C-interop support.
nuclear@0	421 typedef typename CompatibleTypes<Vector3<T> >::Type CompatibleType;
nuclear@0	422
nuclear@0	423 Vector3(const CompatibleType& s) : x(s.x), y(s.y), z(s.z) { }
nuclear@0	424
nuclear@0	425 operator const CompatibleType& () const
nuclear@0	426 {
nuclear@0	427 static_assert(sizeof(Vector3<T>) == sizeof(CompatibleType), "sizeof(Vector3<T>) failure");
nuclear@0	428 return reinterpret_cast<const CompatibleType&>(*this);
nuclear@0	429 }
nuclear@0	430
nuclear@0	431 bool operator== (const Vector3& b) const { return x == b.x && y == b.y && z == b.z; }
nuclear@0	432 bool operator!= (const Vector3& b) const { return x != b.x \|\| y != b.y \|\| z != b.z; }
nuclear@0	433
nuclear@0	434 Vector3 operator+ (const Vector3& b) const { return Vector3(x + b.x, y + b.y, z + b.z); }
nuclear@0	435 Vector3& operator+= (const Vector3& b) { x += b.x; y += b.y; z += b.z; return *this; }
nuclear@0	436 Vector3 operator- (const Vector3& b) const { return Vector3(x - b.x, y - b.y, z - b.z); }
nuclear@0	437 Vector3& operator-= (const Vector3& b) { x -= b.x; y -= b.y; z -= b.z; return *this; }
nuclear@0	438 Vector3 operator- () const { return Vector3(-x, -y, -z); }
nuclear@0	439
nuclear@0	440 // Scalar multiplication/division scales vector.
nuclear@0	441 Vector3 operator* (T s) const { return Vector3(xs, ys, z*s); }
nuclear@0	442 Vector3& operator= (T s) { x = s; y = s; z = s; return *this; }
nuclear@0	443
nuclear@0	444 Vector3 operator/ (T s) const { T rcp = T(1)/s;
nuclear@0	445 return Vector3(xrcp, yrcp, z*rcp); }
nuclear@0	446 Vector3& operator/= (T s) { T rcp = T(1)/s;
nuclear@0	447 x = rcp; y = rcp; z *= rcp;
nuclear@0	448 return *this; }
nuclear@0	449
nuclear@0	450 static Vector3 Min(const Vector3& a, const Vector3& b)
nuclear@0	451 {
nuclear@0	452 return Vector3((a.x < b.x) ? a.x : b.x,
nuclear@0	453 (a.y < b.y) ? a.y : b.y,
nuclear@0	454 (a.z < b.z) ? a.z : b.z);
nuclear@0	455 }
nuclear@0	456 static Vector3 Max(const Vector3& a, const Vector3& b)
nuclear@0	457 {
nuclear@0	458 return Vector3((a.x > b.x) ? a.x : b.x,
nuclear@0	459 (a.y > b.y) ? a.y : b.y,
nuclear@0	460 (a.z > b.z) ? a.z : b.z);
nuclear@0	461 }
nuclear@0	462
nuclear@0	463 // Compare two vectors for equality with tolerance. Returns true if vectors match withing tolerance.
nuclear@0	464 bool Compare(const Vector3&b, T tolerance = ((T)MATH_DOUBLE_TOLERANCE))
nuclear@0	465 {
nuclear@0	466 return (fabs(b.x-x) < tolerance) &&
nuclear@0	467 (fabs(b.y-y) < tolerance) &&
nuclear@0	468 (fabs(b.z-z) < tolerance);
nuclear@0	469 }
nuclear@0	470
nuclear@0	471 T& operator[] (int idx)
nuclear@0	472 {
nuclear@0	473 OVR_ASSERT(0 <= idx && idx < 3);
nuclear@0	474 return *(&x + idx);
nuclear@0	475 }
nuclear@0	476
nuclear@0	477 const T& operator[] (int idx) const
nuclear@0	478 {
nuclear@0	479 OVR_ASSERT(0 <= idx && idx < 3);
nuclear@0	480 return *(&x + idx);
nuclear@0	481 }
nuclear@0	482
nuclear@0	483 // Entrywise product of two vectors
nuclear@0	484 Vector3 EntrywiseMultiply(const Vector3& b) const { return Vector3(x * b.x,
nuclear@0	485 y * b.y,
nuclear@0	486 z * b.z);}
nuclear@0	487
nuclear@0	488 // Multiply and divide operators do entry-wise math
nuclear@0	489 Vector3 operator* (const Vector3& b) const { return Vector3(x * b.x,
nuclear@0	490 y * b.y,
nuclear@0	491 z * b.z); }
nuclear@0	492
nuclear@0	493 Vector3 operator/ (const Vector3& b) const { return Vector3(x / b.x,
nuclear@0	494 y / b.y,
nuclear@0	495 z / b.z); }
nuclear@0	496
nuclear@0	497
nuclear@0	498 // Dot product
nuclear@0	499 // Used to calculate angle q between two vectors among other things,
nuclear@0	500 // as (A dot B) = \|a\|\|b\|cos(q).
nuclear@0	501 T Dot(const Vector3& b) const { return xb.x + yb.y + z*b.z; }
nuclear@0	502
nuclear@0	503 // Compute cross product, which generates a normal vector.
nuclear@0	504 // Direction vector can be determined by right-hand rule: Pointing index finder in
nuclear@0	505 // direction a and middle finger in direction b, thumb will point in a.Cross(b).
nuclear@0	506 Vector3 Cross(const Vector3& b) const { return Vector3(yb.z - zb.y,
nuclear@0	507 zb.x - xb.z,
nuclear@0	508 xb.y - yb.x); }
nuclear@0	509
nuclear@0	510 // Returns the angle from this vector to b, in radians.
nuclear@0	511 T Angle(const Vector3& b) const
nuclear@0	512 {
nuclear@0	513 T div = LengthSq()*b.LengthSq();
nuclear@0	514 OVR_ASSERT(div != T(0));
nuclear@0	515 T result = Acos((this->Dot(b))/sqrt(div));
nuclear@0	516 return result;
nuclear@0	517 }
nuclear@0	518
nuclear@0	519 // Return Length of the vector squared.
nuclear@0	520 T LengthSq() const { return (x * x + y * y + z * z); }
nuclear@0	521
nuclear@0	522 // Return vector length.
nuclear@0	523 T Length() const { return sqrt(LengthSq()); }
nuclear@0	524
nuclear@0	525 // Returns squared distance between two points represented by vectors.
nuclear@0	526 T DistanceSq(Vector3 const& b) const { return (*this - b).LengthSq(); }
nuclear@0	527
nuclear@0	528 // Returns distance between two points represented by vectors.
nuclear@0	529 T Distance(Vector3 const& b) const { return (*this - b).Length(); }
nuclear@0	530
nuclear@0	531 // Determine if this a unit vector.
nuclear@0	532 bool IsNormalized() const { return fabs(LengthSq() - T(1)) < ((T)MATH_DOUBLE_TOLERANCE); }
nuclear@0	533
nuclear@0	534 // Normalize, convention vector length to 1.
nuclear@0	535 void Normalize()
nuclear@0	536 {
nuclear@0	537 T l = Length();
nuclear@0	538 OVR_ASSERT(l != T(0));
nuclear@0	539 *this /= l;
nuclear@0	540 }
nuclear@0	541
nuclear@0	542 // Returns normalized (unit) version of the vector without modifying itself.
nuclear@0	543 Vector3 Normalized() const
nuclear@0	544 {
nuclear@0	545 T l = Length();
nuclear@0	546 OVR_ASSERT(l != T(0));
nuclear@0	547 return *this / l;
nuclear@0	548 }
nuclear@0	549
nuclear@0	550 // Linearly interpolates from this vector to another.
nuclear@0	551 // Factor should be between 0.0 and 1.0, with 0 giving full value to this.
nuclear@0	552 Vector3 Lerp(const Vector3& b, T f) const { return this(T(1) - f) + b*f; }
nuclear@0	553
nuclear@0	554 // Projects this vector onto the argument; in other words,
nuclear@0	555 // A.Project(B) returns projection of vector A onto B.
nuclear@0	556 Vector3 ProjectTo(const Vector3& b) const
nuclear@0	557 {
nuclear@0	558 T l2 = b.LengthSq();
nuclear@0	559 OVR_ASSERT(l2 != T(0));
nuclear@0	560 return b * ( Dot(b) / l2 );
nuclear@0	561 }
nuclear@0	562
nuclear@0	563 // Projects this vector onto a plane defined by a normal vector
nuclear@0	564 Vector3 ProjectToPlane(const Vector3& normal) const { return *this - this->ProjectTo(normal); }
nuclear@0	565 };
nuclear@0	566
nuclear@0	567 typedef Vector3<float> Vector3f;
nuclear@0	568 typedef Vector3<double> Vector3d;
nuclear@0	569 typedef Vector3<int32_t> Vector3i;
nuclear@0	570
nuclear@0	571 static_assert((sizeof(Vector3f) == 3*sizeof(float)), "sizeof(Vector3f) failure");
nuclear@0	572 static_assert((sizeof(Vector3d) == 3*sizeof(double)), "sizeof(Vector3d) failure");
nuclear@0	573 static_assert((sizeof(Vector3i) == 3*sizeof(int32_t)), "sizeof(Vector3i) failure");
nuclear@0	574
nuclear@0	575 typedef Vector3<float> Point3f;
nuclear@0	576 typedef Vector3<double> Point3d;
nuclear@0	577 typedef Vector3<int32_t> Point3i;
nuclear@0	578
nuclear@0	579
nuclear@0	580 //-------------------------------------------------------------------------------------
nuclear@0	581 // ***** Vector4<> - 4D vector of {x, y, z, w}
nuclear@0	582
nuclear@0	583 //
nuclear@0	584 // Vector4f (Vector4d) represents a 3-dimensional vector or point in space,
nuclear@0	585 // consisting of coordinates x, y, z and w.
nuclear@0	586
nuclear@0	587 template<class T>
nuclear@0	588 class Vector4
nuclear@0	589 {
nuclear@0	590 public:
nuclear@0	591 T x, y, z, w;
nuclear@0	592
nuclear@0	593 // FIXME: default initialization of a vector class can be very expensive in a full-blown
nuclear@0	594 // application. A few hundred thousand vector constructions is not unlikely and can add
nuclear@0	595 // up to milliseconds of time on processors like the PS3 PPU.
nuclear@0	596 Vector4() : x(0), y(0), z(0), w(0) { }
nuclear@0	597 Vector4(T x_, T y_, T z_, T w_) : x(x_), y(y_), z(z_), w(w_) { }
nuclear@0	598 explicit Vector4(T s) : x(s), y(s), z(s), w(s) { }
nuclear@0	599 explicit Vector4(const Vector3<T>& v, const float w_=1) : x(v.x), y(v.y), z(v.z), w(w_) { }
nuclear@0	600 explicit Vector4(const Vector4<typename Math<T>::OtherFloatType> &src)
nuclear@0	601 : x((T)src.x), y((T)src.y), z((T)src.z), w((T)src.w) { }
nuclear@0	602
nuclear@0	603 static const Vector4 ZERO;
nuclear@0	604
nuclear@0	605 // C-interop support.
nuclear@0	606 typedef typename CompatibleTypes< Vector4<T> >::Type CompatibleType;
nuclear@0	607
nuclear@0	608 Vector4(const CompatibleType& s) : x(s.x), y(s.y), z(s.z), w(s.w) { }
nuclear@0	609
nuclear@0	610 operator const CompatibleType& () const
nuclear@0	611 {
nuclear@0	612 static_assert(sizeof(Vector4<T>) == sizeof(CompatibleType), "sizeof(Vector4<T>) failure");
nuclear@0	613 return reinterpret_cast<const CompatibleType&>(*this);
nuclear@0	614 }
nuclear@0	615
nuclear@0	616 Vector4& operator= (const Vector3<T>& other) { x=other.x; y=other.y; z=other.z; w=1; return *this; }
nuclear@0	617 bool operator== (const Vector4& b) const { return x == b.x && y == b.y && z == b.z && w == b.w; }
nuclear@0	618 bool operator!= (const Vector4& b) const { return x != b.x \|\| y != b.y \|\| z != b.z \|\| w != b.w; }
nuclear@0	619
nuclear@0	620 Vector4 operator+ (const Vector4& b) const { return Vector4(x + b.x, y + b.y, z + b.z, w + b.w); }
nuclear@0	621 Vector4& operator+= (const Vector4& b) { x += b.x; y += b.y; z += b.z; w += b.w; return *this; }
nuclear@0	622 Vector4 operator- (const Vector4& b) const { return Vector4(x - b.x, y - b.y, z - b.z, w - b.w); }
nuclear@0	623 Vector4& operator-= (const Vector4& b) { x -= b.x; y -= b.y; z -= b.z; w -= b.w; return *this; }
nuclear@0	624 Vector4 operator- () const { return Vector4(-x, -y, -z, -w); }
nuclear@0	625
nuclear@0	626 // Scalar multiplication/division scales vector.
nuclear@0	627 Vector4 operator* (T s) const { return Vector4(xs, ys, zs, ws); }
nuclear@0	628 Vector4& operator= (T s) { x = s; y = s; z = s; w = s;return this; }
nuclear@0	629
nuclear@0	630 Vector4 operator/ (T s) const { T rcp = T(1)/s;
nuclear@0	631 return Vector4(xrcp, yrcp, zrcp, wrcp); }
nuclear@0	632 Vector4& operator/= (T s) { T rcp = T(1)/s;
nuclear@0	633 x = rcp; y = rcp; z = rcp; w = rcp;
nuclear@0	634 return *this; }
nuclear@0	635
nuclear@0	636 static Vector4 Min(const Vector4& a, const Vector4& b)
nuclear@0	637 {
nuclear@0	638 return Vector4((a.x < b.x) ? a.x : b.x,
nuclear@0	639 (a.y < b.y) ? a.y : b.y,
nuclear@0	640 (a.z < b.z) ? a.z : b.z,
nuclear@0	641 (a.w < b.w) ? a.w : b.w);
nuclear@0	642 }
nuclear@0	643 static Vector4 Max(const Vector4& a, const Vector4& b)
nuclear@0	644 {
nuclear@0	645 return Vector4((a.x > b.x) ? a.x : b.x,
nuclear@0	646 (a.y > b.y) ? a.y : b.y,
nuclear@0	647 (a.z > b.z) ? a.z : b.z,
nuclear@0	648 (a.w > b.w) ? a.w : b.w);
nuclear@0	649 }
nuclear@0	650
nuclear@0	651 // Compare two vectors for equality with tolerance. Returns true if vectors match withing tolerance.
nuclear@0	652 bool Compare(const Vector4&b, T tolerance = ((T)MATH_DOUBLE_TOLERANCE))
nuclear@0	653 {
nuclear@0	654 return (fabs(b.x-x) < tolerance) &&
nuclear@0	655 (fabs(b.y-y) < tolerance) &&
nuclear@0	656 (fabs(b.z-z) < tolerance) &&
nuclear@0	657 (fabs(b.w-w) < tolerance);
nuclear@0	658 }
nuclear@0	659
nuclear@0	660 T& operator[] (int idx)
nuclear@0	661 {
nuclear@0	662 OVR_ASSERT(0 <= idx && idx < 4);
nuclear@0	663 return *(&x + idx);
nuclear@0	664 }
nuclear@0	665
nuclear@0	666 const T& operator[] (int idx) const
nuclear@0	667 {
nuclear@0	668 OVR_ASSERT(0 <= idx && idx < 4);
nuclear@0	669 return *(&x + idx);
nuclear@0	670 }
nuclear@0	671
nuclear@0	672 // Entry wise product of two vectors
nuclear@0	673 Vector4 EntrywiseMultiply(const Vector4& b) const { return Vector4(x * b.x,
nuclear@0	674 y * b.y,
nuclear@0	675 z * b.z);}
nuclear@0	676
nuclear@0	677 // Multiply and divide operators do entry-wise math
nuclear@0	678 Vector4 operator* (const Vector4& b) const { return Vector4(x * b.x,
nuclear@0	679 y * b.y,
nuclear@0	680 z * b.z,
nuclear@0	681 w * b.w); }
nuclear@0	682
nuclear@0	683 Vector4 operator/ (const Vector4& b) const { return Vector4(x / b.x,
nuclear@0	684 y / b.y,
nuclear@0	685 z / b.z,
nuclear@0	686 w / b.w); }
nuclear@0	687
nuclear@0	688
nuclear@0	689 // Dot product
nuclear@0	690 T Dot(const Vector4& b) const { return xb.x + yb.y + zb.z + wb.w; }
nuclear@0	691
nuclear@0	692 // Return Length of the vector squared.
nuclear@0	693 T LengthSq() const { return (x * x + y * y + z * z + w * w); }
nuclear@0	694
nuclear@0	695 // Return vector length.
nuclear@0	696 T Length() const { return sqrt(LengthSq()); }
nuclear@0	697
nuclear@0	698 // Determine if this a unit vector.
nuclear@0	699 bool IsNormalized() const { return fabs(LengthSq() - T(1)) < Math<T>::Tolerance; }
nuclear@0	700
nuclear@0	701 // Normalize, convention vector length to 1.
nuclear@0	702 void Normalize()
nuclear@0	703 {
nuclear@0	704 T l = Length();
nuclear@0	705 OVR_ASSERT(l != T(0));
nuclear@0	706 *this /= l;
nuclear@0	707 }
nuclear@0	708
nuclear@0	709 // Returns normalized (unit) version of the vector without modifying itself.
nuclear@0	710 Vector4 Normalized() const
nuclear@0	711 {
nuclear@0	712 T l = Length();
nuclear@0	713 OVR_ASSERT(l != T(0));
nuclear@0	714 return *this / l;
nuclear@0	715 }
nuclear@0	716 };
nuclear@0	717
nuclear@0	718 typedef Vector4<float> Vector4f;
nuclear@0	719 typedef Vector4<double> Vector4d;
nuclear@0	720 typedef Vector4<int> Vector4i;
nuclear@0	721
nuclear@0	722
nuclear@0	723 //-------------------------------------------------------------------------------------
nuclear@0	724 // ***** Bounds3
nuclear@0	725
nuclear@0	726 // Bounds class used to describe a 3D axis aligned bounding box.
nuclear@0	727
nuclear@0	728 template<class T>
nuclear@0	729 class Bounds3
nuclear@0	730 {
nuclear@0	731 public:
nuclear@0	732 Vector3<T> b[2];
nuclear@0	733
nuclear@0	734 Bounds3()
nuclear@0	735 {
nuclear@0	736 }
nuclear@0	737
nuclear@0	738 Bounds3( const Vector3<T> & mins, const Vector3<T> & maxs )
nuclear@0	739 {
nuclear@0	740 b[0] = mins;
nuclear@0	741 b[1] = maxs;
nuclear@0	742 }
nuclear@0	743
nuclear@0	744 void Clear()
nuclear@0	745 {
nuclear@0	746 b[0].x = b[0].y = b[0].z = Math<T>::MaxValue;
nuclear@0	747 b[1].x = b[1].y = b[1].z = -Math<T>::MaxValue;
nuclear@0	748 }
nuclear@0	749
nuclear@0	750 void AddPoint( const Vector3<T> & v )
nuclear@0	751 {
nuclear@0	752 b[0].x = Alg::Min( b[0].x, v.x );
nuclear@0	753 b[0].y = Alg::Min( b[0].y, v.y );
nuclear@0	754 b[0].z = Alg::Min( b[0].z, v.z );
nuclear@0	755 b[1].x = Alg::Max( b[1].x, v.x );
nuclear@0	756 b[1].y = Alg::Max( b[1].y, v.y );
nuclear@0	757 b[1].z = Alg::Max( b[1].z, v.z );
nuclear@0	758 }
nuclear@0	759
nuclear@0	760 const Vector3<T> & GetMins() const { return b[0]; }
nuclear@0	761 const Vector3<T> & GetMaxs() const { return b[1]; }
nuclear@0	762
nuclear@0	763 Vector3<T> & GetMins() { return b[0]; }
nuclear@0	764 Vector3<T> & GetMaxs() { return b[1]; }
nuclear@0	765 };
nuclear@0	766
nuclear@0	767 typedef Bounds3<float> Bounds3f;
nuclear@0	768 typedef Bounds3<double> Bounds3d;
nuclear@0	769
nuclear@0	770
nuclear@0	771 //-------------------------------------------------------------------------------------
nuclear@0	772 // ***** Size
nuclear@0	773
nuclear@0	774 // Size class represents 2D size with Width, Height components.
nuclear@0	775 // Used to describe distentions of render targets, etc.
nuclear@0	776
nuclear@0	777 template<class T>
nuclear@0	778 class Size
nuclear@0	779 {
nuclear@0	780 public:
nuclear@0	781 T w, h;
nuclear@0	782
nuclear@0	783 Size() : w(0), h(0) { }
nuclear@0	784 Size(T w_, T h_) : w(w_), h(h_) { }
nuclear@0	785 explicit Size(T s) : w(s), h(s) { }
nuclear@0	786 explicit Size(const Size<typename Math<T>::OtherFloatType> &src)
nuclear@0	787 : w((T)src.w), h((T)src.h) { }
nuclear@0	788
nuclear@0	789 // C-interop support.
nuclear@0	790 typedef typename CompatibleTypes<Size<T> >::Type CompatibleType;
nuclear@0	791
nuclear@0	792 Size(const CompatibleType& s) : w(s.w), h(s.h) { }
nuclear@0	793
nuclear@0	794 operator const CompatibleType& () const
nuclear@0	795 {
nuclear@0	796 static_assert(sizeof(Size<T>) == sizeof(CompatibleType), "sizeof(Size<T>) failure");
nuclear@0	797 return reinterpret_cast<const CompatibleType&>(*this);
nuclear@0	798 }
nuclear@0	799
nuclear@0	800 bool operator== (const Size& b) const { return w == b.w && h == b.h; }
nuclear@0	801 bool operator!= (const Size& b) const { return w != b.w \|\| h != b.h; }
nuclear@0	802
nuclear@0	803 Size operator+ (const Size& b) const { return Size(w + b.w, h + b.h); }
nuclear@0	804 Size& operator+= (const Size& b) { w += b.w; h += b.h; return *this; }
nuclear@0	805 Size operator- (const Size& b) const { return Size(w - b.w, h - b.h); }
nuclear@0	806 Size& operator-= (const Size& b) { w -= b.w; h -= b.h; return *this; }
nuclear@0	807 Size operator- () const { return Size(-w, -h); }
nuclear@0	808 Size operator* (const Size& b) const { return Size(w * b.w, h * b.h); }
nuclear@0	809 Size& operator= (const Size& b) { w = b.w; h = b.h; return this; }
nuclear@0	810 Size operator/ (const Size& b) const { return Size(w / b.w, h / b.h); }
nuclear@0	811 Size& operator/= (const Size& b) { w /= b.w; h /= b.h; return *this; }
nuclear@0	812
nuclear@0	813 // Scalar multiplication/division scales both components.
nuclear@0	814 Size operator* (T s) const { return Size(ws, hs); }
nuclear@0	815 Size& operator= (T s) { w = s; h = s; return this; }
nuclear@0	816 Size operator/ (T s) const { return Size(w/s, h/s); }
nuclear@0	817 Size& operator/= (T s) { w /= s; h /= s; return *this; }
nuclear@0	818
nuclear@0	819 static Size Min(const Size& a, const Size& b) { return Size((a.w < b.w) ? a.w : b.w,
nuclear@0	820 (a.h < b.h) ? a.h : b.h); }
nuclear@0	821 static Size Max(const Size& a, const Size& b) { return Size((a.w > b.w) ? a.w : b.w,
nuclear@0	822 (a.h > b.h) ? a.h : b.h); }
nuclear@0	823
nuclear@0	824 T Area() const { return w * h; }
nuclear@0	825
nuclear@0	826 inline Vector2<T> ToVector() const { return Vector2<T>(w, h); }
nuclear@0	827 };
nuclear@0	828
nuclear@0	829
nuclear@0	830 typedef Size<int> Sizei;
nuclear@0	831 typedef Size<unsigned> Sizeu;
nuclear@0	832 typedef Size<float> Sizef;
nuclear@0	833 typedef Size<double> Sized;
nuclear@0	834
nuclear@0	835
nuclear@0	836
nuclear@0	837 //-----------------------------------------------------------------------------------
nuclear@0	838 // ***** Rect
nuclear@0	839
nuclear@0	840 // Rect describes a rectangular area for rendering, that includes position and size.
nuclear@0	841 template<class T>
nuclear@0	842 class Rect
nuclear@0	843 {
nuclear@0	844 public:
nuclear@0	845 T x, y;
nuclear@0	846 T w, h;
nuclear@0	847
nuclear@0	848 Rect() { }
nuclear@0	849 Rect(T x1, T y1, T w1, T h1) : x(x1), y(y1), w(w1), h(h1) { }
nuclear@0	850 Rect(const Vector2<T>& pos, const Size<T>& sz) : x(pos.x), y(pos.y), w(sz.w), h(sz.h) { }
nuclear@0	851 Rect(const Size<T>& sz) : x(0), y(0), w(sz.w), h(sz.h) { }
nuclear@0	852
nuclear@0	853 // C-interop support.
nuclear@0	854 typedef typename CompatibleTypes<Rect<T> >::Type CompatibleType;
nuclear@0	855
nuclear@0	856 Rect(const CompatibleType& s) : x(s.Pos.x), y(s.Pos.y), w(s.Size.w), h(s.Size.h) { }
nuclear@0	857
nuclear@0	858 operator const CompatibleType& () const
nuclear@0	859 {
nuclear@0	860 static_assert(sizeof(Rect<T>) == sizeof(CompatibleType), "sizeof(Rect<T>) failure");
nuclear@0	861 return reinterpret_cast<const CompatibleType&>(*this);
nuclear@0	862 }
nuclear@0	863
nuclear@0	864 Vector2<T> GetPos() const { return Vector2<T>(x, y); }
nuclear@0	865 Size<T> GetSize() const { return Size<T>(w, h); }
nuclear@0	866 void SetPos(const Vector2<T>& pos) { x = pos.x; y = pos.y; }
nuclear@0	867 void SetSize(const Size<T>& sz) { w = sz.w; h = sz.h; }
nuclear@0	868
nuclear@0	869 bool operator == (const Rect& vp) const
nuclear@0	870 { return (x == vp.x) && (y == vp.y) && (w == vp.w) && (h == vp.h); }
nuclear@0	871 bool operator != (const Rect& vp) const
nuclear@0	872 { return !operator == (vp); }
nuclear@0	873 };
nuclear@0	874
nuclear@0	875 typedef Rect<int> Recti;
nuclear@0	876
nuclear@0	877
nuclear@0	878 //-------------------------------------------------------------------------------------//
nuclear@0	879 // ***** Quat
nuclear@0	880 //
nuclear@0	881 // Quatf represents a quaternion class used for rotations.
nuclear@0	882 //
nuclear@0	883 // Quaternion multiplications are done in right-to-left order, to match the
nuclear@0	884 // behavior of matrices.
nuclear@0	885
nuclear@0	886
nuclear@0	887 template<class T>
nuclear@0	888 class Quat
nuclear@0	889 {
nuclear@0	890 public:
nuclear@0	891 // w + Xi + Yj + Zk
nuclear@0	892 T x, y, z, w;
nuclear@0	893
nuclear@0	894 Quat() : x(0), y(0), z(0), w(1) { }
nuclear@0	895 Quat(T x_, T y_, T z_, T w_) : x(x_), y(y_), z(z_), w(w_) { }
nuclear@0	896 explicit Quat(const Quat<typename Math<T>::OtherFloatType> &src)
nuclear@0	897 : x((T)src.x), y((T)src.y), z((T)src.z), w((T)src.w) { }
nuclear@0	898
nuclear@0	899 typedef typename CompatibleTypes<Quat<T> >::Type CompatibleType;
nuclear@0	900
nuclear@0	901 // C-interop support.
nuclear@0	902 Quat(const CompatibleType& s) : x(s.x), y(s.y), z(s.z), w(s.w) { }
nuclear@0	903
nuclear@0	904 operator CompatibleType () const
nuclear@0	905 {
nuclear@0	906 CompatibleType result;
nuclear@0	907 result.x = x;
nuclear@0	908 result.y = y;
nuclear@0	909 result.z = z;
nuclear@0	910 result.w = w;
nuclear@0	911 return result;
nuclear@0	912 }
nuclear@0	913
nuclear@0	914 // Constructs quaternion for rotation around the axis by an angle.
nuclear@0	915 Quat(const Vector3<T>& axis, T angle)
nuclear@0	916 {
nuclear@0	917 // Make sure we don't divide by zero.
nuclear@0	918 if (axis.LengthSq() == 0)
nuclear@0	919 {
nuclear@0	920 // Assert if the axis is zero, but the angle isn't
nuclear@0	921 OVR_ASSERT(angle == 0);
nuclear@0	922 x = 0; y = 0; z = 0; w = 1;
nuclear@0	923 return;
nuclear@0	924 }
nuclear@0	925
nuclear@0	926 Vector3<T> unitAxis = axis.Normalized();
nuclear@0	927 T sinHalfAngle = sin(angle * T(0.5));
nuclear@0	928
nuclear@0	929 w = cos(angle * T(0.5));
nuclear@0	930 x = unitAxis.x * sinHalfAngle;
nuclear@0	931 y = unitAxis.y * sinHalfAngle;
nuclear@0	932 z = unitAxis.z * sinHalfAngle;
nuclear@0	933 }
nuclear@0	934
nuclear@0	935 // Constructs quaternion for rotation around one of the coordinate axis by an angle.
nuclear@0	936 Quat(Axis A, T angle, RotateDirection d = Rotate_CCW, HandedSystem s = Handed_R)
nuclear@0	937 {
nuclear@0	938 T sinHalfAngle = s * d sin(angle T(0.5));
nuclear@0	939 T v[3];
nuclear@0	940 v[0] = v[1] = v[2] = T(0);
nuclear@0	941 v[A] = sinHalfAngle;
nuclear@0	942
nuclear@0	943 w = cos(angle * T(0.5));
nuclear@0	944 x = v[0];
nuclear@0	945 y = v[1];
nuclear@0	946 z = v[2];
nuclear@0	947 }
nuclear@0	948
nuclear@0	949 // Compute axis and angle from quaternion
nuclear@0	950 void GetAxisAngle(Vector3<T>* axis, T* angle) const
nuclear@0	951 {
nuclear@0	952 if ( xx + yy + zz > ((T)MATH_DOUBLE_TOLERANCE) ((T)MATH_DOUBLE_TOLERANCE) ) {
nuclear@0	953 *axis = Vector3<T>(x, y, z).Normalized();
nuclear@0	954 angle = 2 Acos(w);
nuclear@0	955 if (*angle > ((T)MATH_DOUBLE_PI)) // Reduce the magnitude of the angle, if necessary
nuclear@0	956 {
nuclear@0	957 angle = ((T)MATH_DOUBLE_TWOPI) - angle;
nuclear@0	958 axis = axis * (-1);
nuclear@0	959 }
nuclear@0	960 }
nuclear@0	961 else
nuclear@0	962 {
nuclear@0	963 *axis = Vector3<T>(1, 0, 0);
nuclear@0	964 *angle= 0;
nuclear@0	965 }
nuclear@0	966 }
nuclear@0	967
nuclear@0	968 // Constructs the quaternion from a rotation matrix
nuclear@0	969 explicit Quat(const Matrix4<T>& m)
nuclear@0	970 {
nuclear@0	971 T trace = m.M[0][0] + m.M[1][1] + m.M[2][2];
nuclear@0	972
nuclear@0	973 // In almost all cases, the first part is executed.
nuclear@0	974 // However, if the trace is not positive, the other
nuclear@0	975 // cases arise.
nuclear@0	976 if (trace > T(0))
nuclear@0	977 {
nuclear@0	978 T s = sqrt(trace + T(1)) * T(2); // s=4*qw
nuclear@0	979 w = T(0.25) * s;
nuclear@0	980 x = (m.M[2][1] - m.M[1][2]) / s;
nuclear@0	981 y = (m.M[0][2] - m.M[2][0]) / s;
nuclear@0	982 z = (m.M[1][0] - m.M[0][1]) / s;
nuclear@0	983 }
nuclear@0	984 else if ((m.M[0][0] > m.M[1][1])&&(m.M[0][0] > m.M[2][2]))
nuclear@0	985 {
nuclear@0	986 T s = sqrt(T(1) + m.M[0][0] - m.M[1][1] - m.M[2][2]) * T(2);
nuclear@0	987 w = (m.M[2][1] - m.M[1][2]) / s;
nuclear@0	988 x = T(0.25) * s;
nuclear@0	989 y = (m.M[0][1] + m.M[1][0]) / s;
nuclear@0	990 z = (m.M[2][0] + m.M[0][2]) / s;
nuclear@0	991 }
nuclear@0	992 else if (m.M[1][1] > m.M[2][2])
nuclear@0	993 {
nuclear@0	994 T s = sqrt(T(1) + m.M[1][1] - m.M[0][0] - m.M[2][2]) * T(2); // S=4*qy
nuclear@0	995 w = (m.M[0][2] - m.M[2][0]) / s;
nuclear@0	996 x = (m.M[0][1] + m.M[1][0]) / s;
nuclear@0	997 y = T(0.25) * s;
nuclear@0	998 z = (m.M[1][2] + m.M[2][1]) / s;
nuclear@0	999 }
nuclear@0	1000 else
nuclear@0	1001 {
nuclear@0	1002 T s = sqrt(T(1) + m.M[2][2] - m.M[0][0] - m.M[1][1]) * T(2); // S=4*qz
nuclear@0	1003 w = (m.M[1][0] - m.M[0][1]) / s;
nuclear@0	1004 x = (m.M[0][2] + m.M[2][0]) / s;
nuclear@0	1005 y = (m.M[1][2] + m.M[2][1]) / s;
nuclear@0	1006 z = T(0.25) * s;
nuclear@0	1007 }
nuclear@0	1008 }
nuclear@0	1009
nuclear@0	1010 // Constructs the quaternion from a rotation matrix
nuclear@0	1011 explicit Quat(const Matrix3<T>& m)
nuclear@0	1012 {
nuclear@0	1013 T trace = m.M[0][0] + m.M[1][1] + m.M[2][2];
nuclear@0	1014
nuclear@0	1015 // In almost all cases, the first part is executed.
nuclear@0	1016 // However, if the trace is not positive, the other
nuclear@0	1017 // cases arise.
nuclear@0	1018 if (trace > T(0))
nuclear@0	1019 {
nuclear@0	1020 T s = sqrt(trace + T(1)) * T(2); // s=4*qw
nuclear@0	1021 w = T(0.25) * s;
nuclear@0	1022 x = (m.M[2][1] - m.M[1][2]) / s;
nuclear@0	1023 y = (m.M[0][2] - m.M[2][0]) / s;
nuclear@0	1024 z = (m.M[1][0] - m.M[0][1]) / s;
nuclear@0	1025 }
nuclear@0	1026 else if ((m.M[0][0] > m.M[1][1])&&(m.M[0][0] > m.M[2][2]))
nuclear@0	1027 {
nuclear@0	1028 T s = sqrt(T(1) + m.M[0][0] - m.M[1][1] - m.M[2][2]) * T(2);
nuclear@0	1029 w = (m.M[2][1] - m.M[1][2]) / s;
nuclear@0	1030 x = T(0.25) * s;
nuclear@0	1031 y = (m.M[0][1] + m.M[1][0]) / s;
nuclear@0	1032 z = (m.M[2][0] + m.M[0][2]) / s;
nuclear@0	1033 }
nuclear@0	1034 else if (m.M[1][1] > m.M[2][2])
nuclear@0	1035 {
nuclear@0	1036 T s = sqrt(T(1) + m.M[1][1] - m.M[0][0] - m.M[2][2]) * T(2); // S=4*qy
nuclear@0	1037 w = (m.M[0][2] - m.M[2][0]) / s;
nuclear@0	1038 x = (m.M[0][1] + m.M[1][0]) / s;
nuclear@0	1039 y = T(0.25) * s;
nuclear@0	1040 z = (m.M[1][2] + m.M[2][1]) / s;
nuclear@0	1041 }
nuclear@0	1042 else
nuclear@0	1043 {
nuclear@0	1044 T s = sqrt(T(1) + m.M[2][2] - m.M[0][0] - m.M[1][1]) * T(2); // S=4*qz
nuclear@0	1045 w = (m.M[1][0] - m.M[0][1]) / s;
nuclear@0	1046 x = (m.M[0][2] + m.M[2][0]) / s;
nuclear@0	1047 y = (m.M[1][2] + m.M[2][1]) / s;
nuclear@0	1048 z = T(0.25) * s;
nuclear@0	1049 }
nuclear@0	1050 }
nuclear@0	1051
nuclear@0	1052 bool operator== (const Quat& b) const { return x == b.x && y == b.y && z == b.z && w == b.w; }
nuclear@0	1053 bool operator!= (const Quat& b) const { return x != b.x \|\| y != b.y \|\| z != b.z \|\| w != b.w; }
nuclear@0	1054
nuclear@0	1055 Quat operator+ (const Quat& b) const { return Quat(x + b.x, y + b.y, z + b.z, w + b.w); }
nuclear@0	1056 Quat& operator+= (const Quat& b) { w += b.w; x += b.x; y += b.y; z += b.z; return *this; }
nuclear@0	1057 Quat operator- (const Quat& b) const { return Quat(x - b.x, y - b.y, z - b.z, w - b.w); }
nuclear@0	1058 Quat& operator-= (const Quat& b) { w -= b.w; x -= b.x; y -= b.y; z -= b.z; return *this; }
nuclear@0	1059
nuclear@0	1060 Quat operator* (T s) const { return Quat(x * s, y * s, z * s, w * s); }
nuclear@0	1061 Quat& operator= (T s) { w = s; x = s; y = s; z = s; return this; }
nuclear@0	1062 Quat operator/ (T s) const { T rcp = T(1)/s; return Quat(x * rcp, y * rcp, z * rcp, w *rcp); }
nuclear@0	1063 Quat& operator/= (T s) { T rcp = T(1)/s; w = rcp; x = rcp; y = rcp; z = rcp; return *this; }
nuclear@0	1064
nuclear@0	1065
nuclear@0	1066 // Get Imaginary part vector
nuclear@0	1067 Vector3<T> Imag() const { return Vector3<T>(x,y,z); }
nuclear@0	1068
nuclear@0	1069 // Get quaternion length.
nuclear@0	1070 T Length() const { return sqrt(LengthSq()); }
nuclear@0	1071
nuclear@0	1072 // Get quaternion length squared.
nuclear@0	1073 T LengthSq() const { return (x * x + y * y + z * z + w * w); }
nuclear@0	1074
nuclear@0	1075 // Simple Euclidean distance in R^4 (not SLERP distance, but at least respects Haar measure)
nuclear@0	1076 T Distance(const Quat& q) const
nuclear@0	1077 {
nuclear@0	1078 T d1 = (*this - q).Length();
nuclear@0	1079 T d2 = (*this + q).Length(); // Antipodal point check
nuclear@0	1080 return (d1 < d2) ? d1 : d2;
nuclear@0	1081 }
nuclear@0	1082
nuclear@0	1083 T DistanceSq(const Quat& q) const
nuclear@0	1084 {
nuclear@0	1085 T d1 = (*this - q).LengthSq();
nuclear@0	1086 T d2 = (*this + q).LengthSq(); // Antipodal point check
nuclear@0	1087 return (d1 < d2) ? d1 : d2;
nuclear@0	1088 }
nuclear@0	1089
nuclear@0	1090 T Dot(const Quat& q) const
nuclear@0	1091 {
nuclear@0	1092 return x * q.x + y * q.y + z * q.z + w * q.w;
nuclear@0	1093 }
nuclear@0	1094
nuclear@0	1095 // Angle between two quaternions in radians
nuclear@0	1096 T Angle(const Quat& q) const
nuclear@0	1097 {
nuclear@0	1098 return 2 * Acos(Alg::Abs(Dot(q)));
nuclear@0	1099 }
nuclear@0	1100
nuclear@0	1101 // Normalize
nuclear@0	1102 bool IsNormalized() const { return fabs(LengthSq() - T(1)) < ((T)MATH_DOUBLE_TOLERANCE); }
nuclear@0	1103
nuclear@0	1104 void Normalize()
nuclear@0	1105 {
nuclear@0	1106 T l = Length();
nuclear@0	1107 OVR_ASSERT(l != T(0));
nuclear@0	1108 *this /= l;
nuclear@0	1109 }
nuclear@0	1110
nuclear@0	1111 Quat Normalized() const
nuclear@0	1112 {
nuclear@0	1113 T l = Length();
nuclear@0	1114 OVR_ASSERT(l != T(0));
nuclear@0	1115 return *this / l;
nuclear@0	1116 }
nuclear@0	1117
nuclear@0	1118 // Returns conjugate of the quaternion. Produces inverse rotation if quaternion is normalized.
nuclear@0	1119 Quat Conj() const { return Quat(-x, -y, -z, w); }
nuclear@0	1120
nuclear@0	1121 // Quaternion multiplication. Combines quaternion rotations, performing the one on the
nuclear@0	1122 // right hand side first.
nuclear@0	1123 Quat operator* (const Quat& b) const { return Quat(w * b.x + x * b.w + y * b.z - z * b.y,
nuclear@0	1124 w * b.y - x * b.z + y * b.w + z * b.x,
nuclear@0	1125 w * b.z + x * b.y - y * b.x + z * b.w,
nuclear@0	1126 w * b.w - x * b.x - y * b.y - z * b.z); }
nuclear@0	1127
nuclear@0	1128 //
nuclear@0	1129 // this^p normalized; same as rotating by this p times.
nuclear@0	1130 Quat PowNormalized(T p) const
nuclear@0	1131 {
nuclear@0	1132 Vector3<T> v;
nuclear@0	1133 T a;
nuclear@0	1134 GetAxisAngle(&v, &a);
nuclear@0	1135 return Quat(v, a * p);
nuclear@0	1136 }
nuclear@0	1137
nuclear@0	1138 // Normalized linear interpolation of quaternions
nuclear@0	1139 Quat Nlerp(const Quat& other, T a)
nuclear@0	1140 {
nuclear@0	1141 T sign = (Dot(other) >= 0) ? 1 : -1;
nuclear@0	1142 return (this sign * a + other * (1-a)).Normalized();
nuclear@0	1143 }
nuclear@0	1144
nuclear@0	1145 // Rotate transforms vector in a manner that matches Matrix rotations (counter-clockwise,
nuclear@0	1146 // assuming negative direction of the axis). Standard formula: q(t) * V * q(t)^-1.
nuclear@0	1147 Vector3<T> Rotate(const Vector3<T>& v) const
nuclear@0	1148 {
nuclear@0	1149 return ((this Quat<T>(v.x, v.y, v.z, T(0))) * Inverted()).Imag();
nuclear@0	1150 }
nuclear@0	1151
nuclear@0	1152 // Inversed quaternion rotates in the opposite direction.
nuclear@0	1153 Quat Inverted() const
nuclear@0	1154 {
nuclear@0	1155 return Quat(-x, -y, -z, w);
nuclear@0	1156 }
nuclear@0	1157
nuclear@0	1158 // Sets this quaternion to the one rotates in the opposite direction.
nuclear@0	1159 void Invert()
nuclear@0	1160 {
nuclear@0	1161 *this = Quat(-x, -y, -z, w);
nuclear@0	1162 }
nuclear@0	1163
nuclear@0	1164 // GetEulerAngles extracts Euler angles from the quaternion, in the specified order of
nuclear@0	1165 // axis rotations and the specified coordinate system. Right-handed coordinate system
nuclear@0	1166 // is the default, with CCW rotations while looking in the negative axis direction.
nuclear@0	1167 // Here a,b,c, are the Yaw/Pitch/Roll angles to be returned.
nuclear@0	1168 // rotation a around axis A1
nuclear@0	1169 // is followed by rotation b around axis A2
nuclear@0	1170 // is followed by rotation c around axis A3
nuclear@0	1171 // rotations are CCW or CW (D) in LH or RH coordinate system (S)
nuclear@0	1172 template <Axis A1, Axis A2, Axis A3, RotateDirection D, HandedSystem S>
nuclear@0	1173 void GetEulerAngles(T a, T b, T *c) const
nuclear@0	1174 {
nuclear@0	1175 static_assert((A1 != A2) && (A2 != A3) && (A1 != A3), "(A1 != A2) && (A2 != A3) && (A1 != A3)");
nuclear@0	1176
nuclear@0	1177 T Q[3] = { x, y, z }; //Quaternion components x,y,z
nuclear@0	1178
nuclear@0	1179 T ww = w*w;
nuclear@0	1180 T Q11 = Q[A1]*Q[A1];
nuclear@0	1181 T Q22 = Q[A2]*Q[A2];
nuclear@0	1182 T Q33 = Q[A3]*Q[A3];
nuclear@0	1183
nuclear@0	1184 T psign = T(-1);
nuclear@0	1185 // Determine whether even permutation
nuclear@0	1186 if (((A1 + 1) % 3 == A2) && ((A2 + 1) % 3 == A3))
nuclear@0	1187 psign = T(1);
nuclear@0	1188
nuclear@0	1189 T s2 = psign * T(2) * (psignwQ[A2] + Q[A1]*Q[A3]);
nuclear@0	1190
nuclear@0	1191 if (s2 < T(-1) + ((T)MATH_DOUBLE_SINGULARITYRADIUS))
nuclear@0	1192 { // South pole singularity
nuclear@0	1193 *a = T(0);
nuclear@0	1194 b = -SD*((T)MATH_DOUBLE_PIOVER2);
nuclear@0	1195 c = SDatan2(T(2)(psignQ[A1]Q[A2] + w*Q[A3]),
nuclear@0	1196 ww + Q22 - Q11 - Q33 );
nuclear@0	1197 }
nuclear@0	1198 else if (s2 > T(1) - ((T)MATH_DOUBLE_SINGULARITYRADIUS))
nuclear@0	1199 { // North pole singularity
nuclear@0	1200 *a = T(0);
nuclear@0	1201 b = SD*((T)MATH_DOUBLE_PIOVER2);
nuclear@0	1202 c = SDatan2(T(2)(psignQ[A1]Q[A2] + w*Q[A3]),
nuclear@0	1203 ww + Q22 - Q11 - Q33);
nuclear@0	1204 }
nuclear@0	1205 else
nuclear@0	1206 {
nuclear@0	1207 a = -SDatan2(T(-2)(wQ[A1] - psignQ[A2]*Q[A3]),
nuclear@0	1208 ww + Q33 - Q11 - Q22);
nuclear@0	1209 b = SD*asin(s2);
nuclear@0	1210 c = SDatan2(T(2)(wQ[A3] - psignQ[A1]*Q[A2]),
nuclear@0	1211 ww + Q11 - Q22 - Q33);
nuclear@0	1212 }
nuclear@0	1213 return;
nuclear@0	1214 }
nuclear@0	1215
nuclear@0	1216 template <Axis A1, Axis A2, Axis A3, RotateDirection D>
nuclear@0	1217 void GetEulerAngles(T a, T b, T *c) const
nuclear@0	1218 { GetEulerAngles<A1, A2, A3, D, Handed_R>(a, b, c); }
nuclear@0	1219
nuclear@0	1220 template <Axis A1, Axis A2, Axis A3>
nuclear@0	1221 void GetEulerAngles(T a, T b, T *c) const
nuclear@0	1222 { GetEulerAngles<A1, A2, A3, Rotate_CCW, Handed_R>(a, b, c); }
nuclear@0	1223
nuclear@0	1224 // GetEulerAnglesABA extracts Euler angles from the quaternion, in the specified order of
nuclear@0	1225 // axis rotations and the specified coordinate system. Right-handed coordinate system
nuclear@0	1226 // is the default, with CCW rotations while looking in the negative axis direction.
nuclear@0	1227 // Here a,b,c, are the Yaw/Pitch/Roll angles to be returned.
nuclear@0	1228 // rotation a around axis A1
nuclear@0	1229 // is followed by rotation b around axis A2
nuclear@0	1230 // is followed by rotation c around axis A1
nuclear@0	1231 // Rotations are CCW or CW (D) in LH or RH coordinate system (S)
nuclear@0	1232 template <Axis A1, Axis A2, RotateDirection D, HandedSystem S>
nuclear@0	1233 void GetEulerAnglesABA(T a, T b, T *c) const
nuclear@0	1234 {
nuclear@0	1235 static_assert(A1 != A2, "A1 != A2");
nuclear@0	1236
nuclear@0	1237 T Q[3] = {x, y, z}; // Quaternion components
nuclear@0	1238
nuclear@0	1239 // Determine the missing axis that was not supplied
nuclear@0	1240 int m = 3 - A1 - A2;
nuclear@0	1241
nuclear@0	1242 T ww = w*w;
nuclear@0	1243 T Q11 = Q[A1]*Q[A1];
nuclear@0	1244 T Q22 = Q[A2]*Q[A2];
nuclear@0	1245 T Qmm = Q[m]*Q[m];
nuclear@0	1246
nuclear@0	1247 T psign = T(-1);
nuclear@0	1248 if ((A1 + 1) % 3 == A2) // Determine whether even permutation
nuclear@0	1249 {
nuclear@0	1250 psign = T(1);
nuclear@0	1251 }
nuclear@0	1252
nuclear@0	1253 T c2 = ww + Q11 - Q22 - Qmm;
nuclear@0	1254 if (c2 < T(-1) + Math<T>::SingularityRadius)
nuclear@0	1255 { // South pole singularity
nuclear@0	1256 *a = T(0);
nuclear@0	1257 b = SD*((T)MATH_DOUBLE_PI);
nuclear@0	1258 c = SDatan2( T(2)(wQ[A1] - psignQ[A2]*Q[m]),
nuclear@0	1259 ww + Q22 - Q11 - Qmm);
nuclear@0	1260 }
nuclear@0	1261 else if (c2 > T(1) - Math<T>::SingularityRadius)
nuclear@0	1262 { // North pole singularity
nuclear@0	1263 *a = T(0);
nuclear@0	1264 *b = T(0);
nuclear@0	1265 c = SDatan2( T(2)(wQ[A1] - psignQ[A2]*Q[m]),
nuclear@0	1266 ww + Q22 - Q11 - Qmm);
nuclear@0	1267 }
nuclear@0	1268 else
nuclear@0	1269 {
nuclear@0	1270 a = SDatan2( psignwQ[m] + Q[A1]Q[A2],
nuclear@0	1271 wQ[A2] -psignQ[A1]*Q[m]);
nuclear@0	1272 b = SD*acos(c2);
nuclear@0	1273 c = SDatan2( -psignwQ[m] + Q[A1]Q[A2],
nuclear@0	1274 wQ[A2] + psignQ[A1]*Q[m]);
nuclear@0	1275 }
nuclear@0	1276 return;
nuclear@0	1277 }
nuclear@0	1278 };
nuclear@0	1279
nuclear@0	1280 typedef Quat<float> Quatf;
nuclear@0	1281 typedef Quat<double> Quatd;
nuclear@0	1282
nuclear@0	1283 static_assert((sizeof(Quatf) == 4*sizeof(float)), "sizeof(Quatf) failure");
nuclear@0	1284 static_assert((sizeof(Quatd) == 4*sizeof(double)), "sizeof(Quatd) failure");
nuclear@0	1285
nuclear@0	1286 //-------------------------------------------------------------------------------------
nuclear@0	1287 // ***** Pose
nuclear@0	1288
nuclear@0	1289 // Position and orientation combined.
nuclear@0	1290
nuclear@0	1291 template<class T>
nuclear@0	1292 class Pose
nuclear@0	1293 {
nuclear@0	1294 public:
nuclear@0	1295 typedef typename CompatibleTypes<Pose<T> >::Type CompatibleType;
nuclear@0	1296
nuclear@0	1297 Pose() { }
nuclear@0	1298 Pose(const Quat<T>& orientation, const Vector3<T>& pos)
nuclear@0	1299 : Rotation(orientation), Translation(pos) { }
nuclear@0	1300 Pose(const Pose& s)
nuclear@0	1301 : Rotation(s.Rotation), Translation(s.Translation) { }
nuclear@0	1302 Pose(const CompatibleType& s)
nuclear@0	1303 : Rotation(s.Orientation), Translation(s.Position) { }
nuclear@0	1304 explicit Pose(const Pose<typename Math<T>::OtherFloatType> &s)
nuclear@0	1305 : Rotation(s.Rotation), Translation(s.Translation) { }
nuclear@0	1306
nuclear@0	1307 operator typename CompatibleTypes<Pose<T> >::Type () const
nuclear@0	1308 {
nuclear@0	1309 typename CompatibleTypes<Pose<T> >::Type result;
nuclear@0	1310 result.Orientation = Rotation;
nuclear@0	1311 result.Position = Translation;
nuclear@0	1312 return result;
nuclear@0	1313 }
nuclear@0	1314
nuclear@0	1315 Quat<T> Rotation;
nuclear@0	1316 Vector3<T> Translation;
nuclear@0	1317
nuclear@0	1318 static_assert((sizeof(T) == sizeof(double) \|\| sizeof(T) == sizeof(float)), "(sizeof(T) == sizeof(double) \|\| sizeof(T) == sizeof(float))");
nuclear@0	1319
nuclear@0	1320 void ToArray(T* arr) const
nuclear@0	1321 {
nuclear@0	1322 T temp[7] = { Rotation.x, Rotation.y, Rotation.z, Rotation.w, Translation.x, Translation.y, Translation.z };
nuclear@0	1323 for (int i = 0; i < 7; i++) arr[i] = temp[i];
nuclear@0	1324 }
nuclear@0	1325
nuclear@0	1326 static Pose<T> FromArray(const T* v)
nuclear@0	1327 {
nuclear@0	1328 Quat<T> rotation(v[0], v[1], v[2], v[3]);
nuclear@0	1329 Vector3<T> translation(v[4], v[5], v[6]);
nuclear@0	1330 return Pose<T>(rotation, translation);
nuclear@0	1331 }
nuclear@0	1332
nuclear@0	1333 Vector3<T> Rotate(const Vector3<T>& v) const
nuclear@0	1334 {
nuclear@0	1335 return Rotation.Rotate(v);
nuclear@0	1336 }
nuclear@0	1337
nuclear@0	1338 Vector3<T> Translate(const Vector3<T>& v) const
nuclear@0	1339 {
nuclear@0	1340 return v + Translation;
nuclear@0	1341 }
nuclear@0	1342
nuclear@0	1343 Vector3<T> Apply(const Vector3<T>& v) const
nuclear@0	1344 {
nuclear@0	1345 return Translate(Rotate(v));
nuclear@0	1346 }
nuclear@0	1347
nuclear@0	1348 Pose operator*(const Pose& other) const
nuclear@0	1349 {
nuclear@0	1350 return Pose(Rotation * other.Rotation, Apply(other.Translation));
nuclear@0	1351 }
nuclear@0	1352
nuclear@0	1353 Pose Inverted() const
nuclear@0	1354 {
nuclear@0	1355 Quat<T> inv = Rotation.Inverted();
nuclear@0	1356 return Pose(inv, inv.Rotate(-Translation));
nuclear@0	1357 }
nuclear@0	1358 };
nuclear@0	1359
nuclear@0	1360 typedef Pose<float> Posef;
nuclear@0	1361 typedef Pose<double> Posed;
nuclear@0	1362
nuclear@0	1363 static_assert((sizeof(Posed) == sizeof(Quatd) + sizeof(Vector3d)), "sizeof(Posed) failure");
nuclear@0	1364 static_assert((sizeof(Posef) == sizeof(Quatf) + sizeof(Vector3f)), "sizeof(Posef) failure");
nuclear@0	1365
nuclear@0	1366
nuclear@0	1367 //-------------------------------------------------------------------------------------
nuclear@0	1368 // ***** Matrix4
nuclear@0	1369 //
nuclear@0	1370 // Matrix4 is a 4x4 matrix used for 3d transformations and projections.
nuclear@0	1371 // Translation stored in the last column.
nuclear@0	1372 // The matrix is stored in row-major order in memory, meaning that values
nuclear@0	1373 // of the first row are stored before the next one.
nuclear@0	1374 //
nuclear@0	1375 // The arrangement of the matrix is chosen to be in Right-Handed
nuclear@0	1376 // coordinate system and counterclockwise rotations when looking down
nuclear@0	1377 // the axis
nuclear@0	1378 //
nuclear@0	1379 // Transformation Order:
nuclear@0	1380 // - Transformations are applied from right to left, so the expression
nuclear@0	1381 // M1 * M2 * M3 * V means that the vector V is transformed by M3 first,
nuclear@0	1382 // followed by M2 and M1.
nuclear@0	1383 //
nuclear@0	1384 // Coordinate system: Right Handed
nuclear@0	1385 //
nuclear@0	1386 // Rotations: Counterclockwise when looking down the axis. All angles are in radians.
nuclear@0	1387 //
nuclear@0	1388 // \| sx 01 02 tx \| // First column (sx, 10, 20): Axis X basis vector.
nuclear@0	1389 // \| 10 sy 12 ty \| // Second column (01, sy, 21): Axis Y basis vector.
nuclear@0	1390 // \| 20 21 sz tz \| // Third columnt (02, 12, sz): Axis Z basis vector.
nuclear@0	1391 // \| 30 31 32 33 \|
nuclear@0	1392 //
nuclear@0	1393 // The basis vectors are first three columns.
nuclear@0	1394
nuclear@0	1395 template<class T>
nuclear@0	1396 class Matrix4
nuclear@0	1397 {
nuclear@0	1398 static const Matrix4 IdentityValue;
nuclear@0	1399
nuclear@0	1400 public:
nuclear@0	1401 T M[4][4];
nuclear@0	1402
nuclear@0	1403 enum NoInitType { NoInit };
nuclear@0	1404
nuclear@0	1405 // Construct with no memory initialization.
nuclear@0	1406 Matrix4(NoInitType) { }
nuclear@0	1407
nuclear@0	1408 // By default, we construct identity matrix.
nuclear@0	1409 Matrix4()
nuclear@0	1410 {
nuclear@0	1411 SetIdentity();
nuclear@0	1412 }
nuclear@0	1413
nuclear@0	1414 Matrix4(T m11, T m12, T m13, T m14,
nuclear@0	1415 T m21, T m22, T m23, T m24,
nuclear@0	1416 T m31, T m32, T m33, T m34,
nuclear@0	1417 T m41, T m42, T m43, T m44)
nuclear@0	1418 {
nuclear@0	1419 M[0][0] = m11; M[0][1] = m12; M[0][2] = m13; M[0][3] = m14;
nuclear@0	1420 M[1][0] = m21; M[1][1] = m22; M[1][2] = m23; M[1][3] = m24;
nuclear@0	1421 M[2][0] = m31; M[2][1] = m32; M[2][2] = m33; M[2][3] = m34;
nuclear@0	1422 M[3][0] = m41; M[3][1] = m42; M[3][2] = m43; M[3][3] = m44;
nuclear@0	1423 }
nuclear@0	1424
nuclear@0	1425 Matrix4(T m11, T m12, T m13,
nuclear@0	1426 T m21, T m22, T m23,
nuclear@0	1427 T m31, T m32, T m33)
nuclear@0	1428 {
nuclear@0	1429 M[0][0] = m11; M[0][1] = m12; M[0][2] = m13; M[0][3] = 0;
nuclear@0	1430 M[1][0] = m21; M[1][1] = m22; M[1][2] = m23; M[1][3] = 0;
nuclear@0	1431 M[2][0] = m31; M[2][1] = m32; M[2][2] = m33; M[2][3] = 0;
nuclear@0	1432 M[3][0] = 0; M[3][1] = 0; M[3][2] = 0; M[3][3] = 1;
nuclear@0	1433 }
nuclear@0	1434
nuclear@0	1435 explicit Matrix4(const Quat<T>& q)
nuclear@0	1436 {
nuclear@0	1437 T ww = q.w*q.w;
nuclear@0	1438 T xx = q.x*q.x;
nuclear@0	1439 T yy = q.y*q.y;
nuclear@0	1440 T zz = q.z*q.z;
nuclear@0	1441
nuclear@0	1442 M[0][0] = ww + xx - yy - zz; M[0][1] = 2 * (q.xq.y - q.wq.z); M[0][2] = 2 * (q.xq.z + q.wq.y); M[0][3] = 0;
nuclear@0	1443 M[1][0] = 2 * (q.xq.y + q.wq.z); M[1][1] = ww - xx + yy - zz; M[1][2] = 2 * (q.yq.z - q.wq.x); M[1][3] = 0;
nuclear@0	1444 M[2][0] = 2 * (q.xq.z - q.wq.y); M[2][1] = 2 * (q.yq.z + q.wq.x); M[2][2] = ww - xx - yy + zz; M[2][3] = 0;
nuclear@0	1445 M[3][0] = 0; M[3][1] = 0; M[3][2] = 0; M[3][3] = 1;
nuclear@0	1446 }
nuclear@0	1447
nuclear@0	1448 explicit Matrix4(const Pose<T>& p)
nuclear@0	1449 {
nuclear@0	1450 Matrix4 result(p.Rotation);
nuclear@0	1451 result.SetTranslation(p.Translation);
nuclear@0	1452 *this = result;
nuclear@0	1453 }
nuclear@0	1454
nuclear@0	1455 // C-interop support
nuclear@0	1456 explicit Matrix4(const Matrix4<typename Math<T>::OtherFloatType> &src)
nuclear@0	1457 {
nuclear@0	1458 for (int i = 0; i < 4; i++)
nuclear@0	1459 for (int j = 0; j < 4; j++)
nuclear@0	1460 M[i][j] = (T)src.M[i][j];
nuclear@0	1461 }
nuclear@0	1462
nuclear@0	1463 // C-interop support.
nuclear@0	1464 Matrix4(const typename CompatibleTypes<Matrix4<T> >::Type& s)
nuclear@0	1465 {
nuclear@0	1466 static_assert(sizeof(s) == sizeof(Matrix4), "sizeof(s) == sizeof(Matrix4)");
nuclear@0	1467 memcpy(M, s.M, sizeof(M));
nuclear@0	1468 }
nuclear@0	1469
nuclear@0	1470 operator typename CompatibleTypes<Matrix4<T> >::Type () const
nuclear@0	1471 {
nuclear@0	1472 typename CompatibleTypes<Matrix4<T> >::Type result;
nuclear@0	1473 static_assert(sizeof(result) == sizeof(Matrix4), "sizeof(result) == sizeof(Matrix4)");
nuclear@0	1474 memcpy(result.M, M, sizeof(M));
nuclear@0	1475 return result;
nuclear@0	1476 }
nuclear@0	1477
nuclear@0	1478 void ToString(char* dest, size_t destsize) const
nuclear@0	1479 {
nuclear@0	1480 size_t pos = 0;
nuclear@0	1481 for (int r=0; r<4; r++)
nuclear@0	1482 for (int c=0; c<4; c++)
nuclear@0	1483 pos += OVR_sprintf(dest+pos, destsize-pos, "%g ", M[r][c]);
nuclear@0	1484 }
nuclear@0	1485
nuclear@0	1486 static Matrix4 FromString(const char* src)
nuclear@0	1487 {
nuclear@0	1488 Matrix4 result;
nuclear@0	1489 if (src)
nuclear@0	1490 {
nuclear@0	1491 for (int r=0; r<4; r++)
nuclear@0	1492 {
nuclear@0	1493 for (int c=0; c<4; c++)
nuclear@0	1494 {
nuclear@0	1495 result.M[r][c] = (T)atof(src);
nuclear@0	1496 while (src && *src != ' ')
nuclear@0	1497 {
nuclear@0	1498 src++;
nuclear@0	1499 }
nuclear@0	1500 while (src && *src == ' ')
nuclear@0	1501 {
nuclear@0	1502 src++;
nuclear@0	1503 }
nuclear@0	1504 }
nuclear@0	1505 }
nuclear@0	1506 }
nuclear@0	1507 return result;
nuclear@0	1508 }
nuclear@0	1509
nuclear@0	1510 static const Matrix4& Identity() { return IdentityValue; }
nuclear@0	1511
nuclear@0	1512 void SetIdentity()
nuclear@0	1513 {
nuclear@0	1514 M[0][0] = M[1][1] = M[2][2] = M[3][3] = 1;
nuclear@0	1515 M[0][1] = M[1][0] = M[2][3] = M[3][1] = 0;
nuclear@0	1516 M[0][2] = M[1][2] = M[2][0] = M[3][2] = 0;
nuclear@0	1517 M[0][3] = M[1][3] = M[2][1] = M[3][0] = 0;
nuclear@0	1518 }
nuclear@0	1519
nuclear@0	1520 void SetXBasis(const Vector3f & v)
nuclear@0	1521 {
nuclear@0	1522 M[0][0] = v.x;
nuclear@0	1523 M[1][0] = v.y;
nuclear@0	1524 M[2][0] = v.z;
nuclear@0	1525 }
nuclear@0	1526 Vector3f GetXBasis() const
nuclear@0	1527 {
nuclear@0	1528 return Vector3f(M[0][0], M[1][0], M[2][0]);
nuclear@0	1529 }
nuclear@0	1530
nuclear@0	1531 void SetYBasis(const Vector3f & v)
nuclear@0	1532 {
nuclear@0	1533 M[0][1] = v.x;
nuclear@0	1534 M[1][1] = v.y;
nuclear@0	1535 M[2][1] = v.z;
nuclear@0	1536 }
nuclear@0	1537 Vector3f GetYBasis() const
nuclear@0	1538 {
nuclear@0	1539 return Vector3f(M[0][1], M[1][1], M[2][1]);
nuclear@0	1540 }
nuclear@0	1541
nuclear@0	1542 void SetZBasis(const Vector3f & v)
nuclear@0	1543 {
nuclear@0	1544 M[0][2] = v.x;
nuclear@0	1545 M[1][2] = v.y;
nuclear@0	1546 M[2][2] = v.z;
nuclear@0	1547 }
nuclear@0	1548 Vector3f GetZBasis() const
nuclear@0	1549 {
nuclear@0	1550 return Vector3f(M[0][2], M[1][2], M[2][2]);
nuclear@0	1551 }
nuclear@0	1552
nuclear@0	1553 bool operator== (const Matrix4& b) const
nuclear@0	1554 {
nuclear@0	1555 bool isEqual = true;
nuclear@0	1556 for (int i = 0; i < 4; i++)
nuclear@0	1557 for (int j = 0; j < 4; j++)
nuclear@0	1558 isEqual &= (M[i][j] == b.M[i][j]);
nuclear@0	1559
nuclear@0	1560 return isEqual;
nuclear@0	1561 }
nuclear@0	1562
nuclear@0	1563 Matrix4 operator+ (const Matrix4& b) const
nuclear@0	1564 {
nuclear@0	1565 Matrix4 result(*this);
nuclear@0	1566 result += b;
nuclear@0	1567 return result;
nuclear@0	1568 }
nuclear@0	1569
nuclear@0	1570 Matrix4& operator+= (const Matrix4& b)
nuclear@0	1571 {
nuclear@0	1572 for (int i = 0; i < 4; i++)
nuclear@0	1573 for (int j = 0; j < 4; j++)
nuclear@0	1574 M[i][j] += b.M[i][j];
nuclear@0	1575 return *this;
nuclear@0	1576 }
nuclear@0	1577
nuclear@0	1578 Matrix4 operator- (const Matrix4& b) const
nuclear@0	1579 {
nuclear@0	1580 Matrix4 result(*this);
nuclear@0	1581 result -= b;
nuclear@0	1582 return result;
nuclear@0	1583 }
nuclear@0	1584
nuclear@0	1585 Matrix4& operator-= (const Matrix4& b)
nuclear@0	1586 {
nuclear@0	1587 for (int i = 0; i < 4; i++)
nuclear@0	1588 for (int j = 0; j < 4; j++)
nuclear@0	1589 M[i][j] -= b.M[i][j];
nuclear@0	1590 return *this;
nuclear@0	1591 }
nuclear@0	1592
nuclear@0	1593 // Multiplies two matrices into destination with minimum copying.
nuclear@0	1594 static Matrix4& Multiply(Matrix4* d, const Matrix4& a, const Matrix4& b)
nuclear@0	1595 {
nuclear@0	1596 OVR_ASSERT((d != &a) && (d != &b));
nuclear@0	1597 int i = 0;
nuclear@0	1598 do {
nuclear@0	1599 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@0	1600 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@0	1601 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@0	1602 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@0	1603 } while((++i) < 4);
nuclear@0	1604
nuclear@0	1605 return *d;
nuclear@0	1606 }
nuclear@0	1607
nuclear@0	1608 Matrix4 operator* (const Matrix4& b) const
nuclear@0	1609 {
nuclear@0	1610 Matrix4 result(Matrix4::NoInit);
nuclear@0	1611 Multiply(&result, *this, b);
nuclear@0	1612 return result;
nuclear@0	1613 }
nuclear@0	1614
nuclear@0	1615 Matrix4& operator*= (const Matrix4& b)
nuclear@0	1616 {
nuclear@0	1617 return Multiply(this, Matrix4(*this), b);
nuclear@0	1618 }
nuclear@0	1619
nuclear@0	1620 Matrix4 operator* (T s) const
nuclear@0	1621 {
nuclear@0	1622 Matrix4 result(*this);
nuclear@0	1623 result *= s;
nuclear@0	1624 return result;
nuclear@0	1625 }
nuclear@0	1626
nuclear@0	1627 Matrix4& operator*= (T s)
nuclear@0	1628 {
nuclear@0	1629 for (int i = 0; i < 4; i++)
nuclear@0	1630 for (int j = 0; j < 4; j++)
nuclear@0	1631 M[i][j] *= s;
nuclear@0	1632 return *this;
nuclear@0	1633 }
nuclear@0	1634
nuclear@0	1635
nuclear@0	1636 Matrix4 operator/ (T s) const
nuclear@0	1637 {
nuclear@0	1638 Matrix4 result(*this);
nuclear@0	1639 result /= s;
nuclear@0	1640 return result;
nuclear@0	1641 }
nuclear@0	1642
nuclear@0	1643 Matrix4& operator/= (T s)
nuclear@0	1644 {
nuclear@0	1645 for (int i = 0; i < 4; i++)
nuclear@0	1646 for (int j = 0; j < 4; j++)
nuclear@0	1647 M[i][j] /= s;
nuclear@0	1648 return *this;
nuclear@0	1649 }
nuclear@0	1650
nuclear@0	1651 Vector3<T> Transform(const Vector3<T>& v) const
nuclear@0	1652 {
nuclear@0	1653 const T rcpW = 1.0f / (M[3][0] * v.x + M[3][1] * v.y + M[3][2] * v.z + M[3][3]);
nuclear@0	1654 return Vector3<T>((M[0][0] * v.x + M[0][1] * v.y + M[0][2] * v.z + M[0][3]) * rcpW,
nuclear@0	1655 (M[1][0] * v.x + M[1][1] * v.y + M[1][2] * v.z + M[1][3]) * rcpW,
nuclear@0	1656 (M[2][0] * v.x + M[2][1] * v.y + M[2][2] * v.z + M[2][3]) * rcpW);
nuclear@0	1657 }
nuclear@0	1658
nuclear@0	1659 Vector4<T> Transform(const Vector4<T>& v) const
nuclear@0	1660 {
nuclear@0	1661 return Vector4<T>(M[0][0] * v.x + M[0][1] * v.y + M[0][2] * v.z + M[0][3] * v.w,
nuclear@0	1662 M[1][0] * v.x + M[1][1] * v.y + M[1][2] * v.z + M[1][3] * v.w,
nuclear@0	1663 M[2][0] * v.x + M[2][1] * v.y + M[2][2] * v.z + M[2][3] * v.w,
nuclear@0	1664 M[3][0] * v.x + M[3][1] * v.y + M[3][2] * v.z + M[3][3] * v.w);
nuclear@0	1665 }
nuclear@0	1666
nuclear@0	1667 Matrix4 Transposed() const
nuclear@0	1668 {
nuclear@0	1669 return Matrix4(M[0][0], M[1][0], M[2][0], M[3][0],
nuclear@0	1670 M[0][1], M[1][1], M[2][1], M[3][1],
nuclear@0	1671 M[0][2], M[1][2], M[2][2], M[3][2],
nuclear@0	1672 M[0][3], M[1][3], M[2][3], M[3][3]);
nuclear@0	1673 }
nuclear@0	1674
nuclear@0	1675 void Transpose()
nuclear@0	1676 {
nuclear@0	1677 *this = Transposed();
nuclear@0	1678 }
nuclear@0	1679
nuclear@0	1680
nuclear@0	1681 T SubDet (const size_t* rows, const size_t* cols) const
nuclear@0	1682 {
nuclear@0	1683 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@0	1684 - 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@0	1685 + 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@0	1686 }
nuclear@0	1687
nuclear@0	1688 T Cofactor(size_t I, size_t J) const
nuclear@0	1689 {
nuclear@0	1690 const size_t indices[4][3] = {{1,2,3},{0,2,3},{0,1,3},{0,1,2}};
nuclear@0	1691 return ((I+J)&1) ? -SubDet(indices[I],indices[J]) : SubDet(indices[I],indices[J]);
nuclear@0	1692 }
nuclear@0	1693
nuclear@0	1694 T Determinant() const
nuclear@0	1695 {
nuclear@0	1696 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@0	1697 }
nuclear@0	1698
nuclear@0	1699 Matrix4 Adjugated() const
nuclear@0	1700 {
nuclear@0	1701 return Matrix4(Cofactor(0,0), Cofactor(1,0), Cofactor(2,0), Cofactor(3,0),
nuclear@0	1702 Cofactor(0,1), Cofactor(1,1), Cofactor(2,1), Cofactor(3,1),
nuclear@0	1703 Cofactor(0,2), Cofactor(1,2), Cofactor(2,2), Cofactor(3,2),
nuclear@0	1704 Cofactor(0,3), Cofactor(1,3), Cofactor(2,3), Cofactor(3,3));
nuclear@0	1705 }
nuclear@0	1706
nuclear@0	1707 Matrix4 Inverted() const
nuclear@0	1708 {
nuclear@0	1709 T det = Determinant();
nuclear@0	1710 assert(det != 0);
nuclear@0	1711 return Adjugated() * (1.0f/det);
nuclear@0	1712 }
nuclear@0	1713
nuclear@0	1714 void Invert()
nuclear@0	1715 {
nuclear@0	1716 *this = Inverted();
nuclear@0	1717 }
nuclear@0	1718
nuclear@0	1719 // This is more efficient than general inverse, but ONLY works
nuclear@0	1720 // correctly if it is a homogeneous transform matrix (rot + trans)
nuclear@0	1721 Matrix4 InvertedHomogeneousTransform() const
nuclear@0	1722 {
nuclear@0	1723 // Make the inverse rotation matrix
nuclear@0	1724 Matrix4 rinv = this->Transposed();
nuclear@0	1725 rinv.M[3][0] = rinv.M[3][1] = rinv.M[3][2] = 0.0f;
nuclear@0	1726 // Make the inverse translation matrix
nuclear@0	1727 Vector3<T> tvinv(-M[0][3],-M[1][3],-M[2][3]);
nuclear@0	1728 Matrix4 tinv = Matrix4::Translation(tvinv);
nuclear@0	1729 return rinv * tinv; // "untranslate", then "unrotate"
nuclear@0	1730 }
nuclear@0	1731
nuclear@0	1732 // This is more efficient than general inverse, but ONLY works
nuclear@0	1733 // correctly if it is a homogeneous transform matrix (rot + trans)
nuclear@0	1734 void InvertHomogeneousTransform()
nuclear@0	1735 {
nuclear@0	1736 *this = InvertedHomogeneousTransform();
nuclear@0	1737 }
nuclear@0	1738
nuclear@0	1739 // Matrix to Euler Angles conversion
nuclear@0	1740 // a,b,c, are the YawPitchRoll angles to be returned
nuclear@0	1741 // rotation a around axis A1
nuclear@0	1742 // is followed by rotation b around axis A2
nuclear@0	1743 // is followed by rotation c around axis A3
nuclear@0	1744 // rotations are CCW or CW (D) in LH or RH coordinate system (S)
nuclear@0	1745 template <Axis A1, Axis A2, Axis A3, RotateDirection D, HandedSystem S>
nuclear@0	1746 void ToEulerAngles(T a, T b, T *c) const
nuclear@0	1747 {
nuclear@0	1748 static_assert((A1 != A2) && (A2 != A3) && (A1 != A3), "(A1 != A2) && (A2 != A3) && (A1 != A3)");
nuclear@0	1749
nuclear@0	1750 T psign = -1;
nuclear@0	1751 if (((A1 + 1) % 3 == A2) && ((A2 + 1) % 3 == A3)) // Determine whether even permutation
nuclear@0	1752 psign = 1;
nuclear@0	1753
nuclear@0	1754 T pm = psign*M[A1][A3];
nuclear@0	1755 if (pm < -1.0f + Math<T>::SingularityRadius)
nuclear@0	1756 { // South pole singularity
nuclear@0	1757 *a = 0;
nuclear@0	1758 b = -SD*((T)MATH_DOUBLE_PIOVER2);
nuclear@0	1759 c = SDatan2( psignM[A2][A1], M[A2][A2] );
nuclear@0	1760 }
nuclear@0	1761 else if (pm > 1.0f - Math<T>::SingularityRadius)
nuclear@0	1762 { // North pole singularity
nuclear@0	1763 *a = 0;
nuclear@0	1764 b = SD*((T)MATH_DOUBLE_PIOVER2);
nuclear@0	1765 c = SDatan2( psignM[A2][A1], M[A2][A2] );
nuclear@0	1766 }
nuclear@0	1767 else
nuclear@0	1768 { // Normal case (nonsingular)
nuclear@0	1769 a = SDatan2( -psignM[A2][A3], M[A3][A3] );
nuclear@0	1770 b = SD*asin(pm);
nuclear@0	1771 c = SDatan2( -psignM[A1][A2], M[A1][A1] );
nuclear@0	1772 }
nuclear@0	1773
nuclear@0	1774 return;
nuclear@0	1775 }
nuclear@0	1776
nuclear@0	1777 // Matrix to Euler Angles conversion
nuclear@0	1778 // a,b,c, are the YawPitchRoll angles to be returned
nuclear@0	1779 // rotation a around axis A1
nuclear@0	1780 // is followed by rotation b around axis A2
nuclear@0	1781 // is followed by rotation c around axis A1
nuclear@0	1782 // rotations are CCW or CW (D) in LH or RH coordinate system (S)
nuclear@0	1783 template <Axis A1, Axis A2, RotateDirection D, HandedSystem S>
nuclear@0	1784 void ToEulerAnglesABA(T a, T b, T *c) const
nuclear@0	1785 {
nuclear@0	1786 static_assert(A1 != A2, "A1 != A2");
nuclear@0	1787
nuclear@0	1788 // Determine the axis that was not supplied
nuclear@0	1789 int m = 3 - A1 - A2;
nuclear@0	1790
nuclear@0	1791 T psign = -1;
nuclear@0	1792 if ((A1 + 1) % 3 == A2) // Determine whether even permutation
nuclear@0	1793 psign = 1.0f;
nuclear@0	1794
nuclear@0	1795 T c2 = M[A1][A1];
nuclear@0	1796 if (c2 < -1 + Math<T>::SingularityRadius)
nuclear@0	1797 { // South pole singularity
nuclear@0	1798 *a = 0;
nuclear@0	1799 b = SD*((T)MATH_DOUBLE_PI);
nuclear@0	1800 c = SDatan2( -psignM[A2][m],M[A2][A2]);
nuclear@0	1801 }
nuclear@0	1802 else if (c2 > 1.0f - Math<T>::SingularityRadius)
nuclear@0	1803 { // North pole singularity
nuclear@0	1804 *a = 0;
nuclear@0	1805 *b = 0;
nuclear@0	1806 c = SDatan2( -psignM[A2][m],M[A2][A2]);
nuclear@0	1807 }
nuclear@0	1808 else
nuclear@0	1809 { // Normal case (nonsingular)
nuclear@0	1810 a = SDatan2( M[A2][A1],-psignM[m][A1]);
nuclear@0	1811 b = SD*acos(c2);
nuclear@0	1812 c = SDatan2( M[A1][A2],psignM[A1][m]);
nuclear@0	1813 }
nuclear@0	1814 return;
nuclear@0	1815 }
nuclear@0	1816
nuclear@0	1817 // Creates a matrix that converts the vertices from one coordinate system
nuclear@0	1818 // to another.
nuclear@0	1819 static Matrix4 AxisConversion(const WorldAxes& to, const WorldAxes& from)
nuclear@0	1820 {
nuclear@0	1821 // Holds axis values from the 'to' structure
nuclear@0	1822 int toArray[3] = { to.XAxis, to.YAxis, to.ZAxis };
nuclear@0	1823
nuclear@0	1824 // The inverse of the toArray
nuclear@0	1825 int inv[4];
nuclear@0	1826 inv[0] = inv[abs(to.XAxis)] = 0;
nuclear@0	1827 inv[abs(to.YAxis)] = 1;
nuclear@0	1828 inv[abs(to.ZAxis)] = 2;
nuclear@0	1829
nuclear@0	1830 Matrix4 m(0, 0, 0,
nuclear@0	1831 0, 0, 0,
nuclear@0	1832 0, 0, 0);
nuclear@0	1833
nuclear@0	1834 // Only three values in the matrix need to be changed to 1 or -1.
nuclear@0	1835 m.M[inv[abs(from.XAxis)]][0] = T(from.XAxis/toArray[inv[abs(from.XAxis)]]);
nuclear@0	1836 m.M[inv[abs(from.YAxis)]][1] = T(from.YAxis/toArray[inv[abs(from.YAxis)]]);
nuclear@0	1837 m.M[inv[abs(from.ZAxis)]][2] = T(from.ZAxis/toArray[inv[abs(from.ZAxis)]]);
nuclear@0	1838 return m;
nuclear@0	1839 }
nuclear@0	1840
nuclear@0	1841
nuclear@0	1842 // Creates a matrix for translation by vector
nuclear@0	1843 static Matrix4 Translation(const Vector3<T>& v)
nuclear@0	1844 {
nuclear@0	1845 Matrix4 t;
nuclear@0	1846 t.M[0][3] = v.x;
nuclear@0	1847 t.M[1][3] = v.y;
nuclear@0	1848 t.M[2][3] = v.z;
nuclear@0	1849 return t;
nuclear@0	1850 }
nuclear@0	1851
nuclear@0	1852 // Creates a matrix for translation by vector
nuclear@0	1853 static Matrix4 Translation(T x, T y, T z = 0.0f)
nuclear@0	1854 {
nuclear@0	1855 Matrix4 t;
nuclear@0	1856 t.M[0][3] = x;
nuclear@0	1857 t.M[1][3] = y;
nuclear@0	1858 t.M[2][3] = z;
nuclear@0	1859 return t;
nuclear@0	1860 }
nuclear@0	1861
nuclear@0	1862 // Sets the translation part
nuclear@0	1863 void SetTranslation(const Vector3<T>& v)
nuclear@0	1864 {
nuclear@0	1865 M[0][3] = v.x;
nuclear@0	1866 M[1][3] = v.y;
nuclear@0	1867 M[2][3] = v.z;
nuclear@0	1868 }
nuclear@0	1869
nuclear@0	1870 Vector3<T> GetTranslation() const
nuclear@0	1871 {
nuclear@0	1872 return Vector3<T>( M[0][3], M[1][3], M[2][3] );
nuclear@0	1873 }
nuclear@0	1874
nuclear@0	1875 // Creates a matrix for scaling by vector
nuclear@0	1876 static Matrix4 Scaling(const Vector3<T>& v)
nuclear@0	1877 {
nuclear@0	1878 Matrix4 t;
nuclear@0	1879 t.M[0][0] = v.x;
nuclear@0	1880 t.M[1][1] = v.y;
nuclear@0	1881 t.M[2][2] = v.z;
nuclear@0	1882 return t;
nuclear@0	1883 }
nuclear@0	1884
nuclear@0	1885 // Creates a matrix for scaling by vector
nuclear@0	1886 static Matrix4 Scaling(T x, T y, T z)
nuclear@0	1887 {
nuclear@0	1888 Matrix4 t;
nuclear@0	1889 t.M[0][0] = x;
nuclear@0	1890 t.M[1][1] = y;
nuclear@0	1891 t.M[2][2] = z;
nuclear@0	1892 return t;
nuclear@0	1893 }
nuclear@0	1894
nuclear@0	1895 // Creates a matrix for scaling by constant
nuclear@0	1896 static Matrix4 Scaling(T s)
nuclear@0	1897 {
nuclear@0	1898 Matrix4 t;
nuclear@0	1899 t.M[0][0] = s;
nuclear@0	1900 t.M[1][1] = s;
nuclear@0	1901 t.M[2][2] = s;
nuclear@0	1902 return t;
nuclear@0	1903 }
nuclear@0	1904
nuclear@0	1905 // Simple L1 distance in R^12
nuclear@0	1906 T Distance(const Matrix4& m2) const
nuclear@0	1907 {
nuclear@0	1908 T d = fabs(M[0][0] - m2.M[0][0]) + fabs(M[0][1] - m2.M[0][1]);
nuclear@0	1909 d += fabs(M[0][2] - m2.M[0][2]) + fabs(M[0][3] - m2.M[0][3]);
nuclear@0	1910 d += fabs(M[1][0] - m2.M[1][0]) + fabs(M[1][1] - m2.M[1][1]);
nuclear@0	1911 d += fabs(M[1][2] - m2.M[1][2]) + fabs(M[1][3] - m2.M[1][3]);
nuclear@0	1912 d += fabs(M[2][0] - m2.M[2][0]) + fabs(M[2][1] - m2.M[2][1]);
nuclear@0	1913 d += fabs(M[2][2] - m2.M[2][2]) + fabs(M[2][3] - m2.M[2][3]);
nuclear@0	1914 d += fabs(M[3][0] - m2.M[3][0]) + fabs(M[3][1] - m2.M[3][1]);
nuclear@0	1915 d += fabs(M[3][2] - m2.M[3][2]) + fabs(M[3][3] - m2.M[3][3]);
nuclear@0	1916 return d;
nuclear@0	1917 }
nuclear@0	1918
nuclear@0	1919 // Creates a rotation matrix rotating around the X axis by 'angle' radians.
nuclear@0	1920 // Just for quick testing. Not for final API. Need to remove case.
nuclear@0	1921 static Matrix4 RotationAxis(Axis A, T angle, RotateDirection d, HandedSystem s)
nuclear@0	1922 {
nuclear@0	1923 T sina = s * d *sin(angle);
nuclear@0	1924 T cosa = cos(angle);
nuclear@0	1925
nuclear@0	1926 switch(A)
nuclear@0	1927 {
nuclear@0	1928 case Axis_X:
nuclear@0	1929 return Matrix4(1, 0, 0,
nuclear@0	1930 0, cosa, -sina,
nuclear@0	1931 0, sina, cosa);
nuclear@0	1932 case Axis_Y:
nuclear@0	1933 return Matrix4(cosa, 0, sina,
nuclear@0	1934 0, 1, 0,
nuclear@0	1935 -sina, 0, cosa);
nuclear@0	1936 case Axis_Z:
nuclear@0	1937 return Matrix4(cosa, -sina, 0,
nuclear@0	1938 sina, cosa, 0,
nuclear@0	1939 0, 0, 1);
nuclear@0	1940 }
nuclear@0	1941 }
nuclear@0	1942
nuclear@0	1943
nuclear@0	1944 // Creates a rotation matrix rotating around the X axis by 'angle' radians.
nuclear@0	1945 // Rotation direction is depends on the coordinate system:
nuclear@0	1946 // RHS (Oculus default): Positive angle values rotate Counter-clockwise (CCW),
nuclear@0	1947 // while looking in the negative axis direction. This is the
nuclear@0	1948 // same as looking down from positive axis values towards origin.
nuclear@0	1949 // LHS: Positive angle values rotate clock-wise (CW), while looking in the
nuclear@0	1950 // negative axis direction.
nuclear@0	1951 static Matrix4 RotationX(T angle)
nuclear@0	1952 {
nuclear@0	1953 T sina = sin(angle);
nuclear@0	1954 T cosa = cos(angle);
nuclear@0	1955 return Matrix4(1, 0, 0,
nuclear@0	1956 0, cosa, -sina,
nuclear@0	1957 0, sina, cosa);
nuclear@0	1958 }
nuclear@0	1959
nuclear@0	1960 // Creates a rotation matrix rotating around the Y axis by 'angle' radians.
nuclear@0	1961 // Rotation direction is depends on the coordinate system:
nuclear@0	1962 // RHS (Oculus default): Positive angle values rotate Counter-clockwise (CCW),
nuclear@0	1963 // while looking in the negative axis direction. This is the
nuclear@0	1964 // same as looking down from positive axis values towards origin.
nuclear@0	1965 // LHS: Positive angle values rotate clock-wise (CW), while looking in the
nuclear@0	1966 // negative axis direction.
nuclear@0	1967 static Matrix4 RotationY(T angle)
nuclear@0	1968 {
nuclear@0	1969 T sina = sin(angle);
nuclear@0	1970 T cosa = cos(angle);
nuclear@0	1971 return Matrix4(cosa, 0, sina,
nuclear@0	1972 0, 1, 0,
nuclear@0	1973 -sina, 0, cosa);
nuclear@0	1974 }
nuclear@0	1975
nuclear@0	1976 // Creates a rotation matrix rotating around the Z axis by 'angle' radians.
nuclear@0	1977 // Rotation direction is depends on the coordinate system:
nuclear@0	1978 // RHS (Oculus default): Positive angle values rotate Counter-clockwise (CCW),
nuclear@0	1979 // while looking in the negative axis direction. This is the
nuclear@0	1980 // same as looking down from positive axis values towards origin.
nuclear@0	1981 // LHS: Positive angle values rotate clock-wise (CW), while looking in the
nuclear@0	1982 // negative axis direction.
nuclear@0	1983 static Matrix4 RotationZ(T angle)
nuclear@0	1984 {
nuclear@0	1985 T sina = sin(angle);
nuclear@0	1986 T cosa = cos(angle);
nuclear@0	1987 return Matrix4(cosa, -sina, 0,
nuclear@0	1988 sina, cosa, 0,
nuclear@0	1989 0, 0, 1);
nuclear@0	1990 }
nuclear@0	1991
nuclear@0	1992 // LookAtRH creates a View transformation matrix for right-handed coordinate system.
nuclear@0	1993 // The resulting matrix points camera from 'eye' towards 'at' direction, with 'up'
nuclear@0	1994 // specifying the up vector. The resulting matrix should be used with PerspectiveRH
nuclear@0	1995 // projection.
nuclear@0	1996 static Matrix4 LookAtRH(const Vector3<T>& eye, const Vector3<T>& at, const Vector3<T>& up)
nuclear@0	1997 {
nuclear@0	1998 Vector3<T> z = (eye - at).Normalized(); // Forward
nuclear@0	1999 Vector3<T> x = up.Cross(z).Normalized(); // Right
nuclear@0	2000 Vector3<T> y = z.Cross(x);
nuclear@0	2001
nuclear@0	2002 Matrix4 m(x.x, x.y, x.z, -(x.Dot(eye)),
nuclear@0	2003 y.x, y.y, y.z, -(y.Dot(eye)),
nuclear@0	2004 z.x, z.y, z.z, -(z.Dot(eye)),
nuclear@0	2005 0, 0, 0, 1 );
nuclear@0	2006 return m;
nuclear@0	2007 }
nuclear@0	2008
nuclear@0	2009 // LookAtLH creates a View transformation matrix for left-handed coordinate system.
nuclear@0	2010 // The resulting matrix points camera from 'eye' towards 'at' direction, with 'up'
nuclear@0	2011 // specifying the up vector.
nuclear@0	2012 static Matrix4 LookAtLH(const Vector3<T>& eye, const Vector3<T>& at, const Vector3<T>& up)
nuclear@0	2013 {
nuclear@0	2014 Vector3<T> z = (at - eye).Normalized(); // Forward
nuclear@0	2015 Vector3<T> x = up.Cross(z).Normalized(); // Right
nuclear@0	2016 Vector3<T> y = z.Cross(x);
nuclear@0	2017
nuclear@0	2018 Matrix4 m(x.x, x.y, x.z, -(x.Dot(eye)),
nuclear@0	2019 y.x, y.y, y.z, -(y.Dot(eye)),
nuclear@0	2020 z.x, z.y, z.z, -(z.Dot(eye)),
nuclear@0	2021 0, 0, 0, 1 );
nuclear@0	2022 return m;
nuclear@0	2023 }
nuclear@0	2024
nuclear@0	2025 // PerspectiveRH creates a right-handed perspective projection matrix that can be
nuclear@0	2026 // used with the Oculus sample renderer.
nuclear@0	2027 // yfov - Specifies vertical field of view in radians.
nuclear@0	2028 // aspect - Screen aspect ration, which is usually width/height for square pixels.
nuclear@0	2029 // Note that xfov = yfov * aspect.
nuclear@0	2030 // znear - Absolute value of near Z clipping clipping range.
nuclear@0	2031 // zfar - Absolute value of far Z clipping clipping range (larger then near).
nuclear@0	2032 // Even though RHS usually looks in the direction of negative Z, positive values
nuclear@0	2033 // are expected for znear and zfar.
nuclear@0	2034 static Matrix4 PerspectiveRH(T yfov, T aspect, T znear, T zfar)
nuclear@0	2035 {
nuclear@0	2036 Matrix4 m;
nuclear@0	2037 T tanHalfFov = tan(yfov * 0.5f);
nuclear@0	2038
nuclear@0	2039 m.M[0][0] = 1. / (aspect * tanHalfFov);
nuclear@0	2040 m.M[1][1] = 1. / tanHalfFov;
nuclear@0	2041 m.M[2][2] = zfar / (znear - zfar);
nuclear@0	2042 m.M[3][2] = -1.;
nuclear@0	2043 m.M[2][3] = (zfar * znear) / (znear - zfar);
nuclear@0	2044 m.M[3][3] = 0.;
nuclear@0	2045
nuclear@0	2046 // Note: Post-projection matrix result assumes Left-Handed coordinate system,
nuclear@0	2047 // with Y up, X right and Z forward. This supports positive z-buffer values.
nuclear@0	2048 // This is the case even for RHS coordinate input.
nuclear@0	2049 return m;
nuclear@0	2050 }
nuclear@0	2051
nuclear@0	2052 // PerspectiveLH creates a left-handed perspective projection matrix that can be
nuclear@0	2053 // used with the Oculus sample renderer.
nuclear@0	2054 // yfov - Specifies vertical field of view in radians.
nuclear@0	2055 // aspect - Screen aspect ration, which is usually width/height for square pixels.
nuclear@0	2056 // Note that xfov = yfov * aspect.
nuclear@0	2057 // znear - Absolute value of near Z clipping clipping range.
nuclear@0	2058 // zfar - Absolute value of far Z clipping clipping range (larger then near).
nuclear@0	2059 static Matrix4 PerspectiveLH(T yfov, T aspect, T znear, T zfar)
nuclear@0	2060 {
nuclear@0	2061 Matrix4 m;
nuclear@0	2062 T tanHalfFov = tan(yfov * 0.5f);
nuclear@0	2063
nuclear@0	2064 m.M[0][0] = 1. / (aspect * tanHalfFov);
nuclear@0	2065 m.M[1][1] = 1. / tanHalfFov;
nuclear@0	2066 //m.M[2][2] = zfar / (znear - zfar);
nuclear@0	2067 m.M[2][2] = zfar / (zfar - znear);
nuclear@0	2068 m.M[3][2] = -1.;
nuclear@0	2069 m.M[2][3] = (zfar * znear) / (znear - zfar);
nuclear@0	2070 m.M[3][3] = 0.;
nuclear@0	2071
nuclear@0	2072 // Note: Post-projection matrix result assumes Left-Handed coordinate system,
nuclear@0	2073 // with Y up, X right and Z forward. This supports positive z-buffer values.
nuclear@0	2074 // This is the case even for RHS coordinate input.
nuclear@0	2075 return m;
nuclear@0	2076 }
nuclear@0	2077
nuclear@0	2078 static Matrix4 Ortho2D(T w, T h)
nuclear@0	2079 {
nuclear@0	2080 Matrix4 m;
nuclear@0	2081 m.M[0][0] = 2.0/w;
nuclear@0	2082 m.M[1][1] = -2.0/h;
nuclear@0	2083 m.M[0][3] = -1.0;
nuclear@0	2084 m.M[1][3] = 1.0;
nuclear@0	2085 m.M[2][2] = 0;
nuclear@0	2086 return m;
nuclear@0	2087 }
nuclear@0	2088 };
nuclear@0	2089
nuclear@0	2090 typedef Matrix4<float> Matrix4f;
nuclear@0	2091 typedef Matrix4<double> Matrix4d;
nuclear@0	2092
nuclear@0	2093 //-------------------------------------------------------------------------------------
nuclear@0	2094 // ***** Matrix3
nuclear@0	2095 //
nuclear@0	2096 // Matrix3 is a 3x3 matrix used for representing a rotation matrix.
nuclear@0	2097 // The matrix is stored in row-major order in memory, meaning that values
nuclear@0	2098 // of the first row are stored before the next one.
nuclear@0	2099 //
nuclear@0	2100 // The arrangement of the matrix is chosen to be in Right-Handed
nuclear@0	2101 // coordinate system and counterclockwise rotations when looking down
nuclear@0	2102 // the axis
nuclear@0	2103 //
nuclear@0	2104 // Transformation Order:
nuclear@0	2105 // - Transformations are applied from right to left, so the expression
nuclear@0	2106 // M1 * M2 * M3 * V means that the vector V is transformed by M3 first,
nuclear@0	2107 // followed by M2 and M1.
nuclear@0	2108 //
nuclear@0	2109 // Coordinate system: Right Handed
nuclear@0	2110 //
nuclear@0	2111 // Rotations: Counterclockwise when looking down the axis. All angles are in radians.
nuclear@0	2112
nuclear@0	2113 template<typename T>
nuclear@0	2114 class SymMat3;
nuclear@0	2115
nuclear@0	2116 template<class T>
nuclear@0	2117 class Matrix3
nuclear@0	2118 {
nuclear@0	2119 static const Matrix3 IdentityValue;
nuclear@0	2120
nuclear@0	2121 public:
nuclear@0	2122 T M[3][3];
nuclear@0	2123
nuclear@0	2124 enum NoInitType { NoInit };
nuclear@0	2125
nuclear@0	2126 // Construct with no memory initialization.
nuclear@0	2127 Matrix3(NoInitType) { }
nuclear@0	2128
nuclear@0	2129 // By default, we construct identity matrix.
nuclear@0	2130 Matrix3()
nuclear@0	2131 {
nuclear@0	2132 SetIdentity();
nuclear@0	2133 }
nuclear@0	2134
nuclear@0	2135 Matrix3(T m11, T m12, T m13,
nuclear@0	2136 T m21, T m22, T m23,
nuclear@0	2137 T m31, T m32, T m33)
nuclear@0	2138 {
nuclear@0	2139 M[0][0] = m11; M[0][1] = m12; M[0][2] = m13;
nuclear@0	2140 M[1][0] = m21; M[1][1] = m22; M[1][2] = m23;
nuclear@0	2141 M[2][0] = m31; M[2][1] = m32; M[2][2] = m33;
nuclear@0	2142 }
nuclear@0	2143
nuclear@0	2144 /*
nuclear@0	2145 explicit Matrix3(const Quat<T>& q)
nuclear@0	2146 {
nuclear@0	2147 T ww = q.w*q.w;
nuclear@0	2148 T xx = q.x*q.x;
nuclear@0	2149 T yy = q.y*q.y;
nuclear@0	2150 T zz = q.z*q.z;
nuclear@0	2151
nuclear@0	2152 M[0][0] = ww + xx - yy - zz; M[0][1] = 2 * (q.xq.y - q.wq.z); M[0][2] = 2 * (q.xq.z + q.wq.y);
nuclear@0	2153 M[1][0] = 2 * (q.xq.y + q.wq.z); M[1][1] = ww - xx + yy - zz; M[1][2] = 2 * (q.yq.z - q.wq.x);
nuclear@0	2154 M[2][0] = 2 * (q.xq.z - q.wq.y); M[2][1] = 2 * (q.yq.z + q.wq.x); M[2][2] = ww - xx - yy + zz;
nuclear@0	2155 }
nuclear@0	2156 */
nuclear@0	2157
nuclear@0	2158 explicit Matrix3(const Quat<T>& q)
nuclear@0	2159 {
nuclear@0	2160 const T tx = q.x+q.x, ty = q.y+q.y, tz = q.z+q.z;
nuclear@0	2161 const T twx = q.wtx, twy = q.wty, twz = q.w*tz;
nuclear@0	2162 const T txx = q.xtx, txy = q.xty, txz = q.x*tz;
nuclear@0	2163 const T tyy = q.yty, tyz = q.ytz, tzz = q.z*tz;
nuclear@0	2164 M[0][0] = T(1) - (tyy + tzz); M[0][1] = txy - twz; M[0][2] = txz + twy;
nuclear@0	2165 M[1][0] = txy + twz; M[1][1] = T(1) - (txx + tzz); M[1][2] = tyz - twx;
nuclear@0	2166 M[2][0] = txz - twy; M[2][1] = tyz + twx; M[2][2] = T(1) - (txx + tyy);
nuclear@0	2167 }
nuclear@0	2168
nuclear@0	2169 inline explicit Matrix3(T s)
nuclear@0	2170 {
nuclear@0	2171 M[0][0] = M[1][1] = M[2][2] = s;
nuclear@0	2172 M[0][1] = M[0][2] = M[1][0] = M[1][2] = M[2][0] = M[2][1] = 0;
nuclear@0	2173 }
nuclear@0	2174
nuclear@0	2175 explicit Matrix3(const Pose<T>& p)
nuclear@0	2176 {
nuclear@0	2177 Matrix3 result(p.Rotation);
nuclear@0	2178 result.SetTranslation(p.Translation);
nuclear@0	2179 *this = result;
nuclear@0	2180 }
nuclear@0	2181
nuclear@0	2182 // C-interop support
nuclear@0	2183 explicit Matrix3(const Matrix4<typename Math<T>::OtherFloatType> &src)
nuclear@0	2184 {
nuclear@0	2185 for (int i = 0; i < 3; i++)
nuclear@0	2186 for (int j = 0; j < 3; j++)
nuclear@0	2187 M[i][j] = (T)src.M[i][j];
nuclear@0	2188 }
nuclear@0	2189
nuclear@0	2190 // C-interop support.
nuclear@0	2191 Matrix3(const typename CompatibleTypes<Matrix3<T> >::Type& s)
nuclear@0	2192 {
nuclear@0	2193 static_assert(sizeof(s) == sizeof(Matrix3), "sizeof(s) == sizeof(Matrix3)");
nuclear@0	2194 memcpy(M, s.M, sizeof(M));
nuclear@0	2195 }
nuclear@0	2196
nuclear@0	2197 operator const typename CompatibleTypes<Matrix3<T> >::Type () const
nuclear@0	2198 {
nuclear@0	2199 typename CompatibleTypes<Matrix3<T> >::Type result;
nuclear@0	2200 static_assert(sizeof(result) == sizeof(Matrix3), "sizeof(result) == sizeof(Matrix3)");
nuclear@0	2201 memcpy(result.M, M, sizeof(M));
nuclear@0	2202 return result;
nuclear@0	2203 }
nuclear@0	2204
nuclear@0	2205 void ToString(char* dest, size_t destsize) const
nuclear@0	2206 {
nuclear@0	2207 size_t pos = 0;
nuclear@0	2208 for (int r=0; r<3; r++)
nuclear@0	2209 for (int c=0; c<3; c++)
nuclear@0	2210 pos += OVR_sprintf(dest+pos, destsize-pos, "%g ", M[r][c]);
nuclear@0	2211 }
nuclear@0	2212
nuclear@0	2213 static Matrix3 FromString(const char* src)
nuclear@0	2214 {
nuclear@0	2215 Matrix3 result;
nuclear@0	2216 for (int r=0; r<3; r++)
nuclear@0	2217 for (int c=0; c<3; c++)
nuclear@0	2218 {
nuclear@0	2219 result.M[r][c] = (T)atof(src);
nuclear@0	2220 while (src && *src != ' ')
nuclear@0	2221 src++;
nuclear@0	2222 while (src && *src == ' ')
nuclear@0	2223 src++;
nuclear@0	2224 }
nuclear@0	2225 return result;
nuclear@0	2226 }
nuclear@0	2227
nuclear@0	2228 static const Matrix3& Identity() { return IdentityValue; }
nuclear@0	2229
nuclear@0	2230 void SetIdentity()
nuclear@0	2231 {
nuclear@0	2232 M[0][0] = M[1][1] = M[2][2] = 1;
nuclear@0	2233 M[0][1] = M[1][0] = M[2][0] = 0;
nuclear@0	2234 M[0][2] = M[1][2] = M[2][1] = 0;
nuclear@0	2235 }
nuclear@0	2236
nuclear@0	2237 bool operator== (const Matrix3& b) const
nuclear@0	2238 {
nuclear@0	2239 bool isEqual = true;
nuclear@0	2240 for (int i = 0; i < 3; i++)
nuclear@0	2241 for (int j = 0; j < 3; j++)
nuclear@0	2242 isEqual &= (M[i][j] == b.M[i][j]);
nuclear@0	2243
nuclear@0	2244 return isEqual;
nuclear@0	2245 }
nuclear@0	2246
nuclear@0	2247 Matrix3 operator+ (const Matrix3& b) const
nuclear@0	2248 {
nuclear@0	2249 Matrix4<T> result(*this);
nuclear@0	2250 result += b;
nuclear@0	2251 return result;
nuclear@0	2252 }
nuclear@0	2253
nuclear@0	2254 Matrix3& operator+= (const Matrix3& b)
nuclear@0	2255 {
nuclear@0	2256 for (int i = 0; i < 3; i++)
nuclear@0	2257 for (int j = 0; j < 3; j++)
nuclear@0	2258 M[i][j] += b.M[i][j];
nuclear@0	2259 return *this;
nuclear@0	2260 }
nuclear@0	2261
nuclear@0	2262 void operator= (const Matrix3& b)
nuclear@0	2263 {
nuclear@0	2264 for (int i = 0; i < 3; i++)
nuclear@0	2265 for (int j = 0; j < 3; j++)
nuclear@0	2266 M[i][j] = b.M[i][j];
nuclear@0	2267 return;
nuclear@0	2268 }
nuclear@0	2269
nuclear@0	2270 void operator= (const SymMat3<T>& b)
nuclear@0	2271 {
nuclear@0	2272 for (int i = 0; i < 3; i++)
nuclear@0	2273 for (int j = 0; j < 3; j++)
nuclear@0	2274 M[i][j] = 0;
nuclear@0	2275
nuclear@0	2276 M[0][0] = b.v[0];
nuclear@0	2277 M[0][1] = b.v[1];
nuclear@0	2278 M[0][2] = b.v[2];
nuclear@0	2279 M[1][1] = b.v[3];
nuclear@0	2280 M[1][2] = b.v[4];
nuclear@0	2281 M[2][2] = b.v[5];
nuclear@0	2282
nuclear@0	2283 return;
nuclear@0	2284 }
nuclear@0	2285
nuclear@0	2286 Matrix3 operator- (const Matrix3& b) const
nuclear@0	2287 {
nuclear@0	2288 Matrix3 result(*this);
nuclear@0	2289 result -= b;
nuclear@0	2290 return result;
nuclear@0	2291 }
nuclear@0	2292
nuclear@0	2293 Matrix3& operator-= (const Matrix3& b)
nuclear@0	2294 {
nuclear@0	2295 for (int i = 0; i < 3; i++)
nuclear@0	2296 for (int j = 0; j < 3; j++)
nuclear@0	2297 M[i][j] -= b.M[i][j];
nuclear@0	2298 return *this;
nuclear@0	2299 }
nuclear@0	2300
nuclear@0	2301 // Multiplies two matrices into destination with minimum copying.
nuclear@0	2302 static Matrix3& Multiply(Matrix3* d, const Matrix3& a, const Matrix3& b)
nuclear@0	2303 {
nuclear@0	2304 OVR_ASSERT((d != &a) && (d != &b));
nuclear@0	2305 int i = 0;
nuclear@0	2306 do {
nuclear@0	2307 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];
nuclear@0	2308 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];
nuclear@0	2309 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];
nuclear@0	2310 } while((++i) < 3);
nuclear@0	2311
nuclear@0	2312 return *d;
nuclear@0	2313 }
nuclear@0	2314
nuclear@0	2315 Matrix3 operator* (const Matrix3& b) const
nuclear@0	2316 {
nuclear@0	2317 Matrix3 result(Matrix3::NoInit);
nuclear@0	2318 Multiply(&result, *this, b);
nuclear@0	2319 return result;
nuclear@0	2320 }
nuclear@0	2321
nuclear@0	2322 Matrix3& operator*= (const Matrix3& b)
nuclear@0	2323 {
nuclear@0	2324 return Multiply(this, Matrix3(*this), b);
nuclear@0	2325 }
nuclear@0	2326
nuclear@0	2327 Matrix3 operator* (T s) const
nuclear@0	2328 {
nuclear@0	2329 Matrix3 result(*this);
nuclear@0	2330 result *= s;
nuclear@0	2331 return result;
nuclear@0	2332 }
nuclear@0	2333
nuclear@0	2334 Matrix3& operator*= (T s)
nuclear@0	2335 {
nuclear@0	2336 for (int i = 0; i < 3; i++)
nuclear@0	2337 for (int j = 0; j < 3; j++)
nuclear@0	2338 M[i][j] *= s;
nuclear@0	2339 return *this;
nuclear@0	2340 }
nuclear@0	2341
nuclear@0	2342 Vector3<T> operator* (const Vector3<T> &b) const
nuclear@0	2343 {
nuclear@0	2344 Vector3<T> result;
nuclear@0	2345 result.x = M[0][0]b.x + M[0][1]b.y + M[0][2]*b.z;
nuclear@0	2346 result.y = M[1][0]b.x + M[1][1]b.y + M[1][2]*b.z;
nuclear@0	2347 result.z = M[2][0]b.x + M[2][1]b.y + M[2][2]*b.z;
nuclear@0	2348
nuclear@0	2349 return result;
nuclear@0	2350 }
nuclear@0	2351
nuclear@0	2352 Matrix3 operator/ (T s) const
nuclear@0	2353 {
nuclear@0	2354 Matrix3 result(*this);
nuclear@0	2355 result /= s;
nuclear@0	2356 return result;
nuclear@0	2357 }
nuclear@0	2358
nuclear@0	2359 Matrix3& operator/= (T s)
nuclear@0	2360 {
nuclear@0	2361 for (int i = 0; i < 3; i++)
nuclear@0	2362 for (int j = 0; j < 3; j++)
nuclear@0	2363 M[i][j] /= s;
nuclear@0	2364 return *this;
nuclear@0	2365 }
nuclear@0	2366
nuclear@0	2367 Vector2<T> Transform(const Vector2<T>& v) const
nuclear@0	2368 {
nuclear@0	2369 const float rcpZ = 1.0f / (M[2][0] * v.x + M[2][1] * v.y + M[2][2]);
nuclear@0	2370 return Vector2<T>((M[0][0] * v.x + M[0][1] * v.y + M[0][2]) * rcpZ,
nuclear@0	2371 (M[1][0] * v.x + M[1][1] * v.y + M[1][2]) * rcpZ);
nuclear@0	2372 }
nuclear@0	2373
nuclear@0	2374 Vector3<T> Transform(const Vector3<T>& v) const
nuclear@0	2375 {
nuclear@0	2376 return Vector3<T>(M[0][0] * v.x + M[0][1] * v.y + M[0][2] * v.z,
nuclear@0	2377 M[1][0] * v.x + M[1][1] * v.y + M[1][2] * v.z,
nuclear@0	2378 M[2][0] * v.x + M[2][1] * v.y + M[2][2] * v.z);
nuclear@0	2379 }
nuclear@0	2380
nuclear@0	2381 Matrix3 Transposed() const
nuclear@0	2382 {
nuclear@0	2383 return Matrix3(M[0][0], M[1][0], M[2][0],
nuclear@0	2384 M[0][1], M[1][1], M[2][1],
nuclear@0	2385 M[0][2], M[1][2], M[2][2]);
nuclear@0	2386 }
nuclear@0	2387
nuclear@0	2388 void Transpose()
nuclear@0	2389 {
nuclear@0	2390 *this = Transposed();
nuclear@0	2391 }
nuclear@0	2392
nuclear@0	2393
nuclear@0	2394 T SubDet (const size_t* rows, const size_t* cols) const
nuclear@0	2395 {
nuclear@0	2396 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@0	2397 - 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@0	2398 + 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@0	2399 }
nuclear@0	2400
nuclear@0	2401 // M += a*b.t()
nuclear@0	2402 inline void Rank1Add(const Vector3<T> &a, const Vector3<T> &b)
nuclear@0	2403 {
nuclear@0	2404 M[0][0] += a.xb.x; M[0][1] += a.xb.y; M[0][2] += a.x*b.z;
nuclear@0	2405 M[1][0] += a.yb.x; M[1][1] += a.yb.y; M[1][2] += a.y*b.z;
nuclear@0	2406 M[2][0] += a.zb.x; M[2][1] += a.zb.y; M[2][2] += a.z*b.z;
nuclear@0	2407 }
nuclear@0	2408
nuclear@0	2409 // M -= a*b.t()
nuclear@0	2410 inline void Rank1Sub(const Vector3<T> &a, const Vector3<T> &b)
nuclear@0	2411 {
nuclear@0	2412 M[0][0] -= a.xb.x; M[0][1] -= a.xb.y; M[0][2] -= a.x*b.z;
nuclear@0	2413 M[1][0] -= a.yb.x; M[1][1] -= a.yb.y; M[1][2] -= a.y*b.z;
nuclear@0	2414 M[2][0] -= a.zb.x; M[2][1] -= a.zb.y; M[2][2] -= a.z*b.z;
nuclear@0	2415 }
nuclear@0	2416
nuclear@0	2417 inline Vector3<T> Col(int c) const
nuclear@0	2418 {
nuclear@0	2419 return Vector3<T>(M[0][c], M[1][c], M[2][c]);
nuclear@0	2420 }
nuclear@0	2421
nuclear@0	2422 inline Vector3<T> Row(int r) const
nuclear@0	2423 {
nuclear@0	2424 return Vector3<T>(M[r][0], M[r][1], M[r][2]);
nuclear@0	2425 }
nuclear@0	2426
nuclear@0	2427 inline T Determinant() const
nuclear@0	2428 {
nuclear@0	2429 const Matrix3<T>& m = *this;
nuclear@0	2430 T d;
nuclear@0	2431
nuclear@0	2432 d = m.M[0][0] * (m.M[1][1]m.M[2][2] - m.M[1][2] m.M[2][1]);
nuclear@0	2433 d -= m.M[0][1] * (m.M[1][0]m.M[2][2] - m.M[1][2] m.M[2][0]);
nuclear@0	2434 d += m.M[0][2] * (m.M[1][0]m.M[2][1] - m.M[1][1] m.M[2][0]);
nuclear@0	2435
nuclear@0	2436 return d;
nuclear@0	2437 }
nuclear@0	2438
nuclear@0	2439 inline Matrix3<T> Inverse() const
nuclear@0	2440 {
nuclear@0	2441 Matrix3<T> a;
nuclear@0	2442 const Matrix3<T>& m = *this;
nuclear@0	2443 T d = Determinant();
nuclear@0	2444
nuclear@0	2445 assert(d != 0);
nuclear@0	2446 T s = T(1)/d;
nuclear@0	2447
nuclear@0	2448 a.M[0][0] = s * (m.M[1][1] * m.M[2][2] - m.M[1][2] * m.M[2][1]);
nuclear@0	2449 a.M[1][0] = s * (m.M[1][2] * m.M[2][0] - m.M[1][0] * m.M[2][2]);
nuclear@0	2450 a.M[2][0] = s * (m.M[1][0] * m.M[2][1] - m.M[1][1] * m.M[2][0]);
nuclear@0	2451
nuclear@0	2452 a.M[0][1] = s * (m.M[0][2] * m.M[2][1] - m.M[0][1] * m.M[2][2]);
nuclear@0	2453 a.M[1][1] = s * (m.M[0][0] * m.M[2][2] - m.M[0][2] * m.M[2][0]);
nuclear@0	2454 a.M[2][1] = s * (m.M[0][1] * m.M[2][0] - m.M[0][0] * m.M[2][1]);
nuclear@0	2455
nuclear@0	2456 a.M[0][2] = s * (m.M[0][1] * m.M[1][2] - m.M[0][2] * m.M[1][1]);
nuclear@0	2457 a.M[1][2] = s * (m.M[0][2] * m.M[1][0] - m.M[0][0] * m.M[1][2]);
nuclear@0	2458 a.M[2][2] = s * (m.M[0][0] * m.M[1][1] - m.M[0][1] * m.M[1][0]);
nuclear@0	2459
nuclear@0	2460 return a;
nuclear@0	2461 }
nuclear@0	2462
nuclear@0	2463 };
nuclear@0	2464
nuclear@0	2465 typedef Matrix3<float> Matrix3f;
nuclear@0	2466 typedef Matrix3<double> Matrix3d;
nuclear@0	2467
nuclear@0	2468 //-------------------------------------------------------------------------------------
nuclear@0	2469
nuclear@0	2470 template<typename T>
nuclear@0	2471 class SymMat3
nuclear@0	2472 {
nuclear@0	2473 private:
nuclear@0	2474 typedef SymMat3<T> this_type;
nuclear@0	2475
nuclear@0	2476 public:
nuclear@0	2477 typedef T Value_t;
nuclear@0	2478 // Upper symmetric
nuclear@0	2479 T v[6]; // _00 _01 _02 _11 _12 _22
nuclear@0	2480
nuclear@0	2481 inline SymMat3() {}
nuclear@0	2482
nuclear@0	2483 inline explicit SymMat3(T s)
nuclear@0	2484 {
nuclear@0	2485 v[0] = v[3] = v[5] = s;
nuclear@0	2486 v[1] = v[2] = v[4] = 0;
nuclear@0	2487 }
nuclear@0	2488
nuclear@0	2489 inline explicit SymMat3(T a00, T a01, T a02, T a11, T a12, T a22)
nuclear@0	2490 {
nuclear@0	2491 v[0] = a00; v[1] = a01; v[2] = a02;
nuclear@0	2492 v[3] = a11; v[4] = a12;
nuclear@0	2493 v[5] = a22;
nuclear@0	2494 }
nuclear@0	2495
nuclear@0	2496 static inline int Index(unsigned int i, unsigned int j)
nuclear@0	2497 {
nuclear@0	2498 return (i <= j) ? (3i - i(i+1)/2 + j) : (3j - j(j+1)/2 + i);
nuclear@0	2499 }
nuclear@0	2500
nuclear@0	2501 inline T operator()(int i, int j) const { return v[Index(i,j)]; }
nuclear@0	2502
nuclear@0	2503 inline T &operator()(int i, int j) { return v[Index(i,j)]; }
nuclear@0	2504
nuclear@0	2505 template<typename U>
nuclear@0	2506 inline SymMat3<U> CastTo() const
nuclear@0	2507 {
nuclear@0	2508 return SymMat3<U>(static_cast<U>(v[0]), static_cast<U>(v[1]), static_cast<U>(v[2]),
nuclear@0	2509 static_cast<U>(v[3]), static_cast<U>(v[4]), static_cast<U>(v[5]));
nuclear@0	2510 }
nuclear@0	2511
nuclear@0	2512 inline this_type& operator+=(const this_type& b)
nuclear@0	2513 {
nuclear@0	2514 v[0]+=b.v[0];
nuclear@0	2515 v[1]+=b.v[1];
nuclear@0	2516 v[2]+=b.v[2];
nuclear@0	2517 v[3]+=b.v[3];
nuclear@0	2518 v[4]+=b.v[4];
nuclear@0	2519 v[5]+=b.v[5];
nuclear@0	2520 return *this;
nuclear@0	2521 }
nuclear@0	2522
nuclear@0	2523 inline this_type& operator-=(const this_type& b)
nuclear@0	2524 {
nuclear@0	2525 v[0]-=b.v[0];
nuclear@0	2526 v[1]-=b.v[1];
nuclear@0	2527 v[2]-=b.v[2];
nuclear@0	2528 v[3]-=b.v[3];
nuclear@0	2529 v[4]-=b.v[4];
nuclear@0	2530 v[5]-=b.v[5];
nuclear@0	2531
nuclear@0	2532 return *this;
nuclear@0	2533 }
nuclear@0	2534
nuclear@0	2535 inline this_type& operator*=(T s)
nuclear@0	2536 {
nuclear@0	2537 v[0]*=s;
nuclear@0	2538 v[1]*=s;
nuclear@0	2539 v[2]*=s;
nuclear@0	2540 v[3]*=s;
nuclear@0	2541 v[4]*=s;
nuclear@0	2542 v[5]*=s;
nuclear@0	2543
nuclear@0	2544 return *this;
nuclear@0	2545 }
nuclear@0	2546
nuclear@0	2547 inline SymMat3 operator*(T s) const
nuclear@0	2548 {
nuclear@0	2549 SymMat3 d;
nuclear@0	2550 d.v[0] = v[0]*s;
nuclear@0	2551 d.v[1] = v[1]*s;
nuclear@0	2552 d.v[2] = v[2]*s;
nuclear@0	2553 d.v[3] = v[3]*s;
nuclear@0	2554 d.v[4] = v[4]*s;
nuclear@0	2555 d.v[5] = v[5]*s;
nuclear@0	2556
nuclear@0	2557 return d;
nuclear@0	2558 }
nuclear@0	2559
nuclear@0	2560 // Multiplies two matrices into destination with minimum copying.
nuclear@0	2561 static SymMat3& Multiply(SymMat3* d, const SymMat3& a, const SymMat3& b)
nuclear@0	2562 {
nuclear@0	2563 // _00 _01 _02 _11 _12 _22
nuclear@0	2564
nuclear@0	2565 d->v[0] = a.v[0] * b.v[0];
nuclear@0	2566 d->v[1] = a.v[0] * b.v[1] + a.v[1] * b.v[3];
nuclear@0	2567 d->v[2] = a.v[0] * b.v[2] + a.v[1] * b.v[4];
nuclear@0	2568
nuclear@0	2569 d->v[3] = a.v[3] * b.v[3];
nuclear@0	2570 d->v[4] = a.v[3] * b.v[4] + a.v[4] * b.v[5];
nuclear@0	2571
nuclear@0	2572 d->v[5] = a.v[5] * b.v[5];
nuclear@0	2573
nuclear@0	2574 return *d;
nuclear@0	2575 }
nuclear@0	2576
nuclear@0	2577 inline T Determinant() const
nuclear@0	2578 {
nuclear@0	2579 const this_type& m = *this;
nuclear@0	2580 T d;
nuclear@0	2581
nuclear@0	2582 d = m(0,0) * (m(1,1)m(2,2) - m(1,2) m(2,1));
nuclear@0	2583 d -= m(0,1) * (m(1,0)m(2,2) - m(1,2) m(2,0));
nuclear@0	2584 d += m(0,2) * (m(1,0)m(2,1) - m(1,1) m(2,0));
nuclear@0	2585
nuclear@0	2586 return d;
nuclear@0	2587 }
nuclear@0	2588
nuclear@0	2589 inline this_type Inverse() const
nuclear@0	2590 {
nuclear@0	2591 this_type a;
nuclear@0	2592 const this_type& m = *this;
nuclear@0	2593 T d = Determinant();
nuclear@0	2594
nuclear@0	2595 assert(d != 0);
nuclear@0	2596 T s = T(1)/d;
nuclear@0	2597
nuclear@0	2598 a(0,0) = s * (m(1,1) * m(2,2) - m(1,2) * m(2,1));
nuclear@0	2599
nuclear@0	2600 a(0,1) = s * (m(0,2) * m(2,1) - m(0,1) * m(2,2));
nuclear@0	2601 a(1,1) = s * (m(0,0) * m(2,2) - m(0,2) * m(2,0));
nuclear@0	2602
nuclear@0	2603 a(0,2) = s * (m(0,1) * m(1,2) - m(0,2) * m(1,1));
nuclear@0	2604 a(1,2) = s * (m(0,2) * m(1,0) - m(0,0) * m(1,2));
nuclear@0	2605 a(2,2) = s * (m(0,0) * m(1,1) - m(0,1) * m(1,0));
nuclear@0	2606
nuclear@0	2607 return a;
nuclear@0	2608 }
nuclear@0	2609
nuclear@0	2610 inline T Trace() const { return v[0] + v[3] + v[5]; }
nuclear@0	2611
nuclear@0	2612 // M = a*a.t()
nuclear@0	2613 inline void Rank1(const Vector3<T> &a)
nuclear@0	2614 {
nuclear@0	2615 v[0] = a.xa.x; v[1] = a.xa.y; v[2] = a.x*a.z;
nuclear@0	2616 v[3] = a.ya.y; v[4] = a.ya.z;
nuclear@0	2617 v[5] = a.z*a.z;
nuclear@0	2618 }
nuclear@0	2619
nuclear@0	2620 // M += a*a.t()
nuclear@0	2621 inline void Rank1Add(const Vector3<T> &a)
nuclear@0	2622 {
nuclear@0	2623 v[0] += a.xa.x; v[1] += a.xa.y; v[2] += a.x*a.z;
nuclear@0	2624 v[3] += a.ya.y; v[4] += a.ya.z;
nuclear@0	2625 v[5] += a.z*a.z;
nuclear@0	2626 }
nuclear@0	2627
nuclear@0	2628 // M -= a*a.t()
nuclear@0	2629 inline void Rank1Sub(const Vector3<T> &a)
nuclear@0	2630 {
nuclear@0	2631 v[0] -= a.xa.x; v[1] -= a.xa.y; v[2] -= a.x*a.z;
nuclear@0	2632 v[3] -= a.ya.y; v[4] -= a.ya.z;
nuclear@0	2633 v[5] -= a.z*a.z;
nuclear@0	2634 }
nuclear@0	2635 };
nuclear@0	2636
nuclear@0	2637 typedef SymMat3<float> SymMat3f;
nuclear@0	2638 typedef SymMat3<double> SymMat3d;
nuclear@0	2639
nuclear@0	2640 template<typename T>
nuclear@0	2641 inline Matrix3<T> operator*(const SymMat3<T>& a, const SymMat3<T>& b)
nuclear@0	2642 {
nuclear@0	2643 #define AJB_ARBC(r,c) (a(r,0)b(0,c)+a(r,1)b(1,c)+a(r,2)*b(2,c))
nuclear@0	2644 return Matrix3<T>(
nuclear@0	2645 AJB_ARBC(0,0), AJB_ARBC(0,1), AJB_ARBC(0,2),
nuclear@0	2646 AJB_ARBC(1,0), AJB_ARBC(1,1), AJB_ARBC(1,2),
nuclear@0	2647 AJB_ARBC(2,0), AJB_ARBC(2,1), AJB_ARBC(2,2));
nuclear@0	2648 #undef AJB_ARBC
nuclear@0	2649 }
nuclear@0	2650
nuclear@0	2651 template<typename T>
nuclear@0	2652 inline Matrix3<T> operator*(const Matrix3<T>& a, const SymMat3<T>& b)
nuclear@0	2653 {
nuclear@0	2654 #define AJB_ARBC(r,c) (a(r,0)b(0,c)+a(r,1)b(1,c)+a(r,2)*b(2,c))
nuclear@0	2655 return Matrix3<T>(
nuclear@0	2656 AJB_ARBC(0,0), AJB_ARBC(0,1), AJB_ARBC(0,2),
nuclear@0	2657 AJB_ARBC(1,0), AJB_ARBC(1,1), AJB_ARBC(1,2),
nuclear@0	2658 AJB_ARBC(2,0), AJB_ARBC(2,1), AJB_ARBC(2,2));
nuclear@0	2659 #undef AJB_ARBC
nuclear@0	2660 }
nuclear@0	2661
nuclear@0	2662 //-------------------------------------------------------------------------------------
nuclear@0	2663 // ***** Angle
nuclear@0	2664
nuclear@0	2665 // Cleanly representing the algebra of 2D rotations.
nuclear@0	2666 // The operations maintain the angle between -Pi and Pi, the same range as atan2.
nuclear@0	2667
nuclear@0	2668 template<class T>
nuclear@0	2669 class Angle
nuclear@0	2670 {
nuclear@0	2671 public:
nuclear@0	2672 enum AngularUnits
nuclear@0	2673 {
nuclear@0	2674 Radians = 0,
nuclear@0	2675 Degrees = 1
nuclear@0	2676 };
nuclear@0	2677
nuclear@0	2678 Angle() : a(0) {}
nuclear@0	2679
nuclear@0	2680 // Fix the range to be between -Pi and Pi
nuclear@0	2681 Angle(T a_, AngularUnits u = Radians) : a((u == Radians) ? a_ : a_*((T)MATH_DOUBLE_DEGREETORADFACTOR)) { FixRange(); }
nuclear@0	2682
nuclear@0	2683 T Get(AngularUnits u = Radians) const { return (u == Radians) ? a : a*((T)MATH_DOUBLE_RADTODEGREEFACTOR); }
nuclear@0	2684 void Set(const T& x, AngularUnits u = Radians) { a = (u == Radians) ? x : x*((T)MATH_DOUBLE_DEGREETORADFACTOR); FixRange(); }
nuclear@0	2685 int Sign() const { if (a == 0) return 0; else return (a > 0) ? 1 : -1; }
nuclear@0	2686 T Abs() const { return (a > 0) ? a : -a; }
nuclear@0	2687
nuclear@0	2688 bool operator== (const Angle& b) const { return a == b.a; }
nuclear@0	2689 bool operator!= (const Angle& b) const { return a != b.a; }
nuclear@0	2690 // bool operator< (const Angle& b) const { return a < a.b; }
nuclear@0	2691 // bool operator> (const Angle& b) const { return a > a.b; }
nuclear@0	2692 // bool operator<= (const Angle& b) const { return a <= a.b; }
nuclear@0	2693 // bool operator>= (const Angle& b) const { return a >= a.b; }
nuclear@0	2694 // bool operator= (const T& x) { a = x; FixRange(); }
nuclear@0	2695
nuclear@0	2696 // These operations assume a is already between -Pi and Pi.
nuclear@0	2697 Angle& operator+= (const Angle& b) { a = a + b.a; FastFixRange(); return *this; }
nuclear@0	2698 Angle& operator+= (const T& x) { a = a + x; FixRange(); return *this; }
nuclear@0	2699 Angle operator+ (const Angle& b) const { Angle res = *this; res += b; return res; }
nuclear@0	2700 Angle operator+ (const T& x) const { Angle res = *this; res += x; return res; }
nuclear@0	2701 Angle& operator-= (const Angle& b) { a = a - b.a; FastFixRange(); return *this; }
nuclear@0	2702 Angle& operator-= (const T& x) { a = a - x; FixRange(); return *this; }
nuclear@0	2703 Angle operator- (const Angle& b) const { Angle res = *this; res -= b; return res; }
nuclear@0	2704 Angle operator- (const T& x) const { Angle res = *this; res -= x; return res; }
nuclear@0	2705
nuclear@0	2706 T Distance(const Angle& b) { T c = fabs(a - b.a); return (c <= ((T)MATH_DOUBLE_PI)) ? c : ((T)MATH_DOUBLE_TWOPI) - c; }
nuclear@0	2707
nuclear@0	2708 private:
nuclear@0	2709
nuclear@0	2710 // The stored angle, which should be maintained between -Pi and Pi
nuclear@0	2711 T a;
nuclear@0	2712
nuclear@0	2713 // Fixes the angle range to [-Pi,Pi], but assumes no more than 2Pi away on either side
nuclear@0	2714 inline void FastFixRange()
nuclear@0	2715 {
nuclear@0	2716 if (a < -((T)MATH_DOUBLE_PI))
nuclear@0	2717 a += ((T)MATH_DOUBLE_TWOPI);
nuclear@0	2718 else if (a > ((T)MATH_DOUBLE_PI))
nuclear@0	2719 a -= ((T)MATH_DOUBLE_TWOPI);
nuclear@0	2720 }
nuclear@0	2721
nuclear@0	2722 // Fixes the angle range to [-Pi,Pi] for any given range, but slower then the fast method
nuclear@0	2723 inline void FixRange()
nuclear@0	2724 {
nuclear@0	2725 // do nothing if the value is already in the correct range, since fmod call is expensive
nuclear@0	2726 if (a >= -((T)MATH_DOUBLE_PI) && a <= ((T)MATH_DOUBLE_PI))
nuclear@0	2727 return;
nuclear@0	2728 a = fmod(a,((T)MATH_DOUBLE_TWOPI));
nuclear@0	2729 if (a < -((T)MATH_DOUBLE_PI))
nuclear@0	2730 a += ((T)MATH_DOUBLE_TWOPI);
nuclear@0	2731 else if (a > ((T)MATH_DOUBLE_PI))
nuclear@0	2732 a -= ((T)MATH_DOUBLE_TWOPI);
nuclear@0	2733 }
nuclear@0	2734 };
nuclear@0	2735
nuclear@0	2736
nuclear@0	2737 typedef Angle<float> Anglef;
nuclear@0	2738 typedef Angle<double> Angled;
nuclear@0	2739
nuclear@0	2740
nuclear@0	2741 //-------------------------------------------------------------------------------------
nuclear@0	2742 // ***** Plane
nuclear@0	2743
nuclear@0	2744 // Consists of a normal vector and distance from the origin where the plane is located.
nuclear@0	2745
nuclear@0	2746 template<class T>
nuclear@0	2747 class Plane
nuclear@0	2748 {
nuclear@0	2749 public:
nuclear@0	2750 Vector3<T> N;
nuclear@0	2751 T D;
nuclear@0	2752
nuclear@0	2753 Plane() : D(0) {}
nuclear@0	2754
nuclear@0	2755 // Normals must already be normalized
nuclear@0	2756 Plane(const Vector3<T>& n, T d) : N(n), D(d) {}
nuclear@0	2757 Plane(T x, T y, T z, T d) : N(x,y,z), D(d) {}
nuclear@0	2758
nuclear@0	2759 // construct from a point on the plane and the normal
nuclear@0	2760 Plane(const Vector3<T>& p, const Vector3<T>& n) : N(n), D(-(p * n)) {}
nuclear@0	2761
nuclear@0	2762 // Find the point to plane distance. The sign indicates what side of the plane the point is on (0 = point on plane).
nuclear@0	2763 T TestSide(const Vector3<T>& p) const
nuclear@0	2764 {
nuclear@0	2765 return (N.Dot(p)) + D;
nuclear@0	2766 }
nuclear@0	2767
nuclear@0	2768 Plane<T> Flipped() const
nuclear@0	2769 {
nuclear@0	2770 return Plane(-N, -D);
nuclear@0	2771 }
nuclear@0	2772
nuclear@0	2773 void Flip()
nuclear@0	2774 {
nuclear@0	2775 N = -N;
nuclear@0	2776 D = -D;
nuclear@0	2777 }
nuclear@0	2778
nuclear@0	2779 bool operator==(const Plane<T>& rhs) const
nuclear@0	2780 {
nuclear@0	2781 return (this->D == rhs.D && this->N == rhs.N);
nuclear@0	2782 }
nuclear@0	2783 };
nuclear@0	2784
nuclear@0	2785 typedef Plane<float> Planef;
nuclear@0	2786 typedef Plane<double> Planed;
nuclear@0	2787
nuclear@0	2788
nuclear@0	2789 } // Namespace OVR
nuclear@0	2790
nuclear@0	2791 #endif