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