Extras.OVR_Math.h Maven / Gradle / Ivy
Go to download
Show more of this group Show more artifacts with this name
Show all versions of jovr Show documentation
Show all versions of jovr Show documentation
JNA bindings for the Oculus SDK C API
/********************************************************************************//**
\file OVR_Math.h
\brief Implementation of 3D primitives such as vectors, matrices.
\copyright Copyright 2015 Oculus VR, LLC All Rights reserved.
*************************************************************************************/
#ifndef OVR_Math_h
#define OVR_Math_h
// This file is intended to be independent of the rest of LibOVR and LibOVRKernel and thus
// has no #include dependencies on either.
#include
#include
#include
#include
#include
#include
#include "../OVR_CAPI.h" // Currently required due to a dependence on the ovrFovPort_ declaration.
#if defined(_MSC_VER)
#pragma warning(push)
#pragma warning(disable: 4127) // conditional expression is constant
#endif
#if defined(_MSC_VER)
#define OVRMath_sprintf sprintf_s
#else
#define OVRMath_sprintf snprintf
#endif
//-------------------------------------------------------------------------------------
// ***** OVR_MATH_ASSERT
//
// Independent debug break implementation for OVR_Math.h.
#if !defined(OVR_MATH_DEBUG_BREAK)
#if defined(_DEBUG)
#if defined(_MSC_VER)
#define OVR_MATH_DEBUG_BREAK __debugbreak()
#else
#define OVR_MATH_DEBUG_BREAK __builtin_trap()
#endif
#else
#define OVR_MATH_DEBUG_BREAK ((void)0)
#endif
#endif
//-------------------------------------------------------------------------------------
// ***** OVR_MATH_ASSERT
//
// Independent OVR_MATH_ASSERT implementation for OVR_Math.h.
#if !defined(OVR_MATH_ASSERT)
#if defined(_DEBUG)
#define OVR_MATH_ASSERT(p) if (!(p)) { OVR_MATH_DEBUG_BREAK; }
#else
#define OVR_MATH_ASSERT(p) ((void)0)
#endif
#endif
//-------------------------------------------------------------------------------------
// ***** OVR_MATH_STATIC_ASSERT
//
// Independent OVR_MATH_ASSERT implementation for OVR_Math.h.
#if !defined(OVR_MATH_STATIC_ASSERT)
#if defined(__cplusplus) && ((defined(_MSC_VER) && (defined(_MSC_VER) >= 1600)) || defined(__GXX_EXPERIMENTAL_CXX0X__) || (__cplusplus >= 201103L))
#define OVR_MATH_STATIC_ASSERT static_assert
#else
#if !defined(OVR_SA_UNUSED)
#if defined(__GNUC__) || defined(__clang__)
#define OVR_SA_UNUSED __attribute__((unused))
#else
#define OVR_SA_UNUSED
#endif
#define OVR_SA_PASTE(a,b) a##b
#define OVR_SA_HELP(a,b) OVR_SA_PASTE(a,b)
#endif
#define OVR_MATH_STATIC_ASSERT(expression, msg) typedef char OVR_SA_HELP(compileTimeAssert, __LINE__) [((expression) != 0) ? 1 : -1] OVR_SA_UNUSED
#endif
#endif
namespace OVR {
template
const T OVRMath_Min(const T a, const T b)
{ return (a < b) ? a : b; }
template
const T OVRMath_Max(const T a, const T b)
{ return (b < a) ? a : b; }
template
void OVRMath_Swap(T& a, T& b)
{ T temp(a); a = b; b = temp; }
//-------------------------------------------------------------------------------------
// ***** 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_MATH_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 ovrSized_ ovrSized;
typedef struct ovrRecti_ ovrRecti;
typedef struct ovrVector2i_ ovrVector2i;
typedef struct ovrVector2f_ ovrVector2f;
typedef struct ovrVector2d_ ovrVector2d;
typedef struct ovrVector3f_ ovrVector3f;
typedef struct ovrVector3d_ ovrVector3d;
typedef struct ovrVector4f_ ovrVector4f;
typedef struct ovrVector4d_ ovrVector4d;
typedef struct ovrMatrix2f_ ovrMatrix2f;
typedef struct ovrMatrix2d_ ovrMatrix2d;
typedef struct ovrMatrix3f_ ovrMatrix3f;
typedef struct ovrMatrix3d_ ovrMatrix3d;
typedef struct ovrMatrix4f_ ovrMatrix4f;
typedef struct ovrMatrix4d_ ovrMatrix4d;
typedef struct ovrPosef_ ovrPosef;
typedef struct ovrPosed_ ovrPosed;
typedef struct ovrPoseStatef_ ovrPoseStatef;
typedef struct ovrPoseStated_ ovrPoseStated;
namespace OVR {
// Forward-declare our templates.
template class Quat;
template class Size;
template class Rect;
template class Vector2;
template class Vector3;
template class Vector4;
template class Matrix2;
template class Matrix3;
template class Matrix4;
template class Pose;
template class PoseState;
// CompatibleTypes::Type is used to lookup a compatible C-version of a C++ class.
template
struct CompatibleTypes
{
// Declaration here seems necessary for MSVC; specializations are
// used instead.
typedef struct {} Type;
};
// Specializations providing CompatibleTypes::Type value.
template<> struct CompatibleTypes > { typedef ovrQuatf Type; };
template<> struct CompatibleTypes > { typedef ovrQuatd Type; };
template<> struct CompatibleTypes > { typedef ovrMatrix2f Type; };
template<> struct CompatibleTypes > { typedef ovrMatrix2d Type; };
template<> struct CompatibleTypes > { typedef ovrMatrix3f Type; };
template<> struct CompatibleTypes > { typedef ovrMatrix3d Type; };
template<> struct CompatibleTypes > { typedef ovrMatrix4f Type; };
template<> struct CompatibleTypes > { typedef ovrMatrix4d Type; };
template<> struct CompatibleTypes > { typedef ovrSizei Type; };
template<> struct CompatibleTypes > { typedef ovrSizef Type; };
template<> struct CompatibleTypes > { typedef ovrSized Type; };
template<> struct CompatibleTypes > { typedef ovrRecti Type; };
template<> struct CompatibleTypes > { typedef ovrVector2i Type; };
template<> struct CompatibleTypes > { typedef ovrVector2f Type; };
template<> struct CompatibleTypes > { typedef ovrVector2d Type; };
template<> struct CompatibleTypes > { typedef ovrVector3f Type; };
template<> struct CompatibleTypes > { typedef ovrVector3d Type; };
template<> struct CompatibleTypes > { typedef ovrVector4f Type; };
template<> struct CompatibleTypes > { typedef ovrVector4d Type; };
template<> struct CompatibleTypes > { typedef ovrPosef Type; };
template<> struct CompatibleTypes > { typedef ovrPosed Type; };
//------------------------------------------------------------------------------------//
// ***** Math
//
// Math class contains constants and functions. This class is a template specialized
// per type, with Math and Math being distinct.
template
class Math
{
public:
// By default, support explicit conversion to float. This allows Vector2 to
// compile, for example.
typedef float OtherFloatType;
static int Tolerance() { return 0; } // Default value so integer types compile
};
//------------------------------------------------------------------------------------//
// ***** double constants
#define MATH_DOUBLE_PI 3.14159265358979323846
#define MATH_DOUBLE_TWOPI (2*MATH_DOUBLE_PI)
#define MATH_DOUBLE_PIOVER2 (0.5*MATH_DOUBLE_PI)
#define MATH_DOUBLE_PIOVER4 (0.25*MATH_DOUBLE_PI)
#define MATH_FLOAT_MAXVALUE (FLT_MAX)
#define MATH_DOUBLE_RADTODEGREEFACTOR (360.0 / MATH_DOUBLE_TWOPI)
#define MATH_DOUBLE_DEGREETORADFACTOR (MATH_DOUBLE_TWOPI / 360.0)
#define MATH_DOUBLE_E 2.71828182845904523536
#define MATH_DOUBLE_LOG2E 1.44269504088896340736
#define MATH_DOUBLE_LOG10E 0.434294481903251827651
#define MATH_DOUBLE_LN2 0.693147180559945309417
#define MATH_DOUBLE_LN10 2.30258509299404568402
#define MATH_DOUBLE_SQRT2 1.41421356237309504880
#define MATH_DOUBLE_SQRT1_2 0.707106781186547524401
#define MATH_DOUBLE_TOLERANCE 1e-12 // a default number for value equality tolerance: about 4500*Epsilon;
#define MATH_DOUBLE_SINGULARITYRADIUS 1e-12 // about 1-cos(.0001 degree), for gimbal lock numerical problems
//------------------------------------------------------------------------------------//
// ***** float constants
#define MATH_FLOAT_PI float(MATH_DOUBLE_PI)
#define MATH_FLOAT_TWOPI float(MATH_DOUBLE_TWOPI)
#define MATH_FLOAT_PIOVER2 float(MATH_DOUBLE_PIOVER2)
#define MATH_FLOAT_PIOVER4 float(MATH_DOUBLE_PIOVER4)
#define MATH_FLOAT_RADTODEGREEFACTOR float(MATH_DOUBLE_RADTODEGREEFACTOR)
#define MATH_FLOAT_DEGREETORADFACTOR float(MATH_DOUBLE_DEGREETORADFACTOR)
#define MATH_FLOAT_E float(MATH_DOUBLE_E)
#define MATH_FLOAT_LOG2E float(MATH_DOUBLE_LOG2E)
#define MATH_FLOAT_LOG10E float(MATH_DOUBLE_LOG10E)
#define MATH_FLOAT_LN2 float(MATH_DOUBLE_LN2)
#define MATH_FLOAT_LN10 float(MATH_DOUBLE_LN10)
#define MATH_FLOAT_SQRT2 float(MATH_DOUBLE_SQRT2)
#define MATH_FLOAT_SQRT1_2 float(MATH_DOUBLE_SQRT1_2)
#define MATH_FLOAT_TOLERANCE 1e-5f // a default number for value equality tolerance: 1e-5, about 84*EPSILON;
#define MATH_FLOAT_SINGULARITYRADIUS 1e-7f // about 1-cos(.025 degree), for gimbal lock numerical problems
// Single-precision Math constants class.
template<>
class Math
{
public:
typedef double OtherFloatType;
static inline float Tolerance() { return MATH_FLOAT_TOLERANCE; }; // a default number for value equality tolerance
static inline float SingularityRadius() { return MATH_FLOAT_SINGULARITYRADIUS; }; // for gimbal lock numerical problems
};
// Double-precision Math constants class
template<>
class Math
{
public:
typedef float OtherFloatType;
static inline double Tolerance() { return MATH_DOUBLE_TOLERANCE; }; // a default number for value equality tolerance
static inline double SingularityRadius() { return MATH_DOUBLE_SINGULARITYRADIUS; }; // for gimbal lock numerical problems
};
typedef Math Mathf;
typedef Math Mathd;
// Conversion functions between degrees and radians
// (non-templated to ensure passing int arguments causes warning)
inline float RadToDegree(float rad) { return rad * MATH_FLOAT_RADTODEGREEFACTOR; }
inline double RadToDegree(double rad) { return rad * MATH_DOUBLE_RADTODEGREEFACTOR; }
inline float DegreeToRad(float deg) { return deg * MATH_FLOAT_DEGREETORADFACTOR; }
inline double DegreeToRad(double deg) { return deg * MATH_DOUBLE_DEGREETORADFACTOR; }
// Square function
template
inline T Sqr(T x) { return x*x; }
// Sign: returns 0 if x == 0, -1 if x < 0, and 1 if x > 0
template
inline T Sign(T x) { return (x != T(0)) ? (x < T(0) ? T(-1) : T(1)) : T(0); }
// Numerically stable acos function
inline float Acos(float x) { return (x > 1.0f) ? 0.0f : (x < -1.0f) ? MATH_FLOAT_PI : acosf(x); }
inline double Acos(double x) { return (x > 1.0) ? 0.0 : (x < -1.0) ? MATH_DOUBLE_PI : acos(x); }
// Numerically stable asin function
inline float Asin(float x) { return (x > 1.0f) ? MATH_FLOAT_PIOVER2 : (x < -1.0f) ? -MATH_FLOAT_PIOVER2 : asinf(x); }
inline double Asin(double x) { return (x > 1.0) ? MATH_DOUBLE_PIOVER2 : (x < -1.0) ? -MATH_DOUBLE_PIOVER2 : asin(x); }
#if defined(_MSC_VER)
inline int isnan(double x) { return ::_isnan(x); }
#elif !defined(isnan) // Some libraries #define isnan.
inline int isnan(double x) { return ::isnan(x); }
#endif
template
class Quat;
//-------------------------------------------------------------------------------------
// ***** Vector2<>
// Vector2f (Vector2d) represents a 2-dimensional vector or point in space,
// consisting of coordinates x and y
template
class Vector2
{
public:
typedef T ElementType;
static const size_t ElementCount = 2;
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::OtherFloatType> &src)
: x((T)src.x), y((T)src.y) { }
static Vector2 Zero() { return Vector2(0, 0); }
// C-interop support.
typedef typename CompatibleTypes >::Type CompatibleType;
Vector2(const CompatibleType& s) : x(s.x), y(s.y) { }
operator const CompatibleType& () const
{
OVR_MATH_STATIC_ASSERT(sizeof(Vector2) == sizeof(CompatibleType), "sizeof(Vector2) failure");
return reinterpret_cast(*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); }
Vector2 Clamped(T maxMag) const
{
T magSquared = LengthSq();
if (magSquared <= Sqr(maxMag))
return *this;
else
return *this * (maxMag / sqrt(magSquared));
}
// Compare two vectors for equality with tolerance. Returns true if vectors match withing tolerance.
bool IsEqual(const Vector2& b, T tolerance =Math::Tolerance()) const
{
return (fabs(b.x-x) <= tolerance) &&
(fabs(b.y-y) <= tolerance);
}
bool Compare(const Vector2& b, T tolerance = Math::Tolerance()) const
{
return IsEqual(b, tolerance);
}
// Access element by index
T& operator[] (int idx)
{
OVR_MATH_ASSERT(0 <= idx && idx < 2);
return *(&x + idx);
}
const T& operator[] (int idx) const
{
OVR_MATH_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_MATH_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)) < Math::Tolerance(); }
// Normalize, convention vector length to 1.
void Normalize()
{
T s = Length();
if (s != T(0))
s = T(1) / s;
*this *= s;
}
// Returns normalized (unit) version of the vector without modifying itself.
Vector2 Normalized() const
{
T s = Length();
if (s != T(0))
s = T(1) / s;
return *this * s;
}
// 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_MATH_ASSERT(l2 != T(0));
return b * ( Dot(b) / l2 );
}
// returns true if vector b is clockwise from this vector
bool IsClockwise(const Vector2& b) const
{
return (x * b.y - y * b.x) < 0;
}
};
typedef Vector2 Vector2f;
typedef Vector2 Vector2d;
typedef Vector2 Vector2i;
typedef Vector2 Point2f;
typedef Vector2 Point2d;
typedef Vector2 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 Vector3
{
public:
typedef T ElementType;
static const size_t ElementCount = 3;
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::OtherFloatType> &src)
: x((T)src.x), y((T)src.y), z((T)src.z) { }
static Vector3 Zero() { return Vector3(0, 0, 0); }
// C-interop support.
typedef typename CompatibleTypes >::Type CompatibleType;
Vector3(const CompatibleType& s) : x(s.x), y(s.y), z(s.z) { }
operator const CompatibleType& () const
{
OVR_MATH_STATIC_ASSERT(sizeof(Vector3) == sizeof(CompatibleType), "sizeof(Vector3) failure");
return reinterpret_cast(*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);
}
Vector3 Clamped(T maxMag) const
{
T magSquared = LengthSq();
if (magSquared <= Sqr(maxMag))
return *this;
else
return *this * (maxMag / sqrt(magSquared));
}
// Compare two vectors for equality with tolerance. Returns true if vectors match withing tolerance.
bool IsEqual(const Vector3& b, T tolerance = Math::Tolerance()) const
{
return (fabs(b.x-x) <= tolerance) &&
(fabs(b.y-y) <= tolerance) &&
(fabs(b.z-z) <= tolerance);
}
bool Compare(const Vector3& b, T tolerance = Math::Tolerance()) const
{
return IsEqual(b, tolerance);
}
T& operator[] (int idx)
{
OVR_MATH_ASSERT(0 <= idx && idx < 3);
return *(&x + idx);
}
const T& operator[] (int idx) const
{
OVR_MATH_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_MATH_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 (T)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(); }
bool IsNormalized() const { return fabs(LengthSq() - T(1)) < Math::Tolerance(); }
// Normalize, convention vector length to 1.
void Normalize()
{
T s = Length();
if (s != T(0))
s = T(1) / s;
*this *= s;
}
// Returns normalized (unit) version of the vector without modifying itself.
Vector3 Normalized() const
{
T s = Length();
if (s != T(0))
s = T(1) / s;
return *this * s;
}
// 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_MATH_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 Vector3f;
typedef Vector3 Vector3d;
typedef Vector3 Vector3i;
OVR_MATH_STATIC_ASSERT((sizeof(Vector3f) == 3*sizeof(float)), "sizeof(Vector3f) failure");
OVR_MATH_STATIC_ASSERT((sizeof(Vector3d) == 3*sizeof(double)), "sizeof(Vector3d) failure");
OVR_MATH_STATIC_ASSERT((sizeof(Vector3i) == 3*sizeof(int32_t)), "sizeof(Vector3i) failure");
typedef Vector3 Point3f;
typedef Vector3 Point3d;
typedef Vector3 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 Vector4
{
public:
typedef T ElementType;
static const size_t ElementCount = 4;
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& v, const T w_=T(1)) : x(v.x), y(v.y), z(v.z), w(w_) { }
explicit Vector4(const Vector4::OtherFloatType> &src)
: x((T)src.x), y((T)src.y), z((T)src.z), w((T)src.w) { }
static Vector4 Zero() { return Vector4(0, 0, 0, 0); }
// C-interop support.
typedef typename CompatibleTypes< Vector4 >::Type CompatibleType;
Vector4(const CompatibleType& s) : x(s.x), y(s.y), z(s.z), w(s.w) { }
operator const CompatibleType& () const
{
OVR_MATH_STATIC_ASSERT(sizeof(Vector4) == sizeof(CompatibleType), "sizeof(Vector4) failure");
return reinterpret_cast(*this);
}
Vector4& operator= (const Vector3& 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);
}
Vector4 Clamped(T maxMag) const
{
T magSquared = LengthSq();
if (magSquared <= Sqr(maxMag))
return *this;
else
return *this * (maxMag / sqrt(magSquared));
}
// Compare two vectors for equality with tolerance. Returns true if vectors match withing tolerance.
bool IsEqual(const Vector4& b, T tolerance = Math::Tolerance()) const
{
return (fabs(b.x-x) <= tolerance) &&
(fabs(b.y-y) <= tolerance) &&
(fabs(b.z-z) <= tolerance) &&
(fabs(b.w-w) <= tolerance);
}
bool Compare(const Vector4& b, T tolerance = Math::Tolerance()) const
{
return IsEqual(b, tolerance);
}
T& operator[] (int idx)
{
OVR_MATH_ASSERT(0 <= idx && idx < 4);
return *(&x + idx);
}
const T& operator[] (int idx) const
{
OVR_MATH_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,
w * b.w);}
// 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()); }
bool IsNormalized() const { return fabs(LengthSq() - T(1)) < Math::Tolerance(); }
// Normalize, convention vector length to 1.
void Normalize()
{
T s = Length();
if (s != T(0))
s = T(1) / s;
*this *= s;
}
// Returns normalized (unit) version of the vector without modifying itself.
Vector4 Normalized() const
{
T s = Length();
if (s != T(0))
s = T(1) / s;
return *this * s;
}
// Linearly interpolates from this vector to another.
// Factor should be between 0.0 and 1.0, with 0 giving full value to this.
Vector4 Lerp(const Vector4& b, T f) const { return *this*(T(1) - f) + b*f; }
};
typedef Vector4 Vector4f;
typedef Vector4 Vector4d;
typedef Vector4 Vector4i;
//-------------------------------------------------------------------------------------
// ***** Bounds3
// Bounds class used to describe a 3D axis aligned bounding box.
template
class Bounds3
{
public:
Vector3 b[2];
Bounds3()
{
}
Bounds3( const Vector3 & mins, const Vector3 & maxs )
{
b[0] = mins;
b[1] = maxs;
}
void Clear()
{
b[0].x = b[0].y = b[0].z = Math::MaxValue;
b[1].x = b[1].y = b[1].z = -Math::MaxValue;
}
void AddPoint( const Vector3 & v )
{
b[0].x = (b[0].x < v.x ? b[0].x : v.x);
b[0].y = (b[0].y < v.y ? b[0].y : v.y);
b[0].z = (b[0].z < v.z ? b[0].z : v.z);
b[1].x = (v.x < b[1].x ? b[1].x : v.x);
b[1].y = (v.y < b[1].y ? b[1].y : v.y);
b[1].z = (v.z < b[1].z ? b[1].z : v.z);
}
const Vector3 & GetMins() const { return b[0]; }
const Vector3 & GetMaxs() const { return b[1]; }
Vector3 & GetMins() { return b[0]; }
Vector3 & GetMaxs() { return b[1]; }
};
typedef Bounds3 Bounds3f;
typedef Bounds3 Bounds3d;
//-------------------------------------------------------------------------------------
// ***** Size
// Size class represents 2D size with Width, Height components.
// Used to describe distentions of render targets, etc.
template
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::OtherFloatType> &src)
: w((T)src.w), h((T)src.h) { }
// C-interop support.
typedef typename CompatibleTypes >::Type CompatibleType;
Size(const CompatibleType& s) : w(s.w), h(s.h) { }
operator const CompatibleType& () const
{
OVR_MATH_STATIC_ASSERT(sizeof(Size) == sizeof(CompatibleType), "sizeof(Size) failure");
return reinterpret_cast(*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 ToVector() const { return Vector2(w, h); }
};
typedef Size Sizei;
typedef Size Sizeu;
typedef Size Sizef;
typedef Size Sized;
//-----------------------------------------------------------------------------------
// ***** Rect
// Rect describes a rectangular area for rendering, that includes position and size.
template
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& pos, const Size& sz) : x(pos.x), y(pos.y), w(sz.w), h(sz.h) { }
Rect(const Size& sz) : x(0), y(0), w(sz.w), h(sz.h) { }
// C-interop support.
typedef typename CompatibleTypes >::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_MATH_STATIC_ASSERT(sizeof(Rect) == sizeof(CompatibleType), "sizeof(Rect) failure");
return reinterpret_cast(*this);
}
Vector2 GetPos() const { return Vector2(x, y); }
Size GetSize() const { return Size(w, h); }
void SetPos(const Vector2& pos) { x = pos.x; y = pos.y; }
void SetSize(const Size& 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 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 Quat
{
public:
typedef T ElementType;
static const size_t ElementCount = 4;
// x,y,z = axis*sin(angle), w = cos(angle)
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::OtherFloatType> &src)
: x((T)src.x), y((T)src.y), z((T)src.z), w((T)src.w)
{
// NOTE: Converting a normalized Quat to Quat
// will generally result in an un-normalized quaternion.
// But we don't normalize here in case the quaternion
// being converted is not a normalized rotation quaternion.
}
typedef typename CompatibleTypes >::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& axis, T angle)
{
// Make sure we don't divide by zero.
if (axis.LengthSq() == T(0))
{
// Assert if the axis is zero, but the angle isn't
OVR_MATH_ASSERT(angle == T(0));
x = y = z = T(0); w = T(1);
return;
}
Vector3 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];
}
Quat operator-() { return Quat(-x, -y, -z, -w); } // unary minus
static Quat Identity() { return Quat(0, 0, 0, 1); }
// Compute axis and angle from quaternion
void GetAxisAngle(Vector3* axis, T* angle) const
{
if ( x*x + y*y + z*z > Math::Tolerance() * Math::Tolerance() ) {
*axis = Vector3(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(1, 0, 0);
*angle= T(0);
}
}
// Convert a quaternion to a rotation vector, also known as
// Rodrigues vector, AxisAngle vector, SORA vector, exponential map.
// A rotation vector describes a rotation about an axis:
// the axis of rotation is the vector normalized,
// the angle of rotation is the magnitude of the vector.
Vector3 ToRotationVector() const
{
OVR_MATH_ASSERT(IsNormalized() || LengthSq() == 0);
T s = T(0);
T sinHalfAngle = sqrt(x*x + y*y + z*z);
if (sinHalfAngle > T(0))
{
T cosHalfAngle = w;
T halfAngle = atan2(sinHalfAngle, cosHalfAngle);
// Ensure minimum rotation magnitude
if (cosHalfAngle < 0)
halfAngle -= T(MATH_DOUBLE_PI);
s = T(2) * halfAngle / sinHalfAngle;
}
return Vector3(x*s, y*s, z*s);
}
// Faster version of the above, optimized for use with small rotations, where rotation angle ~= sin(angle)
inline OVR::Vector3 FastToRotationVector() const
{
OVR_MATH_ASSERT(IsNormalized());
T s;
T sinHalfSquared = x*x + y*y + z*z;
if (sinHalfSquared < T(.0037)) // =~ sin(7/2 degrees)^2
{
// Max rotation magnitude error is about .062% at 7 degrees rotation, or about .0043 degrees
s = T(2) * Sign(w);
}
else
{
T sinHalfAngle = sqrt(sinHalfSquared);
T cosHalfAngle = w;
T halfAngle = atan2(sinHalfAngle, cosHalfAngle);
// Ensure minimum rotation magnitude
if (cosHalfAngle < 0)
halfAngle -= T(MATH_DOUBLE_PI);
s = T(2) * halfAngle / sinHalfAngle;
}
return Vector3(x*s, y*s, z*s);
}
// Given a rotation vector of form unitRotationAxis * angle,
// returns the equivalent quaternion (unitRotationAxis * sin(angle), cos(Angle)).
static Quat FromRotationVector(const Vector3& v)
{
T angleSquared = v.LengthSq();
T s = T(0);
T c = T(1);
if (angleSquared > T(0))
{
T angle = sqrt(angleSquared);
s = sin(angle * T(0.5)) / angle; // normalize
c = cos(angle * T(0.5));
}
return Quat(s*v.x, s*v.y, s*v.z, c);
}
// Faster version of above, optimized for use with small rotation magnitudes, where rotation angle =~ sin(angle).
// If normalize is false, small-angle quaternions are returned un-normalized.
inline static Quat FastFromRotationVector(const OVR::Vector3& v, bool normalize = true)
{
T s, c;
T angleSquared = v.LengthSq();
if (angleSquared < T(0.0076)) // =~ (5 degrees*pi/180)^2
{
s = T(0.5);
c = T(1.0);
// Max rotation magnitude error (after normalization) is about .064% at 5 degrees rotation, or .0032 degrees
if (normalize && angleSquared > 0)
{
// sin(angle/2)^2 ~= (angle/2)^2 and cos(angle/2)^2 ~= 1
T invLen = T(1) / sqrt(angleSquared * T(0.25) + T(1)); // normalize
s = s * invLen;
c = c * invLen;
}
}
else
{
T angle = sqrt(angleSquared);
s = sin(angle * T(0.5)) / angle;
c = cos(angle * T(0.5));
}
return Quat(s*v.x, s*v.y, s*v.z, c);
}
// Constructs the quaternion from a rotation matrix
explicit Quat(const Matrix4& 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;
}
OVR_MATH_ASSERT(IsNormalized()); // Ensure input matrix is orthogonal
}
// Constructs the quaternion from a rotation matrix
explicit Quat(const Matrix3& 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;
}
OVR_MATH_ASSERT(IsNormalized()); // Ensure input matrix is orthogonal
}
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; }
// Compare two quats for equality within tolerance. Returns true if quats match withing tolerance.
bool IsEqual(const Quat& b, T tolerance = Math::Tolerance()) const
{
return Abs(Dot(b)) >= T(1) - tolerance;
}
static T Abs(const T v) { return (v >= 0) ? v : -v; }
// Get Imaginary part vector
Vector3 Imag() const { return Vector3(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 T(2) * Acos(Abs(Dot(q)));
}
// Angle of quaternion
T Angle() const
{
return T(2) * Acos(Abs(w));
}
// Normalize
bool IsNormalized() const { return fabs(LengthSq() - T(1)) < Math::Tolerance(); }
void Normalize()
{
T s = Length();
if (s != T(0))
s = T(1) / s;
*this *= s;
}
Quat Normalized() const
{
T s = Length();
if (s != T(0))
s = T(1) / s;
return *this * s;
}
inline void EnsureSameHemisphere(const Quat& o)
{
if (Dot(o) < T(0))
{
x = -x;
y = -y;
z = -z;
w = -w;
}
}
// 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); }
const Quat& operator*= (const Quat& b) { *this = *this * b; return *this; }
//
// this^p normalized; same as rotating by this p times.
Quat PowNormalized(T p) const
{
Vector3 v;
T a;
GetAxisAngle(&v, &a);
return Quat(v, a * p);
}
// Compute quaternion that rotates v into alignTo: alignTo = Quat::Align(alignTo, v).Rotate(v).
// NOTE: alignTo and v must be normalized.
static Quat Align(const Vector3& alignTo, const Vector3& v)
{
OVR_MATH_ASSERT(alignTo.IsNormalized() && v.IsNormalized());
Vector3 bisector = (v + alignTo);
bisector.Normalize();
T cosHalfAngle = v.Dot(bisector); // 0..1
if (cosHalfAngle > T(0))
{
Vector3 imag = v.Cross(bisector);
return Quat(imag.x, imag.y, imag.z, cosHalfAngle);
}
else
{
// cosHalfAngle == 0: a 180 degree rotation.
// sinHalfAngle == 1, rotation axis is any axis perpendicular
// to alignTo. Choose axis to include largest magnitude components
if (fabs(v.x) > fabs(v.y))
{
// x or z is max magnitude component
// = Cross(v, (0,1,0)).Normalized();
T invLen = sqrt(v.x*v.x + v.z*v.z);
if (invLen > T(0))
invLen = T(1) / invLen;
return Quat(-v.z*invLen, 0, v.x*invLen, 0);
}
else
{
// y or z is max magnitude component
// = Cross(v, (1,0,0)).Normalized();
T invLen = sqrt(v.y*v.y + v.z*v.z);
if (invLen > T(0))
invLen = T(1) / invLen;
return Quat(0, v.z*invLen, -v.y*invLen, 0);
}
}
}
// Normalized linear interpolation of quaternions
// NOTE: This function is a bad approximation of Slerp()
// when the angle between the *this and b is large.
// Use FastSlerp() or Slerp() instead.
Quat Lerp(const Quat& b, T s) const
{
return (*this * (T(1) - s) + b * (Dot(b) < 0 ? -s : s)).Normalized();
}
// Spherical linear interpolation between rotations
Quat Slerp(const Quat& b, T s) const
{
Vector3 delta = (b * this->Inverted()).ToRotationVector();
return FromRotationVector(delta * s) * *this;
}
// Spherical linear interpolation: much faster for small rotations, accurate for large rotations. See FastTo/FromRotationVector
Quat FastSlerp(const Quat& b, T s) const
{
Vector3 delta = (b * this->Inverted()).FastToRotationVector();
return (FastFromRotationVector(delta * s, false) * *this).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 Rotate(const Vector3& v) const
{
OVR_MATH_ASSERT(isnan(w) || IsNormalized());
// rv = q * (v,0) * q'
// Same as rv = v + real * cross(imag,v)*2 + cross(imag, cross(imag,v)*2);
// uv = 2 * Imag().Cross(v);
T uvx = T(2) * (y*v.z - z*v.y);
T uvy = T(2) * (z*v.x - x*v.z);
T uvz = T(2) * (x*v.y - y*v.x);
// return v + Real()*uv + Imag().Cross(uv);
return Vector3(v.x + w*uvx + y*uvz - z*uvy,
v.y + w*uvy + z*uvx - x*uvz,
v.z + w*uvz + x*uvy - y*uvx);
}
// Rotation by inverse of *this
Vector3 InverseRotate(const Vector3& v) const
{
OVR_MATH_ASSERT(IsNormalized());
// rv = q' * (v,0) * q
// Same as rv = v + real * cross(-imag,v)*2 + cross(-imag, cross(-imag,v)*2);
// or rv = v - real * cross(imag,v)*2 + cross(imag, cross(imag,v)*2);
// uv = 2 * Imag().Cross(v);
T uvx = T(2) * (y*v.z - z*v.y);
T uvy = T(2) * (z*v.x - x*v.z);
T uvz = T(2) * (x*v.y - y*v.x);
// return v - Real()*uv + Imag().Cross(uv);
return Vector3(v.x - w*uvx + y*uvz - z*uvy,
v.y - w*uvy + z*uvx - x*uvz,
v.z - w*uvz + x*uvy - y*uvx);
}
// Inversed quaternion rotates in the opposite direction.
Quat Inverted() const
{
return Quat(-x, -y, -z, w);
}
Quat Inverse() 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);
}
// Time integration of constant angular velocity over dt
Quat TimeIntegrate(Vector3 angularVelocity, T dt) const
{
// solution is: this * exp( omega*dt/2 ); FromRotationVector(v) gives exp(v*.5).
return (*this * FastFromRotationVector(angularVelocity * dt, false)).Normalized();
}
// Time integration of constant angular acceleration and velocity over dt
// These are the first two terms of the "Magnus expansion" of the solution
//
// o = o * exp( W=(W1 + W2 + W3+...) * 0.5 );
//
// omega1 = (omega + omegaDot*dt)
// W1 = (omega + omega1)*dt/2
// W2 = cross(omega, omega1)/12*dt^2 % (= -cross(omega_dot, omega)/12*dt^3)
// Terms 3 and beyond are vanishingly small:
// W3 = cross(omega_dot, cross(omega_dot, omega))/240*dt^5
//
Quat TimeIntegrate(Vector3 angularVelocity, Vector3 angularAcceleration, T dt) const
{
const Vector3& omega = angularVelocity;
const Vector3& omegaDot = angularAcceleration;
Vector3 omega1 = (omega + omegaDot * dt);
Vector3 W = ( (omega + omega1) + omega.Cross(omega1) * (dt/T(6)) ) * (dt/T(2));
// FromRotationVector(v) is exp(v*.5)
return (*this * FastFromRotationVector(W, false)).Normalized();
}
// Decompose rotation into three rotations:
// roll radians about Z axis, then pitch radians about X axis, then yaw radians about Y axis.
// Call with nullptr if a return value is not needed.
void GetYawPitchRoll(T* yaw, T* pitch, T* roll) const
{
return GetEulerAngles(yaw, pitch, roll);
}
// 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 order is c, b, a:
// rotation c around axis A3
// is followed by rotation b around axis A2
// is followed by rotation a around axis A1
// rotations are CCW or CW (D) in LH or RH coordinate system (S)
//
template
void GetEulerAngles(T *a, T *b, T *c) const
{
OVR_MATH_ASSERT(IsNormalized());
OVR_MATH_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]);
T singularityRadius = Math::SingularityRadius();
if (s2 < T(-1) + singularityRadius)
{ // South pole singularity
if (a) *a = T(0);
if (b) *b = -S*D*((T)MATH_DOUBLE_PIOVER2);
if (c) *c = S*D*atan2(T(2)*(psign*Q[A1] * Q[A2] + w*Q[A3]), ww + Q22 - Q11 - Q33 );
}
else if (s2 > T(1) - singularityRadius)
{ // North pole singularity
if (a) *a = T(0);
if (b) *b = S*D*((T)MATH_DOUBLE_PIOVER2);
if (c) *c = S*D*atan2(T(2)*(psign*Q[A1] * Q[A2] + w*Q[A3]), ww + Q22 - Q11 - Q33);
}
else
{
if (a) *a = -S*D*atan2(T(-2)*(w*Q[A1] - psign*Q[A2] * Q[A3]), ww + Q33 - Q11 - Q22);
if (b) *b = S*D*asin(s2);
if (c) *c = S*D*atan2(T(2)*(w*Q[A3] - psign*Q[A1] * Q[A2]), ww + Q11 - Q22 - Q33);
}
}
template
void GetEulerAngles(T *a, T *b, T *c) const
{ GetEulerAngles(a, b, c); }
template
void GetEulerAngles(T *a, T *b, T *c) const
{ GetEulerAngles(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
void GetEulerAnglesABA(T *a, T *b, T *c) const
{
OVR_MATH_ASSERT(IsNormalized());
OVR_MATH_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;
T singularityRadius = Math::SingularityRadius();
if (c2 < T(-1) + singularityRadius)
{ // South pole singularity
if (a) *a = T(0);
if (b) *b = S*D*((T)MATH_DOUBLE_PI);
if (c) *c = S*D*atan2(T(2)*(w*Q[A1] - psign*Q[A2] * Q[m]),
ww + Q22 - Q11 - Qmm);
}
else if (c2 > T(1) - singularityRadius)
{ // North pole singularity
if (a) *a = T(0);
if (b) *b = T(0);
if (c) *c = S*D*atan2(T(2)*(w*Q[A1] - psign*Q[A2] * Q[m]),
ww + Q22 - Q11 - Qmm);
}
else
{
if (a) *a = S*D*atan2(psign*w*Q[m] + Q[A1] * Q[A2],
w*Q[A2] -psign*Q[A1]*Q[m]);
if (b) *b = S*D*acos(c2);
if (c) *c = S*D*atan2(-psign*w*Q[m] + Q[A1] * Q[A2],
w*Q[A2] + psign*Q[A1]*Q[m]);
}
}
};
typedef Quat Quatf;
typedef Quat Quatd;
OVR_MATH_STATIC_ASSERT((sizeof(Quatf) == 4*sizeof(float)), "sizeof(Quatf) failure");
OVR_MATH_STATIC_ASSERT((sizeof(Quatd) == 4*sizeof(double)), "sizeof(Quatd) failure");
//-------------------------------------------------------------------------------------
// ***** Pose
//
// Position and orientation combined.
//
// This structure needs to be the same size and layout on 32-bit and 64-bit arch.
// Update OVR_PadCheck.cpp when updating this object.
template
class Pose
{
public:
typedef typename CompatibleTypes >::Type CompatibleType;
Pose() { }
Pose(const Quat& orientation, const Vector3& pos)
: Rotation(orientation), Translation(pos) { }
Pose(const Pose& s)
: Rotation(s.Rotation), Translation(s.Translation) { }
Pose(const Matrix3& R, const Vector3& t)
: Rotation((Quat)R), Translation(t) { }
Pose(const CompatibleType& s)
: Rotation(s.Orientation), Translation(s.Position) { }
explicit Pose(const Pose::OtherFloatType> &s)
: Rotation(s.Rotation), Translation(s.Translation)
{
// Ensure normalized rotation if converting from float to double
if (sizeof(T) > sizeof(Math::OtherFloatType))
Rotation.Normalize();
}
static Pose Identity() { return Pose(Quat(0, 0, 0, 1), Vector3(0, 0, 0)); }
void SetIdentity() { Rotation = Quat(0, 0, 0, 1); Translation = Vector3(0, 0, 0); }
// used to make things obviously broken if someone tries to use the value
void SetInvalid() { Rotation = Quat(NAN, NAN, NAN, NAN); Translation = Vector3(NAN, NAN, NAN); }
bool IsEqual(const Pose&b, T tolerance = Math::Tolerance()) const
{
return Translation.IsEqual(b.Translation, tolerance) && Rotation.IsEqual(b.Rotation, tolerance);
}
operator typename CompatibleTypes >::Type () const
{
typename CompatibleTypes >::Type result;
result.Orientation = Rotation;
result.Position = Translation;
return result;
}
Quat Rotation;
Vector3 Translation;
OVR_MATH_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 FromArray(const T* v)
{
Quat rotation(v[0], v[1], v[2], v[3]);
Vector3 translation(v[4], v[5], v[6]);
// Ensure rotation is normalized, in case it was originally a float, stored in a .json file, etc.
return Pose(rotation.Normalized(), translation);
}
Vector3 Rotate(const Vector3& v) const
{
return Rotation.Rotate(v);
}
Vector3 InverseRotate(const Vector3& v) const
{
return Rotation.InverseRotate(v);
}
Vector3 Translate(const Vector3& v) const
{
return v + Translation;
}
Vector3 Transform(const Vector3& v) const
{
return Rotate(v) + Translation;
}
Vector3 InverseTransform(const Vector3& v) const
{
return InverseRotate(v - Translation);
}
Vector3 Apply(const Vector3& v) const
{
return Transform(v);
}
Pose operator*(const Pose& other) const
{
return Pose(Rotation * other.Rotation, Apply(other.Translation));
}
Pose Inverted() const
{
Quat inv = Rotation.Inverted();
return Pose(inv, inv.Rotate(-Translation));
}
// Interpolation between two poses: translation is interpolated with Lerp(),
// and rotations are interpolated with Slerp().
Pose Lerp(const Pose& b, T s)
{
return Pose(Rotation.Slerp(b.Rotation, s), Translation.Lerp(b.Translation, s));
}
// Similar to Lerp above, except faster in case of small rotation differences. See Quat::FastSlerp.
Pose FastLerp(const Pose& b, T s)
{
return Pose(Rotation.FastSlerp(b.Rotation, s), Translation.Lerp(b.Translation, s));
}
Pose TimeIntegrate(const Vector3& linearVelocity, const Vector3& angularVelocity, T dt) const
{
return Pose(
(Rotation * Quat::FastFromRotationVector(angularVelocity * dt, false)).Normalized(),
Translation + linearVelocity * dt);
}
Pose TimeIntegrate(const Vector3& linearVelocity, const Vector3& linearAcceleration,
const Vector3& angularVelocity, const Vector3& angularAcceleration,
T dt) const
{
return Pose(Rotation.TimeIntegrate(angularVelocity, angularAcceleration, dt),
Translation + linearVelocity*dt + linearAcceleration*dt*dt * T(0.5));
}
};
typedef Pose Posef;
typedef Pose Posed;
OVR_MATH_STATIC_ASSERT((sizeof(Posed) == sizeof(Quatd) + sizeof(Vector3d)), "sizeof(Posed) failure");
OVR_MATH_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 Matrix4
{
public:
typedef T ElementType;
static const size_t Dimension = 4;
T M[4][4];
enum NoInitType { NoInit };
// Construct with no memory initialization.
Matrix4(NoInitType) { }
// By default, we construct identity matrix.
Matrix4()
{
M[0][0] = M[1][1] = M[2][2] = M[3][3] = T(1);
M[0][1] = M[1][0] = M[2][3] = M[3][1] = T(0);
M[0][2] = M[1][2] = M[2][0] = M[3][2] = T(0);
M[0][3] = M[1][3] = M[2][1] = M[3][0] = T(0);
}
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] = T(0);
M[1][0] = m21; M[1][1] = m22; M[1][2] = m23; M[1][3] = T(0);
M[2][0] = m31; M[2][1] = m32; M[2][2] = m33; M[2][3] = T(0);
M[3][0] = T(0); M[3][1] = T(0); M[3][2] = T(0); M[3][3] = T(1);
}
explicit Matrix4(const Matrix3& m)
{
M[0][0] = m.M[0][0]; M[0][1] = m.M[0][1]; M[0][2] = m.M[0][2]; M[0][3] = T(0);
M[1][0] = m.M[1][0]; M[1][1] = m.M[1][1]; M[1][2] = m.M[1][2]; M[1][3] = T(0);
M[2][0] = m.M[2][0]; M[2][1] = m.M[2][1]; M[2][2] = m.M[2][2]; M[2][3] = T(0);
M[3][0] = T(0); M[3][1] = T(0); M[3][2] = T(0); M[3][3] = T(1);
}
explicit Matrix4(const Quat& q)
{
OVR_MATH_ASSERT(q.IsNormalized());
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] = T(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] = T(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] = T(0);
M[3][0] = T(0); M[3][1] = T(0); M[3][2] = T(0); M[3][3] = T(1);
}
explicit Matrix4(const Pose& p)
{
Matrix4 result(p.Rotation);
result.SetTranslation(p.Translation);
*this = result;
}
// C-interop support
explicit Matrix4(const Matrix4::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 >::Type& s)
{
OVR_MATH_STATIC_ASSERT(sizeof(s) == sizeof(Matrix4), "sizeof(s) == sizeof(Matrix4)");
memcpy(M, s.M, sizeof(M));
}
operator typename CompatibleTypes >::Type () const
{
typename CompatibleTypes >::Type result;
OVR_MATH_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 += OVRMath_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 Matrix4 Identity() { return Matrix4(); }
void SetIdentity()
{
M[0][0] = M[1][1] = M[2][2] = M[3][3] = T(1);
M[0][1] = M[1][0] = M[2][3] = M[3][1] = T(0);
M[0][2] = M[1][2] = M[2][0] = M[3][2] = T(0);
M[0][3] = M[1][3] = M[2][1] = M[3][0] = T(0);
}
void SetXBasis(const Vector3& v)
{
M[0][0] = v.x;
M[1][0] = v.y;
M[2][0] = v.z;
}
Vector3 GetXBasis() const
{
return Vector3(M[0][0], M[1][0], M[2][0]);
}
void SetYBasis(const Vector3