summaryrefslogtreecommitdiffstats
path: root/LibOVR/Src/Kernel/OVR_Math.h
diff options
context:
space:
mode:
authorSven Gothel <[email protected]>2015-03-28 01:43:35 +0100
committerSven Gothel <[email protected]>2015-03-28 01:43:35 +0100
commit4207f9c279e832e3afcb3f5fc6cd8d84cb4cfe4c (patch)
treecf3671058d55b47ab6cb6f36f369928606137628 /LibOVR/Src/Kernel/OVR_Math.h
parentc29cd1a2fbff6282bab956ad61661ac9d48c4e6e (diff)
Bump OculusVR RIFT SDK to 0.5.0.1vanilla_0.5.0.1
Diffstat (limited to 'LibOVR/Src/Kernel/OVR_Math.h')
-rw-r--r--LibOVR/Src/Kernel/OVR_Math.h2791
1 files changed, 0 insertions, 2791 deletions
diff --git a/LibOVR/Src/Kernel/OVR_Math.h b/LibOVR/Src/Kernel/OVR_Math.h
deleted file mode 100644
index c187cda..0000000
--- a/LibOVR/Src/Kernel/OVR_Math.h
+++ /dev/null
@@ -1,2791 +0,0 @@
-/************************************************************************************
-
-PublicHeader: OVR_Kernel.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, LLC All Rights reserved.
-
-Licensed under the Oculus VR Rift SDK License Version 3.2 (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.2
-
-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 Pose;
-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 struct {} 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<Pose<float> > { typedef ovrPosef Type; };
-template<> struct CompatibleTypes<Pose<double> > { typedef ovrPosed 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;
-};
-
-
-#define MATH_FLOAT_PI (3.1415926f)
-#define MATH_FLOAT_TWOPI (2.0f *MATH_FLOAT_PI)
-#define MATH_FLOAT_PIOVER2 (0.5f *MATH_FLOAT_PI)
-#define MATH_FLOAT_PIOVER4 (0.25f*MATH_FLOAT_PI)
-#define MATH_FLOAT_E (2.7182818f)
-#define MATH_FLOAT_MAXVALUE (FLT_MAX)
-#define MATH_FLOAT MINPOSITIVEVALUE (FLT_MIN)
-#define MATH_FLOAT_RADTODEGREEFACTOR (360.0f / MATH_FLOAT_TWOPI)
-#define MATH_FLOAT_DEGREETORADFACTOR (MATH_FLOAT_TWOPI / 360.0f)
-#define MATH_FLOAT_TOLERANCE (0.00001f)
-#define MATH_FLOAT_SINGULARITYRADIUS (0.0000001f) // Use for Gimbal lock numerical problems
-
-#define MATH_DOUBLE_PI (3.14159265358979)
-#define MATH_DOUBLE_TWOPI (2.0f *MATH_DOUBLE_PI)
-#define MATH_DOUBLE_PIOVER2 (0.5f *MATH_DOUBLE_PI)
-#define MATH_DOUBLE_PIOVER4 (0.25f*MATH_DOUBLE_PI)
-#define MATH_DOUBLE_E (2.71828182845905)
-#define MATH_DOUBLE_MAXVALUE (DBL_MAX)
-#define MATH_DOUBLE MINPOSITIVEVALUE (DBL_MIN)
-#define MATH_DOUBLE_RADTODEGREEFACTOR (360.0f / MATH_DOUBLE_TWOPI)
-#define MATH_DOUBLE_DEGREETORADFACTOR (MATH_DOUBLE_TWOPI / 360.0f)
-#define MATH_DOUBLE_TOLERANCE (0.00001)
-#define MATH_DOUBLE_SINGULARITYRADIUS (0.000000000001) // Use for Gimbal lock numerical problems
-
-
-
-
-// Single-precision Math constants class.
-template<>
-class Math<float>
-{
-public:
- typedef double OtherFloatType;
-};
-
-// Double-precision Math constants class.
-template<>
-class Math<double>
-{
-public:
- 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 * ((T)MATH_DOUBLE_RADTODEGREEFACTOR); }
-template<class T>
-T DegreeToRad(T rads) { return rads * ((T)MATH_DOUBLE_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 ((T)MATH_DOUBLE_PI);
- else return acos(val);
-};
-
-// Numerically stable asin function
-template<class T>
-T Asin(T val) {
- if (val > T(1)) return ((T)MATH_DOUBLE_PIOVER2);
- else if (val < T(-1)) return ((T)MATH_DOUBLE_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
- {
- static_assert(sizeof(Vector2<T>) == sizeof(CompatibleType), "sizeof(Vector2<T>) failure");
- 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 = ((T)MATH_DOUBLE_TOLERANCE))
- {
- return (fabs(b.x-x) < tolerance) && (fabs(b.y-y) < tolerance);
- }
-
- // Access element by index
- T& operator[] (int idx)
- {
- OVR_ASSERT(0 <= idx && idx < 2);
- return *(&x + idx);
- }
- const T& operator[] (int idx) const
- {
- OVR_ASSERT(0 <= idx && idx < 2);
- return *(&x + idx);
- }
-
- // Entry-wise 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(const Vector2& b) const { return (*this - b).LengthSq(); }
-
- // Returns distance between two points represented by vectors.
- T Distance(const Vector2& b) const { return (*this - b).Length(); }
-
- // Determine if this a unit vector.
- bool IsNormalized() const { return fabs(LengthSq() - T(1)) < ((T)MATH_DOUBLE_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;
-
-typedef Vector2<float> Point2f;
-typedef Vector2<double> Point2d;
-typedef Vector2<int> Point2i;
-
-//-------------------------------------------------------------------------------------
-// ***** 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;
-
- // FIXME: default initialization of a vector class can be very expensive in a full-blown
- // application. A few hundred thousand vector constructions is not unlikely and can add
- // up to milliseconds of time on processors like the PS3 PPU.
- 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) { }
-
- static const Vector3 ZERO;
-
- // 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
- {
- static_assert(sizeof(Vector3<T>) == sizeof(CompatibleType), "sizeof(Vector3<T>) failure");
- 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 = ((T)MATH_DOUBLE_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)) < ((T)MATH_DOUBLE_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<int32_t> Vector3i;
-
-static_assert((sizeof(Vector3f) == 3*sizeof(float)), "sizeof(Vector3f) failure");
-static_assert((sizeof(Vector3d) == 3*sizeof(double)), "sizeof(Vector3d) failure");
-static_assert((sizeof(Vector3i) == 3*sizeof(int32_t)), "sizeof(Vector3i) failure");
-
-typedef Vector3<float> Point3f;
-typedef Vector3<double> Point3d;
-typedef Vector3<int32_t> Point3i;
-
-
-//-------------------------------------------------------------------------------------
-// ***** Vector4<> - 4D vector of {x, y, z, w}
-
-//
-// Vector4f (Vector4d) represents a 3-dimensional vector or point in space,
-// consisting of coordinates x, y, z and w.
-
-template<class T>
-class Vector4
-{
-public:
- T x, y, z, w;
-
- // FIXME: default initialization of a vector class can be very expensive in a full-blown
- // application. A few hundred thousand vector constructions is not unlikely and can add
- // up to milliseconds of time on processors like the PS3 PPU.
- Vector4() : x(0), y(0), z(0), w(0) { }
- Vector4(T x_, T y_, T z_, T w_) : x(x_), y(y_), z(z_), w(w_) { }
- explicit Vector4(T s) : x(s), y(s), z(s), w(s) { }
- explicit Vector4(const Vector3<T>& v, const float w_=1) : x(v.x), y(v.y), z(v.z), w(w_) { }
- explicit Vector4(const Vector4<typename Math<T>::OtherFloatType> &src)
- : x((T)src.x), y((T)src.y), z((T)src.z), w((T)src.w) { }
-
- static const Vector4 ZERO;
-
- // C-interop support.
- typedef typename CompatibleTypes< Vector4<T> >::Type CompatibleType;
-
- Vector4(const CompatibleType& s) : x(s.x), y(s.y), z(s.z), w(s.w) { }
-
- operator const CompatibleType& () const
- {
- static_assert(sizeof(Vector4<T>) == sizeof(CompatibleType), "sizeof(Vector4<T>) failure");
- return reinterpret_cast<const CompatibleType&>(*this);
- }
-
- Vector4& operator= (const Vector3<T>& other) { x=other.x; y=other.y; z=other.z; w=1; return *this; }
- bool operator== (const Vector4& b) const { return x == b.x && y == b.y && z == b.z && w == b.w; }
- bool operator!= (const Vector4& b) const { return x != b.x || y != b.y || z != b.z || w != b.w; }
-
- Vector4 operator+ (const Vector4& b) const { return Vector4(x + b.x, y + b.y, z + b.z, w + b.w); }
- Vector4& operator+= (const Vector4& b) { x += b.x; y += b.y; z += b.z; w += b.w; return *this; }
- Vector4 operator- (const Vector4& b) const { return Vector4(x - b.x, y - b.y, z - b.z, w - b.w); }
- Vector4& operator-= (const Vector4& b) { x -= b.x; y -= b.y; z -= b.z; w -= b.w; return *this; }
- Vector4 operator- () const { return Vector4(-x, -y, -z, -w); }
-
- // Scalar multiplication/division scales vector.
- Vector4 operator* (T s) const { return Vector4(x*s, y*s, z*s, w*s); }
- Vector4& operator*= (T s) { x *= s; y *= s; z *= s; w *= s;return *this; }
-
- Vector4 operator/ (T s) const { T rcp = T(1)/s;
- return Vector4(x*rcp, y*rcp, z*rcp, w*rcp); }
- Vector4& operator/= (T s) { T rcp = T(1)/s;
- x *= rcp; y *= rcp; z *= rcp; w *= rcp;
- return *this; }
-
- static Vector4 Min(const Vector4& a, const Vector4& b)
- {
- return Vector4((a.x < b.x) ? a.x : b.x,
- (a.y < b.y) ? a.y : b.y,
- (a.z < b.z) ? a.z : b.z,
- (a.w < b.w) ? a.w : b.w);
- }
- static Vector4 Max(const Vector4& a, const Vector4& b)
- {
- return Vector4((a.x > b.x) ? a.x : b.x,
- (a.y > b.y) ? a.y : b.y,
- (a.z > b.z) ? a.z : b.z,
- (a.w > b.w) ? a.w : b.w);
- }
-
- // Compare two vectors for equality with tolerance. Returns true if vectors match withing tolerance.
- bool Compare(const Vector4&b, T tolerance = ((T)MATH_DOUBLE_TOLERANCE))
- {
- return (fabs(b.x-x) < tolerance) &&
- (fabs(b.y-y) < tolerance) &&
- (fabs(b.z-z) < tolerance) &&
- (fabs(b.w-w) < tolerance);
- }
-
- T& operator[] (int idx)
- {
- OVR_ASSERT(0 <= idx && idx < 4);
- return *(&x + idx);
- }
-
- const T& operator[] (int idx) const
- {
- OVR_ASSERT(0 <= idx && idx < 4);
- return *(&x + idx);
- }
-
- // Entry wise product of two vectors
- Vector4 EntrywiseMultiply(const Vector4& b) const { return Vector4(x * b.x,
- y * b.y,
- z * b.z);}
-
- // Multiply and divide operators do entry-wise math
- Vector4 operator* (const Vector4& b) const { return Vector4(x * b.x,
- y * b.y,
- z * b.z,
- w * b.w); }
-
- Vector4 operator/ (const Vector4& b) const { return Vector4(x / b.x,
- y / b.y,
- z / b.z,
- w / b.w); }
-
-
- // Dot product
- T Dot(const Vector4& b) const { return x*b.x + y*b.y + z*b.z + w*b.w; }
-
- // Return Length of the vector squared.
- T LengthSq() const { return (x * x + y * y + z * z + w * w); }
-
- // Return vector length.
- T Length() const { return sqrt(LengthSq()); }
-
- // 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.
- Vector4 Normalized() const
- {
- T l = Length();
- OVR_ASSERT(l != T(0));
- return *this / l;
- }
-};
-
-typedef Vector4<float> Vector4f;
-typedef Vector4<double> Vector4d;
-typedef Vector4<int> Vector4i;
-
-
-//-------------------------------------------------------------------------------------
-// ***** Bounds3
-
-// Bounds class used to describe a 3D axis aligned bounding box.
-
-template<class T>
-class Bounds3
-{
-public:
- Vector3<T> b[2];
-
- Bounds3()
- {
- }
-
- Bounds3( const Vector3<T> & mins, const Vector3<T> & maxs )
-{
- b[0] = mins;
- b[1] = maxs;
- }
-
- void Clear()
- {
- b[0].x = b[0].y = b[0].z = Math<T>::MaxValue;
- b[1].x = b[1].y = b[1].z = -Math<T>::MaxValue;
- }
-
- void AddPoint( const Vector3<T> & v )
- {
- b[0].x = Alg::Min( b[0].x, v.x );
- b[0].y = Alg::Min( b[0].y, v.y );
- b[0].z = Alg::Min( b[0].z, v.z );
- b[1].x = Alg::Max( b[1].x, v.x );
- b[1].y = Alg::Max( b[1].y, v.y );
- b[1].z = Alg::Max( b[1].z, v.z );
- }
-
- const Vector3<T> & GetMins() const { return b[0]; }
- const Vector3<T> & GetMaxs() const { return b[1]; }
-
- Vector3<T> & GetMins() { return b[0]; }
- Vector3<T> & GetMaxs() { return b[1]; }
-};
-
-typedef Bounds3<float> Bounds3f;
-typedef Bounds3<double> Bounds3d;
-
-
-//-------------------------------------------------------------------------------------
-// ***** 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
- {
- static_assert(sizeof(Size<T>) == sizeof(CompatibleType), "sizeof(Size<T>) failure");
- 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
- {
- static_assert(sizeof(Rect<T>) == sizeof(CompatibleType), "sizeof(Rect<T>) failure");
- 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) { }
-
- typedef typename CompatibleTypes<Quat<T> >::Type CompatibleType;
-
- // C-interop support.
- Quat(const CompatibleType& s) : x(s.x), y(s.y), z(s.z), w(s.w) { }
-
- operator CompatibleType () const
- {
- CompatibleType 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 > ((T)MATH_DOUBLE_TOLERANCE) * ((T)MATH_DOUBLE_TOLERANCE) ) {
- *axis = Vector3<T>(x, y, z).Normalized();
- *angle = 2 * Acos(w);
- if (*angle > ((T)MATH_DOUBLE_PI)) // Reduce the magnitude of the angle, if necessary
- {
- *angle = ((T)MATH_DOUBLE_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)) < ((T)MATH_DOUBLE_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
- {
- static_assert((A1 != A2) && (A2 != A3) && (A1 != A3), "(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) + ((T)MATH_DOUBLE_SINGULARITYRADIUS))
- { // South pole singularity
- *a = T(0);
- *b = -S*D*((T)MATH_DOUBLE_PIOVER2);
- *c = S*D*atan2(T(2)*(psign*Q[A1]*Q[A2] + w*Q[A3]),
- ww + Q22 - Q11 - Q33 );
- }
- else if (s2 > T(1) - ((T)MATH_DOUBLE_SINGULARITYRADIUS))
- { // North pole singularity
- *a = T(0);
- *b = S*D*((T)MATH_DOUBLE_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
- {
- static_assert(A1 != A2, "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*((T)MATH_DOUBLE_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;
-
-static_assert((sizeof(Quatf) == 4*sizeof(float)), "sizeof(Quatf) failure");
-static_assert((sizeof(Quatd) == 4*sizeof(double)), "sizeof(Quatd) failure");
-
-//-------------------------------------------------------------------------------------
-// ***** Pose
-
-// Position and orientation combined.
-
-template<class T>
-class Pose
-{
-public:
- typedef typename CompatibleTypes<Pose<T> >::Type CompatibleType;
-
- Pose() { }
- Pose(const Quat<T>& orientation, const Vector3<T>& pos)
- : Rotation(orientation), Translation(pos) { }
- Pose(const Pose& s)
- : Rotation(s.Rotation), Translation(s.Translation) { }
- Pose(const CompatibleType& s)
- : Rotation(s.Orientation), Translation(s.Position) { }
- explicit Pose(const Pose<typename Math<T>::OtherFloatType> &s)
- : Rotation(s.Rotation), Translation(s.Translation) { }
-
- operator typename CompatibleTypes<Pose<T> >::Type () const
- {
- typename CompatibleTypes<Pose<T> >::Type result;
- result.Orientation = Rotation;
- result.Position = Translation;
- return result;
- }
-
- Quat<T> Rotation;
- Vector3<T> Translation;
-
- static_assert((sizeof(T) == sizeof(double) || sizeof(T) == sizeof(float)), "(sizeof(T) == sizeof(double) || sizeof(T) == sizeof(float))");
-
- void ToArray(T* arr) const
- {
- T temp[7] = { Rotation.x, Rotation.y, Rotation.z, Rotation.w, Translation.x, Translation.y, Translation.z };
- for (int i = 0; i < 7; i++) arr[i] = temp[i];
- }
-
- static Pose<T> FromArray(const T* v)
- {
- Quat<T> rotation(v[0], v[1], v[2], v[3]);
- Vector3<T> translation(v[4], v[5], v[6]);
- return Pose<T>(rotation, 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));
- }
-
- Pose operator*(const Pose& other) const
- {
- return Pose(Rotation * other.Rotation, Apply(other.Translation));
- }
-
- Pose Inverted() const
- {
- Quat<T> inv = Rotation.Inverted();
- return Pose(inv, inv.Rotate(-Translation));
- }
-};
-
-typedef Pose<float> Posef;
-typedef Pose<double> Posed;
-
-static_assert((sizeof(Posed) == sizeof(Quatd) + sizeof(Vector3d)), "sizeof(Posed) failure");
-static_assert((sizeof(Posef) == sizeof(Quatf) + sizeof(Vector3f)), "sizeof(Posef) failure");
-
-
-//-------------------------------------------------------------------------------------
-// ***** 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 Pose<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)
- {
- static_assert(sizeof(s) == sizeof(Matrix4), "sizeof(s) == sizeof(Matrix4)");
- memcpy(M, s.M, sizeof(M));
- }
-
- operator typename CompatibleTypes<Matrix4<T> >::Type () const
- {
- typename CompatibleTypes<Matrix4<T> >::Type result;
- static_assert(sizeof(result) == sizeof(Matrix4), "sizeof(result) == sizeof(Matrix4)");
- memcpy(result.M, M, sizeof(M));
- return result;
- }
-
- void ToString(char* dest, size_t destsize) const
- {
- size_t 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;
- if (src)
- {
- 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;
- }
-
- void SetXBasis(const Vector3f & v)
- {
- M[0][0] = v.x;
- M[1][0] = v.y;
- M[2][0] = v.z;
- }
- Vector3f GetXBasis() const
- {
- return Vector3f(M[0][0], M[1][0], M[2][0]);
- }
-
- void SetYBasis(const Vector3f & v)
- {
- M[0][1] = v.x;
- M[1][1] = v.y;
- M[2][1] = v.z;
- }
- Vector3f GetYBasis() const
- {
- return Vector3f(M[0][1], M[1][1], M[2][1]);
- }
-
- void SetZBasis(const Vector3f & v)
- {
- M[0][2] = v.x;
- M[1][2] = v.y;
- M[2][2] = v.z;
- }
- Vector3f GetZBasis() const
- {
- return Vector3f(M[0][2], M[1][2], M[2][2]);
- }
-
- 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
- {
- const T rcpW = 1.0f / (M[3][0] * v.x + M[3][1] * v.y + M[3][2] * v.z + M[3][3]);
- return Vector3<T>((M[0][0] * v.x + M[0][1] * v.y + M[0][2] * v.z + M[0][3]) * rcpW,
- (M[1][0] * v.x + M[1][1] * v.y + M[1][2] * v.z + M[1][3]) * rcpW,
- (M[2][0] * v.x + M[2][1] * v.y + M[2][2] * v.z + M[2][3]) * rcpW);
- }
-
- Vector4<T> Transform(const Vector4<T>& v) const
- {
- return Vector4<T>(M[0][0] * v.x + M[0][1] * v.y + M[0][2] * v.z + M[0][3] * v.w,
- M[1][0] * v.x + M[1][1] * v.y + M[1][2] * v.z + M[1][3] * v.w,
- M[2][0] * v.x + M[2][1] * v.y + M[2][2] * v.z + M[2][3] * v.w,
- M[3][0] * v.x + M[3][1] * v.y + M[3][2] * v.z + M[3][3] * v.w);
- }
-
- 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 size_t* rows, const size_t* 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(size_t I, size_t J) const
- {
- const size_t 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) const
- {
- static_assert((A1 != A2) && (A2 != A3) && (A1 != A3), "(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*((T)MATH_DOUBLE_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*((T)MATH_DOUBLE_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) const
- {
- static_assert(A1 != A2, "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*((T)MATH_DOUBLE_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 / (znear - zfar);
- 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.
- // This is the case even for RHS coordinate input.
- return m;
- }
-
- // PerspectiveLH 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. / (aspect * tanHalfFov);
- m.M[1][1] = 1. / tanHalfFov;
- //m.M[2][2] = zfar / (znear - zfar);
- 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.
- // This is the case even for RHS coordinate 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 Pose<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)
- {
- static_assert(sizeof(s) == sizeof(Matrix3), "sizeof(s) == sizeof(Matrix3)");
- memcpy(M, s.M, sizeof(M));
- }
-
- operator const typename CompatibleTypes<Matrix3<T> >::Type () const
- {
- typename CompatibleTypes<Matrix3<T> >::Type result;
- static_assert(sizeof(result) == sizeof(Matrix3), "sizeof(result) == sizeof(Matrix3)");
- memcpy(result.M, M, sizeof(M));
- return result;
- }
-
- void ToString(char* dest, size_t destsize) const
- {
- size_t 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;
- }
-
- Vector2<T> Transform(const Vector2<T>& v) const
- {
- const float rcpZ = 1.0f / (M[2][0] * v.x + M[2][1] * v.y + M[2][2]);
- return Vector2<T>((M[0][0] * v.x + M[0][1] * v.y + M[0][2]) * rcpZ,
- (M[1][0] * v.x + M[1][1] * v.y + M[1][2]) * rcpZ);
- }
-
- 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 size_t* rows, const size_t* 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_*((T)MATH_DOUBLE_DEGREETORADFACTOR)) { FixRange(); }
-
- T Get(AngularUnits u = Radians) const { return (u == Radians) ? a : a*((T)MATH_DOUBLE_RADTODEGREEFACTOR); }
- void Set(const T& x, AngularUnits u = Radians) { a = (u == Radians) ? x : x*((T)MATH_DOUBLE_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 <= ((T)MATH_DOUBLE_PI)) ? c : ((T)MATH_DOUBLE_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 < -((T)MATH_DOUBLE_PI))
- a += ((T)MATH_DOUBLE_TWOPI);
- else if (a > ((T)MATH_DOUBLE_PI))
- a -= ((T)MATH_DOUBLE_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 >= -((T)MATH_DOUBLE_PI) && a <= ((T)MATH_DOUBLE_PI))
- return;
- a = fmod(a,((T)MATH_DOUBLE_TWOPI));
- if (a < -((T)MATH_DOUBLE_PI))
- a += ((T)MATH_DOUBLE_TWOPI);
- else if (a > ((T)MATH_DOUBLE_PI))
- a -= ((T)MATH_DOUBLE_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:
- 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;
-typedef Plane<double> Planed;
-
-
-} // Namespace OVR
-
-#endif
generated by cgit v1.2.3 (git 2.39.1) at 2024-12-12 20:52:23 +0000