aboutsummaryrefslogtreecommitdiffstats
path: root/LibOVR/Src/Kernel/OVR_Math.h
diff options
context:
space:
mode:
Diffstat (limited to 'LibOVR/Src/Kernel/OVR_Math.h')
-rw-r--r--LibOVR/Src/Kernel/OVR_Math.h1918
1 files changed, 1482 insertions, 436 deletions
diff --git a/LibOVR/Src/Kernel/OVR_Math.h b/LibOVR/Src/Kernel/OVR_Math.h
index cdcce81..9bd5bad 100644
--- a/LibOVR/Src/Kernel/OVR_Math.h
+++ b/LibOVR/Src/Kernel/OVR_Math.h
@@ -4,18 +4,19 @@ PublicHeader: OVR.h
Filename : OVR_Math.h
Content : Implementation of 3D primitives such as vectors, matrices.
Created : September 4, 2012
-Authors : Andrew Reisse, Michael Antonov, Steve LaValle, Anna Yershova, Max Katsev
+Authors : Andrew Reisse, Michael Antonov, Steve LaValle,
+ Anna Yershova, Max Katsev, Dov Katz
-Copyright : Copyright 2013 Oculus VR, Inc. All Rights reserved.
+Copyright : Copyright 2014 Oculus VR, Inc. All Rights reserved.
-Licensed under the Oculus VR SDK License Version 2.0 (the "License");
-you may not use the Oculus VR SDK except in compliance with the License,
+Licensed under the Oculus VR Rift SDK License Version 3.1 (the "License");
+you may not use the Oculus VR Rift SDK except in compliance with the License,
which is provided at the time of installation or download, or which
otherwise accompanies this software in either electronic or hard copy form.
You may obtain a copy of the License at
-http://www.oculusvr.com/licenses/LICENSE-2.0
+http://www.oculusvr.com/licenses/LICENSE-3.1
Unless required by applicable law or agreed to in writing, the Oculus VR SDK
distributed under the License is distributed on an "AS IS" BASIS,
@@ -35,6 +36,8 @@ limitations under the License.
#include "OVR_Types.h"
#include "OVR_RefCount.h"
#include "OVR_Std.h"
+#include "OVR_Alg.h"
+
namespace OVR {
@@ -86,15 +89,85 @@ struct WorldAxes
{ 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 float Type;
+};
+
+// Specializations providing CompatibleTypes::Type value.
+template<> struct CompatibleTypes<Quat<float> > { typedef ovrQuatf Type; };
+template<> struct CompatibleTypes<Quat<double> > { typedef ovrQuatd Type; };
+template<> struct CompatibleTypes<Matrix3<double> > { typedef ovrMatrix3d Type; };
+template<> struct CompatibleTypes<Matrix4<float> > { typedef ovrMatrix4f Type; };
+template<> struct CompatibleTypes<Size<int> > { typedef ovrSizei Type; };
+template<> struct CompatibleTypes<Size<float> > { typedef ovrSizef Type; };
+template<> struct CompatibleTypes<Rect<int> > { typedef ovrRecti Type; };
+template<> struct CompatibleTypes<Vector2<int> > { typedef ovrVector2i Type; };
+template<> struct CompatibleTypes<Vector2<float> > { typedef ovrVector2f Type; };
+template<> struct CompatibleTypes<Vector3<float> > { typedef ovrVector3f Type; };
+template<> struct CompatibleTypes<Vector3<double> > { typedef ovrVector3d Type; };
+
+template<> struct CompatibleTypes<Pose<float> > { typedef ovrPosef Type; };
+template<> struct CompatibleTypes<PoseState<float> >{ typedef ovrPoseStatef Type; };
+
+template<> struct CompatibleTypes<Pose<double> > { typedef ovrPosed Type; };
+template<> struct CompatibleTypes<PoseState<double> >{ typedef ovrPoseStated Type; };
//------------------------------------------------------------------------------------//
-// ************************************ Math *****************************************//
+// ***** Math
//
// Math class contains constants and functions. This class is a template specialized
// per type, with Math<float> and Math<double> being distinct.
template<class Type>
class Math
{
+public:
+ // By default, support explicit conversion to float. This allows Vector2<int> to
+ // compile, for example.
+ typedef float OtherFloatType;
};
// Single-precision Math constants class.
@@ -116,6 +189,9 @@ public:
static const float Tolerance; // 0.00001f;
static const float SingularityRadius; // 0.0000001f for Gimbal lock numerical problems
+
+ // Used to support direct conversions in template classes.
+ typedef double OtherFloatType;
};
// Double-precision Math constants class.
@@ -136,10 +212,12 @@ public:
static const double DegreeToRadFactor;
static const double Tolerance; // 0.00001;
- static const double SingularityRadius; // 0.000000000001 for Gimbal lock numerical problems
+ static const double SingularityRadius; // 0.000000000001 for Gimbal lock numerical problems
+ typedef float OtherFloatType;
};
+
typedef Math<float> Mathf;
typedef Math<double> Mathd;
@@ -172,6 +250,7 @@ inline int isnan(double x) { return _isnan(x); };
template<class T>
class Quat;
+
//-------------------------------------------------------------------------------------
// ***** Vector2<>
@@ -187,7 +266,22 @@ public:
Vector2() : x(0), y(0) { }
Vector2(T x_, T y_) : x(x_), y(y_) { }
explicit Vector2(T s) : x(s), y(s) { }
+ explicit Vector2(const Vector2<typename Math<T>::OtherFloatType> &src)
+ : x((T)src.x), y((T)src.y) { }
+
+ // C-interop support.
+ typedef typename CompatibleTypes<Vector2<T> >::Type CompatibleType;
+
+ Vector2(const CompatibleType& s) : x(s.x), y(s.y) { }
+
+ operator const CompatibleType& () const
+ {
+ OVR_COMPILER_ASSERT(sizeof(Vector2<T>) == sizeof(CompatibleType));
+ return reinterpret_cast<const CompatibleType&>(*this);
+ }
+
+
bool operator== (const Vector2& b) const { return x == b.x && y == b.y; }
bool operator!= (const Vector2& b) const { return x != b.x || y != b.y; }
@@ -207,6 +301,11 @@ public:
x *= rcp; y *= rcp;
return *this; }
+ static Vector2 Min(const Vector2& a, const Vector2& b) { return Vector2((a.x < b.x) ? a.x : b.x,
+ (a.y < b.y) ? a.y : b.y); }
+ static Vector2 Max(const Vector2& a, const Vector2& b) { return Vector2((a.x > b.x) ? a.x : b.x,
+ (a.y > b.y) ? a.y : b.y); }
+
// Compare two vectors for equality with tolerance. Returns true if vectors match withing tolerance.
bool Compare(const Vector2&b, T tolerance = Mathf::Tolerance)
{
@@ -216,10 +315,15 @@ public:
// Entrywise product of two vectors
Vector2 EntrywiseMultiply(const Vector2& b) const { return Vector2(x * b.x, y * b.y);}
+
+ // Multiply and divide operators do entry-wise math. Used Dot() for dot product.
+ Vector2 operator* (const Vector2& b) const { return Vector2(x * b.x, y * b.y); }
+ Vector2 operator/ (const Vector2& b) const { return Vector2(x / b.x, y / b.y); }
+
// Dot product
// Used to calculate angle q between two vectors among other things,
// as (A dot B) = |a||b|cos(q).
- T Dot(const Vector2& b) const { return x*b.x + y*b.y; }
+ 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
@@ -232,9 +336,13 @@ public:
// Return Length of the vector squared.
T LengthSq() const { return (x * x + y * y); }
+
// Return vector length.
T Length() const { return sqrt(LengthSq()); }
+ // Returns squared distance between two points represented by vectors.
+ T DistanceSq(Vector2& b) const { return (*this - b).LengthSq(); }
+
// Returns distance between two points represented by vectors.
T Distance(Vector2& b) const { return (*this - b).Length(); }
@@ -273,7 +381,7 @@ public:
typedef Vector2<float> Vector2f;
typedef Vector2<double> Vector2d;
-
+typedef Vector2<int> Vector2i;
//-------------------------------------------------------------------------------------
// ***** Vector3<> - 3D vector of {x, y, z}
@@ -291,6 +399,20 @@ public:
Vector3() : x(0), y(0), z(0) { }
Vector3(T x_, T y_, T z_ = 0) : x(x_), y(y_), z(z_) { }
explicit Vector3(T s) : x(s), y(s), z(s) { }
+ explicit Vector3(const Vector3<typename Math<T>::OtherFloatType> &src)
+ : x((T)src.x), y((T)src.y), z((T)src.z) { }
+
+
+ // C-interop support.
+ typedef typename CompatibleTypes<Vector3<T> >::Type CompatibleType;
+
+ Vector3(const CompatibleType& s) : x(s.x), y(s.y), z(s.z) { }
+
+ operator const CompatibleType& () const
+ {
+ OVR_COMPILER_ASSERT(sizeof(Vector3<T>) == sizeof(CompatibleType));
+ return reinterpret_cast<const CompatibleType&>(*this);
+ }
bool operator== (const Vector3& b) const { return x == b.x && y == b.y && z == b.z; }
bool operator!= (const Vector3& b) const { return x != b.x || y != b.y || z != b.z; }
@@ -311,6 +433,19 @@ public:
x *= rcp; y *= rcp; z *= rcp;
return *this; }
+ static Vector3 Min(const Vector3& a, const Vector3& b)
+ {
+ return Vector3((a.x < b.x) ? a.x : b.x,
+ (a.y < b.y) ? a.y : b.y,
+ (a.z < b.z) ? a.z : b.z);
+ }
+ static Vector3 Max(const Vector3& a, const Vector3& b)
+ {
+ return Vector3((a.x > b.x) ? a.x : b.x,
+ (a.y > b.y) ? a.y : b.y,
+ (a.z > b.z) ? a.z : b.z);
+ }
+
// Compare two vectors for equality with tolerance. Returns true if vectors match withing tolerance.
bool Compare(const Vector3&b, T tolerance = Mathf::Tolerance)
{
@@ -319,11 +454,33 @@ public:
(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).
@@ -347,11 +504,15 @@ public:
// 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& b) const { return (*this - b).LengthSq(); }
+
// Returns distance between two points represented by vectors.
- T Distance(Vector3& b) const { return (*this - b).Length(); }
+ T Distance(Vector3 const& b) const { return (*this - b).Length(); }
// Determine if this a unit vector.
bool IsNormalized() const { return fabs(LengthSq() - T(1)) < Math<T>::Tolerance; }
@@ -392,6 +553,24 @@ public:
typedef Vector3<float> Vector3f;
typedef Vector3<double> Vector3d;
+typedef Vector3<SInt32> Vector3i;
+
+
+// JDC: this was defined in Render_Device.h, I moved it here, but it
+// needs to be fleshed out like the other Vector types.
+//
+// A vector with a dummy w component for alignment in uniform buffers (and for float colors).
+// The w component is not used in any calculations.
+
+struct Vector4f : public Vector3f
+{
+ float w;
+
+ Vector4f() : w(1) {}
+ Vector4f(const Vector3f& v) : Vector3f(v), w(1) {}
+ Vector4f(float r, float g, float b, float a) : Vector3f(r,g,b), w(a) {}
+};
+
//-------------------------------------------------------------------------------------
@@ -404,34 +583,53 @@ template<class T>
class Size
{
public:
- T Width, Height;
+ 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() : Width(0), Height(0) { }
- Size(T w_, T h_) : Width(w_), Height(h_) { }
- explicit Size(T s) : Width(s), Height(s) { }
+ Size(const CompatibleType& s) : w(s.w), h(s.h) { }
+
+ operator const CompatibleType& () const
+ {
+ OVR_COMPILER_ASSERT(sizeof(Size<T>) == sizeof(CompatibleType));
+ return reinterpret_cast<const CompatibleType&>(*this);
+ }
- bool operator== (const Size& b) const { return Width == b.Width && Height == b.Height; }
- bool operator!= (const Size& b) const { return Width != b.Width || Height != b.Height; }
+ 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(Width + b.Width, Height + b.Height); }
- Size& operator+= (const Size& b) { Width += b.Width; Height += b.Height; return *this; }
- Size operator- (const Size& b) const { return Size(Width - b.Width, Height - b.Height); }
- Size& operator-= (const Size& b) { Width -= b.Width; Height -= b.Height; return *this; }
- Size operator- () const { return Size(-Width, -Height); }
- Size operator* (const Size& b) const { return Size(Width * b.Width, Height * b.Height); }
- Size& operator*= (const Size& b) { Width *= b.Width; Height *= b.Height; return *this; }
- Size operator/ (const Size& b) const { return Size(Width / b.Width, Height / b.Height); }
- Size& operator/= (const Size& b) { Width /= b.Width; Height /= b.Height; 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 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(Width*s, Height*s); }
- Size& operator*= (T s) { Width *= s; Height *= s; return *this; }
- Size operator/ (T s) const { return Size(Width/s, Height/s); }
- Size& operator/= (T s) { Width /= s; Height /= s; return *this; }
+ 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 Width * Height; }
+ T Area() const { return w * h; }
- inline Vector2<T> ToVector() const { return Vector2<T>(Width, Height); }
+ inline Vector2<T> ToVector() const { return Vector2<T>(w, h); }
};
@@ -441,10 +639,494 @@ typedef Size<float> Sizef;
typedef Size<double> Sized;
+
+//-----------------------------------------------------------------------------------
+// ***** Rect
+
+// Rect describes a rectangular area for rendering, that includes position and size.
+template<class T>
+class Rect
+{
+public:
+ T x, y;
+ T w, h;
+
+ Rect() { }
+ Rect(T x1, T y1, T w1, T h1) : x(x1), y(y1), w(w1), h(h1) { }
+ Rect(const Vector2<T>& pos, const Size<T>& sz) : x(pos.x), y(pos.y), w(sz.w), h(sz.h) { }
+ Rect(const Size<T>& sz) : x(0), y(0), w(sz.w), h(sz.h) { }
+
+ // C-interop support.
+ typedef typename CompatibleTypes<Rect<T> >::Type CompatibleType;
+
+ Rect(const CompatibleType& s) : x(s.Pos.x), y(s.Pos.y), w(s.Size.w), h(s.Size.h) { }
+
+ operator const CompatibleType& () const
+ {
+ OVR_COMPILER_ASSERT(sizeof(Rect<T>) == sizeof(CompatibleType));
+ return reinterpret_cast<const CompatibleType&>(*this);
+ }
+
+ Vector2<T> GetPos() const { return Vector2<T>(x, y); }
+ Size<T> GetSize() const { return Size<T>(w, h); }
+ void SetPos(const Vector2<T>& pos) { x = pos.x; y = pos.y; }
+ void SetSize(const Size<T>& sz) { w = sz.w; h = sz.h; }
+
+ bool operator == (const Rect& vp) const
+ { return (x == vp.x) && (y == vp.y) && (w == vp.w) && (h == vp.h); }
+ bool operator != (const Rect& vp) const
+ { return !operator == (vp); }
+};
+
+typedef Rect<int> Recti;
+
+
+//-------------------------------------------------------------------------------------//
+// ***** Quat
+//
+// Quatf represents a quaternion class used for rotations.
+//
+// Quaternion multiplications are done in right-to-left order, to match the
+// behavior of matrices.
+
+
+template<class T>
+class Quat
+{
+public:
+ // w + Xi + Yj + Zk
+ T x, y, z, w;
+
+ Quat() : x(0), y(0), z(0), w(1) { }
+ Quat(T x_, T y_, T z_, T w_) : x(x_), y(y_), z(z_), w(w_) { }
+ explicit Quat(const Quat<typename Math<T>::OtherFloatType> &src)
+ : x((T)src.x), y((T)src.y), z((T)src.z), w((T)src.w) { }
+
+ // C-interop support.
+ Quat(const typename CompatibleTypes<Quat<T> >::Type& s) : x(s.x), y(s.y), z(s.z), w(s.w) { }
+
+ operator const typename CompatibleTypes<Quat<T> >::Type () const
+ {
+ typename CompatibleTypes<Quat<T> >::Type result;
+ result.x = x;
+ result.y = y;
+ result.z = z;
+ result.w = w;
+ return result;
+ }
+
+ // Constructs quaternion for rotation around the axis by an angle.
+ Quat(const Vector3<T>& axis, T angle)
+ {
+ // Make sure we don't divide by zero.
+ if (axis.LengthSq() == 0)
+ {
+ // Assert if the axis is zero, but the angle isn't
+ OVR_ASSERT(angle == 0);
+ x = 0; y = 0; z = 0; w = 1;
+ return;
+ }
+
+ Vector3<T> unitAxis = axis.Normalized();
+ T sinHalfAngle = sin(angle * T(0.5));
+
+ w = cos(angle * T(0.5));
+ x = unitAxis.x * sinHalfAngle;
+ y = unitAxis.y * sinHalfAngle;
+ z = unitAxis.z * sinHalfAngle;
+ }
+
+ // Constructs quaternion for rotation around one of the coordinate axis by an angle.
+ Quat(Axis A, T angle, RotateDirection d = Rotate_CCW, HandedSystem s = Handed_R)
+ {
+ T sinHalfAngle = s * d *sin(angle * T(0.5));
+ T v[3];
+ v[0] = v[1] = v[2] = T(0);
+ v[A] = sinHalfAngle;
+
+ w = cos(angle * T(0.5));
+ x = v[0];
+ y = v[1];
+ z = v[2];
+ }
+
+ // Compute axis and angle from quaternion
+ void GetAxisAngle(Vector3<T>* axis, T* angle) const
+ {
+ if ( x*x + y*y + z*z > Math<T>::Tolerance * Math<T>::Tolerance ) {
+ *axis = Vector3<T>(x, y, z).Normalized();
+ *angle = 2 * Acos(w);
+ if (*angle > Math<T>::Pi) // Reduce the magnitude of the angle, if necessary
+ {
+ *angle = Math<T>::TwoPi - *angle;
+ *axis = *axis * (-1);
+ }
+ }
+ else
+ {
+ *axis = Vector3<T>(1, 0, 0);
+ *angle= 0;
+ }
+ }
+
+ // Constructs the quaternion from a rotation matrix
+ explicit Quat(const Matrix4<T>& m)
+ {
+ T trace = m.M[0][0] + m.M[1][1] + m.M[2][2];
+
+ // In almost all cases, the first part is executed.
+ // However, if the trace is not positive, the other
+ // cases arise.
+ if (trace > T(0))
+ {
+ T s = sqrt(trace + T(1)) * T(2); // s=4*qw
+ w = T(0.25) * s;
+ x = (m.M[2][1] - m.M[1][2]) / s;
+ y = (m.M[0][2] - m.M[2][0]) / s;
+ z = (m.M[1][0] - m.M[0][1]) / s;
+ }
+ else if ((m.M[0][0] > m.M[1][1])&&(m.M[0][0] > m.M[2][2]))
+ {
+ T s = sqrt(T(1) + m.M[0][0] - m.M[1][1] - m.M[2][2]) * T(2);
+ w = (m.M[2][1] - m.M[1][2]) / s;
+ x = T(0.25) * s;
+ y = (m.M[0][1] + m.M[1][0]) / s;
+ z = (m.M[2][0] + m.M[0][2]) / s;
+ }
+ else if (m.M[1][1] > m.M[2][2])
+ {
+ T s = sqrt(T(1) + m.M[1][1] - m.M[0][0] - m.M[2][2]) * T(2); // S=4*qy
+ w = (m.M[0][2] - m.M[2][0]) / s;
+ x = (m.M[0][1] + m.M[1][0]) / s;
+ y = T(0.25) * s;
+ z = (m.M[1][2] + m.M[2][1]) / s;
+ }
+ else
+ {
+ T s = sqrt(T(1) + m.M[2][2] - m.M[0][0] - m.M[1][1]) * T(2); // S=4*qz
+ w = (m.M[1][0] - m.M[0][1]) / s;
+ x = (m.M[0][2] + m.M[2][0]) / s;
+ y = (m.M[1][2] + m.M[2][1]) / s;
+ z = T(0.25) * s;
+ }
+ }
+
+ // Constructs the quaternion from a rotation matrix
+ explicit Quat(const Matrix3<T>& m)
+ {
+ T trace = m.M[0][0] + m.M[1][1] + m.M[2][2];
+
+ // In almost all cases, the first part is executed.
+ // However, if the trace is not positive, the other
+ // cases arise.
+ if (trace > T(0))
+ {
+ T s = sqrt(trace + T(1)) * T(2); // s=4*qw
+ w = T(0.25) * s;
+ x = (m.M[2][1] - m.M[1][2]) / s;
+ y = (m.M[0][2] - m.M[2][0]) / s;
+ z = (m.M[1][0] - m.M[0][1]) / s;
+ }
+ else if ((m.M[0][0] > m.M[1][1])&&(m.M[0][0] > m.M[2][2]))
+ {
+ T s = sqrt(T(1) + m.M[0][0] - m.M[1][1] - m.M[2][2]) * T(2);
+ w = (m.M[2][1] - m.M[1][2]) / s;
+ x = T(0.25) * s;
+ y = (m.M[0][1] + m.M[1][0]) / s;
+ z = (m.M[2][0] + m.M[0][2]) / s;
+ }
+ else if (m.M[1][1] > m.M[2][2])
+ {
+ T s = sqrt(T(1) + m.M[1][1] - m.M[0][0] - m.M[2][2]) * T(2); // S=4*qy
+ w = (m.M[0][2] - m.M[2][0]) / s;
+ x = (m.M[0][1] + m.M[1][0]) / s;
+ y = T(0.25) * s;
+ z = (m.M[1][2] + m.M[2][1]) / s;
+ }
+ else
+ {
+ T s = sqrt(T(1) + m.M[2][2] - m.M[0][0] - m.M[1][1]) * T(2); // S=4*qz
+ w = (m.M[1][0] - m.M[0][1]) / s;
+ x = (m.M[0][2] + m.M[2][0]) / s;
+ y = (m.M[1][2] + m.M[2][1]) / s;
+ z = T(0.25) * s;
+ }
+ }
+
+ bool operator== (const Quat& b) const { return x == b.x && y == b.y && z == b.z && w == b.w; }
+ bool operator!= (const Quat& b) const { return x != b.x || y != b.y || z != b.z || w != b.w; }
+
+ Quat operator+ (const Quat& b) const { return Quat(x + b.x, y + b.y, z + b.z, w + b.w); }
+ Quat& operator+= (const Quat& b) { w += b.w; x += b.x; y += b.y; z += b.z; return *this; }
+ Quat operator- (const Quat& b) const { return Quat(x - b.x, y - b.y, z - b.z, w - b.w); }
+ Quat& operator-= (const Quat& b) { w -= b.w; x -= b.x; y -= b.y; z -= b.z; return *this; }
+
+ Quat operator* (T s) const { return Quat(x * s, y * s, z * s, w * s); }
+ Quat& operator*= (T s) { w *= s; x *= s; y *= s; z *= s; return *this; }
+ Quat operator/ (T s) const { T rcp = T(1)/s; return Quat(x * rcp, y * rcp, z * rcp, w *rcp); }
+ Quat& operator/= (T s) { T rcp = T(1)/s; w *= rcp; x *= rcp; y *= rcp; z *= rcp; return *this; }
+
+
+ // Get Imaginary part vector
+ Vector3<T> Imag() const { return Vector3<T>(x,y,z); }
+
+ // Get quaternion length.
+ T Length() const { return sqrt(LengthSq()); }
+
+ // Get quaternion length squared.
+ T LengthSq() const { return (x * x + y * y + z * z + w * w); }
+
+ // Simple Euclidean distance in R^4 (not SLERP distance, but at least respects Haar measure)
+ T Distance(const Quat& q) const
+ {
+ T d1 = (*this - q).Length();
+ T d2 = (*this + q).Length(); // Antipodal point check
+ return (d1 < d2) ? d1 : d2;
+ }
+
+ T DistanceSq(const Quat& q) const
+ {
+ T d1 = (*this - q).LengthSq();
+ T d2 = (*this + q).LengthSq(); // Antipodal point check
+ return (d1 < d2) ? d1 : d2;
+ }
+
+ T Dot(const Quat& q) const
+ {
+ return x * q.x + y * q.y + z * q.z + w * q.w;
+ }
+
+ // Angle between two quaternions in radians
+ T Angle(const Quat& q) const
+ {
+ return 2 * Acos(Alg::Abs(Dot(q)));
+ }
+
+ // Normalize
+ bool IsNormalized() const { return fabs(LengthSq() - T(1)) < Math<T>::Tolerance; }
+
+ void Normalize()
+ {
+ T l = Length();
+ OVR_ASSERT(l != T(0));
+ *this /= l;
+ }
+
+ Quat Normalized() const
+ {
+ T l = Length();
+ OVR_ASSERT(l != T(0));
+ return *this / l;
+ }
+
+ // Returns conjugate of the quaternion. Produces inverse rotation if quaternion is normalized.
+ Quat Conj() const { return Quat(-x, -y, -z, w); }
+
+ // Quaternion multiplication. Combines quaternion rotations, performing the one on the
+ // right hand side first.
+ Quat operator* (const Quat& b) const { return Quat(w * b.x + x * b.w + y * b.z - z * b.y,
+ w * b.y - x * b.z + y * b.w + z * b.x,
+ w * b.z + x * b.y - y * b.x + z * b.w,
+ w * b.w - x * b.x - y * b.y - z * b.z); }
+
+ //
+ // this^p normalized; same as rotating by this p times.
+ Quat PowNormalized(T p) const
+ {
+ Vector3<T> v;
+ T a;
+ GetAxisAngle(&v, &a);
+ return Quat(v, a * p);
+ }
+
+ // Normalized linear interpolation of quaternions
+ Quat Nlerp(const Quat& other, T a)
+ {
+ T sign = (Dot(other) >= 0) ? 1 : -1;
+ return (*this * sign * a + other * (1-a)).Normalized();
+ }
+
+ // Rotate transforms vector in a manner that matches Matrix rotations (counter-clockwise,
+ // assuming negative direction of the axis). Standard formula: q(t) * V * q(t)^-1.
+ Vector3<T> Rotate(const Vector3<T>& v) const
+ {
+ return ((*this * Quat<T>(v.x, v.y, v.z, T(0))) * Inverted()).Imag();
+ }
+
+ // Inversed quaternion rotates in the opposite direction.
+ Quat Inverted() const
+ {
+ return Quat(-x, -y, -z, w);
+ }
+
+ // Sets this quaternion to the one rotates in the opposite direction.
+ void Invert()
+ {
+ *this = Quat(-x, -y, -z, w);
+ }
+
+ // GetEulerAngles extracts Euler angles from the quaternion, in the specified order of
+ // axis rotations and the specified coordinate system. Right-handed coordinate system
+ // is the default, with CCW rotations while looking in the negative axis direction.
+ // Here a,b,c, are the Yaw/Pitch/Roll angles to be returned.
+ // rotation a around axis A1
+ // is followed by rotation b around axis A2
+ // is followed by rotation c around axis A3
+ // rotations are CCW or CW (D) in LH or RH coordinate system (S)
+ template <Axis A1, Axis A2, Axis A3, RotateDirection D, HandedSystem S>
+ void GetEulerAngles(T *a, T *b, T *c) const
+ {
+ OVR_COMPILER_ASSERT((A1 != A2) && (A2 != A3) && (A1 != A3));
+
+ T Q[3] = { x, y, z }; //Quaternion components x,y,z
+
+ T ww = w*w;
+ T Q11 = Q[A1]*Q[A1];
+ T Q22 = Q[A2]*Q[A2];
+ T Q33 = Q[A3]*Q[A3];
+
+ T psign = T(-1);
+ // Determine whether even permutation
+ if (((A1 + 1) % 3 == A2) && ((A2 + 1) % 3 == A3))
+ psign = T(1);
+
+ T s2 = psign * T(2) * (psign*w*Q[A2] + Q[A1]*Q[A3]);
+
+ if (s2 < T(-1) + Math<T>::SingularityRadius)
+ { // South pole singularity
+ *a = T(0);
+ *b = -S*D*Math<T>::PiOver2;
+ *c = S*D*atan2(T(2)*(psign*Q[A1]*Q[A2] + w*Q[A3]),
+ ww + Q22 - Q11 - Q33 );
+ }
+ else if (s2 > T(1) - Math<T>::SingularityRadius)
+ { // North pole singularity
+ *a = T(0);
+ *b = S*D*Math<T>::PiOver2;
+ *c = S*D*atan2(T(2)*(psign*Q[A1]*Q[A2] + w*Q[A3]),
+ ww + Q22 - Q11 - Q33);
+ }
+ else
+ {
+ *a = -S*D*atan2(T(-2)*(w*Q[A1] - psign*Q[A2]*Q[A3]),
+ ww + Q33 - Q11 - Q22);
+ *b = S*D*asin(s2);
+ *c = S*D*atan2(T(2)*(w*Q[A3] - psign*Q[A1]*Q[A2]),
+ ww + Q11 - Q22 - Q33);
+ }
+ return;
+ }
+
+ template <Axis A1, Axis A2, Axis A3, RotateDirection D>
+ void GetEulerAngles(T *a, T *b, T *c) const
+ { GetEulerAngles<A1, A2, A3, D, Handed_R>(a, b, c); }
+
+ template <Axis A1, Axis A2, Axis A3>
+ void GetEulerAngles(T *a, T *b, T *c) const
+ { GetEulerAngles<A1, A2, A3, Rotate_CCW, Handed_R>(a, b, c); }
+
+
+ // GetEulerAnglesABA extracts Euler angles from the quaternion, in the specified order of
+ // axis rotations and the specified coordinate system. Right-handed coordinate system
+ // is the default, with CCW rotations while looking in the negative axis direction.
+ // Here a,b,c, are the Yaw/Pitch/Roll angles to be returned.
+ // rotation a around axis A1
+ // is followed by rotation b around axis A2
+ // is followed by rotation c around axis A1
+ // Rotations are CCW or CW (D) in LH or RH coordinate system (S)
+ template <Axis A1, Axis A2, RotateDirection D, HandedSystem S>
+ void GetEulerAnglesABA(T *a, T *b, T *c) const
+ {
+ OVR_COMPILER_ASSERT(A1 != A2);
+
+ T Q[3] = {x, y, z}; // Quaternion components
+
+ // Determine the missing axis that was not supplied
+ int m = 3 - A1 - A2;
+
+ T ww = w*w;
+ T Q11 = Q[A1]*Q[A1];
+ T Q22 = Q[A2]*Q[A2];
+ T Qmm = Q[m]*Q[m];
+
+ T psign = T(-1);
+ if ((A1 + 1) % 3 == A2) // Determine whether even permutation
+ {
+ psign = T(1);
+ }
+
+ T c2 = ww + Q11 - Q22 - Qmm;
+ if (c2 < T(-1) + Math<T>::SingularityRadius)
+ { // South pole singularity
+ *a = T(0);
+ *b = S*D*Math<T>::Pi;
+ *c = S*D*atan2( T(2)*(w*Q[A1] - psign*Q[A2]*Q[m]),
+ ww + Q22 - Q11 - Qmm);
+ }
+ else if (c2 > T(1) - Math<T>::SingularityRadius)
+ { // North pole singularity
+ *a = T(0);
+ *b = T(0);
+ *c = S*D*atan2( T(2)*(w*Q[A1] - psign*Q[A2]*Q[m]),
+ ww + Q22 - Q11 - Qmm);
+ }
+ else
+ {
+ *a = S*D*atan2( psign*w*Q[m] + Q[A1]*Q[A2],
+ w*Q[A2] -psign*Q[A1]*Q[m]);
+ *b = S*D*acos(c2);
+ *c = S*D*atan2( -psign*w*Q[m] + Q[A1]*Q[A2],
+ w*Q[A2] + psign*Q[A1]*Q[m]);
+ }
+ return;
+ }
+};
+
+typedef Quat<float> Quatf;
+typedef Quat<double> Quatd;
+
//-------------------------------------------------------------------------------------
-// ***** Matrix4f
+// ***** 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)
+ : Orientation(orientation), Position(pos) { }
+ Pose(const Pose& s)
+ : Orientation(s.Orientation), Position(s.Position) { }
+ Pose(const CompatibleType& s)
+ : Orientation(s.Orientation), Position(s.Position) { }
+ explicit Pose(const Pose<typename Math<T>::OtherFloatType> &s)
+ : Orientation(s.Orientation), Position(s.Position) { }
+
+ operator const typename CompatibleTypes<Pose<T> >::Type () const
+ {
+ typename CompatibleTypes<Pose<T> >::Type result;
+ result.Orientation = Orientation;
+ result.Position = Position;
+ return result;
+ }
+
+ Quat<T> Orientation;
+ Vector3<T> Position;
+};
+
+typedef Pose<float> Posef;
+typedef Pose<double> Posed;
+
+
+//-------------------------------------------------------------------------------------
+// ***** Matrix4
//
-// Matrix4f is a 4x4 matrix used for 3d transformations and projections.
+// 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.
@@ -469,28 +1151,29 @@ typedef Size<double> Sized;
//
// The basis vectors are first three columns.
-class Matrix4f
+template<class T>
+class Matrix4
{
- static Matrix4f IdentityValue;
+ static const Matrix4 IdentityValue;
public:
- float M[4][4];
+ T M[4][4];
enum NoInitType { NoInit };
// Construct with no memory initialization.
- Matrix4f(NoInitType) { }
+ Matrix4(NoInitType) { }
// By default, we construct identity matrix.
- Matrix4f()
+ Matrix4()
{
SetIdentity();
}
- Matrix4f(float m11, float m12, float m13, float m14,
- float m21, float m22, float m23, float m24,
- float m31, float m32, float m33, float m34,
- float m41, float m42, float m43, float m44)
+ 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;
@@ -498,9 +1181,9 @@ public:
M[3][0] = m41; M[3][1] = m42; M[3][2] = m43; M[3][3] = m44;
}
- Matrix4f(float m11, float m12, float m13,
- float m21, float m22, float m23,
- float m31, float m32, float m33)
+ 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;
@@ -508,7 +1191,50 @@ public:
M[3][0] = 0; M[3][1] = 0; M[3][2] = 0; M[3][3] = 1;
}
- void ToString(char* dest, UPInt destsize)
+ 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.Orientation);
+ result.SetTranslation(p.Position);
+ *this = result;
+ }
+
+ // C-interop support
+ explicit Matrix4(const Matrix4<typename Math<T>::OtherFloatType> &src)
+ {
+ for (int i = 0; i < 4; i++)
+ for (int j = 0; j < 4; j++)
+ M[i][j] = (T)src.M[i][j];
+ }
+
+ // C-interop support.
+ Matrix4(const typename CompatibleTypes<Matrix4<T> >::Type& s)
+ {
+ OVR_COMPILER_ASSERT(sizeof(s) == sizeof(Matrix4));
+ memcpy(M, s.M, sizeof(M));
+ }
+
+ operator const typename CompatibleTypes<Matrix4<T> >::Type () const
+ {
+ typename CompatibleTypes<Matrix4<T> >::Type result;
+ OVR_COMPILER_ASSERT(sizeof(result) == sizeof(Matrix4));
+ memcpy(result.M, M, sizeof(M));
+ return result;
+ }
+
+ void ToString(char* dest, UPInt destsize) const
{
UPInt pos = 0;
for (int r=0; r<4; r++)
@@ -516,13 +1242,13 @@ public:
pos += OVR_sprintf(dest+pos, destsize-pos, "%g ", M[r][c]);
}
- static Matrix4f FromString(const char* src)
+ static Matrix4 FromString(const char* src)
{
- Matrix4f result;
+ Matrix4 result;
for (int r=0; r<4; r++)
for (int c=0; c<4; c++)
{
- result.M[r][c] = (float)atof(src);
+ result.M[r][c] = (T)atof(src);
while (src && *src != ' ')
src++;
while (src && *src == ' ')
@@ -531,7 +1257,7 @@ public:
return result;
}
- static const Matrix4f& Identity() { return IdentityValue; }
+ static const Matrix4& Identity() { return IdentityValue; }
void SetIdentity()
{
@@ -541,14 +1267,24 @@ public:
M[0][3] = M[1][3] = M[2][1] = M[3][0] = 0;
}
- Matrix4f operator+ (const Matrix4f& b) const
+ 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
{
- Matrix4f result(*this);
+ Matrix4 result(*this);
result += b;
return result;
}
- Matrix4f& operator+= (const Matrix4f& b)
+ Matrix4& operator+= (const Matrix4& b)
{
for (int i = 0; i < 4; i++)
for (int j = 0; j < 4; j++)
@@ -556,14 +1292,14 @@ public:
return *this;
}
- Matrix4f operator- (const Matrix4f& b) const
+ Matrix4 operator- (const Matrix4& b) const
{
- Matrix4f result(*this);
+ Matrix4 result(*this);
result -= b;
return result;
}
- Matrix4f& operator-= (const Matrix4f& b)
+ Matrix4& operator-= (const Matrix4& b)
{
for (int i = 0; i < 4; i++)
for (int j = 0; j < 4; j++)
@@ -572,7 +1308,7 @@ public:
}
// Multiplies two matrices into destination with minimum copying.
- static Matrix4f& Multiply(Matrix4f* d, const Matrix4f& a, const Matrix4f& b)
+ static Matrix4& Multiply(Matrix4* d, const Matrix4& a, const Matrix4& b)
{
OVR_ASSERT((d != &a) && (d != &b));
int i = 0;
@@ -586,26 +1322,26 @@ public:
return *d;
}
- Matrix4f operator* (const Matrix4f& b) const
+ Matrix4 operator* (const Matrix4& b) const
{
- Matrix4f result(Matrix4f::NoInit);
+ Matrix4 result(Matrix4::NoInit);
Multiply(&result, *this, b);
return result;
}
- Matrix4f& operator*= (const Matrix4f& b)
+ Matrix4& operator*= (const Matrix4& b)
{
- return Multiply(this, Matrix4f(*this), b);
+ return Multiply(this, Matrix4(*this), b);
}
- Matrix4f operator* (float s) const
+ Matrix4 operator* (T s) const
{
- Matrix4f result(*this);
+ Matrix4 result(*this);
result *= s;
return result;
}
- Matrix4f& operator*= (float s)
+ Matrix4& operator*= (T s)
{
for (int i = 0; i < 4; i++)
for (int j = 0; j < 4; j++)
@@ -614,14 +1350,14 @@ public:
}
- Matrix4f operator/ (float s) const
+ Matrix4 operator/ (T s) const
{
- Matrix4f result(*this);
+ Matrix4 result(*this);
result /= s;
return result;
}
- Matrix4f& operator/= (float s)
+ Matrix4& operator/= (T s)
{
for (int i = 0; i < 4; i++)
for (int j = 0; j < 4; j++)
@@ -629,16 +1365,16 @@ public:
return *this;
}
- Vector3f Transform(const Vector3f& v) const
+ Vector3<T> Transform(const Vector3<T>& v) const
{
- return Vector3f(M[0][0] * v.x + M[0][1] * v.y + M[0][2] * v.z + M[0][3],
- M[1][0] * v.x + M[1][1] * v.y + M[1][2] * v.z + M[1][3],
- M[2][0] * v.x + M[2][1] * v.y + M[2][2] * v.z + M[2][3]);
+ return Vector3<T>(M[0][0] * v.x + M[0][1] * v.y + M[0][2] * v.z + M[0][3],
+ M[1][0] * v.x + M[1][1] * v.y + M[1][2] * v.z + M[1][3],
+ M[2][0] * v.x + M[2][1] * v.y + M[2][2] * v.z + M[2][3]);
}
- Matrix4f Transposed() const
+ Matrix4 Transposed() const
{
- return Matrix4f(M[0][0], M[1][0], M[2][0], M[3][0],
+ 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]);
@@ -650,35 +1386,35 @@ public:
}
- float SubDet (const UPInt* rows, const UPInt* cols) const
+ T SubDet (const UPInt* rows, const UPInt* cols) const
{
return M[rows[0]][cols[0]] * (M[rows[1]][cols[1]] * M[rows[2]][cols[2]] - M[rows[1]][cols[2]] * M[rows[2]][cols[1]])
- M[rows[0]][cols[1]] * (M[rows[1]][cols[0]] * M[rows[2]][cols[2]] - M[rows[1]][cols[2]] * M[rows[2]][cols[0]])
+ M[rows[0]][cols[2]] * (M[rows[1]][cols[0]] * M[rows[2]][cols[1]] - M[rows[1]][cols[1]] * M[rows[2]][cols[0]]);
}
- float Cofactor(UPInt I, UPInt J) const
+ T Cofactor(UPInt I, UPInt J) const
{
const UPInt indices[4][3] = {{1,2,3},{0,2,3},{0,1,3},{0,1,2}};
return ((I+J)&1) ? -SubDet(indices[I],indices[J]) : SubDet(indices[I],indices[J]);
}
- float Determinant() const
+ 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);
}
- Matrix4f Adjugated() const
+ Matrix4 Adjugated() const
{
- return Matrix4f(Cofactor(0,0), Cofactor(1,0), Cofactor(2,0), Cofactor(3,0),
+ 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));
}
- Matrix4f Inverted() const
+ Matrix4 Inverted() const
{
- float det = Determinant();
+ T det = Determinant();
assert(det != 0);
return Adjugated() * (1.0f/det);
}
@@ -690,14 +1426,14 @@ public:
// This is more efficient than general inverse, but ONLY works
// correctly if it is a homogeneous transform matrix (rot + trans)
- Matrix4f InvertedHomogeneousTransform() const
+ Matrix4 InvertedHomogeneousTransform() const
{
// Make the inverse rotation matrix
- Matrix4f rinv = this->Transposed();
+ Matrix4 rinv = this->Transposed();
rinv.M[3][0] = rinv.M[3][1] = rinv.M[3][2] = 0.0f;
// Make the inverse translation matrix
- Vector3f tvinv = Vector3f(-M[0][3],-M[1][3],-M[2][3]);
- Matrix4f tinv = Matrix4f::Translation(tvinv);
+ Vector3<T> tvinv(-M[0][3],-M[1][3],-M[2][3]);
+ Matrix4 tinv = Matrix4::Translation(tvinv);
return rinv * tinv; // "untranslate", then "unrotate"
}
@@ -715,25 +1451,25 @@ public:
// 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(float *a, float *b, float *c)
+ void ToEulerAngles(T *a, T *b, T *c)
{
OVR_COMPILER_ASSERT((A1 != A2) && (A2 != A3) && (A1 != A3));
- float psign = -1.0f;
+ T psign = -1;
if (((A1 + 1) % 3 == A2) && ((A2 + 1) % 3 == A3)) // Determine whether even permutation
- psign = 1.0f;
+ psign = 1;
- float pm = psign*M[A1][A3];
- if (pm < -1.0f + Math<float>::SingularityRadius)
+ T pm = psign*M[A1][A3];
+ if (pm < -1.0f + Math<T>::SingularityRadius)
{ // South pole singularity
- *a = 0.0f;
- *b = -S*D*Math<float>::PiOver2;
+ *a = 0;
+ *b = -S*D*Math<T>::PiOver2;
*c = S*D*atan2( psign*M[A2][A1], M[A2][A2] );
}
- else if (pm > 1.0f - Math<float>::SingularityRadius)
+ else if (pm > 1.0f - Math<T>::SingularityRadius)
{ // North pole singularity
- *a = 0.0f;
- *b = S*D*Math<float>::PiOver2;
+ *a = 0;
+ *b = S*D*Math<T>::PiOver2;
*c = S*D*atan2( psign*M[A2][A1], M[A2][A2] );
}
else
@@ -753,28 +1489,28 @@ public:
// 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(float *a, float *b, float *c)
+ void ToEulerAnglesABA(T *a, T *b, T *c)
{
OVR_COMPILER_ASSERT(A1 != A2);
// Determine the axis that was not supplied
int m = 3 - A1 - A2;
- float psign = -1.0f;
+ T psign = -1;
if ((A1 + 1) % 3 == A2) // Determine whether even permutation
psign = 1.0f;
- float c2 = M[A1][A1];
- if (c2 < -1.0f + Math<float>::SingularityRadius)
+ T c2 = M[A1][A1];
+ if (c2 < -1 + Math<T>::SingularityRadius)
{ // South pole singularity
- *a = 0.0f;
- *b = S*D*Math<float>::Pi;
+ *a = 0;
+ *b = S*D*Math<T>::Pi;
*c = S*D*atan2( -psign*M[A2][m],M[A2][A2]);
}
- else if (c2 > 1.0f - Math<float>::SingularityRadius)
+ else if (c2 > 1.0f - Math<T>::SingularityRadius)
{ // North pole singularity
- *a = 0.0f;
- *b = 0.0f;
+ *a = 0;
+ *b = 0;
*c = S*D*atan2( -psign*M[A2][m],M[A2][A2]);
}
else
@@ -788,7 +1524,7 @@ public:
// Creates a matrix that converts the vertices from one coordinate system
// to another.
- static Matrix4f AxisConversion(const WorldAxes& to, const WorldAxes& from)
+ 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 };
@@ -799,22 +1535,22 @@ public:
inv[abs(to.YAxis)] = 1;
inv[abs(to.ZAxis)] = 2;
- Matrix4f m(0, 0, 0,
- 0, 0, 0,
- 0, 0, 0);
+ 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] = float(from.XAxis/toArray[inv[abs(from.XAxis)]]);
- m.M[inv[abs(from.YAxis)]][1] = float(from.YAxis/toArray[inv[abs(from.YAxis)]]);
- m.M[inv[abs(from.ZAxis)]][2] = float(from.ZAxis/toArray[inv[abs(from.ZAxis)]]);
+ 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 Matrix4f Translation(const Vector3f& v)
+ static Matrix4 Translation(const Vector3<T>& v)
{
- Matrix4f t;
+ Matrix4 t;
t.M[0][3] = v.x;
t.M[1][3] = v.y;
t.M[2][3] = v.z;
@@ -822,19 +1558,32 @@ public:
}
// Creates a matrix for translation by vector
- static Matrix4f Translation(float x, float y, float z = 0.0f)
+ static Matrix4 Translation(T x, T y, T z = 0.0f)
{
- Matrix4f t;
+ 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 Matrix4f Scaling(const Vector3f& v)
+ static Matrix4 Scaling(const Vector3<T>& v)
{
- Matrix4f t;
+ Matrix4 t;
t.M[0][0] = v.x;
t.M[1][1] = v.y;
t.M[2][2] = v.z;
@@ -842,9 +1591,9 @@ public:
}
// Creates a matrix for scaling by vector
- static Matrix4f Scaling(float x, float y, float z)
+ static Matrix4 Scaling(T x, T y, T z)
{
- Matrix4f t;
+ Matrix4 t;
t.M[0][0] = x;
t.M[1][1] = y;
t.M[2][2] = z;
@@ -852,38 +1601,50 @@ public:
}
// Creates a matrix for scaling by constant
- static Matrix4f Scaling(float s)
+ static Matrix4 Scaling(T s)
{
- Matrix4f t;
+ 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 Matrix4f RotationAxis(Axis A, float angle, RotateDirection d, HandedSystem s)
+ static Matrix4 RotationAxis(Axis A, T angle, RotateDirection d, HandedSystem s)
{
- float sina = s * d *sin(angle);
- float cosa = cos(angle);
+ T sina = s * d *sin(angle);
+ T cosa = cos(angle);
switch(A)
{
case Axis_X:
- return Matrix4f(1, 0, 0,
- 0, cosa, -sina,
- 0, sina, cosa);
+ return Matrix4(1, 0, 0,
+ 0, cosa, -sina,
+ 0, sina, cosa);
case Axis_Y:
- return Matrix4f(cosa, 0, sina,
- 0, 1, 0,
- -sina, 0, cosa);
+ return Matrix4(cosa, 0, sina,
+ 0, 1, 0,
+ -sina, 0, cosa);
case Axis_Z:
- return Matrix4f(cosa, -sina, 0,
- sina, cosa, 0,
- 0, 0, 1);
+ return Matrix4(cosa, -sina, 0,
+ sina, cosa, 0,
+ 0, 0, 1);
}
}
@@ -895,13 +1656,13 @@ public:
// 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 Matrix4f RotationX(float angle)
+ static Matrix4 RotationX(T angle)
{
- float sina = sin(angle);
- float cosa = cos(angle);
- return Matrix4f(1, 0, 0,
- 0, cosa, -sina,
- 0, sina, cosa);
+ 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.
@@ -911,13 +1672,13 @@ public:
// 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 Matrix4f RotationY(float angle)
+ static Matrix4 RotationY(T angle)
{
- float sina = sin(angle);
- float cosa = cos(angle);
- return Matrix4f(cosa, 0, sina,
- 0, 1, 0,
- -sina, 0, cosa);
+ 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.
@@ -927,27 +1688,47 @@ public:
// 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 Matrix4f RotationZ(float angle)
+ static Matrix4 RotationZ(T angle)
{
- float sina = sin(angle);
- float cosa = cos(angle);
- return Matrix4f(cosa, -sina, 0,
- sina, cosa, 0,
- 0, 0, 1);
+ 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 Matrix4f LookAtRH(const Vector3f& eye, const Vector3f& at, const Vector3f& up);
-
+ 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 Matrix4f LookAtLH(const Vector3f& eye, const Vector3f& at, const Vector3f& up);
-
+ 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.
@@ -958,8 +1739,22 @@ public:
// 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 Matrix4f PerspectiveRH(float yfov, float aspect, float znear, float zfar);
-
+ static Matrix4 PerspectiveRH(T yfov, T aspect, T znear, T zfar)
+ {
+ Matrix4 m;
+ T tanHalfFov = tan(yfov * 0.5f);
+
+ m.M[0][0] = 1 / (aspect * tanHalfFov);
+ m.M[1][1] = 1 / tanHalfFov;
+ m.M[2][2] = zfar / (zfar - znear);
+ m.M[3][2] = 1;
+ m.M[2][3] = (zfar * znear) / (znear - zfar);
+ m.M[3][3] = 0;
+
+ // Note: Post-projection matrix result assumes Left-Handed coordinate system,
+ // with Y up, X right and Z forward. This supports positive z-buffer values.
+ return m;
+ }
// PerspectiveRH creates a left-handed perspective projection matrix that can be
// used with the Oculus sample renderer.
@@ -968,350 +1763,601 @@ public:
// 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 Matrix4f PerspectiveLH(float yfov, float aspect, float znear, float zfar);
-
+ static Matrix4 PerspectiveLH(T yfov, T aspect, T znear, T zfar)
+ {
+ Matrix4 m;
+ T tanHalfFov = tan(yfov * 0.5f);
+
+ m.M[0][0] = 1.0 / (aspect * tanHalfFov);
+ m.M[1][1] = 1.0 / tanHalfFov;
+ m.M[2][2] = zfar / (znear - zfar);
+ // m.M[2][2] = zfar / (zfar - znear);
+ m.M[3][2] = -1.0;
+ m.M[2][3] = (zfar * znear) / (znear - zfar);
+ m.M[3][3] = 0.0;
+
+ // Note: Post-projection matrix result assumes Left-Handed coordinate system,
+ // with Y up, X right and Z forward. This supports positive z-buffer values.
+ // This is the case even for RHS cooridnate input.
+ return m;
+ }
- static Matrix4f Ortho2D(float w, float h);
+ 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;
-//-------------------------------------------------------------------------------------//
-// **************************************** Quat **************************************//
+//-------------------------------------------------------------------------------------
+// ***** Matrix3
//
-// Quatf represents a quaternion class used for rotations.
-//
-// Quaternion multiplications are done in right-to-left order, to match the
-// behavior of matrices.
+// 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 Quat
+class Matrix3
{
+ static const Matrix3 IdentityValue;
+
public:
- // w + Xi + Yj + Zk
- T x, y, z, w;
+ T M[3][3];
- 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_) {}
+ enum NoInitType { NoInit };
+ // Construct with no memory initialization.
+ Matrix3(NoInitType) { }
- // Constructs quaternion for rotation around the axis by an angle.
- Quat(const Vector3<T>& axis, T angle)
- {
- Vector3<T> unitAxis = axis.Normalized();
- T sinHalfAngle = sin(angle * T(0.5));
+ // By default, we construct identity matrix.
+ Matrix3()
+ {
+ SetIdentity();
+ }
- w = cos(angle * T(0.5));
- x = unitAxis.x * sinHalfAngle;
- y = unitAxis.y * sinHalfAngle;
- z = unitAxis.z * sinHalfAngle;
+ 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;
}
- // Constructs quaternion for rotation around one of the coordinate axis by an angle.
- void AxisAngle(Axis A, T angle, RotateDirection d, HandedSystem s)
- {
- T sinHalfAngle = s * d *sin(angle * T(0.5));
- T v[3];
- v[0] = v[1] = v[2] = T(0);
- v[A] = sinHalfAngle;
+ explicit Matrix3(const Pose<T>& p)
+ {
+ Matrix3 result(p.Orientation);
+ result.SetTranslation(p.Position);
+ *this = result;
+ }
- w = cos(angle * T(0.5));
- x = v[0];
- y = v[1];
- z = v[2];
- }
+ // C-interop support
+ explicit Matrix3(const Matrix4<typename Math<T>::OtherFloatType> &src)
+ {
+ for (int i = 0; i < 3; i++)
+ for (int j = 0; j < 3; j++)
+ M[i][j] = (T)src.M[i][j];
+ }
+ // C-interop support.
+ Matrix3(const typename CompatibleTypes<Matrix3<T> >::Type& s)
+ {
+ OVR_COMPILER_ASSERT(sizeof(s) == sizeof(Matrix3));
+ memcpy(M, s.M, sizeof(M));
+ }
- // Compute axis and angle from quaternion
- void GetAxisAngle(Vector3<T>* axis, T* angle) const
- {
- if ( x*x + y*y + z*z > Math<T>::Tolerance * Math<T>::Tolerance ) {
- *axis = Vector3<T>(x, y, z).Normalized();
- *angle = T(2) * Acos(w);
- }
- else
- {
- *axis = Vector3<T>(1, 0, 0);
- *angle= 0;
- }
- }
+ operator const typename CompatibleTypes<Matrix3<T> >::Type () const
+ {
+ typename CompatibleTypes<Matrix3<T> >::Type result;
+ OVR_COMPILER_ASSERT(sizeof(result) == sizeof(Matrix3));
+ memcpy(result.M, M, sizeof(M));
+ return result;
+ }
- 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; }
+ void ToString(char* dest, UPInt destsize) const
+ {
+ UPInt pos = 0;
+ for (int r=0; r<3; r++)
+ for (int c=0; c<3; c++)
+ pos += OVR_sprintf(dest+pos, destsize-pos, "%g ", M[r][c]);
+ }
- 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; }
+ 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;
+ }
- 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; }
+ 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;
+ }
- // Get Imaginary part vector
- Vector3<T> Imag() const { return Vector3<T>(x,y,z); }
+ 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]);
- // Get quaternion length.
- T Length() const { return sqrt(x * x + y * y + z * z + w * w); }
- // Get quaternion length squared.
- T LengthSq() const { return (x * x + y * y + z * z + w * w); }
+ return isEqual;
+ }
- // Simple Eulidean 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;
+ Matrix3 operator+ (const Matrix3& b) const
+ {
+ Matrix4<T> result(*this);
+ result += b;
+ return result;
}
- T DistanceSq(const Quat& q) const
- {
- T d1 = (*this - q).LengthSq();
- T d2 = (*this + q).LengthSq(); // Antipodal point check
- return (d1 < d2) ? d1 : d2;
- }
+ 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;
+ }
- // Normalize
- bool IsNormalized() const { return fabs(LengthSq() - T(1)) < Math<T>::Tolerance; }
+ 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 Normalize()
+ void operator= (const SymMat3<T>& b)
{
- T l = Length();
- OVR_ASSERT(l != T(0));
- *this /= l;
+ 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;
}
- Quat Normalized() const
- {
- T l = Length();
- OVR_ASSERT(l != T(0));
- return *this / l;
+ Matrix3 operator- (const Matrix3& b) const
+ {
+ Matrix3 result(*this);
+ result -= b;
+ return result;
}
- // Returns conjugate of the quaternion. Produces inverse rotation if quaternion is normalized.
- Quat Conj() const { return Quat(-x, -y, -z, w); }
+ 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;
+ }
- // 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); }
+ // 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;
+ }
- //
- // 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);
- }
-
- // 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();
- }
+ Matrix3 operator* (const Matrix3& b) const
+ {
+ Matrix3 result(Matrix3::NoInit);
+ Multiply(&result, *this, b);
+ return result;
+ }
-
- // Inversed quaternion rotates in the opposite direction.
- Quat Inverted() const
- {
- return Quat(-x, -y, -z, w);
- }
+ Matrix3& operator*= (const Matrix3& b)
+ {
+ return Multiply(this, Matrix3(*this), b);
+ }
- // Sets this quaternion to the one rotates in the opposite direction.
- void Invert()
- {
- *this = Quat(-x, -y, -z, w);
- }
-
- // Converting quaternion to matrix.
- operator Matrix4f() const
- {
- T ww = w*w;
- T xx = x*x;
- T yy = y*y;
- T zz = z*z;
+ Matrix3 operator* (T s) const
+ {
+ Matrix3 result(*this);
+ result *= s;
+ return result;
+ }
- return Matrix4f(float(ww + xx - yy - zz), float(T(2) * (x*y - w*z)), float(T(2) * (x*z + w*y)),
- float(T(2) * (x*y + w*z)), float(ww - xx + yy - zz), float(T(2) * (y*z - w*x)),
- float(T(2) * (x*z - w*y)), float(T(2) * (y*z + w*x)), float(ww - xx - yy + zz) );
- }
+ 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;
+ }
- // Converting matrix to quaternion
- static Quat<T> Matrix4fToQuat(const Matrix4f& m)
+ Matrix3 operator/ (T s) const
{
- T trace = m.M[0][0] + m.M[1][1] + m.M[2][2];
- Quat<T> q;
+ Matrix3 result(*this);
+ result /= s;
+ return result;
+ }
- // 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
- q.w = T(0.25) * s;
- q.x = (m.M[2][1] - m.M[1][2]) / s;
- q.y = (m.M[0][2] - m.M[2][0]) / s;
- q.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);
- q.w = (m.M[2][1] - m.M[1][2]) / s;
- q.x = T(0.25) * s;
- q.y = (m.M[0][1] + m.M[1][0]) / s;
- q.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
- q.w = (m.M[0][2] - m.M[2][0]) / s;
- q.x = (m.M[0][1] + m.M[1][0]) / s;
- q.y = T(0.25) * s;
- q.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
- q.w = (m.M[1][0] - m.M[0][1]) / s;
- q.x = (m.M[0][2] + m.M[2][0]) / s;
- q.y = (m.M[1][2] + m.M[2][1]) / s;
- q.z = T(0.25) * s;
- }
- return q;
+ Matrix3& operator/= (T s)
+ {
+ for (int i = 0; i < 3; i++)
+ for (int j = 0; j < 3; j++)
+ M[i][j] /= s;
+ return *this;
}
+ Vector3<T> Transform(const Vector3<T>& v) const
+ {
+ return Vector3<T>(M[0][0] * v.x + M[0][1] * v.y + M[0][2] * v.z,
+ M[1][0] * v.x + M[1][1] * v.y + M[1][2] * v.z,
+ M[2][0] * v.x + M[2][1] * v.y + M[2][2] * v.z);
+ }
-
- // 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)
+ Matrix3 Transposed() const
+ {
+ return Matrix3(M[0][0], M[1][0], M[2][0],
+ M[0][1], M[1][1], M[2][1],
+ M[0][2], M[1][2], M[2][2]);
+ }
+
+ void Transpose()
+ {
+ *this = Transposed();
+ }
+
+
+ T SubDet (const UPInt* rows, const UPInt* cols) const
+ {
+ return M[rows[0]][cols[0]] * (M[rows[1]][cols[1]] * M[rows[2]][cols[2]] - M[rows[1]][cols[2]] * M[rows[2]][cols[1]])
+ - M[rows[0]][cols[1]] * (M[rows[1]][cols[0]] * M[rows[2]][cols[2]] - M[rows[1]][cols[2]] * M[rows[2]][cols[0]])
+ + M[rows[0]][cols[2]] * (M[rows[1]][cols[0]] * M[rows[2]][cols[1]] - M[rows[1]][cols[1]] * M[rows[2]][cols[0]]);
+ }
+
+ // M += a*b.t()
+ inline void Rank1Add(const Vector3<T> &a, const Vector3<T> &b)
+ {
+ M[0][0] += a.x*b.x; M[0][1] += a.x*b.y; M[0][2] += a.x*b.z;
+ M[1][0] += a.y*b.x; M[1][1] += a.y*b.y; M[1][2] += a.y*b.z;
+ M[2][0] += a.z*b.x; M[2][1] += a.z*b.y; M[2][2] += a.z*b.z;
+ }
+
+ // M -= a*b.t()
+ inline void Rank1Sub(const Vector3<T> &a, const Vector3<T> &b)
+ {
+ M[0][0] -= a.x*b.x; M[0][1] -= a.x*b.y; M[0][2] -= a.x*b.z;
+ M[1][0] -= a.y*b.x; M[1][1] -= a.y*b.y; M[1][2] -= a.y*b.z;
+ M[2][0] -= a.z*b.x; M[2][1] -= a.z*b.y; M[2][2] -= a.z*b.z;
+ }
+
+ inline Vector3<T> Col(int c) const
+ {
+ return Vector3<T>(M[0][c], M[1][c], M[2][c]);
+ }
+
+ inline Vector3<T> Row(int r) const
+ {
+ return Vector3<T>(M[r][0], M[r][1], M[r][2]);
+ }
+
+ inline T Determinant() const
+ {
+ const Matrix3<T>& m = *this;
+ T d;
+
+ d = m.M[0][0] * (m.M[1][1]*m.M[2][2] - m.M[1][2] * m.M[2][1]);
+ d -= m.M[0][1] * (m.M[1][0]*m.M[2][2] - m.M[1][2] * m.M[2][0]);
+ d += m.M[0][2] * (m.M[1][0]*m.M[2][1] - m.M[1][1] * m.M[2][0]);
+
+ return d;
+ }
+
+ inline Matrix3<T> Inverse() const
{
- OVR_COMPILER_ASSERT((A1 != A2) && (A2 != A3) && (A1 != A3));
+ Matrix3<T> a;
+ const Matrix3<T>& m = *this;
+ T d = Determinant();
- T Q[3] = { x, y, z }; //Quaternion components x,y,z
+ assert(d != 0);
+ T s = T(1)/d;
- T ww = w*w;
- T Q11 = Q[A1]*Q[A1];
- T Q22 = Q[A2]*Q[A2];
- T Q33 = Q[A3]*Q[A3];
+ 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]);
- T psign = T(-1);
- // Determine whether even permutation
- if (((A1 + 1) % 3 == A2) && ((A2 + 1) % 3 == A3))
- psign = T(1);
+ 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]);
- T s2 = psign * T(2) * (psign*w*Q[A2] + Q[A1]*Q[A3]);
-
- if (s2 < T(-1) + Math<T>::SingularityRadius)
- { // South pole singularity
- *a = T(0);
- *b = -S*D*Math<T>::PiOver2;
- *c = S*D*atan2(T(2)*(psign*Q[A1]*Q[A2] + w*Q[A3]),
- ww + Q22 - Q11 - Q33 );
- }
- else if (s2 > T(1) - Math<T>::SingularityRadius)
- { // North pole singularity
- *a = T(0);
- *b = S*D*Math<T>::PiOver2;
- *c = S*D*atan2(T(2)*(psign*Q[A1]*Q[A2] + w*Q[A3]),
- ww + Q22 - Q11 - Q33);
- }
- else
- {
- *a = -S*D*atan2(T(-2)*(w*Q[A1] - psign*Q[A2]*Q[A3]),
- ww + Q33 - Q11 - Q22);
- *b = S*D*asin(s2);
- *c = S*D*atan2(T(2)*(w*Q[A3] - psign*Q[A1]*Q[A2]),
- ww + Q11 - Q22 - Q33);
- }
- return;
+ 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;
}
+
+};
- template <Axis A1, Axis A2, Axis A3, RotateDirection D>
- void GetEulerAngles(T *a, T *b, T *c)
- { GetEulerAngles<A1, A2, A3, D, Handed_R>(a, b, c); }
+typedef Matrix3<float> Matrix3f;
+typedef Matrix3<double> Matrix3d;
- template <Axis A1, Axis A2, Axis A3>
- void GetEulerAngles(T *a, T *b, T *c)
- { GetEulerAngles<A1, A2, A3, Rotate_CCW, Handed_R>(a, b, c); }
+//-------------------------------------------------------------------------------------
+template<typename T>
+class SymMat3
+{
+private:
+ typedef SymMat3<T> this_type;
- // 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)
- {
- OVR_COMPILER_ASSERT(A1 != A2);
+public:
+ typedef T Value_t;
+ // Upper symmetric
+ T v[6]; // _00 _01 _02 _11 _12 _22
- T Q[3] = {x, y, z}; // Quaternion components
+ inline SymMat3() {}
- // Determine the missing axis that was not supplied
- int m = 3 - A1 - A2;
+ inline explicit SymMat3(T s)
+ {
+ v[0] = v[3] = v[5] = s;
+ v[1] = v[2] = v[4] = 0;
+ }
- T ww = w*w;
- T Q11 = Q[A1]*Q[A1];
- T Q22 = Q[A2]*Q[A2];
- T Qmm = Q[m]*Q[m];
+ 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;
+ }
- T psign = T(-1);
- if ((A1 + 1) % 3 == A2) // Determine whether even permutation
- {
- psign = T(1);
- }
+ 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);
+ }
- T c2 = ww + Q11 - Q22 - Qmm;
- if (c2 < T(-1) + Math<T>::SingularityRadius)
- { // South pole singularity
- *a = T(0);
- *b = S*D*Math<T>::Pi;
- *c = S*D*atan2( T(2)*(w*Q[A1] - psign*Q[A2]*Q[m]),
- ww + Q22 - Q11 - Qmm);
- }
- else if (c2 > T(1) - Math<T>::SingularityRadius)
- { // North pole singularity
- *a = T(0);
- *b = T(0);
- *c = S*D*atan2( T(2)*(w*Q[A1] - psign*Q[A2]*Q[m]),
- ww + Q22 - Q11 - Qmm);
- }
- else
- {
- *a = S*D*atan2( psign*w*Q[m] + Q[A1]*Q[A2],
- w*Q[A2] -psign*Q[A1]*Q[m]);
- *b = S*D*acos(c2);
- *c = S*D*atan2( -psign*w*Q[m] + Q[A1]*Q[A2],
- w*Q[A2] + psign*Q[A1]*Q[m]);
- }
- return;
- }
+ 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]));
+ }
-typedef Quat<float> Quatf;
-typedef Quat<double> Quatd;
+ 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
generated by cgit v1.2.3 (git 2.39.1) at 2024-11-29 03:42:34 +0000