nuclear@0: /************************************************************************************ nuclear@0: nuclear@0: PublicHeader: OVR_Kernel.h nuclear@0: Filename : OVR_Math.h nuclear@0: Content : Implementation of 3D primitives such as vectors, matrices. nuclear@0: Created : September 4, 2012 nuclear@0: Authors : Andrew Reisse, Michael Antonov, Steve LaValle, nuclear@0: Anna Yershova, Max Katsev, Dov Katz nuclear@0: nuclear@0: Copyright : Copyright 2014 Oculus VR, LLC All Rights reserved. nuclear@0: nuclear@0: Licensed under the Oculus VR Rift SDK License Version 3.2 (the "License"); nuclear@0: you may not use the Oculus VR Rift SDK except in compliance with the License, nuclear@0: which is provided at the time of installation or download, or which nuclear@0: otherwise accompanies this software in either electronic or hard copy form. nuclear@0: nuclear@0: You may obtain a copy of the License at nuclear@0: nuclear@0: http://www.oculusvr.com/licenses/LICENSE-3.2 nuclear@0: nuclear@0: Unless required by applicable law or agreed to in writing, the Oculus VR SDK nuclear@0: distributed under the License is distributed on an "AS IS" BASIS, nuclear@0: WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. nuclear@0: See the License for the specific language governing permissions and nuclear@0: limitations under the License. nuclear@0: nuclear@0: *************************************************************************************/ nuclear@0: nuclear@0: #ifndef OVR_Math_h nuclear@0: #define OVR_Math_h nuclear@0: nuclear@0: #include nuclear@0: #include nuclear@0: #include nuclear@0: nuclear@0: #include "OVR_Types.h" nuclear@0: #include "OVR_RefCount.h" nuclear@0: #include "OVR_Std.h" nuclear@0: #include "OVR_Alg.h" nuclear@0: nuclear@0: nuclear@0: namespace OVR { nuclear@0: nuclear@0: //------------------------------------------------------------------------------------- nuclear@0: // ***** Constants for 3D world/axis definitions. nuclear@0: nuclear@0: // Definitions of axes for coordinate and rotation conversions. nuclear@0: enum Axis nuclear@0: { nuclear@0: Axis_X = 0, Axis_Y = 1, Axis_Z = 2 nuclear@0: }; nuclear@0: nuclear@0: // RotateDirection describes the rotation direction around an axis, interpreted as follows: nuclear@0: // CW - Clockwise while looking "down" from positive axis towards the origin. nuclear@0: // CCW - Counter-clockwise while looking from the positive axis towards the origin, nuclear@0: // which is in the negative axis direction. nuclear@0: // CCW is the default for the RHS coordinate system. Oculus standard RHS coordinate nuclear@0: // system defines Y up, X right, and Z back (pointing out from the screen). In this nuclear@0: // system Rotate_CCW around Z will specifies counter-clockwise rotation in XY plane. nuclear@0: enum RotateDirection nuclear@0: { nuclear@0: Rotate_CCW = 1, nuclear@0: Rotate_CW = -1 nuclear@0: }; nuclear@0: nuclear@0: // Constants for right handed and left handed coordinate systems nuclear@0: enum HandedSystem nuclear@0: { nuclear@0: Handed_R = 1, Handed_L = -1 nuclear@0: }; nuclear@0: nuclear@0: // AxisDirection describes which way the coordinate axis points. Used by WorldAxes. nuclear@0: enum AxisDirection nuclear@0: { nuclear@0: Axis_Up = 2, nuclear@0: Axis_Down = -2, nuclear@0: Axis_Right = 1, nuclear@0: Axis_Left = -1, nuclear@0: Axis_In = 3, nuclear@0: Axis_Out = -3 nuclear@0: }; nuclear@0: nuclear@0: struct WorldAxes nuclear@0: { nuclear@0: AxisDirection XAxis, YAxis, ZAxis; nuclear@0: nuclear@0: WorldAxes(AxisDirection x, AxisDirection y, AxisDirection z) nuclear@0: : XAxis(x), YAxis(y), ZAxis(z) nuclear@0: { OVR_ASSERT(abs(x) != abs(y) && abs(y) != abs(z) && abs(z) != abs(x));} nuclear@0: }; nuclear@0: nuclear@0: } // namespace OVR nuclear@0: nuclear@0: nuclear@0: //------------------------------------------------------------------------------------// nuclear@0: // ***** C Compatibility Types nuclear@0: nuclear@0: // These declarations are used to support conversion between C types used in nuclear@0: // LibOVR C interfaces and their C++ versions. As an example, they allow passing nuclear@0: // Vector3f into a function that expects ovrVector3f. nuclear@0: nuclear@0: typedef struct ovrQuatf_ ovrQuatf; nuclear@0: typedef struct ovrQuatd_ ovrQuatd; nuclear@0: typedef struct ovrSizei_ ovrSizei; nuclear@0: typedef struct ovrSizef_ ovrSizef; nuclear@0: typedef struct ovrRecti_ ovrRecti; nuclear@0: typedef struct ovrVector2i_ ovrVector2i; nuclear@0: typedef struct ovrVector2f_ ovrVector2f; nuclear@0: typedef struct ovrVector3f_ ovrVector3f; nuclear@0: typedef struct ovrVector3d_ ovrVector3d; nuclear@0: typedef struct ovrMatrix3d_ ovrMatrix3d; nuclear@0: typedef struct ovrMatrix4f_ ovrMatrix4f; nuclear@0: typedef struct ovrPosef_ ovrPosef; nuclear@0: typedef struct ovrPosed_ ovrPosed; nuclear@0: typedef struct ovrPoseStatef_ ovrPoseStatef; nuclear@0: typedef struct ovrPoseStated_ ovrPoseStated; nuclear@0: nuclear@0: namespace OVR { nuclear@0: nuclear@0: // Forward-declare our templates. nuclear@0: template class Quat; nuclear@0: template class Size; nuclear@0: template class Rect; nuclear@0: template class Vector2; nuclear@0: template class Vector3; nuclear@0: template class Matrix3; nuclear@0: template class Matrix4; nuclear@0: template class Pose; nuclear@0: template class PoseState; nuclear@0: nuclear@0: // CompatibleTypes::Type is used to lookup a compatible C-version of a C++ class. nuclear@0: template nuclear@0: struct CompatibleTypes nuclear@0: { nuclear@0: // Declaration here seems necessary for MSVC; specializations are nuclear@0: // used instead. nuclear@0: typedef struct {} Type; nuclear@0: }; nuclear@0: nuclear@0: // Specializations providing CompatibleTypes::Type value. nuclear@0: template<> struct CompatibleTypes > { typedef ovrQuatf Type; }; nuclear@0: template<> struct CompatibleTypes > { typedef ovrQuatd Type; }; nuclear@0: template<> struct CompatibleTypes > { typedef ovrMatrix3d Type; }; nuclear@0: template<> struct CompatibleTypes > { typedef ovrMatrix4f Type; }; nuclear@0: template<> struct CompatibleTypes > { typedef ovrSizei Type; }; nuclear@0: template<> struct CompatibleTypes > { typedef ovrSizef Type; }; nuclear@0: template<> struct CompatibleTypes > { typedef ovrRecti Type; }; nuclear@0: template<> struct CompatibleTypes > { typedef ovrVector2i Type; }; nuclear@0: template<> struct CompatibleTypes > { typedef ovrVector2f Type; }; nuclear@0: template<> struct CompatibleTypes > { typedef ovrVector3f Type; }; nuclear@0: template<> struct CompatibleTypes > { typedef ovrVector3d Type; }; nuclear@0: nuclear@0: template<> struct CompatibleTypes > { typedef ovrPosef Type; }; nuclear@0: template<> struct CompatibleTypes > { typedef ovrPosed Type; }; nuclear@0: nuclear@0: //------------------------------------------------------------------------------------// nuclear@0: // ***** Math nuclear@0: // nuclear@0: // Math class contains constants and functions. This class is a template specialized nuclear@0: // per type, with Math and Math being distinct. nuclear@0: template nuclear@0: class Math nuclear@0: { nuclear@0: public: nuclear@0: // By default, support explicit conversion to float. This allows Vector2 to nuclear@0: // compile, for example. nuclear@0: typedef float OtherFloatType; nuclear@0: }; nuclear@0: nuclear@0: nuclear@0: #define MATH_FLOAT_PI (3.1415926f) nuclear@0: #define MATH_FLOAT_TWOPI (2.0f *MATH_FLOAT_PI) nuclear@0: #define MATH_FLOAT_PIOVER2 (0.5f *MATH_FLOAT_PI) nuclear@0: #define MATH_FLOAT_PIOVER4 (0.25f*MATH_FLOAT_PI) nuclear@0: #define MATH_FLOAT_E (2.7182818f) nuclear@0: #define MATH_FLOAT_MAXVALUE (FLT_MAX) nuclear@0: #define MATH_FLOAT MINPOSITIVEVALUE (FLT_MIN) nuclear@0: #define MATH_FLOAT_RADTODEGREEFACTOR (360.0f / MATH_FLOAT_TWOPI) nuclear@0: #define MATH_FLOAT_DEGREETORADFACTOR (MATH_FLOAT_TWOPI / 360.0f) nuclear@0: #define MATH_FLOAT_TOLERANCE (0.00001f) nuclear@0: #define MATH_FLOAT_SINGULARITYRADIUS (0.0000001f) // Use for Gimbal lock numerical problems nuclear@0: nuclear@0: #define MATH_DOUBLE_PI (3.14159265358979) nuclear@0: #define MATH_DOUBLE_TWOPI (2.0f *MATH_DOUBLE_PI) nuclear@0: #define MATH_DOUBLE_PIOVER2 (0.5f *MATH_DOUBLE_PI) nuclear@0: #define MATH_DOUBLE_PIOVER4 (0.25f*MATH_DOUBLE_PI) nuclear@0: #define MATH_DOUBLE_E (2.71828182845905) nuclear@0: #define MATH_DOUBLE_MAXVALUE (DBL_MAX) nuclear@0: #define MATH_DOUBLE MINPOSITIVEVALUE (DBL_MIN) nuclear@0: #define MATH_DOUBLE_RADTODEGREEFACTOR (360.0f / MATH_DOUBLE_TWOPI) nuclear@0: #define MATH_DOUBLE_DEGREETORADFACTOR (MATH_DOUBLE_TWOPI / 360.0f) nuclear@0: #define MATH_DOUBLE_TOLERANCE (0.00001) nuclear@0: #define MATH_DOUBLE_SINGULARITYRADIUS (0.000000000001) // Use for Gimbal lock numerical problems nuclear@0: nuclear@0: nuclear@0: nuclear@0: nuclear@0: // Single-precision Math constants class. nuclear@0: template<> nuclear@0: class Math nuclear@0: { nuclear@0: public: nuclear@0: typedef double OtherFloatType; nuclear@0: }; nuclear@0: nuclear@0: // Double-precision Math constants class. nuclear@0: template<> nuclear@0: class Math nuclear@0: { nuclear@0: public: nuclear@0: typedef float OtherFloatType; nuclear@0: }; nuclear@0: nuclear@0: nuclear@0: typedef Math Mathf; nuclear@0: typedef Math Mathd; nuclear@0: nuclear@0: // Conversion functions between degrees and radians nuclear@0: template nuclear@0: T RadToDegree(T rads) { return rads * ((T)MATH_DOUBLE_RADTODEGREEFACTOR); } nuclear@0: template nuclear@0: T DegreeToRad(T rads) { return rads * ((T)MATH_DOUBLE_DEGREETORADFACTOR); } nuclear@0: nuclear@0: // Numerically stable acos function nuclear@0: template nuclear@0: T Acos(T val) { nuclear@0: if (val > T(1)) return T(0); nuclear@0: else if (val < T(-1)) return ((T)MATH_DOUBLE_PI); nuclear@0: else return acos(val); nuclear@0: }; nuclear@0: nuclear@0: // Numerically stable asin function nuclear@0: template nuclear@0: T Asin(T val) { nuclear@0: if (val > T(1)) return ((T)MATH_DOUBLE_PIOVER2); nuclear@0: else if (val < T(-1)) return ((T)MATH_DOUBLE_PIOVER2) * T(3); nuclear@0: else return asin(val); nuclear@0: }; nuclear@0: nuclear@0: #ifdef OVR_CC_MSVC nuclear@0: inline int isnan(double x) { return _isnan(x); }; nuclear@0: #endif nuclear@0: nuclear@0: template nuclear@0: class Quat; nuclear@0: nuclear@0: nuclear@0: //------------------------------------------------------------------------------------- nuclear@0: // ***** Vector2<> nuclear@0: nuclear@0: // Vector2f (Vector2d) represents a 2-dimensional vector or point in space, nuclear@0: // consisting of coordinates x and y nuclear@0: nuclear@0: template nuclear@0: class Vector2 nuclear@0: { nuclear@0: public: nuclear@0: T x, y; nuclear@0: nuclear@0: Vector2() : x(0), y(0) { } nuclear@0: Vector2(T x_, T y_) : x(x_), y(y_) { } nuclear@0: explicit Vector2(T s) : x(s), y(s) { } nuclear@0: explicit Vector2(const Vector2::OtherFloatType> &src) nuclear@0: : x((T)src.x), y((T)src.y) { } nuclear@0: nuclear@0: nuclear@0: // C-interop support. nuclear@0: typedef typename CompatibleTypes >::Type CompatibleType; nuclear@0: nuclear@0: Vector2(const CompatibleType& s) : x(s.x), y(s.y) { } nuclear@0: nuclear@0: operator const CompatibleType& () const nuclear@0: { nuclear@0: static_assert(sizeof(Vector2) == sizeof(CompatibleType), "sizeof(Vector2) failure"); nuclear@0: return reinterpret_cast(*this); nuclear@0: } nuclear@0: nuclear@0: nuclear@0: bool operator== (const Vector2& b) const { return x == b.x && y == b.y; } nuclear@0: bool operator!= (const Vector2& b) const { return x != b.x || y != b.y; } nuclear@0: nuclear@0: Vector2 operator+ (const Vector2& b) const { return Vector2(x + b.x, y + b.y); } nuclear@0: Vector2& operator+= (const Vector2& b) { x += b.x; y += b.y; return *this; } nuclear@0: Vector2 operator- (const Vector2& b) const { return Vector2(x - b.x, y - b.y); } nuclear@0: Vector2& operator-= (const Vector2& b) { x -= b.x; y -= b.y; return *this; } nuclear@0: Vector2 operator- () const { return Vector2(-x, -y); } nuclear@0: nuclear@0: // Scalar multiplication/division scales vector. nuclear@0: Vector2 operator* (T s) const { return Vector2(x*s, y*s); } nuclear@0: Vector2& operator*= (T s) { x *= s; y *= s; return *this; } nuclear@0: nuclear@0: Vector2 operator/ (T s) const { T rcp = T(1)/s; nuclear@0: return Vector2(x*rcp, y*rcp); } nuclear@0: Vector2& operator/= (T s) { T rcp = T(1)/s; nuclear@0: x *= rcp; y *= rcp; nuclear@0: return *this; } nuclear@0: nuclear@0: static Vector2 Min(const Vector2& a, const Vector2& b) { return Vector2((a.x < b.x) ? a.x : b.x, nuclear@0: (a.y < b.y) ? a.y : b.y); } nuclear@0: static Vector2 Max(const Vector2& a, const Vector2& b) { return Vector2((a.x > b.x) ? a.x : b.x, nuclear@0: (a.y > b.y) ? a.y : b.y); } nuclear@0: nuclear@0: // Compare two vectors for equality with tolerance. Returns true if vectors match withing tolerance. nuclear@0: bool Compare(const Vector2&b, T tolerance = ((T)MATH_DOUBLE_TOLERANCE)) nuclear@0: { nuclear@0: return (fabs(b.x-x) < tolerance) && (fabs(b.y-y) < tolerance); nuclear@0: } nuclear@0: nuclear@0: // Access element by index nuclear@0: T& operator[] (int idx) nuclear@0: { nuclear@0: OVR_ASSERT(0 <= idx && idx < 2); nuclear@0: return *(&x + idx); nuclear@0: } nuclear@0: const T& operator[] (int idx) const nuclear@0: { nuclear@0: OVR_ASSERT(0 <= idx && idx < 2); nuclear@0: return *(&x + idx); nuclear@0: } nuclear@0: nuclear@0: // Entry-wise product of two vectors nuclear@0: Vector2 EntrywiseMultiply(const Vector2& b) const { return Vector2(x * b.x, y * b.y);} nuclear@0: nuclear@0: nuclear@0: // Multiply and divide operators do entry-wise math. Used Dot() for dot product. nuclear@0: Vector2 operator* (const Vector2& b) const { return Vector2(x * b.x, y * b.y); } nuclear@0: Vector2 operator/ (const Vector2& b) const { return Vector2(x / b.x, y / b.y); } nuclear@0: nuclear@0: // Dot product nuclear@0: // Used to calculate angle q between two vectors among other things, nuclear@0: // as (A dot B) = |a||b|cos(q). nuclear@0: T Dot(const Vector2& b) const { return x*b.x + y*b.y; } nuclear@0: nuclear@0: // Returns the angle from this vector to b, in radians. nuclear@0: T Angle(const Vector2& b) const nuclear@0: { nuclear@0: T div = LengthSq()*b.LengthSq(); nuclear@0: OVR_ASSERT(div != T(0)); nuclear@0: T result = Acos((this->Dot(b))/sqrt(div)); nuclear@0: return result; nuclear@0: } nuclear@0: nuclear@0: // Return Length of the vector squared. nuclear@0: T LengthSq() const { return (x * x + y * y); } nuclear@0: nuclear@0: // Return vector length. nuclear@0: T Length() const { return sqrt(LengthSq()); } nuclear@0: nuclear@0: // Returns squared distance between two points represented by vectors. nuclear@0: T DistanceSq(const Vector2& b) const { return (*this - b).LengthSq(); } nuclear@0: nuclear@0: // Returns distance between two points represented by vectors. nuclear@0: T Distance(const Vector2& b) const { return (*this - b).Length(); } nuclear@0: nuclear@0: // Determine if this a unit vector. nuclear@0: bool IsNormalized() const { return fabs(LengthSq() - T(1)) < ((T)MATH_DOUBLE_TOLERANCE); } nuclear@0: nuclear@0: // Normalize, convention vector length to 1. nuclear@0: void Normalize() nuclear@0: { nuclear@0: T l = Length(); nuclear@0: OVR_ASSERT(l != T(0)); nuclear@0: *this /= l; nuclear@0: } nuclear@0: // Returns normalized (unit) version of the vector without modifying itself. nuclear@0: Vector2 Normalized() const nuclear@0: { nuclear@0: T l = Length(); nuclear@0: OVR_ASSERT(l != T(0)); nuclear@0: return *this / l; nuclear@0: } nuclear@0: nuclear@0: // Linearly interpolates from this vector to another. nuclear@0: // Factor should be between 0.0 and 1.0, with 0 giving full value to this. nuclear@0: Vector2 Lerp(const Vector2& b, T f) const { return *this*(T(1) - f) + b*f; } nuclear@0: nuclear@0: // Projects this vector onto the argument; in other words, nuclear@0: // A.Project(B) returns projection of vector A onto B. nuclear@0: Vector2 ProjectTo(const Vector2& b) const nuclear@0: { nuclear@0: T l2 = b.LengthSq(); nuclear@0: OVR_ASSERT(l2 != T(0)); nuclear@0: return b * ( Dot(b) / l2 ); nuclear@0: } nuclear@0: }; nuclear@0: nuclear@0: nuclear@0: typedef Vector2 Vector2f; nuclear@0: typedef Vector2 Vector2d; nuclear@0: typedef Vector2 Vector2i; nuclear@0: nuclear@0: typedef Vector2 Point2f; nuclear@0: typedef Vector2 Point2d; nuclear@0: typedef Vector2 Point2i; nuclear@0: nuclear@0: //------------------------------------------------------------------------------------- nuclear@0: // ***** Vector3<> - 3D vector of {x, y, z} nuclear@0: nuclear@0: // nuclear@0: // Vector3f (Vector3d) represents a 3-dimensional vector or point in space, nuclear@0: // consisting of coordinates x, y and z. nuclear@0: nuclear@0: template nuclear@0: class Vector3 nuclear@0: { nuclear@0: public: nuclear@0: T x, y, z; nuclear@0: nuclear@0: // FIXME: default initialization of a vector class can be very expensive in a full-blown nuclear@0: // application. A few hundred thousand vector constructions is not unlikely and can add nuclear@0: // up to milliseconds of time on processors like the PS3 PPU. nuclear@0: Vector3() : x(0), y(0), z(0) { } nuclear@0: Vector3(T x_, T y_, T z_ = 0) : x(x_), y(y_), z(z_) { } nuclear@0: explicit Vector3(T s) : x(s), y(s), z(s) { } nuclear@0: explicit Vector3(const Vector3::OtherFloatType> &src) nuclear@0: : x((T)src.x), y((T)src.y), z((T)src.z) { } nuclear@0: nuclear@0: static const Vector3 ZERO; nuclear@0: nuclear@0: // C-interop support. nuclear@0: typedef typename CompatibleTypes >::Type CompatibleType; nuclear@0: nuclear@0: Vector3(const CompatibleType& s) : x(s.x), y(s.y), z(s.z) { } nuclear@0: nuclear@0: operator const CompatibleType& () const nuclear@0: { nuclear@0: static_assert(sizeof(Vector3) == sizeof(CompatibleType), "sizeof(Vector3) failure"); nuclear@0: return reinterpret_cast(*this); nuclear@0: } nuclear@0: nuclear@0: bool operator== (const Vector3& b) const { return x == b.x && y == b.y && z == b.z; } nuclear@0: bool operator!= (const Vector3& b) const { return x != b.x || y != b.y || z != b.z; } nuclear@0: nuclear@0: Vector3 operator+ (const Vector3& b) const { return Vector3(x + b.x, y + b.y, z + b.z); } nuclear@0: Vector3& operator+= (const Vector3& b) { x += b.x; y += b.y; z += b.z; return *this; } nuclear@0: Vector3 operator- (const Vector3& b) const { return Vector3(x - b.x, y - b.y, z - b.z); } nuclear@0: Vector3& operator-= (const Vector3& b) { x -= b.x; y -= b.y; z -= b.z; return *this; } nuclear@0: Vector3 operator- () const { return Vector3(-x, -y, -z); } nuclear@0: nuclear@0: // Scalar multiplication/division scales vector. nuclear@0: Vector3 operator* (T s) const { return Vector3(x*s, y*s, z*s); } nuclear@0: Vector3& operator*= (T s) { x *= s; y *= s; z *= s; return *this; } nuclear@0: nuclear@0: Vector3 operator/ (T s) const { T rcp = T(1)/s; nuclear@0: return Vector3(x*rcp, y*rcp, z*rcp); } nuclear@0: Vector3& operator/= (T s) { T rcp = T(1)/s; nuclear@0: x *= rcp; y *= rcp; z *= rcp; nuclear@0: return *this; } nuclear@0: nuclear@0: static Vector3 Min(const Vector3& a, const Vector3& b) nuclear@0: { nuclear@0: return Vector3((a.x < b.x) ? a.x : b.x, nuclear@0: (a.y < b.y) ? a.y : b.y, nuclear@0: (a.z < b.z) ? a.z : b.z); nuclear@0: } nuclear@0: static Vector3 Max(const Vector3& a, const Vector3& b) nuclear@0: { nuclear@0: return Vector3((a.x > b.x) ? a.x : b.x, nuclear@0: (a.y > b.y) ? a.y : b.y, nuclear@0: (a.z > b.z) ? a.z : b.z); nuclear@0: } nuclear@0: nuclear@0: // Compare two vectors for equality with tolerance. Returns true if vectors match withing tolerance. nuclear@0: bool Compare(const Vector3&b, T tolerance = ((T)MATH_DOUBLE_TOLERANCE)) nuclear@0: { nuclear@0: return (fabs(b.x-x) < tolerance) && nuclear@0: (fabs(b.y-y) < tolerance) && nuclear@0: (fabs(b.z-z) < tolerance); nuclear@0: } nuclear@0: nuclear@0: T& operator[] (int idx) nuclear@0: { nuclear@0: OVR_ASSERT(0 <= idx && idx < 3); nuclear@0: return *(&x + idx); nuclear@0: } nuclear@0: nuclear@0: const T& operator[] (int idx) const nuclear@0: { nuclear@0: OVR_ASSERT(0 <= idx && idx < 3); nuclear@0: return *(&x + idx); nuclear@0: } nuclear@0: nuclear@0: // Entrywise product of two vectors nuclear@0: Vector3 EntrywiseMultiply(const Vector3& b) const { return Vector3(x * b.x, nuclear@0: y * b.y, nuclear@0: z * b.z);} nuclear@0: nuclear@0: // Multiply and divide operators do entry-wise math nuclear@0: Vector3 operator* (const Vector3& b) const { return Vector3(x * b.x, nuclear@0: y * b.y, nuclear@0: z * b.z); } nuclear@0: nuclear@0: Vector3 operator/ (const Vector3& b) const { return Vector3(x / b.x, nuclear@0: y / b.y, nuclear@0: z / b.z); } nuclear@0: nuclear@0: nuclear@0: // Dot product nuclear@0: // Used to calculate angle q between two vectors among other things, nuclear@0: // as (A dot B) = |a||b|cos(q). nuclear@0: T Dot(const Vector3& b) const { return x*b.x + y*b.y + z*b.z; } nuclear@0: nuclear@0: // Compute cross product, which generates a normal vector. nuclear@0: // Direction vector can be determined by right-hand rule: Pointing index finder in nuclear@0: // direction a and middle finger in direction b, thumb will point in a.Cross(b). nuclear@0: Vector3 Cross(const Vector3& b) const { return Vector3(y*b.z - z*b.y, nuclear@0: z*b.x - x*b.z, nuclear@0: x*b.y - y*b.x); } nuclear@0: nuclear@0: // Returns the angle from this vector to b, in radians. nuclear@0: T Angle(const Vector3& b) const nuclear@0: { nuclear@0: T div = LengthSq()*b.LengthSq(); nuclear@0: OVR_ASSERT(div != T(0)); nuclear@0: T result = Acos((this->Dot(b))/sqrt(div)); nuclear@0: return result; nuclear@0: } nuclear@0: nuclear@0: // Return Length of the vector squared. nuclear@0: T LengthSq() const { return (x * x + y * y + z * z); } nuclear@0: nuclear@0: // Return vector length. nuclear@0: T Length() const { return sqrt(LengthSq()); } nuclear@0: nuclear@0: // Returns squared distance between two points represented by vectors. nuclear@0: T DistanceSq(Vector3 const& b) const { return (*this - b).LengthSq(); } nuclear@0: nuclear@0: // Returns distance between two points represented by vectors. nuclear@0: T Distance(Vector3 const& b) const { return (*this - b).Length(); } nuclear@0: nuclear@0: // Determine if this a unit vector. nuclear@0: bool IsNormalized() const { return fabs(LengthSq() - T(1)) < ((T)MATH_DOUBLE_TOLERANCE); } nuclear@0: nuclear@0: // Normalize, convention vector length to 1. nuclear@0: void Normalize() nuclear@0: { nuclear@0: T l = Length(); nuclear@0: OVR_ASSERT(l != T(0)); nuclear@0: *this /= l; nuclear@0: } nuclear@0: nuclear@0: // Returns normalized (unit) version of the vector without modifying itself. nuclear@0: Vector3 Normalized() const nuclear@0: { nuclear@0: T l = Length(); nuclear@0: OVR_ASSERT(l != T(0)); nuclear@0: return *this / l; nuclear@0: } nuclear@0: nuclear@0: // Linearly interpolates from this vector to another. nuclear@0: // Factor should be between 0.0 and 1.0, with 0 giving full value to this. nuclear@0: Vector3 Lerp(const Vector3& b, T f) const { return *this*(T(1) - f) + b*f; } nuclear@0: nuclear@0: // Projects this vector onto the argument; in other words, nuclear@0: // A.Project(B) returns projection of vector A onto B. nuclear@0: Vector3 ProjectTo(const Vector3& b) const nuclear@0: { nuclear@0: T l2 = b.LengthSq(); nuclear@0: OVR_ASSERT(l2 != T(0)); nuclear@0: return b * ( Dot(b) / l2 ); nuclear@0: } nuclear@0: nuclear@0: // Projects this vector onto a plane defined by a normal vector nuclear@0: Vector3 ProjectToPlane(const Vector3& normal) const { return *this - this->ProjectTo(normal); } nuclear@0: }; nuclear@0: nuclear@0: typedef Vector3 Vector3f; nuclear@0: typedef Vector3 Vector3d; nuclear@0: typedef Vector3 Vector3i; nuclear@0: nuclear@0: static_assert((sizeof(Vector3f) == 3*sizeof(float)), "sizeof(Vector3f) failure"); nuclear@0: static_assert((sizeof(Vector3d) == 3*sizeof(double)), "sizeof(Vector3d) failure"); nuclear@0: static_assert((sizeof(Vector3i) == 3*sizeof(int32_t)), "sizeof(Vector3i) failure"); nuclear@0: nuclear@0: typedef Vector3 Point3f; nuclear@0: typedef Vector3 Point3d; nuclear@0: typedef Vector3 Point3i; nuclear@0: nuclear@0: nuclear@0: //------------------------------------------------------------------------------------- nuclear@0: // ***** Vector4<> - 4D vector of {x, y, z, w} nuclear@0: nuclear@0: // nuclear@0: // Vector4f (Vector4d) represents a 3-dimensional vector or point in space, nuclear@0: // consisting of coordinates x, y, z and w. nuclear@0: nuclear@0: template nuclear@0: class Vector4 nuclear@0: { nuclear@0: public: nuclear@0: T x, y, z, w; nuclear@0: nuclear@0: // FIXME: default initialization of a vector class can be very expensive in a full-blown nuclear@0: // application. A few hundred thousand vector constructions is not unlikely and can add nuclear@0: // up to milliseconds of time on processors like the PS3 PPU. nuclear@0: Vector4() : x(0), y(0), z(0), w(0) { } nuclear@0: Vector4(T x_, T y_, T z_, T w_) : x(x_), y(y_), z(z_), w(w_) { } nuclear@0: explicit Vector4(T s) : x(s), y(s), z(s), w(s) { } nuclear@0: explicit Vector4(const Vector3& v, const float w_=1) : x(v.x), y(v.y), z(v.z), w(w_) { } nuclear@0: explicit Vector4(const Vector4::OtherFloatType> &src) nuclear@0: : x((T)src.x), y((T)src.y), z((T)src.z), w((T)src.w) { } nuclear@0: nuclear@0: static const Vector4 ZERO; nuclear@0: nuclear@0: // C-interop support. nuclear@0: typedef typename CompatibleTypes< Vector4 >::Type CompatibleType; nuclear@0: nuclear@0: Vector4(const CompatibleType& s) : x(s.x), y(s.y), z(s.z), w(s.w) { } nuclear@0: nuclear@0: operator const CompatibleType& () const nuclear@0: { nuclear@0: static_assert(sizeof(Vector4) == sizeof(CompatibleType), "sizeof(Vector4) failure"); nuclear@0: return reinterpret_cast(*this); nuclear@0: } nuclear@0: nuclear@0: Vector4& operator= (const Vector3& other) { x=other.x; y=other.y; z=other.z; w=1; return *this; } nuclear@0: bool operator== (const Vector4& b) const { return x == b.x && y == b.y && z == b.z && w == b.w; } nuclear@0: bool operator!= (const Vector4& b) const { return x != b.x || y != b.y || z != b.z || w != b.w; } nuclear@0: nuclear@0: Vector4 operator+ (const Vector4& b) const { return Vector4(x + b.x, y + b.y, z + b.z, w + b.w); } nuclear@0: Vector4& operator+= (const Vector4& b) { x += b.x; y += b.y; z += b.z; w += b.w; return *this; } nuclear@0: Vector4 operator- (const Vector4& b) const { return Vector4(x - b.x, y - b.y, z - b.z, w - b.w); } nuclear@0: Vector4& operator-= (const Vector4& b) { x -= b.x; y -= b.y; z -= b.z; w -= b.w; return *this; } nuclear@0: Vector4 operator- () const { return Vector4(-x, -y, -z, -w); } nuclear@0: nuclear@0: // Scalar multiplication/division scales vector. nuclear@0: Vector4 operator* (T s) const { return Vector4(x*s, y*s, z*s, w*s); } nuclear@0: Vector4& operator*= (T s) { x *= s; y *= s; z *= s; w *= s;return *this; } nuclear@0: nuclear@0: Vector4 operator/ (T s) const { T rcp = T(1)/s; nuclear@0: return Vector4(x*rcp, y*rcp, z*rcp, w*rcp); } nuclear@0: Vector4& operator/= (T s) { T rcp = T(1)/s; nuclear@0: x *= rcp; y *= rcp; z *= rcp; w *= rcp; nuclear@0: return *this; } nuclear@0: nuclear@0: static Vector4 Min(const Vector4& a, const Vector4& b) nuclear@0: { nuclear@0: return Vector4((a.x < b.x) ? a.x : b.x, nuclear@0: (a.y < b.y) ? a.y : b.y, nuclear@0: (a.z < b.z) ? a.z : b.z, nuclear@0: (a.w < b.w) ? a.w : b.w); nuclear@0: } nuclear@0: static Vector4 Max(const Vector4& a, const Vector4& b) nuclear@0: { nuclear@0: return Vector4((a.x > b.x) ? a.x : b.x, nuclear@0: (a.y > b.y) ? a.y : b.y, nuclear@0: (a.z > b.z) ? a.z : b.z, nuclear@0: (a.w > b.w) ? a.w : b.w); nuclear@0: } nuclear@0: nuclear@0: // Compare two vectors for equality with tolerance. Returns true if vectors match withing tolerance. nuclear@0: bool Compare(const Vector4&b, T tolerance = ((T)MATH_DOUBLE_TOLERANCE)) nuclear@0: { nuclear@0: return (fabs(b.x-x) < tolerance) && nuclear@0: (fabs(b.y-y) < tolerance) && nuclear@0: (fabs(b.z-z) < tolerance) && nuclear@0: (fabs(b.w-w) < tolerance); nuclear@0: } nuclear@0: nuclear@0: T& operator[] (int idx) nuclear@0: { nuclear@0: OVR_ASSERT(0 <= idx && idx < 4); nuclear@0: return *(&x + idx); nuclear@0: } nuclear@0: nuclear@0: const T& operator[] (int idx) const nuclear@0: { nuclear@0: OVR_ASSERT(0 <= idx && idx < 4); nuclear@0: return *(&x + idx); nuclear@0: } nuclear@0: nuclear@0: // Entry wise product of two vectors nuclear@0: Vector4 EntrywiseMultiply(const Vector4& b) const { return Vector4(x * b.x, nuclear@0: y * b.y, nuclear@0: z * b.z);} nuclear@0: nuclear@0: // Multiply and divide operators do entry-wise math nuclear@0: Vector4 operator* (const Vector4& b) const { return Vector4(x * b.x, nuclear@0: y * b.y, nuclear@0: z * b.z, nuclear@0: w * b.w); } nuclear@0: nuclear@0: Vector4 operator/ (const Vector4& b) const { return Vector4(x / b.x, nuclear@0: y / b.y, nuclear@0: z / b.z, nuclear@0: w / b.w); } nuclear@0: nuclear@0: nuclear@0: // Dot product nuclear@0: T Dot(const Vector4& b) const { return x*b.x + y*b.y + z*b.z + w*b.w; } nuclear@0: nuclear@0: // Return Length of the vector squared. nuclear@0: T LengthSq() const { return (x * x + y * y + z * z + w * w); } nuclear@0: nuclear@0: // Return vector length. nuclear@0: T Length() const { return sqrt(LengthSq()); } nuclear@0: nuclear@0: // Determine if this a unit vector. nuclear@0: bool IsNormalized() const { return fabs(LengthSq() - T(1)) < Math::Tolerance; } nuclear@0: nuclear@0: // Normalize, convention vector length to 1. nuclear@0: void Normalize() nuclear@0: { nuclear@0: T l = Length(); nuclear@0: OVR_ASSERT(l != T(0)); nuclear@0: *this /= l; nuclear@0: } nuclear@0: nuclear@0: // Returns normalized (unit) version of the vector without modifying itself. nuclear@0: Vector4 Normalized() const nuclear@0: { nuclear@0: T l = Length(); nuclear@0: OVR_ASSERT(l != T(0)); nuclear@0: return *this / l; nuclear@0: } nuclear@0: }; nuclear@0: nuclear@0: typedef Vector4 Vector4f; nuclear@0: typedef Vector4 Vector4d; nuclear@0: typedef Vector4 Vector4i; nuclear@0: nuclear@0: nuclear@0: //------------------------------------------------------------------------------------- nuclear@0: // ***** Bounds3 nuclear@0: nuclear@0: // Bounds class used to describe a 3D axis aligned bounding box. nuclear@0: nuclear@0: template nuclear@0: class Bounds3 nuclear@0: { nuclear@0: public: nuclear@0: Vector3 b[2]; nuclear@0: nuclear@0: Bounds3() nuclear@0: { nuclear@0: } nuclear@0: nuclear@0: Bounds3( const Vector3 & mins, const Vector3 & maxs ) nuclear@0: { nuclear@0: b[0] = mins; nuclear@0: b[1] = maxs; nuclear@0: } nuclear@0: nuclear@0: void Clear() nuclear@0: { nuclear@0: b[0].x = b[0].y = b[0].z = Math::MaxValue; nuclear@0: b[1].x = b[1].y = b[1].z = -Math::MaxValue; nuclear@0: } nuclear@0: nuclear@0: void AddPoint( const Vector3 & v ) nuclear@0: { nuclear@0: b[0].x = Alg::Min( b[0].x, v.x ); nuclear@0: b[0].y = Alg::Min( b[0].y, v.y ); nuclear@0: b[0].z = Alg::Min( b[0].z, v.z ); nuclear@0: b[1].x = Alg::Max( b[1].x, v.x ); nuclear@0: b[1].y = Alg::Max( b[1].y, v.y ); nuclear@0: b[1].z = Alg::Max( b[1].z, v.z ); nuclear@0: } nuclear@0: nuclear@0: const Vector3 & GetMins() const { return b[0]; } nuclear@0: const Vector3 & GetMaxs() const { return b[1]; } nuclear@0: nuclear@0: Vector3 & GetMins() { return b[0]; } nuclear@0: Vector3 & GetMaxs() { return b[1]; } nuclear@0: }; nuclear@0: nuclear@0: typedef Bounds3 Bounds3f; nuclear@0: typedef Bounds3 Bounds3d; nuclear@0: nuclear@0: nuclear@0: //------------------------------------------------------------------------------------- nuclear@0: // ***** Size nuclear@0: nuclear@0: // Size class represents 2D size with Width, Height components. nuclear@0: // Used to describe distentions of render targets, etc. nuclear@0: nuclear@0: template nuclear@0: class Size nuclear@0: { nuclear@0: public: nuclear@0: T w, h; nuclear@0: nuclear@0: Size() : w(0), h(0) { } nuclear@0: Size(T w_, T h_) : w(w_), h(h_) { } nuclear@0: explicit Size(T s) : w(s), h(s) { } nuclear@0: explicit Size(const Size::OtherFloatType> &src) nuclear@0: : w((T)src.w), h((T)src.h) { } nuclear@0: nuclear@0: // C-interop support. nuclear@0: typedef typename CompatibleTypes >::Type CompatibleType; nuclear@0: nuclear@0: Size(const CompatibleType& s) : w(s.w), h(s.h) { } nuclear@0: nuclear@0: operator const CompatibleType& () const nuclear@0: { nuclear@0: static_assert(sizeof(Size) == sizeof(CompatibleType), "sizeof(Size) failure"); nuclear@0: return reinterpret_cast(*this); nuclear@0: } nuclear@0: nuclear@0: bool operator== (const Size& b) const { return w == b.w && h == b.h; } nuclear@0: bool operator!= (const Size& b) const { return w != b.w || h != b.h; } nuclear@0: nuclear@0: Size operator+ (const Size& b) const { return Size(w + b.w, h + b.h); } nuclear@0: Size& operator+= (const Size& b) { w += b.w; h += b.h; return *this; } nuclear@0: Size operator- (const Size& b) const { return Size(w - b.w, h - b.h); } nuclear@0: Size& operator-= (const Size& b) { w -= b.w; h -= b.h; return *this; } nuclear@0: Size operator- () const { return Size(-w, -h); } nuclear@0: Size operator* (const Size& b) const { return Size(w * b.w, h * b.h); } nuclear@0: Size& operator*= (const Size& b) { w *= b.w; h *= b.h; return *this; } nuclear@0: Size operator/ (const Size& b) const { return Size(w / b.w, h / b.h); } nuclear@0: Size& operator/= (const Size& b) { w /= b.w; h /= b.h; return *this; } nuclear@0: nuclear@0: // Scalar multiplication/division scales both components. nuclear@0: Size operator* (T s) const { return Size(w*s, h*s); } nuclear@0: Size& operator*= (T s) { w *= s; h *= s; return *this; } nuclear@0: Size operator/ (T s) const { return Size(w/s, h/s); } nuclear@0: Size& operator/= (T s) { w /= s; h /= s; return *this; } nuclear@0: nuclear@0: static Size Min(const Size& a, const Size& b) { return Size((a.w < b.w) ? a.w : b.w, nuclear@0: (a.h < b.h) ? a.h : b.h); } nuclear@0: static Size Max(const Size& a, const Size& b) { return Size((a.w > b.w) ? a.w : b.w, nuclear@0: (a.h > b.h) ? a.h : b.h); } nuclear@0: nuclear@0: T Area() const { return w * h; } nuclear@0: nuclear@0: inline Vector2 ToVector() const { return Vector2(w, h); } nuclear@0: }; nuclear@0: nuclear@0: nuclear@0: typedef Size Sizei; nuclear@0: typedef Size Sizeu; nuclear@0: typedef Size Sizef; nuclear@0: typedef Size Sized; nuclear@0: nuclear@0: nuclear@0: nuclear@0: //----------------------------------------------------------------------------------- nuclear@0: // ***** Rect nuclear@0: nuclear@0: // Rect describes a rectangular area for rendering, that includes position and size. nuclear@0: template nuclear@0: class Rect nuclear@0: { nuclear@0: public: nuclear@0: T x, y; nuclear@0: T w, h; nuclear@0: nuclear@0: Rect() { } nuclear@0: Rect(T x1, T y1, T w1, T h1) : x(x1), y(y1), w(w1), h(h1) { } nuclear@0: Rect(const Vector2& pos, const Size& sz) : x(pos.x), y(pos.y), w(sz.w), h(sz.h) { } nuclear@0: Rect(const Size& sz) : x(0), y(0), w(sz.w), h(sz.h) { } nuclear@0: nuclear@0: // C-interop support. nuclear@0: typedef typename CompatibleTypes >::Type CompatibleType; nuclear@0: nuclear@0: Rect(const CompatibleType& s) : x(s.Pos.x), y(s.Pos.y), w(s.Size.w), h(s.Size.h) { } nuclear@0: nuclear@0: operator const CompatibleType& () const nuclear@0: { nuclear@0: static_assert(sizeof(Rect) == sizeof(CompatibleType), "sizeof(Rect) failure"); nuclear@0: return reinterpret_cast(*this); nuclear@0: } nuclear@0: nuclear@0: Vector2 GetPos() const { return Vector2(x, y); } nuclear@0: Size GetSize() const { return Size(w, h); } nuclear@0: void SetPos(const Vector2& pos) { x = pos.x; y = pos.y; } nuclear@0: void SetSize(const Size& sz) { w = sz.w; h = sz.h; } nuclear@0: nuclear@0: bool operator == (const Rect& vp) const nuclear@0: { return (x == vp.x) && (y == vp.y) && (w == vp.w) && (h == vp.h); } nuclear@0: bool operator != (const Rect& vp) const nuclear@0: { return !operator == (vp); } nuclear@0: }; nuclear@0: nuclear@0: typedef Rect Recti; nuclear@0: nuclear@0: nuclear@0: //-------------------------------------------------------------------------------------// nuclear@0: // ***** Quat nuclear@0: // nuclear@0: // Quatf represents a quaternion class used for rotations. nuclear@0: // nuclear@0: // Quaternion multiplications are done in right-to-left order, to match the nuclear@0: // behavior of matrices. nuclear@0: nuclear@0: nuclear@0: template nuclear@0: class Quat nuclear@0: { nuclear@0: public: nuclear@0: // w + Xi + Yj + Zk nuclear@0: T x, y, z, w; nuclear@0: nuclear@0: Quat() : x(0), y(0), z(0), w(1) { } nuclear@0: Quat(T x_, T y_, T z_, T w_) : x(x_), y(y_), z(z_), w(w_) { } nuclear@0: explicit Quat(const Quat::OtherFloatType> &src) nuclear@0: : x((T)src.x), y((T)src.y), z((T)src.z), w((T)src.w) { } nuclear@0: nuclear@0: typedef typename CompatibleTypes >::Type CompatibleType; nuclear@0: nuclear@0: // C-interop support. nuclear@0: Quat(const CompatibleType& s) : x(s.x), y(s.y), z(s.z), w(s.w) { } nuclear@0: nuclear@0: operator CompatibleType () const nuclear@0: { nuclear@0: CompatibleType result; nuclear@0: result.x = x; nuclear@0: result.y = y; nuclear@0: result.z = z; nuclear@0: result.w = w; nuclear@0: return result; nuclear@0: } nuclear@0: nuclear@0: // Constructs quaternion for rotation around the axis by an angle. nuclear@0: Quat(const Vector3& axis, T angle) nuclear@0: { nuclear@0: // Make sure we don't divide by zero. nuclear@0: if (axis.LengthSq() == 0) nuclear@0: { nuclear@0: // Assert if the axis is zero, but the angle isn't nuclear@0: OVR_ASSERT(angle == 0); nuclear@0: x = 0; y = 0; z = 0; w = 1; nuclear@0: return; nuclear@0: } nuclear@0: nuclear@0: Vector3 unitAxis = axis.Normalized(); nuclear@0: T sinHalfAngle = sin(angle * T(0.5)); nuclear@0: nuclear@0: w = cos(angle * T(0.5)); nuclear@0: x = unitAxis.x * sinHalfAngle; nuclear@0: y = unitAxis.y * sinHalfAngle; nuclear@0: z = unitAxis.z * sinHalfAngle; nuclear@0: } nuclear@0: nuclear@0: // Constructs quaternion for rotation around one of the coordinate axis by an angle. nuclear@0: Quat(Axis A, T angle, RotateDirection d = Rotate_CCW, HandedSystem s = Handed_R) nuclear@0: { nuclear@0: T sinHalfAngle = s * d *sin(angle * T(0.5)); nuclear@0: T v[3]; nuclear@0: v[0] = v[1] = v[2] = T(0); nuclear@0: v[A] = sinHalfAngle; nuclear@0: nuclear@0: w = cos(angle * T(0.5)); nuclear@0: x = v[0]; nuclear@0: y = v[1]; nuclear@0: z = v[2]; nuclear@0: } nuclear@0: nuclear@0: // Compute axis and angle from quaternion nuclear@0: void GetAxisAngle(Vector3* axis, T* angle) const nuclear@0: { nuclear@0: if ( x*x + y*y + z*z > ((T)MATH_DOUBLE_TOLERANCE) * ((T)MATH_DOUBLE_TOLERANCE) ) { nuclear@0: *axis = Vector3(x, y, z).Normalized(); nuclear@0: *angle = 2 * Acos(w); nuclear@0: if (*angle > ((T)MATH_DOUBLE_PI)) // Reduce the magnitude of the angle, if necessary nuclear@0: { nuclear@0: *angle = ((T)MATH_DOUBLE_TWOPI) - *angle; nuclear@0: *axis = *axis * (-1); nuclear@0: } nuclear@0: } nuclear@0: else nuclear@0: { nuclear@0: *axis = Vector3(1, 0, 0); nuclear@0: *angle= 0; nuclear@0: } nuclear@0: } nuclear@0: nuclear@0: // Constructs the quaternion from a rotation matrix nuclear@0: explicit Quat(const Matrix4& m) nuclear@0: { nuclear@0: T trace = m.M[0][0] + m.M[1][1] + m.M[2][2]; nuclear@0: nuclear@0: // In almost all cases, the first part is executed. nuclear@0: // However, if the trace is not positive, the other nuclear@0: // cases arise. nuclear@0: if (trace > T(0)) nuclear@0: { nuclear@0: T s = sqrt(trace + T(1)) * T(2); // s=4*qw nuclear@0: w = T(0.25) * s; nuclear@0: x = (m.M[2][1] - m.M[1][2]) / s; nuclear@0: y = (m.M[0][2] - m.M[2][0]) / s; nuclear@0: z = (m.M[1][0] - m.M[0][1]) / s; nuclear@0: } nuclear@0: else if ((m.M[0][0] > m.M[1][1])&&(m.M[0][0] > m.M[2][2])) nuclear@0: { nuclear@0: T s = sqrt(T(1) + m.M[0][0] - m.M[1][1] - m.M[2][2]) * T(2); nuclear@0: w = (m.M[2][1] - m.M[1][2]) / s; nuclear@0: x = T(0.25) * s; nuclear@0: y = (m.M[0][1] + m.M[1][0]) / s; nuclear@0: z = (m.M[2][0] + m.M[0][2]) / s; nuclear@0: } nuclear@0: else if (m.M[1][1] > m.M[2][2]) nuclear@0: { nuclear@0: T s = sqrt(T(1) + m.M[1][1] - m.M[0][0] - m.M[2][2]) * T(2); // S=4*qy nuclear@0: w = (m.M[0][2] - m.M[2][0]) / s; nuclear@0: x = (m.M[0][1] + m.M[1][0]) / s; nuclear@0: y = T(0.25) * s; nuclear@0: z = (m.M[1][2] + m.M[2][1]) / s; nuclear@0: } nuclear@0: else nuclear@0: { nuclear@0: T s = sqrt(T(1) + m.M[2][2] - m.M[0][0] - m.M[1][1]) * T(2); // S=4*qz nuclear@0: w = (m.M[1][0] - m.M[0][1]) / s; nuclear@0: x = (m.M[0][2] + m.M[2][0]) / s; nuclear@0: y = (m.M[1][2] + m.M[2][1]) / s; nuclear@0: z = T(0.25) * s; nuclear@0: } nuclear@0: } nuclear@0: nuclear@0: // Constructs the quaternion from a rotation matrix nuclear@0: explicit Quat(const Matrix3& m) nuclear@0: { nuclear@0: T trace = m.M[0][0] + m.M[1][1] + m.M[2][2]; nuclear@0: nuclear@0: // In almost all cases, the first part is executed. nuclear@0: // However, if the trace is not positive, the other nuclear@0: // cases arise. nuclear@0: if (trace > T(0)) nuclear@0: { nuclear@0: T s = sqrt(trace + T(1)) * T(2); // s=4*qw nuclear@0: w = T(0.25) * s; nuclear@0: x = (m.M[2][1] - m.M[1][2]) / s; nuclear@0: y = (m.M[0][2] - m.M[2][0]) / s; nuclear@0: z = (m.M[1][0] - m.M[0][1]) / s; nuclear@0: } nuclear@0: else if ((m.M[0][0] > m.M[1][1])&&(m.M[0][0] > m.M[2][2])) nuclear@0: { nuclear@0: T s = sqrt(T(1) + m.M[0][0] - m.M[1][1] - m.M[2][2]) * T(2); nuclear@0: w = (m.M[2][1] - m.M[1][2]) / s; nuclear@0: x = T(0.25) * s; nuclear@0: y = (m.M[0][1] + m.M[1][0]) / s; nuclear@0: z = (m.M[2][0] + m.M[0][2]) / s; nuclear@0: } nuclear@0: else if (m.M[1][1] > m.M[2][2]) nuclear@0: { nuclear@0: T s = sqrt(T(1) + m.M[1][1] - m.M[0][0] - m.M[2][2]) * T(2); // S=4*qy nuclear@0: w = (m.M[0][2] - m.M[2][0]) / s; nuclear@0: x = (m.M[0][1] + m.M[1][0]) / s; nuclear@0: y = T(0.25) * s; nuclear@0: z = (m.M[1][2] + m.M[2][1]) / s; nuclear@0: } nuclear@0: else nuclear@0: { nuclear@0: T s = sqrt(T(1) + m.M[2][2] - m.M[0][0] - m.M[1][1]) * T(2); // S=4*qz nuclear@0: w = (m.M[1][0] - m.M[0][1]) / s; nuclear@0: x = (m.M[0][2] + m.M[2][0]) / s; nuclear@0: y = (m.M[1][2] + m.M[2][1]) / s; nuclear@0: z = T(0.25) * s; nuclear@0: } nuclear@0: } nuclear@0: nuclear@0: bool operator== (const Quat& b) const { return x == b.x && y == b.y && z == b.z && w == b.w; } nuclear@0: bool operator!= (const Quat& b) const { return x != b.x || y != b.y || z != b.z || w != b.w; } nuclear@0: nuclear@0: Quat operator+ (const Quat& b) const { return Quat(x + b.x, y + b.y, z + b.z, w + b.w); } nuclear@0: Quat& operator+= (const Quat& b) { w += b.w; x += b.x; y += b.y; z += b.z; return *this; } nuclear@0: Quat operator- (const Quat& b) const { return Quat(x - b.x, y - b.y, z - b.z, w - b.w); } nuclear@0: Quat& operator-= (const Quat& b) { w -= b.w; x -= b.x; y -= b.y; z -= b.z; return *this; } nuclear@0: nuclear@0: Quat operator* (T s) const { return Quat(x * s, y * s, z * s, w * s); } nuclear@0: Quat& operator*= (T s) { w *= s; x *= s; y *= s; z *= s; return *this; } nuclear@0: Quat operator/ (T s) const { T rcp = T(1)/s; return Quat(x * rcp, y * rcp, z * rcp, w *rcp); } nuclear@0: Quat& operator/= (T s) { T rcp = T(1)/s; w *= rcp; x *= rcp; y *= rcp; z *= rcp; return *this; } nuclear@0: nuclear@0: nuclear@0: // Get Imaginary part vector nuclear@0: Vector3 Imag() const { return Vector3(x,y,z); } nuclear@0: nuclear@0: // Get quaternion length. nuclear@0: T Length() const { return sqrt(LengthSq()); } nuclear@0: nuclear@0: // Get quaternion length squared. nuclear@0: T LengthSq() const { return (x * x + y * y + z * z + w * w); } nuclear@0: nuclear@0: // Simple Euclidean distance in R^4 (not SLERP distance, but at least respects Haar measure) nuclear@0: T Distance(const Quat& q) const nuclear@0: { nuclear@0: T d1 = (*this - q).Length(); nuclear@0: T d2 = (*this + q).Length(); // Antipodal point check nuclear@0: return (d1 < d2) ? d1 : d2; nuclear@0: } nuclear@0: nuclear@0: T DistanceSq(const Quat& q) const nuclear@0: { nuclear@0: T d1 = (*this - q).LengthSq(); nuclear@0: T d2 = (*this + q).LengthSq(); // Antipodal point check nuclear@0: return (d1 < d2) ? d1 : d2; nuclear@0: } nuclear@0: nuclear@0: T Dot(const Quat& q) const nuclear@0: { nuclear@0: return x * q.x + y * q.y + z * q.z + w * q.w; nuclear@0: } nuclear@0: nuclear@0: // Angle between two quaternions in radians nuclear@0: T Angle(const Quat& q) const nuclear@0: { nuclear@0: return 2 * Acos(Alg::Abs(Dot(q))); nuclear@0: } nuclear@0: nuclear@0: // Normalize nuclear@0: bool IsNormalized() const { return fabs(LengthSq() - T(1)) < ((T)MATH_DOUBLE_TOLERANCE); } nuclear@0: nuclear@0: void Normalize() nuclear@0: { nuclear@0: T l = Length(); nuclear@0: OVR_ASSERT(l != T(0)); nuclear@0: *this /= l; nuclear@0: } nuclear@0: nuclear@0: Quat Normalized() const nuclear@0: { nuclear@0: T l = Length(); nuclear@0: OVR_ASSERT(l != T(0)); nuclear@0: return *this / l; nuclear@0: } nuclear@0: nuclear@0: // Returns conjugate of the quaternion. Produces inverse rotation if quaternion is normalized. nuclear@0: Quat Conj() const { return Quat(-x, -y, -z, w); } nuclear@0: nuclear@0: // Quaternion multiplication. Combines quaternion rotations, performing the one on the nuclear@0: // right hand side first. nuclear@0: Quat operator* (const Quat& b) const { return Quat(w * b.x + x * b.w + y * b.z - z * b.y, nuclear@0: w * b.y - x * b.z + y * b.w + z * b.x, nuclear@0: w * b.z + x * b.y - y * b.x + z * b.w, nuclear@0: w * b.w - x * b.x - y * b.y - z * b.z); } nuclear@0: nuclear@0: // nuclear@0: // this^p normalized; same as rotating by this p times. nuclear@0: Quat PowNormalized(T p) const nuclear@0: { nuclear@0: Vector3 v; nuclear@0: T a; nuclear@0: GetAxisAngle(&v, &a); nuclear@0: return Quat(v, a * p); nuclear@0: } nuclear@0: nuclear@0: // Normalized linear interpolation of quaternions nuclear@0: Quat Nlerp(const Quat& other, T a) nuclear@0: { nuclear@0: T sign = (Dot(other) >= 0) ? 1 : -1; nuclear@0: return (*this * sign * a + other * (1-a)).Normalized(); nuclear@0: } nuclear@0: nuclear@0: // Rotate transforms vector in a manner that matches Matrix rotations (counter-clockwise, nuclear@0: // assuming negative direction of the axis). Standard formula: q(t) * V * q(t)^-1. nuclear@0: Vector3 Rotate(const Vector3& v) const nuclear@0: { nuclear@0: return ((*this * Quat(v.x, v.y, v.z, T(0))) * Inverted()).Imag(); nuclear@0: } nuclear@0: nuclear@0: // Inversed quaternion rotates in the opposite direction. nuclear@0: Quat Inverted() const nuclear@0: { nuclear@0: return Quat(-x, -y, -z, w); nuclear@0: } nuclear@0: nuclear@0: // Sets this quaternion to the one rotates in the opposite direction. nuclear@0: void Invert() nuclear@0: { nuclear@0: *this = Quat(-x, -y, -z, w); nuclear@0: } nuclear@0: nuclear@0: // GetEulerAngles extracts Euler angles from the quaternion, in the specified order of nuclear@0: // axis rotations and the specified coordinate system. Right-handed coordinate system nuclear@0: // is the default, with CCW rotations while looking in the negative axis direction. nuclear@0: // Here a,b,c, are the Yaw/Pitch/Roll angles to be returned. nuclear@0: // rotation a around axis A1 nuclear@0: // is followed by rotation b around axis A2 nuclear@0: // is followed by rotation c around axis A3 nuclear@0: // rotations are CCW or CW (D) in LH or RH coordinate system (S) nuclear@0: template nuclear@0: void GetEulerAngles(T *a, T *b, T *c) const nuclear@0: { nuclear@0: static_assert((A1 != A2) && (A2 != A3) && (A1 != A3), "(A1 != A2) && (A2 != A3) && (A1 != A3)"); nuclear@0: nuclear@0: T Q[3] = { x, y, z }; //Quaternion components x,y,z nuclear@0: nuclear@0: T ww = w*w; nuclear@0: T Q11 = Q[A1]*Q[A1]; nuclear@0: T Q22 = Q[A2]*Q[A2]; nuclear@0: T Q33 = Q[A3]*Q[A3]; nuclear@0: nuclear@0: T psign = T(-1); nuclear@0: // Determine whether even permutation nuclear@0: if (((A1 + 1) % 3 == A2) && ((A2 + 1) % 3 == A3)) nuclear@0: psign = T(1); nuclear@0: nuclear@0: T s2 = psign * T(2) * (psign*w*Q[A2] + Q[A1]*Q[A3]); nuclear@0: nuclear@0: if (s2 < T(-1) + ((T)MATH_DOUBLE_SINGULARITYRADIUS)) nuclear@0: { // South pole singularity nuclear@0: *a = T(0); nuclear@0: *b = -S*D*((T)MATH_DOUBLE_PIOVER2); nuclear@0: *c = S*D*atan2(T(2)*(psign*Q[A1]*Q[A2] + w*Q[A3]), nuclear@0: ww + Q22 - Q11 - Q33 ); nuclear@0: } nuclear@0: else if (s2 > T(1) - ((T)MATH_DOUBLE_SINGULARITYRADIUS)) nuclear@0: { // North pole singularity nuclear@0: *a = T(0); nuclear@0: *b = S*D*((T)MATH_DOUBLE_PIOVER2); nuclear@0: *c = S*D*atan2(T(2)*(psign*Q[A1]*Q[A2] + w*Q[A3]), nuclear@0: ww + Q22 - Q11 - Q33); nuclear@0: } nuclear@0: else nuclear@0: { nuclear@0: *a = -S*D*atan2(T(-2)*(w*Q[A1] - psign*Q[A2]*Q[A3]), nuclear@0: ww + Q33 - Q11 - Q22); nuclear@0: *b = S*D*asin(s2); nuclear@0: *c = S*D*atan2(T(2)*(w*Q[A3] - psign*Q[A1]*Q[A2]), nuclear@0: ww + Q11 - Q22 - Q33); nuclear@0: } nuclear@0: return; nuclear@0: } nuclear@0: nuclear@0: template nuclear@0: void GetEulerAngles(T *a, T *b, T *c) const nuclear@0: { GetEulerAngles(a, b, c); } nuclear@0: nuclear@0: template nuclear@0: void GetEulerAngles(T *a, T *b, T *c) const nuclear@0: { GetEulerAngles(a, b, c); } nuclear@0: nuclear@0: // GetEulerAnglesABA extracts Euler angles from the quaternion, in the specified order of nuclear@0: // axis rotations and the specified coordinate system. Right-handed coordinate system nuclear@0: // is the default, with CCW rotations while looking in the negative axis direction. nuclear@0: // Here a,b,c, are the Yaw/Pitch/Roll angles to be returned. nuclear@0: // rotation a around axis A1 nuclear@0: // is followed by rotation b around axis A2 nuclear@0: // is followed by rotation c around axis A1 nuclear@0: // Rotations are CCW or CW (D) in LH or RH coordinate system (S) nuclear@0: template nuclear@0: void GetEulerAnglesABA(T *a, T *b, T *c) const nuclear@0: { nuclear@0: static_assert(A1 != A2, "A1 != A2"); nuclear@0: nuclear@0: T Q[3] = {x, y, z}; // Quaternion components nuclear@0: nuclear@0: // Determine the missing axis that was not supplied nuclear@0: int m = 3 - A1 - A2; nuclear@0: nuclear@0: T ww = w*w; nuclear@0: T Q11 = Q[A1]*Q[A1]; nuclear@0: T Q22 = Q[A2]*Q[A2]; nuclear@0: T Qmm = Q[m]*Q[m]; nuclear@0: nuclear@0: T psign = T(-1); nuclear@0: if ((A1 + 1) % 3 == A2) // Determine whether even permutation nuclear@0: { nuclear@0: psign = T(1); nuclear@0: } nuclear@0: nuclear@0: T c2 = ww + Q11 - Q22 - Qmm; nuclear@0: if (c2 < T(-1) + Math::SingularityRadius) nuclear@0: { // South pole singularity nuclear@0: *a = T(0); nuclear@0: *b = S*D*((T)MATH_DOUBLE_PI); nuclear@0: *c = S*D*atan2( T(2)*(w*Q[A1] - psign*Q[A2]*Q[m]), nuclear@0: ww + Q22 - Q11 - Qmm); nuclear@0: } nuclear@0: else if (c2 > T(1) - Math::SingularityRadius) nuclear@0: { // North pole singularity nuclear@0: *a = T(0); nuclear@0: *b = T(0); nuclear@0: *c = S*D*atan2( T(2)*(w*Q[A1] - psign*Q[A2]*Q[m]), nuclear@0: ww + Q22 - Q11 - Qmm); nuclear@0: } nuclear@0: else nuclear@0: { nuclear@0: *a = S*D*atan2( psign*w*Q[m] + Q[A1]*Q[A2], nuclear@0: w*Q[A2] -psign*Q[A1]*Q[m]); nuclear@0: *b = S*D*acos(c2); nuclear@0: *c = S*D*atan2( -psign*w*Q[m] + Q[A1]*Q[A2], nuclear@0: w*Q[A2] + psign*Q[A1]*Q[m]); nuclear@0: } nuclear@0: return; nuclear@0: } nuclear@0: }; nuclear@0: nuclear@0: typedef Quat Quatf; nuclear@0: typedef Quat Quatd; nuclear@0: nuclear@0: static_assert((sizeof(Quatf) == 4*sizeof(float)), "sizeof(Quatf) failure"); nuclear@0: static_assert((sizeof(Quatd) == 4*sizeof(double)), "sizeof(Quatd) failure"); nuclear@0: nuclear@0: //------------------------------------------------------------------------------------- nuclear@0: // ***** Pose nuclear@0: nuclear@0: // Position and orientation combined. nuclear@0: nuclear@0: template nuclear@0: class Pose nuclear@0: { nuclear@0: public: nuclear@0: typedef typename CompatibleTypes >::Type CompatibleType; nuclear@0: nuclear@0: Pose() { } nuclear@0: Pose(const Quat& orientation, const Vector3& pos) nuclear@0: : Rotation(orientation), Translation(pos) { } nuclear@0: Pose(const Pose& s) nuclear@0: : Rotation(s.Rotation), Translation(s.Translation) { } nuclear@0: Pose(const CompatibleType& s) nuclear@0: : Rotation(s.Orientation), Translation(s.Position) { } nuclear@0: explicit Pose(const Pose::OtherFloatType> &s) nuclear@0: : Rotation(s.Rotation), Translation(s.Translation) { } nuclear@0: nuclear@0: operator typename CompatibleTypes >::Type () const nuclear@0: { nuclear@0: typename CompatibleTypes >::Type result; nuclear@0: result.Orientation = Rotation; nuclear@0: result.Position = Translation; nuclear@0: return result; nuclear@0: } nuclear@0: nuclear@0: Quat Rotation; nuclear@0: Vector3 Translation; nuclear@0: nuclear@0: static_assert((sizeof(T) == sizeof(double) || sizeof(T) == sizeof(float)), "(sizeof(T) == sizeof(double) || sizeof(T) == sizeof(float))"); nuclear@0: nuclear@0: void ToArray(T* arr) const nuclear@0: { nuclear@0: T temp[7] = { Rotation.x, Rotation.y, Rotation.z, Rotation.w, Translation.x, Translation.y, Translation.z }; nuclear@0: for (int i = 0; i < 7; i++) arr[i] = temp[i]; nuclear@0: } nuclear@0: nuclear@0: static Pose FromArray(const T* v) nuclear@0: { nuclear@0: Quat rotation(v[0], v[1], v[2], v[3]); nuclear@0: Vector3 translation(v[4], v[5], v[6]); nuclear@0: return Pose(rotation, translation); nuclear@0: } nuclear@0: nuclear@0: Vector3 Rotate(const Vector3& v) const nuclear@0: { nuclear@0: return Rotation.Rotate(v); nuclear@0: } nuclear@0: nuclear@0: Vector3 Translate(const Vector3& v) const nuclear@0: { nuclear@0: return v + Translation; nuclear@0: } nuclear@0: nuclear@0: Vector3 Apply(const Vector3& v) const nuclear@0: { nuclear@0: return Translate(Rotate(v)); nuclear@0: } nuclear@0: nuclear@0: Pose operator*(const Pose& other) const nuclear@0: { nuclear@0: return Pose(Rotation * other.Rotation, Apply(other.Translation)); nuclear@0: } nuclear@0: nuclear@0: Pose Inverted() const nuclear@0: { nuclear@0: Quat inv = Rotation.Inverted(); nuclear@0: return Pose(inv, inv.Rotate(-Translation)); nuclear@0: } nuclear@0: }; nuclear@0: nuclear@0: typedef Pose Posef; nuclear@0: typedef Pose Posed; nuclear@0: nuclear@0: static_assert((sizeof(Posed) == sizeof(Quatd) + sizeof(Vector3d)), "sizeof(Posed) failure"); nuclear@0: static_assert((sizeof(Posef) == sizeof(Quatf) + sizeof(Vector3f)), "sizeof(Posef) failure"); nuclear@0: nuclear@0: nuclear@0: //------------------------------------------------------------------------------------- nuclear@0: // ***** Matrix4 nuclear@0: // nuclear@0: // Matrix4 is a 4x4 matrix used for 3d transformations and projections. nuclear@0: // Translation stored in the last column. nuclear@0: // The matrix is stored in row-major order in memory, meaning that values nuclear@0: // of the first row are stored before the next one. nuclear@0: // nuclear@0: // The arrangement of the matrix is chosen to be in Right-Handed nuclear@0: // coordinate system and counterclockwise rotations when looking down nuclear@0: // the axis nuclear@0: // nuclear@0: // Transformation Order: nuclear@0: // - Transformations are applied from right to left, so the expression nuclear@0: // M1 * M2 * M3 * V means that the vector V is transformed by M3 first, nuclear@0: // followed by M2 and M1. nuclear@0: // nuclear@0: // Coordinate system: Right Handed nuclear@0: // nuclear@0: // Rotations: Counterclockwise when looking down the axis. All angles are in radians. nuclear@0: // nuclear@0: // | sx 01 02 tx | // First column (sx, 10, 20): Axis X basis vector. nuclear@0: // | 10 sy 12 ty | // Second column (01, sy, 21): Axis Y basis vector. nuclear@0: // | 20 21 sz tz | // Third columnt (02, 12, sz): Axis Z basis vector. nuclear@0: // | 30 31 32 33 | nuclear@0: // nuclear@0: // The basis vectors are first three columns. nuclear@0: nuclear@0: template nuclear@0: class Matrix4 nuclear@0: { nuclear@0: static const Matrix4 IdentityValue; nuclear@0: nuclear@0: public: nuclear@0: T M[4][4]; nuclear@0: nuclear@0: enum NoInitType { NoInit }; nuclear@0: nuclear@0: // Construct with no memory initialization. nuclear@0: Matrix4(NoInitType) { } nuclear@0: nuclear@0: // By default, we construct identity matrix. nuclear@0: Matrix4() nuclear@0: { nuclear@0: SetIdentity(); nuclear@0: } nuclear@0: nuclear@0: Matrix4(T m11, T m12, T m13, T m14, nuclear@0: T m21, T m22, T m23, T m24, nuclear@0: T m31, T m32, T m33, T m34, nuclear@0: T m41, T m42, T m43, T m44) nuclear@0: { nuclear@0: M[0][0] = m11; M[0][1] = m12; M[0][2] = m13; M[0][3] = m14; nuclear@0: M[1][0] = m21; M[1][1] = m22; M[1][2] = m23; M[1][3] = m24; nuclear@0: M[2][0] = m31; M[2][1] = m32; M[2][2] = m33; M[2][3] = m34; nuclear@0: M[3][0] = m41; M[3][1] = m42; M[3][2] = m43; M[3][3] = m44; nuclear@0: } nuclear@0: nuclear@0: Matrix4(T m11, T m12, T m13, nuclear@0: T m21, T m22, T m23, nuclear@0: T m31, T m32, T m33) nuclear@0: { nuclear@0: M[0][0] = m11; M[0][1] = m12; M[0][2] = m13; M[0][3] = 0; nuclear@0: M[1][0] = m21; M[1][1] = m22; M[1][2] = m23; M[1][3] = 0; nuclear@0: M[2][0] = m31; M[2][1] = m32; M[2][2] = m33; M[2][3] = 0; nuclear@0: M[3][0] = 0; M[3][1] = 0; M[3][2] = 0; M[3][3] = 1; nuclear@0: } nuclear@0: nuclear@0: explicit Matrix4(const Quat& q) nuclear@0: { nuclear@0: T ww = q.w*q.w; nuclear@0: T xx = q.x*q.x; nuclear@0: T yy = q.y*q.y; nuclear@0: T zz = q.z*q.z; nuclear@0: nuclear@0: M[0][0] = ww + xx - yy - zz; M[0][1] = 2 * (q.x*q.y - q.w*q.z); M[0][2] = 2 * (q.x*q.z + q.w*q.y); M[0][3] = 0; nuclear@0: M[1][0] = 2 * (q.x*q.y + q.w*q.z); M[1][1] = ww - xx + yy - zz; M[1][2] = 2 * (q.y*q.z - q.w*q.x); M[1][3] = 0; nuclear@0: M[2][0] = 2 * (q.x*q.z - q.w*q.y); M[2][1] = 2 * (q.y*q.z + q.w*q.x); M[2][2] = ww - xx - yy + zz; M[2][3] = 0; nuclear@0: M[3][0] = 0; M[3][1] = 0; M[3][2] = 0; M[3][3] = 1; nuclear@0: } nuclear@0: nuclear@0: explicit Matrix4(const Pose& p) nuclear@0: { nuclear@0: Matrix4 result(p.Rotation); nuclear@0: result.SetTranslation(p.Translation); nuclear@0: *this = result; nuclear@0: } nuclear@0: nuclear@0: // C-interop support nuclear@0: explicit Matrix4(const Matrix4::OtherFloatType> &src) nuclear@0: { nuclear@0: for (int i = 0; i < 4; i++) nuclear@0: for (int j = 0; j < 4; j++) nuclear@0: M[i][j] = (T)src.M[i][j]; nuclear@0: } nuclear@0: nuclear@0: // C-interop support. nuclear@0: Matrix4(const typename CompatibleTypes >::Type& s) nuclear@0: { nuclear@0: static_assert(sizeof(s) == sizeof(Matrix4), "sizeof(s) == sizeof(Matrix4)"); nuclear@0: memcpy(M, s.M, sizeof(M)); nuclear@0: } nuclear@0: nuclear@0: operator typename CompatibleTypes >::Type () const nuclear@0: { nuclear@0: typename CompatibleTypes >::Type result; nuclear@0: static_assert(sizeof(result) == sizeof(Matrix4), "sizeof(result) == sizeof(Matrix4)"); nuclear@0: memcpy(result.M, M, sizeof(M)); nuclear@0: return result; nuclear@0: } nuclear@0: nuclear@0: void ToString(char* dest, size_t destsize) const nuclear@0: { nuclear@0: size_t pos = 0; nuclear@0: for (int r=0; r<4; r++) nuclear@0: for (int c=0; c<4; c++) nuclear@0: pos += OVR_sprintf(dest+pos, destsize-pos, "%g ", M[r][c]); nuclear@0: } nuclear@0: nuclear@0: static Matrix4 FromString(const char* src) nuclear@0: { nuclear@0: Matrix4 result; nuclear@0: if (src) nuclear@0: { nuclear@0: for (int r=0; r<4; r++) nuclear@0: { nuclear@0: for (int c=0; c<4; c++) nuclear@0: { nuclear@0: result.M[r][c] = (T)atof(src); nuclear@0: while (src && *src != ' ') nuclear@0: { nuclear@0: src++; nuclear@0: } nuclear@0: while (src && *src == ' ') nuclear@0: { nuclear@0: src++; nuclear@0: } nuclear@0: } nuclear@0: } nuclear@0: } nuclear@0: return result; nuclear@0: } nuclear@0: nuclear@0: static const Matrix4& Identity() { return IdentityValue; } nuclear@0: nuclear@0: void SetIdentity() nuclear@0: { nuclear@0: M[0][0] = M[1][1] = M[2][2] = M[3][3] = 1; nuclear@0: M[0][1] = M[1][0] = M[2][3] = M[3][1] = 0; nuclear@0: M[0][2] = M[1][2] = M[2][0] = M[3][2] = 0; nuclear@0: M[0][3] = M[1][3] = M[2][1] = M[3][0] = 0; nuclear@0: } nuclear@0: nuclear@0: void SetXBasis(const Vector3f & v) nuclear@0: { nuclear@0: M[0][0] = v.x; nuclear@0: M[1][0] = v.y; nuclear@0: M[2][0] = v.z; nuclear@0: } nuclear@0: Vector3f GetXBasis() const nuclear@0: { nuclear@0: return Vector3f(M[0][0], M[1][0], M[2][0]); nuclear@0: } nuclear@0: nuclear@0: void SetYBasis(const Vector3f & v) nuclear@0: { nuclear@0: M[0][1] = v.x; nuclear@0: M[1][1] = v.y; nuclear@0: M[2][1] = v.z; nuclear@0: } nuclear@0: Vector3f GetYBasis() const nuclear@0: { nuclear@0: return Vector3f(M[0][1], M[1][1], M[2][1]); nuclear@0: } nuclear@0: nuclear@0: void SetZBasis(const Vector3f & v) nuclear@0: { nuclear@0: M[0][2] = v.x; nuclear@0: M[1][2] = v.y; nuclear@0: M[2][2] = v.z; nuclear@0: } nuclear@0: Vector3f GetZBasis() const nuclear@0: { nuclear@0: return Vector3f(M[0][2], M[1][2], M[2][2]); nuclear@0: } nuclear@0: nuclear@0: bool operator== (const Matrix4& b) const nuclear@0: { nuclear@0: bool isEqual = true; nuclear@0: for (int i = 0; i < 4; i++) nuclear@0: for (int j = 0; j < 4; j++) nuclear@0: isEqual &= (M[i][j] == b.M[i][j]); nuclear@0: nuclear@0: return isEqual; nuclear@0: } nuclear@0: nuclear@0: Matrix4 operator+ (const Matrix4& b) const nuclear@0: { nuclear@0: Matrix4 result(*this); nuclear@0: result += b; nuclear@0: return result; nuclear@0: } nuclear@0: nuclear@0: Matrix4& operator+= (const Matrix4& b) nuclear@0: { nuclear@0: for (int i = 0; i < 4; i++) nuclear@0: for (int j = 0; j < 4; j++) nuclear@0: M[i][j] += b.M[i][j]; nuclear@0: return *this; nuclear@0: } nuclear@0: nuclear@0: Matrix4 operator- (const Matrix4& b) const nuclear@0: { nuclear@0: Matrix4 result(*this); nuclear@0: result -= b; nuclear@0: return result; nuclear@0: } nuclear@0: nuclear@0: Matrix4& operator-= (const Matrix4& b) nuclear@0: { nuclear@0: for (int i = 0; i < 4; i++) nuclear@0: for (int j = 0; j < 4; j++) nuclear@0: M[i][j] -= b.M[i][j]; nuclear@0: return *this; nuclear@0: } nuclear@0: nuclear@0: // Multiplies two matrices into destination with minimum copying. nuclear@0: static Matrix4& Multiply(Matrix4* d, const Matrix4& a, const Matrix4& b) nuclear@0: { nuclear@0: OVR_ASSERT((d != &a) && (d != &b)); nuclear@0: int i = 0; nuclear@0: do { nuclear@0: 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: 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: 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: 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: } while((++i) < 4); nuclear@0: nuclear@0: return *d; nuclear@0: } nuclear@0: nuclear@0: Matrix4 operator* (const Matrix4& b) const nuclear@0: { nuclear@0: Matrix4 result(Matrix4::NoInit); nuclear@0: Multiply(&result, *this, b); nuclear@0: return result; nuclear@0: } nuclear@0: nuclear@0: Matrix4& operator*= (const Matrix4& b) nuclear@0: { nuclear@0: return Multiply(this, Matrix4(*this), b); nuclear@0: } nuclear@0: nuclear@0: Matrix4 operator* (T s) const nuclear@0: { nuclear@0: Matrix4 result(*this); nuclear@0: result *= s; nuclear@0: return result; nuclear@0: } nuclear@0: nuclear@0: Matrix4& operator*= (T s) nuclear@0: { nuclear@0: for (int i = 0; i < 4; i++) nuclear@0: for (int j = 0; j < 4; j++) nuclear@0: M[i][j] *= s; nuclear@0: return *this; nuclear@0: } nuclear@0: nuclear@0: nuclear@0: Matrix4 operator/ (T s) const nuclear@0: { nuclear@0: Matrix4 result(*this); nuclear@0: result /= s; nuclear@0: return result; nuclear@0: } nuclear@0: nuclear@0: Matrix4& operator/= (T s) nuclear@0: { nuclear@0: for (int i = 0; i < 4; i++) nuclear@0: for (int j = 0; j < 4; j++) nuclear@0: M[i][j] /= s; nuclear@0: return *this; nuclear@0: } nuclear@0: nuclear@0: Vector3 Transform(const Vector3& v) const nuclear@0: { nuclear@0: 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: return Vector3((M[0][0] * v.x + M[0][1] * v.y + M[0][2] * v.z + M[0][3]) * rcpW, nuclear@0: (M[1][0] * v.x + M[1][1] * v.y + M[1][2] * v.z + M[1][3]) * rcpW, nuclear@0: (M[2][0] * v.x + M[2][1] * v.y + M[2][2] * v.z + M[2][3]) * rcpW); nuclear@0: } nuclear@0: nuclear@0: Vector4 Transform(const Vector4& v) const nuclear@0: { nuclear@0: return Vector4(M[0][0] * v.x + M[0][1] * v.y + M[0][2] * v.z + M[0][3] * v.w, nuclear@0: M[1][0] * v.x + M[1][1] * v.y + M[1][2] * v.z + M[1][3] * v.w, nuclear@0: M[2][0] * v.x + M[2][1] * v.y + M[2][2] * v.z + M[2][3] * v.w, nuclear@0: M[3][0] * v.x + M[3][1] * v.y + M[3][2] * v.z + M[3][3] * v.w); nuclear@0: } nuclear@0: nuclear@0: Matrix4 Transposed() const nuclear@0: { nuclear@0: return Matrix4(M[0][0], M[1][0], M[2][0], M[3][0], nuclear@0: M[0][1], M[1][1], M[2][1], M[3][1], nuclear@0: M[0][2], M[1][2], M[2][2], M[3][2], nuclear@0: M[0][3], M[1][3], M[2][3], M[3][3]); nuclear@0: } nuclear@0: nuclear@0: void Transpose() nuclear@0: { nuclear@0: *this = Transposed(); nuclear@0: } nuclear@0: nuclear@0: nuclear@0: T SubDet (const size_t* rows, const size_t* cols) const nuclear@0: { nuclear@0: 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: - 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: + 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: } nuclear@0: nuclear@0: T Cofactor(size_t I, size_t J) const nuclear@0: { nuclear@0: const size_t indices[4][3] = {{1,2,3},{0,2,3},{0,1,3},{0,1,2}}; nuclear@0: return ((I+J)&1) ? -SubDet(indices[I],indices[J]) : SubDet(indices[I],indices[J]); nuclear@0: } nuclear@0: nuclear@0: T Determinant() const nuclear@0: { nuclear@0: 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: } nuclear@0: nuclear@0: Matrix4 Adjugated() const nuclear@0: { nuclear@0: return Matrix4(Cofactor(0,0), Cofactor(1,0), Cofactor(2,0), Cofactor(3,0), nuclear@0: Cofactor(0,1), Cofactor(1,1), Cofactor(2,1), Cofactor(3,1), nuclear@0: Cofactor(0,2), Cofactor(1,2), Cofactor(2,2), Cofactor(3,2), nuclear@0: Cofactor(0,3), Cofactor(1,3), Cofactor(2,3), Cofactor(3,3)); nuclear@0: } nuclear@0: nuclear@0: Matrix4 Inverted() const nuclear@0: { nuclear@0: T det = Determinant(); nuclear@0: assert(det != 0); nuclear@0: return Adjugated() * (1.0f/det); nuclear@0: } nuclear@0: nuclear@0: void Invert() nuclear@0: { nuclear@0: *this = Inverted(); nuclear@0: } nuclear@0: nuclear@0: // This is more efficient than general inverse, but ONLY works nuclear@0: // correctly if it is a homogeneous transform matrix (rot + trans) nuclear@0: Matrix4 InvertedHomogeneousTransform() const nuclear@0: { nuclear@0: // Make the inverse rotation matrix nuclear@0: Matrix4 rinv = this->Transposed(); nuclear@0: rinv.M[3][0] = rinv.M[3][1] = rinv.M[3][2] = 0.0f; nuclear@0: // Make the inverse translation matrix nuclear@0: Vector3 tvinv(-M[0][3],-M[1][3],-M[2][3]); nuclear@0: Matrix4 tinv = Matrix4::Translation(tvinv); nuclear@0: return rinv * tinv; // "untranslate", then "unrotate" nuclear@0: } nuclear@0: nuclear@0: // This is more efficient than general inverse, but ONLY works nuclear@0: // correctly if it is a homogeneous transform matrix (rot + trans) nuclear@0: void InvertHomogeneousTransform() nuclear@0: { nuclear@0: *this = InvertedHomogeneousTransform(); nuclear@0: } nuclear@0: nuclear@0: // Matrix to Euler Angles conversion nuclear@0: // a,b,c, are the YawPitchRoll angles to be returned nuclear@0: // rotation a around axis A1 nuclear@0: // is followed by rotation b around axis A2 nuclear@0: // is followed by rotation c around axis A3 nuclear@0: // rotations are CCW or CW (D) in LH or RH coordinate system (S) nuclear@0: template nuclear@0: void ToEulerAngles(T *a, T *b, T *c) const nuclear@0: { nuclear@0: static_assert((A1 != A2) && (A2 != A3) && (A1 != A3), "(A1 != A2) && (A2 != A3) && (A1 != A3)"); nuclear@0: nuclear@0: T psign = -1; nuclear@0: if (((A1 + 1) % 3 == A2) && ((A2 + 1) % 3 == A3)) // Determine whether even permutation nuclear@0: psign = 1; nuclear@0: nuclear@0: T pm = psign*M[A1][A3]; nuclear@0: if (pm < -1.0f + Math::SingularityRadius) nuclear@0: { // South pole singularity nuclear@0: *a = 0; nuclear@0: *b = -S*D*((T)MATH_DOUBLE_PIOVER2); nuclear@0: *c = S*D*atan2( psign*M[A2][A1], M[A2][A2] ); nuclear@0: } nuclear@0: else if (pm > 1.0f - Math::SingularityRadius) nuclear@0: { // North pole singularity nuclear@0: *a = 0; nuclear@0: *b = S*D*((T)MATH_DOUBLE_PIOVER2); nuclear@0: *c = S*D*atan2( psign*M[A2][A1], M[A2][A2] ); nuclear@0: } nuclear@0: else nuclear@0: { // Normal case (nonsingular) nuclear@0: *a = S*D*atan2( -psign*M[A2][A3], M[A3][A3] ); nuclear@0: *b = S*D*asin(pm); nuclear@0: *c = S*D*atan2( -psign*M[A1][A2], M[A1][A1] ); nuclear@0: } nuclear@0: nuclear@0: return; nuclear@0: } nuclear@0: nuclear@0: // Matrix to Euler Angles conversion nuclear@0: // a,b,c, are the YawPitchRoll angles to be returned nuclear@0: // rotation a around axis A1 nuclear@0: // is followed by rotation b around axis A2 nuclear@0: // is followed by rotation c around axis A1 nuclear@0: // rotations are CCW or CW (D) in LH or RH coordinate system (S) nuclear@0: template nuclear@0: void ToEulerAnglesABA(T *a, T *b, T *c) const nuclear@0: { nuclear@0: static_assert(A1 != A2, "A1 != A2"); nuclear@0: nuclear@0: // Determine the axis that was not supplied nuclear@0: int m = 3 - A1 - A2; nuclear@0: nuclear@0: T psign = -1; nuclear@0: if ((A1 + 1) % 3 == A2) // Determine whether even permutation nuclear@0: psign = 1.0f; nuclear@0: nuclear@0: T c2 = M[A1][A1]; nuclear@0: if (c2 < -1 + Math::SingularityRadius) nuclear@0: { // South pole singularity nuclear@0: *a = 0; nuclear@0: *b = S*D*((T)MATH_DOUBLE_PI); nuclear@0: *c = S*D*atan2( -psign*M[A2][m],M[A2][A2]); nuclear@0: } nuclear@0: else if (c2 > 1.0f - Math::SingularityRadius) nuclear@0: { // North pole singularity nuclear@0: *a = 0; nuclear@0: *b = 0; nuclear@0: *c = S*D*atan2( -psign*M[A2][m],M[A2][A2]); nuclear@0: } nuclear@0: else nuclear@0: { // Normal case (nonsingular) nuclear@0: *a = S*D*atan2( M[A2][A1],-psign*M[m][A1]); nuclear@0: *b = S*D*acos(c2); nuclear@0: *c = S*D*atan2( M[A1][A2],psign*M[A1][m]); nuclear@0: } nuclear@0: return; nuclear@0: } nuclear@0: nuclear@0: // Creates a matrix that converts the vertices from one coordinate system nuclear@0: // to another. nuclear@0: static Matrix4 AxisConversion(const WorldAxes& to, const WorldAxes& from) nuclear@0: { nuclear@0: // Holds axis values from the 'to' structure nuclear@0: int toArray[3] = { to.XAxis, to.YAxis, to.ZAxis }; nuclear@0: nuclear@0: // The inverse of the toArray nuclear@0: int inv[4]; nuclear@0: inv[0] = inv[abs(to.XAxis)] = 0; nuclear@0: inv[abs(to.YAxis)] = 1; nuclear@0: inv[abs(to.ZAxis)] = 2; nuclear@0: nuclear@0: Matrix4 m(0, 0, 0, nuclear@0: 0, 0, 0, nuclear@0: 0, 0, 0); nuclear@0: nuclear@0: // Only three values in the matrix need to be changed to 1 or -1. nuclear@0: m.M[inv[abs(from.XAxis)]][0] = T(from.XAxis/toArray[inv[abs(from.XAxis)]]); nuclear@0: m.M[inv[abs(from.YAxis)]][1] = T(from.YAxis/toArray[inv[abs(from.YAxis)]]); nuclear@0: m.M[inv[abs(from.ZAxis)]][2] = T(from.ZAxis/toArray[inv[abs(from.ZAxis)]]); nuclear@0: return m; nuclear@0: } nuclear@0: nuclear@0: nuclear@0: // Creates a matrix for translation by vector nuclear@0: static Matrix4 Translation(const Vector3& v) nuclear@0: { nuclear@0: Matrix4 t; nuclear@0: t.M[0][3] = v.x; nuclear@0: t.M[1][3] = v.y; nuclear@0: t.M[2][3] = v.z; nuclear@0: return t; nuclear@0: } nuclear@0: nuclear@0: // Creates a matrix for translation by vector nuclear@0: static Matrix4 Translation(T x, T y, T z = 0.0f) nuclear@0: { nuclear@0: Matrix4 t; nuclear@0: t.M[0][3] = x; nuclear@0: t.M[1][3] = y; nuclear@0: t.M[2][3] = z; nuclear@0: return t; nuclear@0: } nuclear@0: nuclear@0: // Sets the translation part nuclear@0: void SetTranslation(const Vector3& v) nuclear@0: { nuclear@0: M[0][3] = v.x; nuclear@0: M[1][3] = v.y; nuclear@0: M[2][3] = v.z; nuclear@0: } nuclear@0: nuclear@0: Vector3 GetTranslation() const nuclear@0: { nuclear@0: return Vector3( M[0][3], M[1][3], M[2][3] ); nuclear@0: } nuclear@0: nuclear@0: // Creates a matrix for scaling by vector nuclear@0: static Matrix4 Scaling(const Vector3& v) nuclear@0: { nuclear@0: Matrix4 t; nuclear@0: t.M[0][0] = v.x; nuclear@0: t.M[1][1] = v.y; nuclear@0: t.M[2][2] = v.z; nuclear@0: return t; nuclear@0: } nuclear@0: nuclear@0: // Creates a matrix for scaling by vector nuclear@0: static Matrix4 Scaling(T x, T y, T z) nuclear@0: { nuclear@0: Matrix4 t; nuclear@0: t.M[0][0] = x; nuclear@0: t.M[1][1] = y; nuclear@0: t.M[2][2] = z; nuclear@0: return t; nuclear@0: } nuclear@0: nuclear@0: // Creates a matrix for scaling by constant nuclear@0: static Matrix4 Scaling(T s) nuclear@0: { nuclear@0: Matrix4 t; nuclear@0: t.M[0][0] = s; nuclear@0: t.M[1][1] = s; nuclear@0: t.M[2][2] = s; nuclear@0: return t; nuclear@0: } nuclear@0: nuclear@0: // Simple L1 distance in R^12 nuclear@0: T Distance(const Matrix4& m2) const nuclear@0: { nuclear@0: T d = fabs(M[0][0] - m2.M[0][0]) + fabs(M[0][1] - m2.M[0][1]); nuclear@0: d += fabs(M[0][2] - m2.M[0][2]) + fabs(M[0][3] - m2.M[0][3]); nuclear@0: d += fabs(M[1][0] - m2.M[1][0]) + fabs(M[1][1] - m2.M[1][1]); nuclear@0: d += fabs(M[1][2] - m2.M[1][2]) + fabs(M[1][3] - m2.M[1][3]); nuclear@0: d += fabs(M[2][0] - m2.M[2][0]) + fabs(M[2][1] - m2.M[2][1]); nuclear@0: d += fabs(M[2][2] - m2.M[2][2]) + fabs(M[2][3] - m2.M[2][3]); nuclear@0: d += fabs(M[3][0] - m2.M[3][0]) + fabs(M[3][1] - m2.M[3][1]); nuclear@0: d += fabs(M[3][2] - m2.M[3][2]) + fabs(M[3][3] - m2.M[3][3]); nuclear@0: return d; nuclear@0: } nuclear@0: nuclear@0: // Creates a rotation matrix rotating around the X axis by 'angle' radians. nuclear@0: // Just for quick testing. Not for final API. Need to remove case. nuclear@0: static Matrix4 RotationAxis(Axis A, T angle, RotateDirection d, HandedSystem s) nuclear@0: { nuclear@0: T sina = s * d *sin(angle); nuclear@0: T cosa = cos(angle); nuclear@0: nuclear@0: switch(A) nuclear@0: { nuclear@0: case Axis_X: nuclear@0: return Matrix4(1, 0, 0, nuclear@0: 0, cosa, -sina, nuclear@0: 0, sina, cosa); nuclear@0: case Axis_Y: nuclear@0: return Matrix4(cosa, 0, sina, nuclear@0: 0, 1, 0, nuclear@0: -sina, 0, cosa); nuclear@0: case Axis_Z: nuclear@0: return Matrix4(cosa, -sina, 0, nuclear@0: sina, cosa, 0, nuclear@0: 0, 0, 1); nuclear@0: } nuclear@0: } nuclear@0: nuclear@0: nuclear@0: // Creates a rotation matrix rotating around the X axis by 'angle' radians. nuclear@0: // Rotation direction is depends on the coordinate system: nuclear@0: // RHS (Oculus default): Positive angle values rotate Counter-clockwise (CCW), nuclear@0: // while looking in the negative axis direction. This is the nuclear@0: // same as looking down from positive axis values towards origin. nuclear@0: // LHS: Positive angle values rotate clock-wise (CW), while looking in the nuclear@0: // negative axis direction. nuclear@0: static Matrix4 RotationX(T angle) nuclear@0: { nuclear@0: T sina = sin(angle); nuclear@0: T cosa = cos(angle); nuclear@0: return Matrix4(1, 0, 0, nuclear@0: 0, cosa, -sina, nuclear@0: 0, sina, cosa); nuclear@0: } nuclear@0: nuclear@0: // Creates a rotation matrix rotating around the Y axis by 'angle' radians. nuclear@0: // Rotation direction is depends on the coordinate system: nuclear@0: // RHS (Oculus default): Positive angle values rotate Counter-clockwise (CCW), nuclear@0: // while looking in the negative axis direction. This is the nuclear@0: // same as looking down from positive axis values towards origin. nuclear@0: // LHS: Positive angle values rotate clock-wise (CW), while looking in the nuclear@0: // negative axis direction. nuclear@0: static Matrix4 RotationY(T angle) nuclear@0: { nuclear@0: T sina = sin(angle); nuclear@0: T cosa = cos(angle); nuclear@0: return Matrix4(cosa, 0, sina, nuclear@0: 0, 1, 0, nuclear@0: -sina, 0, cosa); nuclear@0: } nuclear@0: nuclear@0: // Creates a rotation matrix rotating around the Z axis by 'angle' radians. nuclear@0: // Rotation direction is depends on the coordinate system: nuclear@0: // RHS (Oculus default): Positive angle values rotate Counter-clockwise (CCW), nuclear@0: // while looking in the negative axis direction. This is the nuclear@0: // same as looking down from positive axis values towards origin. nuclear@0: // LHS: Positive angle values rotate clock-wise (CW), while looking in the nuclear@0: // negative axis direction. nuclear@0: static Matrix4 RotationZ(T angle) nuclear@0: { nuclear@0: T sina = sin(angle); nuclear@0: T cosa = cos(angle); nuclear@0: return Matrix4(cosa, -sina, 0, nuclear@0: sina, cosa, 0, nuclear@0: 0, 0, 1); nuclear@0: } nuclear@0: nuclear@0: // LookAtRH creates a View transformation matrix for right-handed coordinate system. nuclear@0: // The resulting matrix points camera from 'eye' towards 'at' direction, with 'up' nuclear@0: // specifying the up vector. The resulting matrix should be used with PerspectiveRH nuclear@0: // projection. nuclear@0: static Matrix4 LookAtRH(const Vector3& eye, const Vector3& at, const Vector3& up) nuclear@0: { nuclear@0: Vector3 z = (eye - at).Normalized(); // Forward nuclear@0: Vector3 x = up.Cross(z).Normalized(); // Right nuclear@0: Vector3 y = z.Cross(x); nuclear@0: nuclear@0: Matrix4 m(x.x, x.y, x.z, -(x.Dot(eye)), nuclear@0: y.x, y.y, y.z, -(y.Dot(eye)), nuclear@0: z.x, z.y, z.z, -(z.Dot(eye)), nuclear@0: 0, 0, 0, 1 ); nuclear@0: return m; nuclear@0: } nuclear@0: nuclear@0: // LookAtLH creates a View transformation matrix for left-handed coordinate system. nuclear@0: // The resulting matrix points camera from 'eye' towards 'at' direction, with 'up' nuclear@0: // specifying the up vector. nuclear@0: static Matrix4 LookAtLH(const Vector3& eye, const Vector3& at, const Vector3& up) nuclear@0: { nuclear@0: Vector3 z = (at - eye).Normalized(); // Forward nuclear@0: Vector3 x = up.Cross(z).Normalized(); // Right nuclear@0: Vector3 y = z.Cross(x); nuclear@0: nuclear@0: Matrix4 m(x.x, x.y, x.z, -(x.Dot(eye)), nuclear@0: y.x, y.y, y.z, -(y.Dot(eye)), nuclear@0: z.x, z.y, z.z, -(z.Dot(eye)), nuclear@0: 0, 0, 0, 1 ); nuclear@0: return m; nuclear@0: } nuclear@0: nuclear@0: // PerspectiveRH creates a right-handed perspective projection matrix that can be nuclear@0: // used with the Oculus sample renderer. nuclear@0: // yfov - Specifies vertical field of view in radians. nuclear@0: // aspect - Screen aspect ration, which is usually width/height for square pixels. nuclear@0: // Note that xfov = yfov * aspect. nuclear@0: // znear - Absolute value of near Z clipping clipping range. nuclear@0: // zfar - Absolute value of far Z clipping clipping range (larger then near). nuclear@0: // Even though RHS usually looks in the direction of negative Z, positive values nuclear@0: // are expected for znear and zfar. nuclear@0: static Matrix4 PerspectiveRH(T yfov, T aspect, T znear, T zfar) nuclear@0: { nuclear@0: Matrix4 m; nuclear@0: T tanHalfFov = tan(yfov * 0.5f); nuclear@0: nuclear@0: m.M[0][0] = 1. / (aspect * tanHalfFov); nuclear@0: m.M[1][1] = 1. / tanHalfFov; nuclear@0: m.M[2][2] = zfar / (znear - zfar); nuclear@0: m.M[3][2] = -1.; nuclear@0: m.M[2][3] = (zfar * znear) / (znear - zfar); nuclear@0: m.M[3][3] = 0.; nuclear@0: nuclear@0: // Note: Post-projection matrix result assumes Left-Handed coordinate system, nuclear@0: // with Y up, X right and Z forward. This supports positive z-buffer values. nuclear@0: // This is the case even for RHS coordinate input. nuclear@0: return m; nuclear@0: } nuclear@0: nuclear@0: // PerspectiveLH creates a left-handed perspective projection matrix that can be nuclear@0: // used with the Oculus sample renderer. nuclear@0: // yfov - Specifies vertical field of view in radians. nuclear@0: // aspect - Screen aspect ration, which is usually width/height for square pixels. nuclear@0: // Note that xfov = yfov * aspect. nuclear@0: // znear - Absolute value of near Z clipping clipping range. nuclear@0: // zfar - Absolute value of far Z clipping clipping range (larger then near). nuclear@0: static Matrix4 PerspectiveLH(T yfov, T aspect, T znear, T zfar) nuclear@0: { nuclear@0: Matrix4 m; nuclear@0: T tanHalfFov = tan(yfov * 0.5f); nuclear@0: nuclear@0: m.M[0][0] = 1. / (aspect * tanHalfFov); nuclear@0: m.M[1][1] = 1. / tanHalfFov; nuclear@0: //m.M[2][2] = zfar / (znear - zfar); nuclear@0: m.M[2][2] = zfar / (zfar - znear); nuclear@0: m.M[3][2] = -1.; nuclear@0: m.M[2][3] = (zfar * znear) / (znear - zfar); nuclear@0: m.M[3][3] = 0.; nuclear@0: nuclear@0: // Note: Post-projection matrix result assumes Left-Handed coordinate system, nuclear@0: // with Y up, X right and Z forward. This supports positive z-buffer values. nuclear@0: // This is the case even for RHS coordinate input. nuclear@0: return m; nuclear@0: } nuclear@0: nuclear@0: static Matrix4 Ortho2D(T w, T h) nuclear@0: { nuclear@0: Matrix4 m; nuclear@0: m.M[0][0] = 2.0/w; nuclear@0: m.M[1][1] = -2.0/h; nuclear@0: m.M[0][3] = -1.0; nuclear@0: m.M[1][3] = 1.0; nuclear@0: m.M[2][2] = 0; nuclear@0: return m; nuclear@0: } nuclear@0: }; nuclear@0: nuclear@0: typedef Matrix4 Matrix4f; nuclear@0: typedef Matrix4 Matrix4d; nuclear@0: nuclear@0: //------------------------------------------------------------------------------------- nuclear@0: // ***** Matrix3 nuclear@0: // nuclear@0: // Matrix3 is a 3x3 matrix used for representing a rotation matrix. nuclear@0: // The matrix is stored in row-major order in memory, meaning that values nuclear@0: // of the first row are stored before the next one. nuclear@0: // nuclear@0: // The arrangement of the matrix is chosen to be in Right-Handed nuclear@0: // coordinate system and counterclockwise rotations when looking down nuclear@0: // the axis nuclear@0: // nuclear@0: // Transformation Order: nuclear@0: // - Transformations are applied from right to left, so the expression nuclear@0: // M1 * M2 * M3 * V means that the vector V is transformed by M3 first, nuclear@0: // followed by M2 and M1. nuclear@0: // nuclear@0: // Coordinate system: Right Handed nuclear@0: // nuclear@0: // Rotations: Counterclockwise when looking down the axis. All angles are in radians. nuclear@0: nuclear@0: template nuclear@0: class SymMat3; nuclear@0: nuclear@0: template nuclear@0: class Matrix3 nuclear@0: { nuclear@0: static const Matrix3 IdentityValue; nuclear@0: nuclear@0: public: nuclear@0: T M[3][3]; nuclear@0: nuclear@0: enum NoInitType { NoInit }; nuclear@0: nuclear@0: // Construct with no memory initialization. nuclear@0: Matrix3(NoInitType) { } nuclear@0: nuclear@0: // By default, we construct identity matrix. nuclear@0: Matrix3() nuclear@0: { nuclear@0: SetIdentity(); nuclear@0: } nuclear@0: nuclear@0: Matrix3(T m11, T m12, T m13, nuclear@0: T m21, T m22, T m23, nuclear@0: T m31, T m32, T m33) nuclear@0: { nuclear@0: M[0][0] = m11; M[0][1] = m12; M[0][2] = m13; nuclear@0: M[1][0] = m21; M[1][1] = m22; M[1][2] = m23; nuclear@0: M[2][0] = m31; M[2][1] = m32; M[2][2] = m33; nuclear@0: } nuclear@0: nuclear@0: /* nuclear@0: explicit Matrix3(const Quat& q) nuclear@0: { nuclear@0: T ww = q.w*q.w; nuclear@0: T xx = q.x*q.x; nuclear@0: T yy = q.y*q.y; nuclear@0: T zz = q.z*q.z; nuclear@0: nuclear@0: M[0][0] = ww + xx - yy - zz; M[0][1] = 2 * (q.x*q.y - q.w*q.z); M[0][2] = 2 * (q.x*q.z + q.w*q.y); nuclear@0: M[1][0] = 2 * (q.x*q.y + q.w*q.z); M[1][1] = ww - xx + yy - zz; M[1][2] = 2 * (q.y*q.z - q.w*q.x); nuclear@0: M[2][0] = 2 * (q.x*q.z - q.w*q.y); M[2][1] = 2 * (q.y*q.z + q.w*q.x); M[2][2] = ww - xx - yy + zz; nuclear@0: } nuclear@0: */ nuclear@0: nuclear@0: explicit Matrix3(const Quat& q) nuclear@0: { nuclear@0: const T tx = q.x+q.x, ty = q.y+q.y, tz = q.z+q.z; nuclear@0: const T twx = q.w*tx, twy = q.w*ty, twz = q.w*tz; nuclear@0: const T txx = q.x*tx, txy = q.x*ty, txz = q.x*tz; nuclear@0: const T tyy = q.y*ty, tyz = q.y*tz, tzz = q.z*tz; nuclear@0: M[0][0] = T(1) - (tyy + tzz); M[0][1] = txy - twz; M[0][2] = txz + twy; nuclear@0: M[1][0] = txy + twz; M[1][1] = T(1) - (txx + tzz); M[1][2] = tyz - twx; nuclear@0: M[2][0] = txz - twy; M[2][1] = tyz + twx; M[2][2] = T(1) - (txx + tyy); nuclear@0: } nuclear@0: nuclear@0: inline explicit Matrix3(T s) nuclear@0: { nuclear@0: M[0][0] = M[1][1] = M[2][2] = s; nuclear@0: M[0][1] = M[0][2] = M[1][0] = M[1][2] = M[2][0] = M[2][1] = 0; nuclear@0: } nuclear@0: nuclear@0: explicit Matrix3(const Pose& p) nuclear@0: { nuclear@0: Matrix3 result(p.Rotation); nuclear@0: result.SetTranslation(p.Translation); nuclear@0: *this = result; nuclear@0: } nuclear@0: nuclear@0: // C-interop support nuclear@0: explicit Matrix3(const Matrix4::OtherFloatType> &src) nuclear@0: { nuclear@0: for (int i = 0; i < 3; i++) nuclear@0: for (int j = 0; j < 3; j++) nuclear@0: M[i][j] = (T)src.M[i][j]; nuclear@0: } nuclear@0: nuclear@0: // C-interop support. nuclear@0: Matrix3(const typename CompatibleTypes >::Type& s) nuclear@0: { nuclear@0: static_assert(sizeof(s) == sizeof(Matrix3), "sizeof(s) == sizeof(Matrix3)"); nuclear@0: memcpy(M, s.M, sizeof(M)); nuclear@0: } nuclear@0: nuclear@0: operator const typename CompatibleTypes >::Type () const nuclear@0: { nuclear@0: typename CompatibleTypes >::Type result; nuclear@0: static_assert(sizeof(result) == sizeof(Matrix3), "sizeof(result) == sizeof(Matrix3)"); nuclear@0: memcpy(result.M, M, sizeof(M)); nuclear@0: return result; nuclear@0: } nuclear@0: nuclear@0: void ToString(char* dest, size_t destsize) const nuclear@0: { nuclear@0: size_t pos = 0; nuclear@0: for (int r=0; r<3; r++) nuclear@0: for (int c=0; c<3; c++) nuclear@0: pos += OVR_sprintf(dest+pos, destsize-pos, "%g ", M[r][c]); nuclear@0: } nuclear@0: nuclear@0: static Matrix3 FromString(const char* src) nuclear@0: { nuclear@0: Matrix3 result; nuclear@0: for (int r=0; r<3; r++) nuclear@0: for (int c=0; c<3; c++) nuclear@0: { nuclear@0: result.M[r][c] = (T)atof(src); nuclear@0: while (src && *src != ' ') nuclear@0: src++; nuclear@0: while (src && *src == ' ') nuclear@0: src++; nuclear@0: } nuclear@0: return result; nuclear@0: } nuclear@0: nuclear@0: static const Matrix3& Identity() { return IdentityValue; } nuclear@0: nuclear@0: void SetIdentity() nuclear@0: { nuclear@0: M[0][0] = M[1][1] = M[2][2] = 1; nuclear@0: M[0][1] = M[1][0] = M[2][0] = 0; nuclear@0: M[0][2] = M[1][2] = M[2][1] = 0; nuclear@0: } nuclear@0: nuclear@0: bool operator== (const Matrix3& b) const nuclear@0: { nuclear@0: bool isEqual = true; nuclear@0: for (int i = 0; i < 3; i++) nuclear@0: for (int j = 0; j < 3; j++) nuclear@0: isEqual &= (M[i][j] == b.M[i][j]); nuclear@0: nuclear@0: return isEqual; nuclear@0: } nuclear@0: nuclear@0: Matrix3 operator+ (const Matrix3& b) const nuclear@0: { nuclear@0: Matrix4 result(*this); nuclear@0: result += b; nuclear@0: return result; nuclear@0: } nuclear@0: nuclear@0: Matrix3& operator+= (const Matrix3& b) nuclear@0: { nuclear@0: for (int i = 0; i < 3; i++) nuclear@0: for (int j = 0; j < 3; j++) nuclear@0: M[i][j] += b.M[i][j]; nuclear@0: return *this; nuclear@0: } nuclear@0: nuclear@0: void operator= (const Matrix3& b) nuclear@0: { nuclear@0: for (int i = 0; i < 3; i++) nuclear@0: for (int j = 0; j < 3; j++) nuclear@0: M[i][j] = b.M[i][j]; nuclear@0: return; nuclear@0: } nuclear@0: nuclear@0: void operator= (const SymMat3& b) nuclear@0: { nuclear@0: for (int i = 0; i < 3; i++) nuclear@0: for (int j = 0; j < 3; j++) nuclear@0: M[i][j] = 0; nuclear@0: nuclear@0: M[0][0] = b.v[0]; nuclear@0: M[0][1] = b.v[1]; nuclear@0: M[0][2] = b.v[2]; nuclear@0: M[1][1] = b.v[3]; nuclear@0: M[1][2] = b.v[4]; nuclear@0: M[2][2] = b.v[5]; nuclear@0: nuclear@0: return; nuclear@0: } nuclear@0: nuclear@0: Matrix3 operator- (const Matrix3& b) const nuclear@0: { nuclear@0: Matrix3 result(*this); nuclear@0: result -= b; nuclear@0: return result; nuclear@0: } nuclear@0: nuclear@0: Matrix3& operator-= (const Matrix3& b) nuclear@0: { nuclear@0: for (int i = 0; i < 3; i++) nuclear@0: for (int j = 0; j < 3; j++) nuclear@0: M[i][j] -= b.M[i][j]; nuclear@0: return *this; nuclear@0: } nuclear@0: nuclear@0: // Multiplies two matrices into destination with minimum copying. nuclear@0: static Matrix3& Multiply(Matrix3* d, const Matrix3& a, const Matrix3& b) nuclear@0: { nuclear@0: OVR_ASSERT((d != &a) && (d != &b)); nuclear@0: int i = 0; nuclear@0: do { nuclear@0: 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: 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: 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: } while((++i) < 3); nuclear@0: nuclear@0: return *d; nuclear@0: } nuclear@0: nuclear@0: Matrix3 operator* (const Matrix3& b) const nuclear@0: { nuclear@0: Matrix3 result(Matrix3::NoInit); nuclear@0: Multiply(&result, *this, b); nuclear@0: return result; nuclear@0: } nuclear@0: nuclear@0: Matrix3& operator*= (const Matrix3& b) nuclear@0: { nuclear@0: return Multiply(this, Matrix3(*this), b); nuclear@0: } nuclear@0: nuclear@0: Matrix3 operator* (T s) const nuclear@0: { nuclear@0: Matrix3 result(*this); nuclear@0: result *= s; nuclear@0: return result; nuclear@0: } nuclear@0: nuclear@0: Matrix3& operator*= (T s) nuclear@0: { nuclear@0: for (int i = 0; i < 3; i++) nuclear@0: for (int j = 0; j < 3; j++) nuclear@0: M[i][j] *= s; nuclear@0: return *this; nuclear@0: } nuclear@0: nuclear@0: Vector3 operator* (const Vector3 &b) const nuclear@0: { nuclear@0: Vector3 result; nuclear@0: result.x = M[0][0]*b.x + M[0][1]*b.y + M[0][2]*b.z; nuclear@0: result.y = M[1][0]*b.x + M[1][1]*b.y + M[1][2]*b.z; nuclear@0: result.z = M[2][0]*b.x + M[2][1]*b.y + M[2][2]*b.z; nuclear@0: nuclear@0: return result; nuclear@0: } nuclear@0: nuclear@0: Matrix3 operator/ (T s) const nuclear@0: { nuclear@0: Matrix3 result(*this); nuclear@0: result /= s; nuclear@0: return result; nuclear@0: } nuclear@0: nuclear@0: Matrix3& operator/= (T s) nuclear@0: { nuclear@0: for (int i = 0; i < 3; i++) nuclear@0: for (int j = 0; j < 3; j++) nuclear@0: M[i][j] /= s; nuclear@0: return *this; nuclear@0: } nuclear@0: nuclear@0: Vector2 Transform(const Vector2& v) const nuclear@0: { nuclear@0: const float rcpZ = 1.0f / (M[2][0] * v.x + M[2][1] * v.y + M[2][2]); nuclear@0: return Vector2((M[0][0] * v.x + M[0][1] * v.y + M[0][2]) * rcpZ, nuclear@0: (M[1][0] * v.x + M[1][1] * v.y + M[1][2]) * rcpZ); nuclear@0: } nuclear@0: nuclear@0: Vector3 Transform(const Vector3& v) const nuclear@0: { nuclear@0: return Vector3(M[0][0] * v.x + M[0][1] * v.y + M[0][2] * v.z, nuclear@0: M[1][0] * v.x + M[1][1] * v.y + M[1][2] * v.z, nuclear@0: M[2][0] * v.x + M[2][1] * v.y + M[2][2] * v.z); nuclear@0: } nuclear@0: nuclear@0: Matrix3 Transposed() const nuclear@0: { nuclear@0: return Matrix3(M[0][0], M[1][0], M[2][0], nuclear@0: M[0][1], M[1][1], M[2][1], nuclear@0: M[0][2], M[1][2], M[2][2]); nuclear@0: } nuclear@0: nuclear@0: void Transpose() nuclear@0: { nuclear@0: *this = Transposed(); nuclear@0: } nuclear@0: nuclear@0: nuclear@0: T SubDet (const size_t* rows, const size_t* cols) const nuclear@0: { nuclear@0: 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: - 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: + 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: } nuclear@0: nuclear@0: // M += a*b.t() nuclear@0: inline void Rank1Add(const Vector3 &a, const Vector3 &b) nuclear@0: { nuclear@0: M[0][0] += a.x*b.x; M[0][1] += a.x*b.y; M[0][2] += a.x*b.z; nuclear@0: M[1][0] += a.y*b.x; M[1][1] += a.y*b.y; M[1][2] += a.y*b.z; nuclear@0: M[2][0] += a.z*b.x; M[2][1] += a.z*b.y; M[2][2] += a.z*b.z; nuclear@0: } nuclear@0: nuclear@0: // M -= a*b.t() nuclear@0: inline void Rank1Sub(const Vector3 &a, const Vector3 &b) nuclear@0: { nuclear@0: M[0][0] -= a.x*b.x; M[0][1] -= a.x*b.y; M[0][2] -= a.x*b.z; nuclear@0: M[1][0] -= a.y*b.x; M[1][1] -= a.y*b.y; M[1][2] -= a.y*b.z; nuclear@0: M[2][0] -= a.z*b.x; M[2][1] -= a.z*b.y; M[2][2] -= a.z*b.z; nuclear@0: } nuclear@0: nuclear@0: inline Vector3 Col(int c) const nuclear@0: { nuclear@0: return Vector3(M[0][c], M[1][c], M[2][c]); nuclear@0: } nuclear@0: nuclear@0: inline Vector3 Row(int r) const nuclear@0: { nuclear@0: return Vector3(M[r][0], M[r][1], M[r][2]); nuclear@0: } nuclear@0: nuclear@0: inline T Determinant() const nuclear@0: { nuclear@0: const Matrix3& m = *this; nuclear@0: T d; nuclear@0: nuclear@0: d = m.M[0][0] * (m.M[1][1]*m.M[2][2] - m.M[1][2] * m.M[2][1]); nuclear@0: d -= m.M[0][1] * (m.M[1][0]*m.M[2][2] - m.M[1][2] * m.M[2][0]); nuclear@0: d += m.M[0][2] * (m.M[1][0]*m.M[2][1] - m.M[1][1] * m.M[2][0]); nuclear@0: nuclear@0: return d; nuclear@0: } nuclear@0: nuclear@0: inline Matrix3 Inverse() const nuclear@0: { nuclear@0: Matrix3 a; nuclear@0: const Matrix3& m = *this; nuclear@0: T d = Determinant(); nuclear@0: nuclear@0: assert(d != 0); nuclear@0: T s = T(1)/d; nuclear@0: nuclear@0: a.M[0][0] = s * (m.M[1][1] * m.M[2][2] - m.M[1][2] * m.M[2][1]); nuclear@0: a.M[1][0] = s * (m.M[1][2] * m.M[2][0] - m.M[1][0] * m.M[2][2]); nuclear@0: a.M[2][0] = s * (m.M[1][0] * m.M[2][1] - m.M[1][1] * m.M[2][0]); nuclear@0: nuclear@0: a.M[0][1] = s * (m.M[0][2] * m.M[2][1] - m.M[0][1] * m.M[2][2]); nuclear@0: a.M[1][1] = s * (m.M[0][0] * m.M[2][2] - m.M[0][2] * m.M[2][0]); nuclear@0: a.M[2][1] = s * (m.M[0][1] * m.M[2][0] - m.M[0][0] * m.M[2][1]); nuclear@0: nuclear@0: a.M[0][2] = s * (m.M[0][1] * m.M[1][2] - m.M[0][2] * m.M[1][1]); nuclear@0: a.M[1][2] = s * (m.M[0][2] * m.M[1][0] - m.M[0][0] * m.M[1][2]); nuclear@0: a.M[2][2] = s * (m.M[0][0] * m.M[1][1] - m.M[0][1] * m.M[1][0]); nuclear@0: nuclear@0: return a; nuclear@0: } nuclear@0: nuclear@0: }; nuclear@0: nuclear@0: typedef Matrix3 Matrix3f; nuclear@0: typedef Matrix3 Matrix3d; nuclear@0: nuclear@0: //------------------------------------------------------------------------------------- nuclear@0: nuclear@0: template nuclear@0: class SymMat3 nuclear@0: { nuclear@0: private: nuclear@0: typedef SymMat3 this_type; nuclear@0: nuclear@0: public: nuclear@0: typedef T Value_t; nuclear@0: // Upper symmetric nuclear@0: T v[6]; // _00 _01 _02 _11 _12 _22 nuclear@0: nuclear@0: inline SymMat3() {} nuclear@0: nuclear@0: inline explicit SymMat3(T s) nuclear@0: { nuclear@0: v[0] = v[3] = v[5] = s; nuclear@0: v[1] = v[2] = v[4] = 0; nuclear@0: } nuclear@0: nuclear@0: inline explicit SymMat3(T a00, T a01, T a02, T a11, T a12, T a22) nuclear@0: { nuclear@0: v[0] = a00; v[1] = a01; v[2] = a02; nuclear@0: v[3] = a11; v[4] = a12; nuclear@0: v[5] = a22; nuclear@0: } nuclear@0: nuclear@0: static inline int Index(unsigned int i, unsigned int j) nuclear@0: { nuclear@0: return (i <= j) ? (3*i - i*(i+1)/2 + j) : (3*j - j*(j+1)/2 + i); nuclear@0: } nuclear@0: nuclear@0: inline T operator()(int i, int j) const { return v[Index(i,j)]; } nuclear@0: nuclear@0: inline T &operator()(int i, int j) { return v[Index(i,j)]; } nuclear@0: nuclear@0: template nuclear@0: inline SymMat3 CastTo() const nuclear@0: { nuclear@0: return SymMat3(static_cast(v[0]), static_cast(v[1]), static_cast(v[2]), nuclear@0: static_cast(v[3]), static_cast(v[4]), static_cast(v[5])); nuclear@0: } nuclear@0: nuclear@0: inline this_type& operator+=(const this_type& b) nuclear@0: { nuclear@0: v[0]+=b.v[0]; nuclear@0: v[1]+=b.v[1]; nuclear@0: v[2]+=b.v[2]; nuclear@0: v[3]+=b.v[3]; nuclear@0: v[4]+=b.v[4]; nuclear@0: v[5]+=b.v[5]; nuclear@0: return *this; nuclear@0: } nuclear@0: nuclear@0: inline this_type& operator-=(const this_type& b) nuclear@0: { nuclear@0: v[0]-=b.v[0]; nuclear@0: v[1]-=b.v[1]; nuclear@0: v[2]-=b.v[2]; nuclear@0: v[3]-=b.v[3]; nuclear@0: v[4]-=b.v[4]; nuclear@0: v[5]-=b.v[5]; nuclear@0: nuclear@0: return *this; nuclear@0: } nuclear@0: nuclear@0: inline this_type& operator*=(T s) nuclear@0: { nuclear@0: v[0]*=s; nuclear@0: v[1]*=s; nuclear@0: v[2]*=s; nuclear@0: v[3]*=s; nuclear@0: v[4]*=s; nuclear@0: v[5]*=s; nuclear@0: nuclear@0: return *this; nuclear@0: } nuclear@0: nuclear@0: inline SymMat3 operator*(T s) const nuclear@0: { nuclear@0: SymMat3 d; nuclear@0: d.v[0] = v[0]*s; nuclear@0: d.v[1] = v[1]*s; nuclear@0: d.v[2] = v[2]*s; nuclear@0: d.v[3] = v[3]*s; nuclear@0: d.v[4] = v[4]*s; nuclear@0: d.v[5] = v[5]*s; nuclear@0: nuclear@0: return d; nuclear@0: } nuclear@0: nuclear@0: // Multiplies two matrices into destination with minimum copying. nuclear@0: static SymMat3& Multiply(SymMat3* d, const SymMat3& a, const SymMat3& b) nuclear@0: { nuclear@0: // _00 _01 _02 _11 _12 _22 nuclear@0: nuclear@0: d->v[0] = a.v[0] * b.v[0]; nuclear@0: d->v[1] = a.v[0] * b.v[1] + a.v[1] * b.v[3]; nuclear@0: d->v[2] = a.v[0] * b.v[2] + a.v[1] * b.v[4]; nuclear@0: nuclear@0: d->v[3] = a.v[3] * b.v[3]; nuclear@0: d->v[4] = a.v[3] * b.v[4] + a.v[4] * b.v[5]; nuclear@0: nuclear@0: d->v[5] = a.v[5] * b.v[5]; nuclear@0: nuclear@0: return *d; nuclear@0: } nuclear@0: nuclear@0: inline T Determinant() const nuclear@0: { nuclear@0: const this_type& m = *this; nuclear@0: T d; nuclear@0: nuclear@0: d = m(0,0) * (m(1,1)*m(2,2) - m(1,2) * m(2,1)); nuclear@0: d -= m(0,1) * (m(1,0)*m(2,2) - m(1,2) * m(2,0)); nuclear@0: d += m(0,2) * (m(1,0)*m(2,1) - m(1,1) * m(2,0)); nuclear@0: nuclear@0: return d; nuclear@0: } nuclear@0: nuclear@0: inline this_type Inverse() const nuclear@0: { nuclear@0: this_type a; nuclear@0: const this_type& m = *this; nuclear@0: T d = Determinant(); nuclear@0: nuclear@0: assert(d != 0); nuclear@0: T s = T(1)/d; nuclear@0: nuclear@0: a(0,0) = s * (m(1,1) * m(2,2) - m(1,2) * m(2,1)); nuclear@0: nuclear@0: a(0,1) = s * (m(0,2) * m(2,1) - m(0,1) * m(2,2)); nuclear@0: a(1,1) = s * (m(0,0) * m(2,2) - m(0,2) * m(2,0)); nuclear@0: nuclear@0: a(0,2) = s * (m(0,1) * m(1,2) - m(0,2) * m(1,1)); nuclear@0: a(1,2) = s * (m(0,2) * m(1,0) - m(0,0) * m(1,2)); nuclear@0: a(2,2) = s * (m(0,0) * m(1,1) - m(0,1) * m(1,0)); nuclear@0: nuclear@0: return a; nuclear@0: } nuclear@0: nuclear@0: inline T Trace() const { return v[0] + v[3] + v[5]; } nuclear@0: nuclear@0: // M = a*a.t() nuclear@0: inline void Rank1(const Vector3 &a) nuclear@0: { nuclear@0: v[0] = a.x*a.x; v[1] = a.x*a.y; v[2] = a.x*a.z; nuclear@0: v[3] = a.y*a.y; v[4] = a.y*a.z; nuclear@0: v[5] = a.z*a.z; nuclear@0: } nuclear@0: nuclear@0: // M += a*a.t() nuclear@0: inline void Rank1Add(const Vector3 &a) nuclear@0: { nuclear@0: v[0] += a.x*a.x; v[1] += a.x*a.y; v[2] += a.x*a.z; nuclear@0: v[3] += a.y*a.y; v[4] += a.y*a.z; nuclear@0: v[5] += a.z*a.z; nuclear@0: } nuclear@0: nuclear@0: // M -= a*a.t() nuclear@0: inline void Rank1Sub(const Vector3 &a) nuclear@0: { nuclear@0: v[0] -= a.x*a.x; v[1] -= a.x*a.y; v[2] -= a.x*a.z; nuclear@0: v[3] -= a.y*a.y; v[4] -= a.y*a.z; nuclear@0: v[5] -= a.z*a.z; nuclear@0: } nuclear@0: }; nuclear@0: nuclear@0: typedef SymMat3 SymMat3f; nuclear@0: typedef SymMat3 SymMat3d; nuclear@0: nuclear@0: template nuclear@0: inline Matrix3 operator*(const SymMat3& a, const SymMat3& b) nuclear@0: { nuclear@0: #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: return Matrix3( nuclear@0: AJB_ARBC(0,0), AJB_ARBC(0,1), AJB_ARBC(0,2), nuclear@0: AJB_ARBC(1,0), AJB_ARBC(1,1), AJB_ARBC(1,2), nuclear@0: AJB_ARBC(2,0), AJB_ARBC(2,1), AJB_ARBC(2,2)); nuclear@0: #undef AJB_ARBC nuclear@0: } nuclear@0: nuclear@0: template nuclear@0: inline Matrix3 operator*(const Matrix3& a, const SymMat3& b) nuclear@0: { nuclear@0: #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: return Matrix3( nuclear@0: AJB_ARBC(0,0), AJB_ARBC(0,1), AJB_ARBC(0,2), nuclear@0: AJB_ARBC(1,0), AJB_ARBC(1,1), AJB_ARBC(1,2), nuclear@0: AJB_ARBC(2,0), AJB_ARBC(2,1), AJB_ARBC(2,2)); nuclear@0: #undef AJB_ARBC nuclear@0: } nuclear@0: nuclear@0: //------------------------------------------------------------------------------------- nuclear@0: // ***** Angle nuclear@0: nuclear@0: // Cleanly representing the algebra of 2D rotations. nuclear@0: // The operations maintain the angle between -Pi and Pi, the same range as atan2. nuclear@0: nuclear@0: template nuclear@0: class Angle nuclear@0: { nuclear@0: public: nuclear@0: enum AngularUnits nuclear@0: { nuclear@0: Radians = 0, nuclear@0: Degrees = 1 nuclear@0: }; nuclear@0: nuclear@0: Angle() : a(0) {} nuclear@0: nuclear@0: // Fix the range to be between -Pi and Pi nuclear@0: Angle(T a_, AngularUnits u = Radians) : a((u == Radians) ? a_ : a_*((T)MATH_DOUBLE_DEGREETORADFACTOR)) { FixRange(); } nuclear@0: nuclear@0: T Get(AngularUnits u = Radians) const { return (u == Radians) ? a : a*((T)MATH_DOUBLE_RADTODEGREEFACTOR); } nuclear@0: void Set(const T& x, AngularUnits u = Radians) { a = (u == Radians) ? x : x*((T)MATH_DOUBLE_DEGREETORADFACTOR); FixRange(); } nuclear@0: int Sign() const { if (a == 0) return 0; else return (a > 0) ? 1 : -1; } nuclear@0: T Abs() const { return (a > 0) ? a : -a; } nuclear@0: nuclear@0: bool operator== (const Angle& b) const { return a == b.a; } nuclear@0: bool operator!= (const Angle& b) const { return a != b.a; } nuclear@0: // bool operator< (const Angle& b) const { return a < a.b; } nuclear@0: // bool operator> (const Angle& b) const { return a > a.b; } nuclear@0: // bool operator<= (const Angle& b) const { return a <= a.b; } nuclear@0: // bool operator>= (const Angle& b) const { return a >= a.b; } nuclear@0: // bool operator= (const T& x) { a = x; FixRange(); } nuclear@0: nuclear@0: // These operations assume a is already between -Pi and Pi. nuclear@0: Angle& operator+= (const Angle& b) { a = a + b.a; FastFixRange(); return *this; } nuclear@0: Angle& operator+= (const T& x) { a = a + x; FixRange(); return *this; } nuclear@0: Angle operator+ (const Angle& b) const { Angle res = *this; res += b; return res; } nuclear@0: Angle operator+ (const T& x) const { Angle res = *this; res += x; return res; } nuclear@0: Angle& operator-= (const Angle& b) { a = a - b.a; FastFixRange(); return *this; } nuclear@0: Angle& operator-= (const T& x) { a = a - x; FixRange(); return *this; } nuclear@0: Angle operator- (const Angle& b) const { Angle res = *this; res -= b; return res; } nuclear@0: Angle operator- (const T& x) const { Angle res = *this; res -= x; return res; } nuclear@0: nuclear@0: 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: nuclear@0: private: nuclear@0: nuclear@0: // The stored angle, which should be maintained between -Pi and Pi nuclear@0: T a; nuclear@0: nuclear@0: // Fixes the angle range to [-Pi,Pi], but assumes no more than 2Pi away on either side nuclear@0: inline void FastFixRange() nuclear@0: { nuclear@0: if (a < -((T)MATH_DOUBLE_PI)) nuclear@0: a += ((T)MATH_DOUBLE_TWOPI); nuclear@0: else if (a > ((T)MATH_DOUBLE_PI)) nuclear@0: a -= ((T)MATH_DOUBLE_TWOPI); nuclear@0: } nuclear@0: nuclear@0: // Fixes the angle range to [-Pi,Pi] for any given range, but slower then the fast method nuclear@0: inline void FixRange() nuclear@0: { nuclear@0: // do nothing if the value is already in the correct range, since fmod call is expensive nuclear@0: if (a >= -((T)MATH_DOUBLE_PI) && a <= ((T)MATH_DOUBLE_PI)) nuclear@0: return; nuclear@0: a = fmod(a,((T)MATH_DOUBLE_TWOPI)); nuclear@0: if (a < -((T)MATH_DOUBLE_PI)) nuclear@0: a += ((T)MATH_DOUBLE_TWOPI); nuclear@0: else if (a > ((T)MATH_DOUBLE_PI)) nuclear@0: a -= ((T)MATH_DOUBLE_TWOPI); nuclear@0: } nuclear@0: }; nuclear@0: nuclear@0: nuclear@0: typedef Angle Anglef; nuclear@0: typedef Angle Angled; nuclear@0: nuclear@0: nuclear@0: //------------------------------------------------------------------------------------- nuclear@0: // ***** Plane nuclear@0: nuclear@0: // Consists of a normal vector and distance from the origin where the plane is located. nuclear@0: nuclear@0: template nuclear@0: class Plane nuclear@0: { nuclear@0: public: nuclear@0: Vector3 N; nuclear@0: T D; nuclear@0: nuclear@0: Plane() : D(0) {} nuclear@0: nuclear@0: // Normals must already be normalized nuclear@0: Plane(const Vector3& n, T d) : N(n), D(d) {} nuclear@0: Plane(T x, T y, T z, T d) : N(x,y,z), D(d) {} nuclear@0: nuclear@0: // construct from a point on the plane and the normal nuclear@0: Plane(const Vector3& p, const Vector3& n) : N(n), D(-(p * n)) {} nuclear@0: nuclear@0: // Find the point to plane distance. The sign indicates what side of the plane the point is on (0 = point on plane). nuclear@0: T TestSide(const Vector3& p) const nuclear@0: { nuclear@0: return (N.Dot(p)) + D; nuclear@0: } nuclear@0: nuclear@0: Plane Flipped() const nuclear@0: { nuclear@0: return Plane(-N, -D); nuclear@0: } nuclear@0: nuclear@0: void Flip() nuclear@0: { nuclear@0: N = -N; nuclear@0: D = -D; nuclear@0: } nuclear@0: nuclear@0: bool operator==(const Plane& rhs) const nuclear@0: { nuclear@0: return (this->D == rhs.D && this->N == rhs.N); nuclear@0: } nuclear@0: }; nuclear@0: nuclear@0: typedef Plane Planef; nuclear@0: typedef Plane Planed; nuclear@0: nuclear@0: nuclear@0: } // Namespace OVR nuclear@0: nuclear@0: #endif