diff options
| author | Brad Davis <[email protected]> | 2013-10-10 23:31:48 -0700 |
|---|---|---|
| committer | Brad Davis <[email protected]> | 2013-10-10 23:31:48 -0700 |
| commit | cf0441a1dc790d6fdca7400a7db7a939c2fa6000 (patch) | |
| tree | edaf4e78195d789b1e3b724328951324db085324 | |
| parent | abfcde20cd83aa36e83186a0745425bcdead6144 (diff) | |
Adding Linux 0.2.5 changes
28 files changed, 1378 insertions, 1036 deletions
diff --git a/ConfigurePermissionsAndPackages.sh b/ConfigurePermissionsAndPackages.sh index e19a913..ca12a56 100755 --- a/ConfigurePermissionsAndPackages.sh +++ b/ConfigurePermissionsAndPackages.sh @@ -11,10 +11,24 @@ # Instruction : Ensure that the install.sh has execute permissions. # Navigate to a command shell, enter: # -# ./install.sh +# ./install.sh # # Enter the administrator password for sudo access. # +# Notes : UBUNTU 13 USERS +# --------------- +# The OculusConfigUtil does not currently support Ubuntu 13 +# out of the box. If you see an error similar to this upon +# launching OculusConfigUtil: +# +# "error while loading shared libraries: libudev.so.0" +# +# Then please try the following solution, until we officially +# support Ubuntu 13: +# +# cd /lib/x86_64-linux-gnu/ +# sudo ln -sf libudev.so.1 libudev.so.0 +# ############################################################################# echo "Installing OculusVR Rift udev rules file..." diff --git a/LibOVR/Include/OVR.h b/LibOVR/Include/OVR.h index 0308b9f..c832338 100644 --- a/LibOVR/Include/OVR.h +++ b/LibOVR/Include/OVR.h @@ -28,7 +28,6 @@ otherwise accompanies this software in either electronic or hard copy form. #include "../Src/OVR_Profile.h" #include "../Src/Util/Util_LatencyTest.h" #include "../Src/Util/Util_Render_Stereo.h" -#include "../Src/Util/Util_MagCalibration.h" #endif diff --git a/LibOVR/Include/OVRVersion.h b/LibOVR/Include/OVRVersion.h index 453db75..a1c7ea6 100644 --- a/LibOVR/Include/OVRVersion.h +++ b/LibOVR/Include/OVRVersion.h @@ -16,7 +16,7 @@ otherwise accompanies this software in either electronic or hard copy form. #define OVR_MAJOR_VERSION 0 #define OVR_MINOR_VERSION 2 -#define OVR_BUILD_VERSION 4 -#define OVR_VERSION_STRING "0.2.4" +#define OVR_BUILD_VERSION 5 +#define OVR_VERSION_STRING "0.2.5" #endif diff --git a/LibOVR/Makefile b/LibOVR/Makefile index 174e8a5..ff8d399 100644 --- a/LibOVR/Makefile +++ b/LibOVR/Makefile @@ -91,7 +91,6 @@ OBJECTS = $(OBJPATH)/OVR_DeviceHandle.o \ $(OBJPATH)/OVR_Timer.o \ $(OBJPATH)/OVR_UTF8Util.o \ $(OBJPATH)/Util_LatencyTest.o \ - $(OBJPATH)/Util_MagCalibration.o \ $(OBJPATH)/Util_Render_Stereo.o \ $(OBJPATH)/OVR_ThreadsPthread.o \ $(OBJPATH)/OVR_Linux_HIDDevice.o \ @@ -185,9 +184,6 @@ $(OBJPATH)/OVR_UTF8Util.o: $(LIBOVRPATH)/Src/Kernel/OVR_UTF8Util.cpp $(OBJPATH)/Util_LatencyTest.o: $(LIBOVRPATH)/Src/Util/Util_LatencyTest.cpp $(CXXBUILD)Util_LatencyTest.o $(LIBOVRPATH)/Src/Util/Util_LatencyTest.cpp -$(OBJPATH)/Util_MagCalibration.o: $(LIBOVRPATH)/Src/Util/Util_MagCalibration.cpp - $(CXXBUILD)Util_MagCalibration.o $(LIBOVRPATH)/Src/Util/Util_MagCalibration.cpp - $(OBJPATH)/Util_Render_Stereo.o: $(LIBOVRPATH)/Src/Util/Util_Render_Stereo.cpp $(CXXBUILD)Util_Render_Stereo.o $(LIBOVRPATH)/Src/Util/Util_Render_Stereo.cpp diff --git a/LibOVR/Src/Kernel/OVR_Atomic.h b/LibOVR/Src/Kernel/OVR_Atomic.h index a8591ff..6e04b0f 100644 --- a/LibOVR/Src/Kernel/OVR_Atomic.h +++ b/LibOVR/Src/Kernel/OVR_Atomic.h @@ -824,6 +824,7 @@ public: Lock (unsigned dummy = 0) { + OVR_UNUSED(dummy); if (!RecursiveAttrInit) { pthread_mutexattr_init(&RecursiveAttr); diff --git a/LibOVR/Src/Kernel/OVR_FileFILE.cpp b/LibOVR/Src/Kernel/OVR_FileFILE.cpp index dd77fde..e140158 100644 --- a/LibOVR/Src/Kernel/OVR_FileFILE.cpp +++ b/LibOVR/Src/Kernel/OVR_FileFILE.cpp @@ -580,4 +580,4 @@ bool SysFile::GetFileStat(FileStat* pfileStat, const String& path) return true; } -} // Scaleform +} // Namespace OVR diff --git a/LibOVR/Src/Kernel/OVR_Math.cpp b/LibOVR/Src/Kernel/OVR_Math.cpp index 3c35bde..d971d65 100644 --- a/LibOVR/Src/Kernel/OVR_Math.cpp +++ b/LibOVR/Src/Kernel/OVR_Math.cpp @@ -3,7 +3,7 @@ Filename : OVR_Math.h Content : Implementation of 3D primitives such as vectors, matrices. Created : September 4, 2012 -Authors : Andrew Reisse, Michael Antonov +Authors : Andrew Reisse, Michael Antonov, Anna Yershova Copyright : Copyright 2012 Oculus VR, Inc. All Rights reserved. @@ -14,9 +14,11 @@ otherwise accompanies this software in either electronic or hard copy form. *************************************************************************************/ #include "OVR_Math.h" +#include "OVR_Log.h" #include <float.h> + namespace OVR { @@ -31,15 +33,14 @@ const float Math<float>::PiOver2 = 3.1415926f / 2.0f; const float Math<float>::PiOver4 = 3.1415926f / 4.0f; const float Math<float>::E = 2.7182818f; -const float Math<float>::MaxValue = FLT_MAX; -const float Math<float>::MinPositiveValue = FLT_MIN; - -const float Math<float>::RadToDegreeFactor = 360.0f / Math<float>::TwoPi; -const float Math<float>::DegreeToRadFactor = Math<float>::TwoPi / 360.0f; +const float Math<float>::MaxValue = FLT_MAX; +const float Math<float>::MinPositiveValue = FLT_MIN; -const float Math<float>::Tolerance = 0.00001f; -const float Math<float>::SingularityRadius = 0.0000001f; // Use for Gimbal lock numerical problems +const float Math<float>::RadToDegreeFactor = 360.0f / Math<float>::TwoPi; +const float Math<float>::DegreeToRadFactor = Math<float>::TwoPi / 360.0f; +const float Math<float>::Tolerance = 0.00001f; +const float Math<float>::SingularityRadius = 0.0000001f; // Use for Gimbal lock numerical problems // Double-precision Math constants class. const double Math<double>::Pi = 3.14159265358979; @@ -48,14 +49,14 @@ const double Math<double>::PiOver2 = 3.14159265358979 / 2.0; const double Math<double>::PiOver4 = 3.14159265358979 / 4.0; const double Math<double>::E = 2.71828182845905; -const double Math<double>::MaxValue = DBL_MAX; -const double Math<double>::MinPositiveValue = DBL_MIN; +const double Math<double>::MaxValue = DBL_MAX; +const double Math<double>::MinPositiveValue = DBL_MIN; -const double Math<double>::RadToDegreeFactor = 360.0 / Math<double>::TwoPi; -const double Math<double>::DegreeToRadFactor = Math<double>::TwoPi / 360.0; +const double Math<double>::RadToDegreeFactor = 360.0 / Math<double>::TwoPi; +const double Math<double>::DegreeToRadFactor = Math<double>::TwoPi / 360.0; -const double Math<double>::Tolerance = 0.00001; -const double Math<double>::SingularityRadius = 0.000000000001; // Use for Gimbal lock numerical problems +const double Math<double>::Tolerance = 0.00001; +const double Math<double>::SingularityRadius = 0.000000000001; // Use for Gimbal lock numerical problems @@ -69,9 +70,9 @@ Matrix4f Matrix4f::LookAtRH(const Vector3f& eye, const Vector3f& at, const Vecto Vector3f x = up.Cross(z).Normalized(); // Right Vector3f y = z.Cross(x); - Matrix4f m(x.x, x.y, x.z, -(x * eye), - y.x, y.y, y.z, -(y * eye), - z.x, z.y, z.z, -(z * eye), + Matrix4f 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; } @@ -82,9 +83,9 @@ Matrix4f Matrix4f::LookAtLH(const Vector3f& eye, const Vector3f& at, const Vecto Vector3f x = up.Cross(z).Normalized(); // Right Vector3f y = z.Cross(x); - Matrix4f m(x.x, x.y, x.z, -(x * eye), - y.x, y.y, y.z, -(y * eye), - z.x, z.y, z.z, -(z * eye), + Matrix4f 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; } @@ -127,18 +128,6 @@ Matrix4f Matrix4f::PerspectiveRH(float yfov, float aspect, float znear, float zf return m; } - -/* -OffCenterLH - -2*zn/(r-l) 0 0 0 -0 2*zn/(t-b) 0 0 -(l+r)/(l-r) (t+b)/(b-t) zf/(zf-zn) 1 -0 0 zn*zf/(zn-zf) 0 - -*/ - - Matrix4f Matrix4f::Ortho2D(float w, float h) { Matrix4f m; @@ -150,4 +139,5 @@ Matrix4f Matrix4f::Ortho2D(float w, float h) return m; } -} + +} // Namespace OVR diff --git a/LibOVR/Src/Kernel/OVR_Math.h b/LibOVR/Src/Kernel/OVR_Math.h index 567ea9c..108b9d3 100644 --- a/LibOVR/Src/Kernel/OVR_Math.h +++ b/LibOVR/Src/Kernel/OVR_Math.h @@ -4,7 +4,7 @@ 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 +Authors : Andrew Reisse, Michael Antonov, Steve LaValle, Anna Yershova, Max Katsev Copyright : Copyright 2012 Oculus VR, Inc. All Rights reserved. @@ -23,11 +23,12 @@ otherwise accompanies this software in either electronic or hard copy form. #include "OVR_Types.h" #include "OVR_RefCount.h" +#include "OVR_Std.h" namespace OVR { //------------------------------------------------------------------------------------- -// Constants for 3D world/axis definitions. +// ***** Constants for 3D world/axis definitions. // Definitions of axes for coordinate and rotation conversions. enum Axis @@ -48,12 +49,13 @@ enum RotateDirection 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 axis points. Used by WorldAxes. +// AxisDirection describes which way the coordinate axis points. Used by WorldAxes. enum AxisDirection { Axis_Up = 2, @@ -74,9 +76,9 @@ struct WorldAxes }; -//------------------------------------------------------------------------------------- -// ***** 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> @@ -95,14 +97,14 @@ public: static const float PiOver4; static const float E; - static const float MaxValue; // Largest positive float Value - static const float MinPositiveValue; // Smallest possible positive value + static const float MaxValue; // Largest positive float Value + static const float MinPositiveValue; // Smallest possible positive value static const float RadToDegreeFactor; static const float DegreeToRadFactor; - static const float Tolerance; // 0.00001f; - static const float SingularityRadius; //0.00000000001f for Gimbal lock numerical problems + static const float Tolerance; // 0.00001f; + static const float SingularityRadius; // 0.0000001f for Gimbal lock numerical problems }; // Double-precision Math constants class. @@ -116,33 +118,54 @@ public: static const double PiOver4; static const double E; - static const double MaxValue; // Largest positive double Value - static const double MinPositiveValue; // Smallest possible positive value + static const double MaxValue; // Largest positive double Value + static const double MinPositiveValue; // Smallest possible positive value static const double RadToDegreeFactor; static const double DegreeToRadFactor; - static const double Tolerance; // 0.00001f; - static const double SingularityRadius; //0.00000000001 for Gimbal lock numerical problems + static const double Tolerance; // 0.00001; + static const double SingularityRadius; // 0.000000000001 for Gimbal lock numerical problems + }; typedef Math<float> Mathf; typedef Math<double> Mathd; // Conversion functions between degrees and radians -template<class FT> -FT RadToDegree(FT rads) { return rads * Math<FT>::RadToDegreeFactor; } -template<class FT> -FT DegreeToRad(FT rads) { return rads * Math<FT>::DegreeToRadFactor; } +template<class T> +T RadToDegree(T rads) { return rads * Math<T>::RadToDegreeFactor; } +template<class T> +T DegreeToRad(T rads) { return rads * Math<T>::DegreeToRadFactor; } + +// Numerically stable acos function +template<class T> +T Acos(T val) { + if (val > T(1)) return T(0); + else if (val < T(-1)) return Math<T>::Pi; + else return acos(val); +}; + +// Numerically stable asin function +template<class T> +T Asin(T val) { + if (val > T(1)) return Math<T>::PiOver2; + else if (val < T(-1)) return Math<T>::PiOver2 * T(3); + else return asin(val); +}; + +#ifdef OVR_CC_MSVC +inline int isnan(double x) { return _isnan(x); }; +#endif template<class T> class Quat; //------------------------------------------------------------------------------------- -// ***** Vector2f - 2D Vector2f +// ***** Vector2<> -// Vector2f represents a 2-dimensional vector or point in space, -// consisting of coordinates x and y, +// Vector2f (Vector2d) represents a 2-dimensional vector or point in space, +// consisting of coordinates x and y template<class T> class Vector2 @@ -174,33 +197,53 @@ public: return *this; } // Compare two vectors for equality with tolerance. Returns true if vectors match withing tolerance. - bool Compare(const Vector2&b, T tolerance = Mathf::Tolerance) + bool Compare(const Vector2&b, T tolerance = Mathf::Tolerance) { return (fabs(b.x-x) < tolerance) && (fabs(b.y-y) < tolerance); } - // Dot product overload. + // Entrywise product of two vectors + Vector2 EntrywiseMultiply(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 operator* (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 { return acos((*this * b)/(Length()*b.Length())); } + T Angle(const Vector2& b) const + { + T div = LengthSq()*b.LengthSq(); + OVR_ASSERT(div != T(0)); + T result = Acos((this->Dot(b))/sqrt(div)); + return result; + } // Return Length of the vector squared. T LengthSq() const { return (x * x + y * y); } // Return vector length. T Length() const { return sqrt(LengthSq()); } - // Returns distance between two points represented by vectors. + // Returns distance between two points represented by vectors. T Distance(Vector2& b) const { return (*this - b).Length(); } - - // Determine if this a unit vector. + + // Determine if this a unit vector. bool IsNormalized() const { return fabs(LengthSq() - T(1)) < Math<T>::Tolerance; } + // Normalize, convention vector length to 1. - void Normalize() { *this /= Length(); } + void Normalize() + { + T l = Length(); + OVR_ASSERT(l != T(0)); + *this /= l; + } // Returns normalized (unit) version of the vector without modifying itself. - Vector2 Normalized() const { return *this / Length(); } + Vector2 Normalized() const + { + T l = Length(); + OVR_ASSERT(l != T(0)); + return *this / l; + } // Linearly interpolates from this vector to another. // Factor should be between 0.0 and 1.0, with 0 giving full value to this. @@ -208,17 +251,24 @@ public: // 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 { return b * ((*this * b) / b.LengthSq()); } + Vector2 ProjectTo(const Vector2& b) const + { + T l2 = b.LengthSq(); + OVR_ASSERT(l2 != T(0)); + return b * ( Dot(b) / l2 ); + } }; typedef Vector2<float> Vector2f; typedef Vector2<double> Vector2d; + //------------------------------------------------------------------------------------- -// ***** Vector3f - 3D Vector3f +// ***** Vector3<> - 3D vector of {x, y, z} -// Vector3f represents a 3-dimensional vector or point in space, +// +// Vector3f (Vector3d) represents a 3-dimensional vector or point in space, // consisting of coordinates x, y and z. template<class T> @@ -253,13 +303,20 @@ public: // Compare two vectors for equality with tolerance. Returns true if vectors match withing tolerance. bool Compare(const Vector3&b, T tolerance = Mathf::Tolerance) { - return (fabs(b.x-x) < tolerance) && (fabs(b.y-y) < tolerance) && (fabs(b.z-z) < tolerance); + return (fabs(b.x-x) < tolerance) && + (fabs(b.y-y) < tolerance) && + (fabs(b.z-z) < tolerance); } - // Dot product overload. + // Entrywise product of two vectors + Vector3 EntrywiseMultiply(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 operator* (const Vector3& b) const { return x*b.x + y*b.y + z*b.z; } + 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 @@ -269,7 +326,13 @@ public: x*b.y - y*b.x); } // Returns the angle from this vector to b, in radians. - T Angle(const Vector3& b) const { return acos((*this * b)/(Length()*b.Length())); } + T Angle(const Vector3& b) const + { + T div = LengthSq()*b.LengthSq(); + OVR_ASSERT(div != T(0)); + T result = Acos((this->Dot(b))/sqrt(div)); + return result; + } // Return Length of the vector squared. T LengthSq() const { return (x * x + y * y + z * z); } @@ -281,10 +344,22 @@ public: // Determine if this a unit vector. bool IsNormalized() const { return fabs(LengthSq() - T(1)) < Math<T>::Tolerance; } + // Normalize, convention vector length to 1. - void Normalize() { *this /= Length(); } + void Normalize() + { + T l = Length(); + OVR_ASSERT(l != T(0)); + *this /= l; + } + // Returns normalized (unit) version of the vector without modifying itself. - Vector3 Normalized() const { return *this / Length(); } + Vector3 Normalized() const + { + T l = Length(); + OVR_ASSERT(l != T(0)); + return *this / l; + } // Linearly interpolates from this vector to another. // Factor should be between 0.0 and 1.0, with 0 giving full value to this. @@ -292,7 +367,15 @@ public: // 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 { return b * ((*this * b) / b.LengthSq()); } + Vector3 ProjectTo(const Vector3& b) const + { + T l2 = b.LengthSq(); + OVR_ASSERT(l2 != T(0)); + return b * ( Dot(b) / l2 ); + } + + // Projects this vector onto a plane defined by a normal vector + Vector3 ProjectToPlane(const Vector3& normal) const { return *this - this->ProjectTo(normal); } }; @@ -301,8 +384,55 @@ typedef Vector3<double> Vector3d; //------------------------------------------------------------------------------------- -// ***** Matrix4f +// ***** Size + +// Size class represents 2D size with Width, Height components. +// Used to describe distentions of render targets, etc. + +template<class T> +class Size +{ +public: + T Width, Height; + + Size() : Width(0), Height(0) { } + Size(T w_, T h_) : Width(w_), Height(h_) { } + explicit Size(T s) : Width(s), Height(s) { } + + 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; } + + 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; } + + // 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; } + + T Area() const { return Width * Height; } + + inline Vector2<T> ToVector() const { return Vector2<T>(Width, Height); } +}; + +typedef Size<int> Sizei; +typedef Size<unsigned> Sizeu; +typedef Size<float> Sizef; +typedef Size<double> Sized; + + +//------------------------------------------------------------------------------------- +// ***** Matrix4f +// // Matrix4f 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 @@ -367,6 +497,29 @@ public: M[3][0] = 0; M[3][1] = 0; M[3][2] = 0; M[3][3] = 1; } + void ToString(char* dest, UPInt destsize) + { + UPInt pos = 0; + for (int r=0; r<4; r++) + for (int c=0; c<4; c++) + pos += OVR_sprintf(dest+pos, destsize-pos, "%g ", M[r][c]); + } + + static Matrix4f FromString(const char* src) + { + Matrix4f result; + for (int r=0; r<4; r++) + for (int c=0; c<4; c++) + { + result.M[r][c] = (float)atof(src); + while (src && *src != ' ') + src++; + while (src && *src == ' ') + src++; + } + return result; + } + static const Matrix4f& Identity() { return IdentityValue; } void SetIdentity() @@ -377,6 +530,36 @@ public: M[0][3] = M[1][3] = M[2][1] = M[3][0] = 0; } + Matrix4f operator+ (const Matrix4f& b) const + { + Matrix4f result(*this); + result += b; + return result; + } + + Matrix4f& operator+= (const Matrix4f& b) + { + for (int i = 0; i < 4; i++) + for (int j = 0; j < 4; j++) + M[i][j] += b.M[i][j]; + return *this; + } + + Matrix4f operator- (const Matrix4f& b) const + { + Matrix4f result(*this); + result -= b; + return result; + } + + Matrix4f& operator-= (const Matrix4f& b) + { + for (int i = 0; i < 4; i++) + for (int j = 0; j < 4; j++) + M[i][j] -= b.M[i][j]; + return *this; + } + // Multiplies two matrices into destination with minimum copying. static Matrix4f& Multiply(Matrix4f* d, const Matrix4f& a, const Matrix4f& b) { @@ -406,18 +589,32 @@ public: Matrix4f operator* (float s) const { - return Matrix4f(M[0][0] * s, M[0][1] * s, M[0][2] * s, M[0][3] * s, - M[1][0] * s, M[1][1] * s, M[1][2] * s, M[1][3] * s, - M[2][0] * s, M[2][1] * s, M[2][2] * s, M[2][3] * s, - M[3][0] * s, M[3][1] * s, M[3][2] * s, M[3][3] * s); + Matrix4f result(*this); + result *= s; + return result; } Matrix4f& operator*= (float s) { - M[0][0] *= s; M[0][1] *= s; M[0][2] *= s; M[0][3] *= s; - M[1][0] *= s; M[1][1] *= s; M[1][2] *= s; M[1][3] *= s; - M[2][0] *= s; M[2][1] *= s; M[2][2] *= s; M[2][3] *= s; - M[3][0] *= s; M[3][1] *= s; M[3][2] *= s; M[3][3] *= s; + for (int i = 0; i < 4; i++) + for (int j = 0; j < 4; j++) + M[i][j] *= s; + return *this; + } + + + Matrix4f operator/ (float s) const + { + Matrix4f result(*this); + result /= s; + return result; + } + + Matrix4f& operator/= (float s) + { + for (int i = 0; i < 4; i++) + for (int j = 0; j < 4; j++) + M[i][j] /= s; return *this; } @@ -442,16 +639,16 @@ public: } - float SubDet (const int* rows, const int* cols) const + float 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(int I, int J) const + float Cofactor(UPInt I, UPInt J) const { - const int indices[4][3] = {{1,2,3},{0,2,3},{0,1,3},{0,1,2}}; + 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]); } @@ -480,7 +677,27 @@ public: *this = Inverted(); } - //AnnaSteve: + // This is more efficient than general inverse, but ONLY works + // correctly if it is a homogeneous transform matrix (rot + trans) + Matrix4f InvertedHomogeneousTransform() const + { + // Make the inverse rotation matrix + Matrix4f 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); + return rinv * tinv; // "untranslate", then "unrotate" + } + + // This is more efficient than general inverse, but ONLY works + // correctly if it is a homogeneous transform matrix (rot + trans) + void InvertHomogeneousTransform() + { + *this = InvertedHomogeneousTransform(); + } + + // Matrix to Euler Angles conversion // a,b,c, are the YawPitchRoll angles to be returned // rotation a around axis A1 // is followed by rotation b around axis A2 @@ -502,7 +719,7 @@ public: *b = -S*D*Math<float>::PiOver2; *c = S*D*atan2( psign*M[A2][A1], M[A2][A2] ); } - else if (pm > 1.0 - Math<float>::SingularityRadius) + else if (pm > 1.0f - Math<float>::SingularityRadius) { // North pole singularity *a = 0.0f; *b = S*D*Math<float>::PiOver2; @@ -518,7 +735,7 @@ public: return; } - //AnnaSteve: + // Matrix to Euler Angles conversion // a,b,c, are the YawPitchRoll angles to be returned // rotation a around axis A1 // is followed by rotation b around axis A2 @@ -537,13 +754,13 @@ public: psign = 1.0f; float c2 = M[A1][A1]; - if (c2 < -1.0 + Math<float>::SingularityRadius) + if (c2 < -1.0f + Math<float>::SingularityRadius) { // South pole singularity *a = 0.0f; *b = S*D*Math<float>::Pi; *c = S*D*atan2( -psign*M[A2][m],M[A2][A2]); } - else if (c2 > 1.0 - Math<float>::SingularityRadius) + else if (c2 > 1.0f - Math<float>::SingularityRadius) { // North pole singularity *a = 0.0f; *b = 0.0f; @@ -560,7 +777,6 @@ public: // Creates a matrix that converts the vertices from one coordinate system // to another. - // static Matrix4f AxisConversion(const WorldAxes& to, const WorldAxes& from) { // Holds axis values from the 'to' structure @@ -584,7 +800,7 @@ public: } - + // Creates a matrix for translation by vector static Matrix4f Translation(const Vector3f& v) { Matrix4f t; @@ -594,6 +810,7 @@ public: return t; } + // Creates a matrix for translation by vector static Matrix4f Translation(float x, float y, float z = 0.0f) { Matrix4f t; @@ -603,6 +820,7 @@ public: return t; } + // Creates a matrix for scaling by vector static Matrix4f Scaling(const Vector3f& v) { Matrix4f t; @@ -612,6 +830,7 @@ public: return t; } + // Creates a matrix for scaling by vector static Matrix4f Scaling(float x, float y, float z) { Matrix4f t; @@ -621,6 +840,7 @@ public: return t; } + // Creates a matrix for scaling by constant static Matrix4f Scaling(float s) { Matrix4f t; @@ -632,7 +852,8 @@ public: - //AnnaSteve : Just for quick testing. Not for final API. Need to remove case. + // 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) { float sina = s * d *sin(angle); @@ -658,10 +879,10 @@ public: // Creates a rotation matrix rotating around the X axis by 'angle' radians. // Rotation direction is depends on the coordinate system: - // RHS (Oculus default): Positive angle values rotate Counter-clockwise (CCW), + // RHS (Oculus default): Positive angle values rotate Counter-clockwise (CCW), // while looking in the negative axis direction. This is the // same as looking down from positive axis values towards origin. - // LHS: Positive angle values rotate clock-wise (CW), while looking in the + // LHS: Positive angle values rotate clock-wise (CW), while looking in the // negative axis direction. static Matrix4f RotationX(float angle) { @@ -743,9 +964,9 @@ public: }; -//------------------------------------------------------------------------------------- -// ***** Quat - +//-------------------------------------------------------------------------------------// +// **************************************** Quat **************************************// +// // Quatf represents a quaternion class used for rotations. // // Quaternion multiplications are done in right-to-left order, to match the @@ -763,7 +984,7 @@ public: Quat(T x_, T y_, T z_, T w_) : x(x_), y(y_), z(z_), w(w_) {} - // Constructs rotation quaternion around the axis. + // Constructs quaternion for rotation around the axis by an angle. Quat(const Vector3<T>& axis, T angle) { Vector3<T> unitAxis = axis.Normalized(); @@ -775,33 +996,33 @@ public: z = unitAxis.z * sinHalfAngle; } - //AnnaSteve: + // 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 sinHalfAngle = s * d *sin(angle * T(0.5)); T v[3]; - v[0] = v[1] = v[2] = (T)0; + v[0] = v[1] = v[2] = T(0); v[A] = sinHalfAngle; - //return Quat(v[0], v[1], v[2], cos(angle * (T)0.5)); - w = cos(angle * (T)0.5); + + w = cos(angle * T(0.5)); x = v[0]; y = v[1]; z = v[2]; } - void GetAxisAngle(Vector3<T>* axis, T* angle) const + // Compute axis and angle from quaternion + void GetAxisAngle(Vector3<T>* axis, T* angle) const { - if (LengthSq() > Math<T>::Tolerance * Math<T>::Tolerance) - { - *axis = Vector3<T>(x, y, z).Normalized(); - *angle = 2 * acos(w); - } - else - { - *axis = Vector3<T>(1, 0, 0); - *angle= 0; - } + 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; + } } bool operator== (const Quat& b) const { return x == b.x && y == b.y && z == b.z && w == b.w; } @@ -817,6 +1038,7 @@ public: 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); } @@ -824,29 +1046,42 @@ public: 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); } + // Simple Eulidean distance in R^4 (not SLERP distance, but at least respects Haar measure) - T Distance(const Quat& q) const - { + T Distance(const Quat& q) const + { T d1 = (*this - q).Length(); - T d2 = (*this + q).Length(); // Antipoldal point check + 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(); // Antipoldal point check + T d2 = (*this + q).LengthSq(); // Antipodal point check return (d1 < d2) ? d1 : d2; } // Normalize - bool IsNormalized() const { return fabs(LengthSq() - 1) < Math<T>::Tolerance; } - void Normalize() { *this /= Length(); } - Quat Normalized() const { return *this / Length(); } + 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); } - // AnnaSteve fixed: order of quaternion multiplication // 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, @@ -868,7 +1103,7 @@ public: // 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, 0)) * Inverted()).Imag(); + return ((*this * Quat<T>(v.x, v.y, v.z, T(0))) * Inverted()).Imag(); } @@ -897,6 +1132,52 @@ public: float(T(2) * (x*z - w*y)), float(T(2) * (y*z + w*x)), float(ww - xx - yy + zz) ); } + + // Converting matrix to quaternion + static Quat<T> Matrix4fToQuat(const Matrix4f& m) + { + T trace = m.M[0][0] + m.M[1][1] + m.M[2][2]; + Quat<T> q; + + // 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; + } + + // GetEulerAngles extracts Euler angles from the quaternion, in the specified order of // axis rotations and the specified coordinate system. Right-handed coordinate system @@ -918,33 +1199,33 @@ public: T Q22 = Q[A2]*Q[A2]; T Q33 = Q[A3]*Q[A3]; - T psign = T(-1.0); + T psign = T(-1); // Determine whether even permutation if (((A1 + 1) % 3 == A2) && ((A2 + 1) % 3 == A3)) - psign = T(1.0); + psign = T(1); - T s2 = psign * T(2.0) * (psign*w*Q[A2] + Q[A1]*Q[A3]); + T s2 = psign * T(2) * (psign*w*Q[A2] + Q[A1]*Q[A3]); - if (s2 < (T)-1.0 + Math<T>::SingularityRadius) + if (s2 < T(-1) + Math<T>::SingularityRadius) { // South pole singularity - *a = T(0.0); + *a = T(0); *b = -S*D*Math<T>::PiOver2; - *c = S*D*atan2((T)2.0*(psign*Q[A1]*Q[A2] + w*Q[A3]), + *c = S*D*atan2(T(2)*(psign*Q[A1]*Q[A2] + w*Q[A3]), ww + Q22 - Q11 - Q33 ); } - else if (s2 > (T)1.0 - Math<T>::SingularityRadius) + else if (s2 > T(1) - Math<T>::SingularityRadius) { // North pole singularity - *a = (T)0.0; + *a = T(0); *b = S*D*Math<T>::PiOver2; - *c = S*D*atan2((T)2.0*(psign*Q[A1]*Q[A2] + w*Q[A3]), + *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.0*(w*Q[A1] - psign*Q[A2]*Q[A3]), + *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.0*(w*Q[A3] - psign*Q[A1]*Q[A2]), + *c = S*D*atan2(T(2)*(w*Q[A3] - psign*Q[A1]*Q[A2]), ww + Q11 - Q22 - Q33); } return; @@ -982,25 +1263,25 @@ public: T Q22 = Q[A2]*Q[A2]; T Qmm = Q[m]*Q[m]; - T psign = T(-1.0); + T psign = T(-1); if ((A1 + 1) % 3 == A2) // Determine whether even permutation { - psign = (T)1.0; + psign = T(1); } T c2 = ww + Q11 - Q22 - Qmm; - if (c2 < (T)-1.0 + Math<T>::SingularityRadius) + if (c2 < T(-1) + Math<T>::SingularityRadius) { // South pole singularity - *a = (T)0.0; + *a = T(0); *b = S*D*Math<T>::Pi; - *c = S*D*atan2( (T)2.0*(w*Q[A1] - psign*Q[A2]*Q[m]), + *c = S*D*atan2( T(2)*(w*Q[A1] - psign*Q[A2]*Q[m]), ww + Q22 - Q11 - Qmm); } - else if (c2 > (T)1.0 - Math<T>::SingularityRadius) + else if (c2 > T(1) - Math<T>::SingularityRadius) { // North pole singularity - *a = (T)0.0; - *b = (T)0.0; - *c = S*D*atan2( (T)2.0*(w*Q[A1] - psign*Q[A2]*Q[m]), + *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 @@ -1013,8 +1294,8 @@ public: } return; } -}; +}; typedef Quat<float> Quatf; typedef Quat<double> Quatd; @@ -1026,7 +1307,6 @@ typedef Quat<double> Quatd; // Cleanly representing the algebra of 2D rotations. // The operations maintain the angle between -Pi and Pi, the same range as atan2. -// template<class T> class Angle @@ -1057,14 +1337,14 @@ public: // bool operator= (const T& x) { a = x; FixRange(); } // These operations assume a is already between -Pi and Pi. - Angle operator+ (const Angle& b) const { return Angle(a + b.a); } - Angle operator+ (const T& x) const { return Angle(a + x); } Angle& operator+= (const Angle& b) { a = a + b.a; FastFixRange(); return *this; } Angle& operator+= (const T& x) { a = a + x; FixRange(); return *this; } - Angle operator- (const Angle& b) const { return Angle(a - b.a); } - Angle operator- (const T& x) const { return Angle(a - x); } + Angle operator+ (const Angle& b) const { Angle res = *this; res += b; return res; } + Angle operator+ (const T& x) const { Angle res = *this; res += x; return res; } Angle& operator-= (const Angle& b) { a = a - b.a; FastFixRange(); return *this; } Angle& operator-= (const T& x) { a = a - x; FixRange(); return *this; } + Angle operator- (const Angle& b) const { Angle res = *this; res -= b; return res; } + Angle operator- (const T& x) const { Angle res = *this; res -= x; return res; } T Distance(const Angle& b) { T c = fabs(a - b.a); return (c <= Math<T>::Pi) ? c : Math<T>::TwoPi - c; } @@ -1085,6 +1365,9 @@ private: // Fixes the angle range to [-Pi,Pi] for any given range, but slower then the fast method inline void FixRange() { + // do nothing if the value is already in the correct range, since fmod call is expensive + if (a >= -Math<T>::Pi && a <= Math<T>::Pi) + return; a = fmod(a,Math<T>::TwoPi); if (a < -Math<T>::Pi) a += Math<T>::TwoPi; @@ -1122,7 +1405,7 @@ public: // Find the point to plane distance. The sign indicates what side of the plane the point is on (0 = point on plane). T TestSide(const Vector3<T>& p) const { - return (N * p) + D; + return (N.Dot(p)) + D; } Plane<T> Flipped() const @@ -1144,6 +1427,6 @@ public: typedef Plane<float> Planef; -} +} // Namespace OVR #endif diff --git a/LibOVR/Src/Kernel/OVR_SysFile.h b/LibOVR/Src/Kernel/OVR_SysFile.h index d078df1..892d16b 100644 --- a/LibOVR/Src/Kernel/OVR_SysFile.h +++ b/LibOVR/Src/Kernel/OVR_SysFile.h @@ -88,6 +88,6 @@ public: virtual bool Close(); }; -} // Scaleform +} // Namespace OVR #endif diff --git a/LibOVR/Src/Kernel/OVR_Timer.h b/LibOVR/Src/Kernel/OVR_Timer.h index 778dce8..34f72c4 100644 --- a/LibOVR/Src/Kernel/OVR_Timer.h +++ b/LibOVR/Src/Kernel/OVR_Timer.h @@ -95,6 +95,6 @@ private: }; -} // Scaleform::Timer +} // Namespace OVR #endif diff --git a/LibOVR/Src/OVR_DeviceConstants.h b/LibOVR/Src/OVR_DeviceConstants.h index 41342ce..37b95cf 100644 --- a/LibOVR/Src/OVR_DeviceConstants.h +++ b/LibOVR/Src/OVR_DeviceConstants.h @@ -30,6 +30,7 @@ enum DeviceType Device_HMD = 2, Device_Sensor = 3, Device_LatencyTester = 4, + Device_BootLoader = 5, Device_All = 0xFF // Set for enumeration only, to enumerate all device types. }; diff --git a/LibOVR/Src/OVR_LatencyTestImpl.cpp b/LibOVR/Src/OVR_LatencyTestImpl.cpp index 2bdbd52..4ab28e5 100644 --- a/LibOVR/Src/OVR_LatencyTestImpl.cpp +++ b/LibOVR/Src/OVR_LatencyTestImpl.cpp @@ -463,13 +463,13 @@ bool LatencyTestDeviceCreateDesc::GetDeviceInfo(DeviceInfo* info) const OVR_strcpy(info->ProductName, DeviceInfo::MaxNameLength, HIDDesc.Product.ToCStr()); OVR_strcpy(info->Manufacturer, DeviceInfo::MaxNameLength, HIDDesc.Manufacturer.ToCStr()); info->Type = Device_LatencyTester; - info->Version = 0; if (info->InfoClassType == Device_LatencyTester) { SensorInfo* sinfo = (SensorInfo*)info; sinfo->VendorId = HIDDesc.VendorId; sinfo->ProductId = HIDDesc.ProductId; + sinfo->Version = HIDDesc.VersionNumber; OVR_strcpy(sinfo->SerialNumber, sizeof(sinfo->SerialNumber),HIDDesc.SerialNumber.ToCStr()); } return true; diff --git a/LibOVR/Src/OVR_Linux_HIDDevice.cpp b/LibOVR/Src/OVR_Linux_HIDDevice.cpp index 062d566..c07c1ad 100644 --- a/LibOVR/Src/OVR_Linux_HIDDevice.cpp +++ b/LibOVR/Src/OVR_Linux_HIDDevice.cpp @@ -173,6 +173,11 @@ bool HIDDeviceManager::initVendorProductVersion(udev_device* device, HIDDeviceDe else return false; + if (getIntProperty(device, "bcdDevice", &result)) + pDevDesc->VersionNumber = result; + else + return false; + return true; } diff --git a/LibOVR/Src/OVR_OSX_HIDDevice.cpp b/LibOVR/Src/OVR_OSX_HIDDevice.cpp index e954ef4..e93cf67 100644 --- a/LibOVR/Src/OVR_OSX_HIDDevice.cpp +++ b/LibOVR/Src/OVR_OSX_HIDDevice.cpp @@ -128,6 +128,13 @@ bool HIDDeviceManager::initVendorProductVersion(IOHIDDeviceRef device, HIDDevice { return false; } + + SInt32 result; + if (!getIntProperty(device, CFSTR(kIOHIDVersionNumberKey), &result)) + { + return false; + } + pDevDesc->VersionNumber = result; return true; } diff --git a/LibOVR/Src/OVR_OSX_HMDDevice.cpp b/LibOVR/Src/OVR_OSX_HMDDevice.cpp index ee2899d..937b30f 100644 --- a/LibOVR/Src/OVR_OSX_HMDDevice.cpp +++ b/LibOVR/Src/OVR_OSX_HMDDevice.cpp @@ -186,7 +186,7 @@ void HMDDeviceFactory::EnumerateDevices(EnumerateVisitor& visitor) unsigned mheight = (unsigned)CGDisplayPixelsHigh(Displays[i]); CGRect desktop = CGDisplayBounds(Displays[i]); - if (vendor == 16082 && product == 1) + if (vendor == 16082 && ( (product == 1)||(product == 2) ) ) // 7" or HD { char idstring[9]; idstring[0] = 'A'-1+((vendor>>10) & 31); diff --git a/LibOVR/Src/OVR_Profile.cpp b/LibOVR/Src/OVR_Profile.cpp index 83eade3..3be43db 100644 --- a/LibOVR/Src/OVR_Profile.cpp +++ b/LibOVR/Src/OVR_Profile.cpp @@ -41,6 +41,7 @@ otherwise accompanies this software in either electronic or hard copy form. #endif #define PROFILE_VERSION 1.0 +#define MAX_PROFILE_MAJOR_VERSION 1 namespace OVR { @@ -154,11 +155,18 @@ Profile* ProfileManager::CreateProfileObject(const char* user, Profile* profile = NULL; switch (device) { + case Profile_GenericHMD: + *device_name = NULL; + profile = new HMDProfile(Profile_GenericHMD, user); + break; case Profile_RiftDK1: *device_name = "RiftDK1"; profile = new RiftDK1Profile(user); break; case Profile_RiftDKHD: + *device_name = "RiftDKHD"; + profile = new RiftDKHDProfile(user); + break; case Profile_Unknown: break; } @@ -176,7 +184,6 @@ void ProfileManager::ClearCache() CacheDevice = Profile_Unknown; } - // Poplulates the local profile cache. This occurs on the first access of the profile // data. All profile operations are performed against the local cache until the // ProfileManager is released or goes out of scope at which time the cache is serialized @@ -198,8 +205,11 @@ void ProfileManager::LoadCache(ProfileType device) JSON* item1 = root->GetNextItem(item0); JSON* item2 = root->GetNextItem(item1); - if (OVR_strcmp(item0->Name, "Oculus Profile Version") == 0) - { // In the future I may need to check versioning to determine parse method + if (item0->Name == "Oculus Profile Version") + { + int major = atoi(item0->Value.ToCStr()); + if (major > MAX_PROFILE_MAJOR_VERSION) + return; // don't parse the file on unsupported major version number } else { @@ -214,55 +224,57 @@ void ProfileManager::LoadCache(ProfileType device) for (int p=0; p<profileCount; p++) { - profileItem = profileItem->GetNextItem(profileItem); - if (!profileItem) + profileItem = root->GetNextItem(profileItem); + if (profileItem == NULL) break; - - // Read the required Name field - const char* profileName; - JSON* item = profileItem->GetFirstItem(); - - if (item && (OVR_strcmp(item->Name, "Name") == 0)) - { - profileName = item->Value; - } - else + + if (profileItem->Name == "Profile") { - return; // invalid field - } + // Read the required Name field + const char* profileName; + JSON* item = profileItem->GetFirstItem(); + + if (item && (item->Name == "Name")) + { + profileName = item->Value; + } + else + { + return; // invalid field + } - const char* deviceName = 0; - bool deviceFound = false; - Ptr<Profile> profile = *CreateProfileObject(profileName, device, &deviceName); + const char* deviceName = 0; + bool deviceFound = false; + Ptr<Profile> profile = *CreateProfileObject(profileName, device, &deviceName); - // Read the base profile fields. - if (profile) - { - while (item = profileItem->GetNextItem(item), item) + // Read the base profile fields. + if (profile) { - if (item->Type != JSON_Object) + while (item = profileItem->GetNextItem(item), item) { - profile->ParseProperty(item->Name, item->Value); - } - else - { // Search for the matching device to get device specific fields - if (!deviceFound && OVR_strcmp(item->Name, deviceName) == 0) + if (item->Type != JSON_Object) { - deviceFound = true; - - for (JSON* deviceItem = item->GetFirstItem(); deviceItem; - deviceItem = item->GetNextItem(deviceItem)) + profile->ParseProperty(item->Name, item->Value); + } + else + { // Search for the matching device to get device specific fields + if (!deviceFound && deviceName && OVR_strcmp(item->Name, deviceName) == 0) { - profile->ParseProperty(deviceItem->Name, deviceItem->Value); + deviceFound = true; + + for (JSON* deviceItem = item->GetFirstItem(); deviceItem; + deviceItem = item->GetNextItem(deviceItem)) + { + profile->ParseProperty(deviceItem->Name, deviceItem->Value); + } } } } } - } - // Add the new profile - if (deviceFound) + // Add the new profile ProfileCache.PushBack(profile); + } } CacheDevice = device; @@ -276,10 +288,33 @@ void ProfileManager::SaveCache() Lock::Locker lockScope(&ProfileLock); - // TODO: Since there is only a single device type now, a full tree overwrite - // is sufficient but in the future a selective device branch replacement will - // be necessary + Ptr<JSON> oldroot = *JSON::Load(path); + if (oldroot) + { + if (oldroot->GetItemCount() >= 3) + { + JSON* item0 = oldroot->GetFirstItem(); + JSON* item1 = oldroot->GetNextItem(item0); + oldroot->GetNextItem(item1); + if (item0->Name == "Oculus Profile Version") + { + int major = atoi(item0->Value.ToCStr()); + if (major > MAX_PROFILE_MAJOR_VERSION) + oldroot.Clear(); // don't use the file on unsupported major version number + } + else + { + oldroot.Clear(); + } + } + else + { + oldroot.Clear(); + } + } + + // Create a new json root Ptr<JSON> root = *JSON::CreateObject(); root->AddNumberItem("Oculus Profile Version", PROFILE_VERSION); root->AddStringItem("CurrentProfile", DefaultProfile); @@ -290,6 +325,7 @@ void ProfileManager::SaveCache() { Profile* profile = ProfileCache[i]; + // Write the base profile information JSON* json_profile = JSON::CreateObject(); json_profile->Name = "Profile"; json_profile->AddStringItem("Name", profile->Name); @@ -304,29 +340,100 @@ void ProfileManager::SaveCache() json_profile->AddNumberItem("PlayerHeight", profile->PlayerHeight); json_profile->AddNumberItem("IPD", profile->IPD); + char* device_name = NULL; + // Create a device-specific subtree for the cached device if (profile->Type == Profile_RiftDK1) { - RiftDK1Profile* riftdk1 = (RiftDK1Profile*)profile; - JSON* json_riftdk1 = JSON::CreateObject(); - json_profile->AddItem("RiftDK1", json_riftdk1); + device_name = "RiftDK1"; + + RiftDK1Profile* rift = (RiftDK1Profile*)profile; + JSON* json_rift = JSON::CreateObject(); + json_profile->AddItem(device_name, json_rift); const char* eyecup = "A"; - switch (riftdk1->EyeCups) + switch (rift->EyeCups) { - case RiftDK1Profile::EyeCup_A: eyecup = "A"; break; - case RiftDK1Profile::EyeCup_B: eyecup = "B"; break; - case RiftDK1Profile::EyeCup_C: eyecup = "C"; break; + case EyeCup_A: eyecup = "A"; break; + case EyeCup_B: eyecup = "B"; break; + case EyeCup_C: eyecup = "C"; break; } - json_riftdk1->AddStringItem("EyeCup", eyecup); - json_riftdk1->AddNumberItem("LL", riftdk1->LL); - json_riftdk1->AddNumberItem("LR", riftdk1->LR); - json_riftdk1->AddNumberItem("RL", riftdk1->RL); - json_riftdk1->AddNumberItem("RR", riftdk1->RR); + json_rift->AddStringItem("EyeCup", eyecup); + json_rift->AddNumberItem("LL", rift->LL); + json_rift->AddNumberItem("LR", rift->LR); + json_rift->AddNumberItem("RL", rift->RL); + json_rift->AddNumberItem("RR", rift->RR); } + else if (profile->Type == Profile_RiftDKHD) + { + device_name = "RiftDKHD"; + + RiftDKHDProfile* rift = (RiftDKHDProfile*)profile; + JSON* json_rift = JSON::CreateObject(); + json_profile->AddItem(device_name, json_rift); + const char* eyecup = "A"; + switch (rift->EyeCups) + { + case EyeCup_A: eyecup = "A"; break; + case EyeCup_B: eyecup = "B"; break; + case EyeCup_C: eyecup = "C"; break; + } + json_rift->AddStringItem("EyeCup", eyecup); + //json_rift->AddNumberItem("LL", rift->LL); + //json_rift->AddNumberItem("LR", rift->LR); + //json_rift->AddNumberItem("RL", rift->RL); + //json_rift->AddNumberItem("RR", rift->RR); + } + + // There may be multiple devices stored per user, but only a single + // device is represented by this root. We don't want to overwrite + // the other devices so we need to examine the older root + // and merge previous devices into new json root + if (oldroot) + { + JSON* old_profile = oldroot->GetFirstItem(); + while (old_profile) + { + if (old_profile->Name == "Profile") + { + JSON* profile_name = old_profile->GetItemByName("Name"); + if (profile_name && OVR_strcmp(profile->Name, profile_name->Value) == 0) + { // Now that we found the user in the older root, add all the + // object children to the new root - except for the one for the + // current device + JSON* old_item = old_profile->GetFirstItem(); + while (old_item) + { + if (old_item->Type == JSON_Object + && (device_name == NULL || OVR_strcmp(old_item->Name, device_name) != 0)) + { + JSON* old_device = old_item; + old_item = old_profile->GetNextItem(old_item); + + // remove the node from the older root to avoid multiple reference + old_device->RemoveNode(); + // add the node pointer to the new root + json_profile->AddItem(old_device->Name, old_device); + } + else + { + old_item = old_profile->GetNextItem(old_item); + } + } + + break; + } + } + + old_profile = oldroot->GetNextItem(old_profile); + } + } + + // Add the completed user profile to the new root root->AddItem("Profile", json_profile); } + // Save the profile to disk root->Save(path); } @@ -613,32 +720,20 @@ float Profile::GetEyeHeight() return eye_height; } - //----------------------------------------------------------------------------- -// ***** RiftDK1Profile +// ***** HMDProfile -RiftDK1Profile::RiftDK1Profile(const char* name) : Profile(Profile_RiftDK1, name) +HMDProfile::HMDProfile(ProfileType type, const char* name) : Profile(type, name) { - EyeCups = EyeCup_A; LL = 0; LR = 0; RL = 0; RR = 0; } -bool RiftDK1Profile::ParseProperty(const char* prop, const char* sval) +bool HMDProfile::ParseProperty(const char* prop, const char* sval) { - if (OVR_strcmp(prop, "EyeCup") == 0) - { - switch (sval[0]) - { - case 'C': EyeCups = EyeCup_C; break; - case 'B': EyeCups = EyeCup_B; break; - default: EyeCups = EyeCup_A; break; - } - return true; - } - else if (OVR_strcmp(prop, "LL") == 0) + if (OVR_strcmp(prop, "LL") == 0) { LL = atoi(sval); return true; @@ -662,10 +757,70 @@ bool RiftDK1Profile::ParseProperty(const char* prop, const char* sval) return Profile::ParseProperty(prop, sval); } +Profile* HMDProfile::Clone() const +{ + HMDProfile* profile = new HMDProfile(*this); + return profile; +} + +//----------------------------------------------------------------------------- +// ***** RiftDK1Profile + +RiftDK1Profile::RiftDK1Profile(const char* name) : HMDProfile(Profile_RiftDK1, name) +{ + EyeCups = EyeCup_A; +} + +bool RiftDK1Profile::ParseProperty(const char* prop, const char* sval) +{ + if (OVR_strcmp(prop, "EyeCup") == 0) + { + switch (sval[0]) + { + case 'C': EyeCups = EyeCup_C; break; + case 'B': EyeCups = EyeCup_B; break; + default: EyeCups = EyeCup_A; break; + } + return true; + } + + return HMDProfile::ParseProperty(prop, sval); +} + Profile* RiftDK1Profile::Clone() const { RiftDK1Profile* profile = new RiftDK1Profile(*this); return profile; } +//----------------------------------------------------------------------------- +// ***** RiftDKHDProfile + +RiftDKHDProfile::RiftDKHDProfile(const char* name) : HMDProfile(Profile_RiftDKHD, name) +{ + EyeCups = EyeCup_A; +} + +bool RiftDKHDProfile::ParseProperty(const char* prop, const char* sval) +{ + if (OVR_strcmp(prop, "EyeCup") == 0) + { + switch (sval[0]) + { + case 'C': EyeCups = EyeCup_C; break; + case 'B': EyeCups = EyeCup_B; break; + default: EyeCups = EyeCup_A; break; + } + return true; + } + + return HMDProfile::ParseProperty(prop, sval); +} + +Profile* RiftDKHDProfile::Clone() const +{ + RiftDKHDProfile* profile = new RiftDKHDProfile(*this); + return profile; +} + } // OVR diff --git a/LibOVR/Src/OVR_Profile.h b/LibOVR/Src/OVR_Profile.h index 7184aae..df25fea 100644 --- a/LibOVR/Src/OVR_Profile.h +++ b/LibOVR/Src/OVR_Profile.h @@ -32,8 +32,9 @@ namespace OVR { enum ProfileType { Profile_Unknown = 0, - Profile_RiftDK1 = 1, - Profile_RiftDKHD = 2, + Profile_GenericHMD = 10, + Profile_RiftDK1 = 11, + Profile_RiftDKHD = 12, }; class Profile; @@ -99,10 +100,9 @@ protected: //------------------------------------------------------------------- // ***** Profile -// The base profile for all HMD devices. This object is never created directly. -// Instead derived objects provide specific data implementations. Some settings -// such as IPD will be tied to a specific user and be consistent between , -// implementations but other settings like optical distortion may vary between devices. +// The base profile for all users. This object is not created directly. +// Instead derived device objects provide add specific device members to +// the base profile class Profile : public RefCountBase<Profile> { @@ -116,66 +116,120 @@ public: Gender_Female = 2 }; - ProfileType Type; // The type of device profile - char Name[MaxNameLen]; // The name given to this profile + ProfileType Type; // The type of device profile + char Name[MaxNameLen]; // The name given to this profile protected: - GenderType Gender; // The gender of the user - float PlayerHeight; // The height of the user in meters - float IPD; // Distance between eyes in meters + GenderType Gender; // The gender of the user + float PlayerHeight; // The height of the user in meters + float IPD; // Distance between eyes in meters public: + virtual Profile* Clone() const = 0; + // These are properties which are intrinsic to the user and affect scene setup - GenderType GetGender() { return Gender; }; - float GetPlayerHeight() { return PlayerHeight; }; - float GetIPD() { return IPD; }; - float GetEyeHeight(); + GenderType GetGender() { return Gender; }; + float GetPlayerHeight() { return PlayerHeight; }; + float GetIPD() { return IPD; }; + float GetEyeHeight(); - void SetGender(GenderType gender) { Gender = gender; }; - void SetPlayerHeight(float height) { PlayerHeight = height; }; - void SetIPD(float ipd) { IPD = ipd; }; - + void SetGender(GenderType gender) { Gender = gender; }; + void SetPlayerHeight(float height) { PlayerHeight = height; }; + void SetIPD(float ipd) { IPD = ipd; }; protected: Profile(ProfileType type, const char* name); - - virtual Profile* Clone() const = 0; + virtual bool ParseProperty(const char* prop, const char* sval); friend class ProfileManager; }; +//----------------------------------------------------------------------------- +// ***** HMDProfile + +// The generic HMD profile is used for properties that are common to all headsets +class HMDProfile : public Profile +{ +protected: + // FOV extents in pixels measured by a user + int LL; // left eye outer extent + int LR; // left eye inner extent + int RL; // right eye inner extent + int RR; // right eye outer extent + +public: + virtual Profile* Clone() const; + + void SetLL(int val) { LL = val; }; + void SetLR(int val) { LR = val; }; + void SetRL(int val) { RL = val; }; + void SetRR(int val) { RR = val; }; + + int GetLL() { return LL; }; + int GetLR() { return LR; }; + int GetRL() { return RL; }; + int GetRR() { return RR; }; + +protected: + HMDProfile(ProfileType type, const char* name); + + virtual bool ParseProperty(const char* prop, const char* sval); + + friend class ProfileManager; +}; + +// For headsets that use eye cups +enum EyeCupType +{ + EyeCup_A = 0, + EyeCup_B = 1, + EyeCup_C = 2 +}; //----------------------------------------------------------------------------- // ***** RiftDK1Profile // This profile is specific to the Rift Dev Kit 1 and contains overrides specific // to that device and lens cup settings. -class RiftDK1Profile : public Profile +class RiftDK1Profile : public HMDProfile { -public: - enum EyeCupType - { - EyeCup_A = 0, - EyeCup_B = 1, - EyeCup_C = 2 - }; - protected: - EyeCupType EyeCups; // Which eye cup does the player use - int LL; // Configuration Utility IPD setting - int LR; // Configuration Utility IPD setting - int RL; // Configuration Utility IPD setting - int RR; // Configuration Utility IPD setting + EyeCupType EyeCups; // Which eye cup does the player use public: + virtual Profile* Clone() const; + EyeCupType GetEyeCup() { return EyeCups; }; void SetEyeCup(EyeCupType cup) { EyeCups = cup; }; protected: RiftDK1Profile(const char* name); + virtual bool ParseProperty(const char* prop, const char* sval); + + friend class ProfileManager; +}; + +//----------------------------------------------------------------------------- +// ***** RiftDKHDProfile + +// This profile is specific to the Rift HD Dev Kit and contains overrides specific +// to that device and lens cup settings. +class RiftDKHDProfile : public HMDProfile +{ +protected: + EyeCupType EyeCups; // Which eye cup does the player use + +public: virtual Profile* Clone() const; + + EyeCupType GetEyeCup() { return EyeCups; }; + void SetEyeCup(EyeCupType cup) { EyeCups = cup; }; + +protected: + RiftDKHDProfile(const char* name); + virtual bool ParseProperty(const char* prop, const char* sval); friend class ProfileManager; diff --git a/LibOVR/Src/OVR_SensorFilter.cpp b/LibOVR/Src/OVR_SensorFilter.cpp index 75c3b5b..7df84da 100644 --- a/LibOVR/Src/OVR_SensorFilter.cpp +++ b/LibOVR/Src/OVR_SensorFilter.cpp @@ -4,7 +4,7 @@ PublicHeader: OVR.h Filename : OVR_SensorFilter.cpp Content : Basic filtering of sensor data Created : March 7, 2013 -Authors : Steve LaValle, Anna Yershova +Authors : Steve LaValle, Anna Yershova, Max Katsev Copyright : Copyright 2012 Oculus VR, Inc. All Rights reserved. @@ -18,35 +18,15 @@ otherwise accompanies this software in either electronic or hard copy form. namespace OVR { -Vector3f SensorFilter::Total() const -{ - Vector3f total = Vector3f(0.0f, 0.0f, 0.0f); - for (int i = 0; i < Size; i++) - total += Elements[i]; - return total; -} - -Vector3f SensorFilter::Mean() const -{ - Vector3f total = Vector3f(0.0f, 0.0f, 0.0f); - for (int i = 0; i < Size; i++) - total += Elements[i]; - return total / (float) Size; -} - Vector3f SensorFilter::Median() const { - int half_window = (int) Size / 2; - float sortx[MaxFilterSize]; - float resultx = 0.0f; + int half_window = Count / 2; + float* sortx = (float*) OVR_ALLOC(Count * sizeof(float)); + float* sorty = (float*) OVR_ALLOC(Count * sizeof(float)); + float* sortz = (float*) OVR_ALLOC(Count * sizeof(float)); + float resultx = 0.0f, resulty = 0.0f, resultz = 0.0f; - float sorty[MaxFilterSize]; - float resulty = 0.0f; - - float sortz[MaxFilterSize]; - float resultz = 0.0f; - - for (int i = 0; i < Size; i++) + for (int i = 0; i < Count; i++) { sortx[i] = Elements[i].x; sorty[i] = Elements[i].y; @@ -57,7 +37,7 @@ Vector3f SensorFilter::Median() const int minx = j; int miny = j; int minz = j; - for (int k = j + 1; k < Size; k++) + for (int k = j + 1; k < Count; k++) { if (sortx[k] < sortx[minx]) minx = k; if (sorty[k] < sorty[miny]) miny = k; @@ -79,6 +59,10 @@ Vector3f SensorFilter::Median() const resulty = sorty[half_window]; resultz = sortz[half_window]; + OVR_FREE(sortx); + OVR_FREE(sorty); + OVR_FREE(sortz); + return Vector3f(resultx, resulty, resultz); } @@ -87,13 +71,13 @@ Vector3f SensorFilter::Variance() const { Vector3f mean = Mean(); Vector3f total = Vector3f(0.0f, 0.0f, 0.0f); - for (int i = 0; i < Size; i++) + for (int i = 0; i < Count; i++) { total.x += (Elements[i].x - mean.x) * (Elements[i].x - mean.x); total.y += (Elements[i].y - mean.y) * (Elements[i].y - mean.y); total.z += (Elements[i].z - mean.z) * (Elements[i].z - mean.z); } - return total / (float) Size; + return total / (float) Count; } // Should be a 3x3 matrix returned, but OVR_math.h doesn't have one @@ -101,7 +85,7 @@ Matrix4f SensorFilter::Covariance() const { Vector3f mean = Mean(); Matrix4f total = Matrix4f(0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0); - for (int i = 0; i < Size; i++) + for (int i = 0; i < Count; i++) { total.M[0][0] += (Elements[i].x - mean.x) * (Elements[i].x - mean.x); total.M[1][0] += (Elements[i].y - mean.y) * (Elements[i].x - mean.x); @@ -115,7 +99,7 @@ Matrix4f SensorFilter::Covariance() const total.M[1][2] = total.M[2][1]; for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++) - total.M[i][j] *= 1.0f / Size; + total.M[i][j] *= 1.0f / Count; return total; } @@ -130,72 +114,4 @@ Vector3f SensorFilter::PearsonCoefficient() const return pearson; } - -Vector3f SensorFilter::SavitzkyGolaySmooth8() const -{ - OVR_ASSERT(Size >= 8); - return GetPrev(0)*0.41667f + - GetPrev(1)*0.33333f + - GetPrev(2)*0.25f + - GetPrev(3)*0.16667f + - GetPrev(4)*0.08333f - - GetPrev(6)*0.08333f - - GetPrev(7)*0.16667f; -} - - -Vector3f SensorFilter::SavitzkyGolayDerivative4() const -{ - OVR_ASSERT(Size >= 4); - return GetPrev(0)*0.3f + - GetPrev(1)*0.1f - - GetPrev(2)*0.1f - - GetPrev(3)*0.3f; -} - -Vector3f SensorFilter::SavitzkyGolayDerivative5() const -{ - OVR_ASSERT(Size >= 5); - return GetPrev(0)*0.2f + - GetPrev(1)*0.1f - - GetPrev(3)*0.1f - - GetPrev(4)*0.2f; -} - -Vector3f SensorFilter::SavitzkyGolayDerivative12() const -{ - OVR_ASSERT(Size >= 12); - return GetPrev(0)*0.03846f + - GetPrev(1)*0.03147f + - GetPrev(2)*0.02448f + - GetPrev(3)*0.01748f + - GetPrev(4)*0.01049f + - GetPrev(5)*0.0035f - - GetPrev(6)*0.0035f - - GetPrev(7)*0.01049f - - GetPrev(8)*0.01748f - - GetPrev(9)*0.02448f - - GetPrev(10)*0.03147f - - GetPrev(11)*0.03846f; -} - -Vector3f SensorFilter::SavitzkyGolayDerivativeN(int n) const -{ - OVR_ASSERT(Size >= n); - int m = (n-1)/2; - Vector3f result = Vector3f(); - for (int k = 1; k <= m; k++) - { - int ind1 = m - k; - int ind2 = n - m + k - 1; - result += (GetPrev(ind1) - GetPrev(ind2)) * (float) k; - } - float coef = 3.0f/(m*(m+1.0f)*(2.0f*m+1.0f)); - result = result*coef; - return result; -} - - - - -} //namespace OVR
\ No newline at end of file +} //namespace OVR diff --git a/LibOVR/Src/OVR_SensorFilter.h b/LibOVR/Src/OVR_SensorFilter.h index 857e719..e6ff01a 100644 --- a/LibOVR/Src/OVR_SensorFilter.h +++ b/LibOVR/Src/OVR_SensorFilter.h @@ -4,7 +4,7 @@ PublicHeader: OVR.h Filename : OVR_SensorFilter.h Content : Basic filtering of sensor data Created : March 7, 2013 -Authors : Steve LaValle, Anna Yershova +Authors : Steve LaValle, Anna Yershova, Max Katsev Copyright : Copyright 2012 Oculus VR, Inc. All Rights reserved. @@ -22,76 +22,179 @@ otherwise accompanies this software in either electronic or hard copy form. namespace OVR { -// This class maintains a sliding window of sensor data taken over time and implements -// various simple filters, most of which are linear functions of the data history. -class SensorFilter +// A simple circular buffer data structure that stores last N elements in an array +template <typename T> +class CircularBuffer { +protected: enum { - MaxFilterSize = 100, - DefaultFilterSize = 20 + DefaultFilterCapacity = 20 }; -private: - int LastIdx; // The index of the last element that was added to the array - int Size; // The window size (number of elements) - Vector3f Elements[MaxFilterSize]; + int LastIdx; // The index of the last element that was added to the buffer + int Capacity; // The buffer size (maximum number of elements) + int Count; // Number of elements in the filter + T* Elements; public: - // Create a new filter with default size - SensorFilter() + CircularBuffer(int capacity = DefaultFilterCapacity) + : LastIdx(-1), Capacity(capacity), Count(0) { - LastIdx = -1; - Size = DefaultFilterSize; - }; + Elements = (T*)OVR_ALLOC(capacity * sizeof(T)); + for (int i = 0; i < Capacity; i++) + Elements[i] = T(); + } - // Create a new filter with size i - SensorFilter(int i) - { - OVR_ASSERT(i <= MaxFilterSize); - LastIdx = -1; - Size = i; - }; + ~CircularBuffer() { + OVR_FREE(Elements); + } +private: + // Make the class non-copyable + CircularBuffer(const CircularBuffer& other); + CircularBuffer& operator=(const CircularBuffer& other); - // Create a new element to the filter - void AddElement (const Vector3f &e) +public: + // Add a new element to the filter + void AddElement (const T &e) { - if (LastIdx == Size - 1) - LastIdx = 0; - else - LastIdx++; - + LastIdx = (LastIdx + 1) % Capacity; Elements[LastIdx] = e; - }; + if (Count < Capacity) + Count++; + } // Get element i. 0 is the most recent, 1 is one step ago, 2 is two steps ago, ... - Vector3f GetPrev(int i) const + T GetPrev(int i = 0) const { - OVR_ASSERT(i >= 0); // + OVR_ASSERT(i >= 0); + if (i >= Count) // return 0 if the filter doesn't have enough elements + return T(); int idx = (LastIdx - i); if (idx < 0) // Fix the wraparound case - idx += Size; + idx += Capacity; OVR_ASSERT(idx >= 0); // Multiple wraparounds not allowed return Elements[idx]; - }; + } +}; + +// A base class for filters that maintains a buffer of sensor data taken over time and implements +// various simple filters, most of which are linear functions of the data history. +// Maintains the running sum of its elements for better performance on large capacity values +template <typename T> +class SensorFilterBase : public CircularBuffer<T> +{ +protected: + T RunningTotal; // Cached sum of the elements + +public: + SensorFilterBase(int capacity = CircularBuffer<T>::DefaultFilterCapacity) : CircularBuffer<T>(capacity), RunningTotal() { }; + + // Add a new element to the filter + // Updates the running sum value + void AddElement (const T &e) + { + int NextIdx = (this->LastIdx + 1) % this->Capacity; + RunningTotal += (e - this->Elements[NextIdx]); + CircularBuffer<T>::AddElement(e); + if (this->LastIdx == 0) + { + // update the cached total to avoid error accumulation + RunningTotal = T(); + for (int i = 0; i < this->Count; i++) + RunningTotal += this->Elements[i]; + } + } + + // Simple statistics + T Total() const + { + return RunningTotal; + } + + T Mean() const + { + return (this->Count == 0) ? T() : (Total() / (float) this->Count); + } + + // A popular family of smoothing filters and smoothed derivatives + T SavitzkyGolaySmooth8() const + { + OVR_ASSERT(this->Capacity >= 8); + return this->GetPrev(0)*0.41667f + + this->GetPrev(1)*0.33333f + + this->GetPrev(2)*0.25f + + this->GetPrev(3)*0.16667f + + this->GetPrev(4)*0.08333f - + this->GetPrev(6)*0.08333f - + this->GetPrev(7)*0.16667f; + } + + T SavitzkyGolayDerivative4() const + { + OVR_ASSERT(this->Capacity >= 4); + return this->GetPrev(0)*0.3f + + this->GetPrev(1)*0.1f - + this->GetPrev(2)*0.1f - + this->GetPrev(3)*0.3f; + } + + T SavitzkyGolayDerivative5() const + { + OVR_ASSERT(this->Capacity >= 5); + return this->GetPrev(0)*0.2f + + this->GetPrev(1)*0.1f - + this->GetPrev(3)*0.1f - + this->GetPrev(4)*0.2f; + } + + T SavitzkyGolayDerivative12() const + { + OVR_ASSERT(this->Capacity >= 12); + return this->GetPrev(0)*0.03846f + + this->GetPrev(1)*0.03147f + + this->GetPrev(2)*0.02448f + + this->GetPrev(3)*0.01748f + + this->GetPrev(4)*0.01049f + + this->GetPrev(5)*0.0035f - + this->GetPrev(6)*0.0035f - + this->GetPrev(7)*0.01049f - + this->GetPrev(8)*0.01748f - + this->GetPrev(9)*0.02448f - + this->GetPrev(10)*0.03147f - + this->GetPrev(11)*0.03846f; + } + + T SavitzkyGolayDerivativeN(int n) const + { + OVR_ASSERT(this->Capacity >= n); + int m = (n-1)/2; + T result = T(); + for (int k = 1; k <= m; k++) + { + int ind1 = m - k; + int ind2 = n - m + k - 1; + result += (this->GetPrev(ind1) - this->GetPrev(ind2)) * (float) k; + } + float coef = 3.0f/(m*(m+1.0f)*(2.0f*m+1.0f)); + result = result*coef; + return result; + } +}; + +// This class maintains a buffer of sensor data taken over time and implements +// various simple filters, most of which are linear functions of the data history. +class SensorFilter : public SensorFilterBase<Vector3f> +{ +public: + SensorFilter(int capacity = DefaultFilterCapacity) : SensorFilterBase<Vector3f>(capacity) { }; // Simple statistics - Vector3f Total() const; - Vector3f Mean() const; Vector3f Median() const; Vector3f Variance() const; // The diagonal of covariance matrix Matrix4f Covariance() const; Vector3f PearsonCoefficient() const; - - // A popular family of smoothing filters and smoothed derivatives - Vector3f SavitzkyGolaySmooth8() const; - Vector3f SavitzkyGolayDerivative4() const; - Vector3f SavitzkyGolayDerivative5() const; - Vector3f SavitzkyGolayDerivative12() const; - Vector3f SavitzkyGolayDerivativeN(int n) const; - - ~SensorFilter() {}; }; } //namespace OVR diff --git a/LibOVR/Src/OVR_SensorFusion.cpp b/LibOVR/Src/OVR_SensorFusion.cpp index ba913c1..f396775 100644 --- a/LibOVR/Src/OVR_SensorFusion.cpp +++ b/LibOVR/Src/OVR_SensorFusion.cpp @@ -3,7 +3,7 @@ Filename : OVR_SensorFusion.cpp Content : Methods that determine head orientation from sensor data over time Created : October 9, 2012 -Authors : Michael Antonov, Steve LaValle +Authors : Michael Antonov, Steve LaValle, Max Katsev Copyright : Copyright 2012 Oculus VR, Inc. All Rights reserved. @@ -19,22 +19,21 @@ otherwise accompanies this software in either electronic or hard copy form. #include "OVR_JSON.h" #include "OVR_Profile.h" +#define MAX_DEVICE_PROFILE_MAJOR_VERSION 1 + namespace OVR { //------------------------------------------------------------------------------------- // ***** Sensor Fusion SensorFusion::SensorFusion(SensorDevice* sensor) - : Handler(getThis()), pDelegate(0), - Gain(0.05f), YawMult(1), EnableGravity(true), Stage(0), RunningTime(0), DeltaT(0.001f), - EnablePrediction(true), PredictionDT(0.03f), PredictionTimeIncrement(0.001f), - FRawMag(10), FAccW(20), FAngV(20), - TiltCondCount(0), TiltErrorAngle(0), - TiltErrorAxis(0,1,0), - MagCondCount(0), MagCalibrated(false), MagRefQ(0, 0, 0, 1), - MagRefM(0), MagRefYaw(0), YawErrorAngle(0), MagRefDistance(0.5f), - YawErrorCount(0), YawCorrectionActivated(false), YawCorrectionInProgress(false), - EnableYawCorrection(false), MagNumReferences(0), MagHasNearbyReference(false), + : Stage(0), RunningTime(0), DeltaT(0.001f), + Handler(getThis()), pDelegate(0), + Gain(0.05f), EnableGravity(true), + EnablePrediction(true), PredictionDT(0.03f), PredictionTimeIncrement(0.001f), + FRawMag(10), FAngV(20), + GyroOffset(), TiltAngleFilter(1000), + EnableYawCorrection(false), MagCalibrated(false), MagNumReferences(0), MagRefIdx(-1), MagRefScore(0), MotionTrackingEnabled(true) { if (sensor) @@ -49,10 +48,17 @@ SensorFusion::~SensorFusion() bool SensorFusion::AttachToSensor(SensorDevice* sensor) { - - pSensor = sensor; + // clear the cached device information + CachedSensorInfo.SerialNumber[0] = 0; + CachedSensorInfo.VendorId = 0; + CachedSensorInfo.ProductId = 0; + if (sensor != NULL) { + // Cache the sensor device so we can access this information during + // mag saving and loading (avoid holding a reference to sensor to prevent + // deadlock on shutdown) + sensor->GetDeviceInfo(&CachedSensorInfo); // save the device information MessageHandler* pCurrentHandler = sensor->GetMessageHandler(); if (pCurrentHandler == &Handler) @@ -94,315 +100,214 @@ void SensorFusion::Reset() Q = Quatf(); QUncorrected = Quatf(); Stage = 0; - RunningTime = 0; - MagNumReferences = 0; - MagHasNearbyReference = false; + RunningTime = 0; + MagNumReferences = 0; + MagRefIdx = -1; + GyroOffset = Vector3f(); } +// Compute a rotation required to transform "estimated" into "measured" +// Returns an approximation of the goal rotation in the Simultaneous Orthogonal Rotations Angle representation +// (vector direction is the axis of rotation, norm is the angle) +Vector3f SensorFusion_ComputeCorrection(Vector3f measured, Vector3f estimated) +{ + measured.Normalize(); + estimated.Normalize(); + Vector3f correction = measured.Cross(estimated); + float cosError = measured.Dot(estimated); + // from the def. of cross product, correction.Length() = sin(error) + // therefore sin(error) * sqrt(2 / (1 + cos(error))) = 2 * sin(error / 2) ~= error in [-pi, pi] + // Mathf::Tolerance is used to avoid div by 0 if cos(error) = -1 + return correction * sqrt(2 / (1 + cosError + Mathf::Tolerance)); +} void SensorFusion::handleMessage(const MessageBodyFrame& msg) { if (msg.Type != Message_BodyFrame || !IsMotionTrackingEnabled()) return; - - // Put the sensor readings into convenient local variables - Vector3f angVel = msg.RotationRate; - Vector3f rawAccel = msg.Acceleration; - Vector3f mag = msg.MagneticField; - // Set variables accessible through the class API - DeltaT = msg.TimeDelta; - AngV = msg.RotationRate; - AngV.y *= YawMult; // Warning: If YawMult != 1, then AngV is not true angular velocity - A = rawAccel; + // Put the sensor readings into convenient local variables + Vector3f gyro = msg.RotationRate; + Vector3f accel = msg.Acceleration; + Vector3f mag = msg.MagneticField; - // Allow external access to uncalibrated magnetometer values - RawMag = mag; + // Insert current sensor data into filter history + FRawMag.AddElement(mag); + FAngV.AddElement(gyro); // Apply the calibration parameters to raw mag - if (HasMagCalibration()) - { - mag.x += MagCalibrationMatrix.M[0][3]; - mag.y += MagCalibrationMatrix.M[1][3]; - mag.z += MagCalibrationMatrix.M[2][3]; - } + Vector3f calMag = MagCalibrated ? GetCalibratedMagValue(FRawMag.Mean()) : FRawMag.Mean(); - // Provide external access to calibrated mag values - // (if the mag is not calibrated, then the raw value is returned) - CalMag = mag; - - float angVelLength = angVel.Length(); - float accLength = rawAccel.Length(); - - - // Acceleration in the world frame (Q is current HMD orientation) - Vector3f accWorld = Q.Rotate(rawAccel); + // Set variables accessible through the class API + DeltaT = msg.TimeDelta; + AngV = gyro; + A = accel; + RawMag = mag; + CalMag = calMag; // Keep track of time Stage++; RunningTime += DeltaT; - // Insert current sensor data into filter history - FRawMag.AddElement(RawMag); - FAccW.AddElement(accWorld); - FAngV.AddElement(angVel); - - // Update orientation Q based on gyro outputs. This technique is - // based on direct properties of the angular velocity vector: - // Its direction is the current rotation axis, and its magnitude - // is the rotation rate (rad/sec) about that axis. Our sensor - // sampling rate is so fast that we need not worry about integral - // approximation error (not yet, anyway). - if (angVelLength > 0.0f) + // Small preprocessing + Quatf Qinv = Q.Inverted(); + Vector3f up = Qinv.Rotate(Vector3f(0, 1, 0)); + + Vector3f gyroCorrected = gyro; + + // Apply integral term + // All the corrections are stored in the Simultaneous Orthogonal Rotations Angle representation, + // which allows to combine and scale them by just addition and multiplication + if (EnableGravity || EnableYawCorrection) + gyroCorrected -= GyroOffset; + + if (EnableGravity) { - Vector3f rotAxis = angVel / angVelLength; - float halfRotAngle = angVelLength * DeltaT * 0.5f; - float sinHRA = sin(halfRotAngle); - Quatf deltaQ(rotAxis.x*sinHRA, rotAxis.y*sinHRA, rotAxis.z*sinHRA, cos(halfRotAngle)); + const float spikeThreshold = 0.01f; + const float gravityThreshold = 0.1f; + float proportionalGain = 5 * Gain; // Gain parameter should be removed in a future release + float integralGain = 0.0125f; + + Vector3f tiltCorrection = SensorFusion_ComputeCorrection(accel, up); - Q = Q * deltaQ; + if (Stage > 5) + { + // Spike detection + float tiltAngle = up.Angle(accel); + TiltAngleFilter.AddElement(tiltAngle); + if (tiltAngle > TiltAngleFilter.Mean() + spikeThreshold) + proportionalGain = integralGain = 0; + // Acceleration detection + const float gravity = 9.8f; + if (fabs(accel.Length() / gravity - 1) > gravityThreshold) + integralGain = 0; + } + else // Apply full correction at the startup + { + proportionalGain = 1 / DeltaT; + integralGain = 0; + } + + gyroCorrected += (tiltCorrection * proportionalGain); + GyroOffset -= (tiltCorrection * integralGain * DeltaT); } - - // The quaternion magnitude may slowly drift due to numerical error, - // so it is periodically normalized. - if (Stage % 5000 == 0) - Q.Normalize(); - - // Maintain the uncorrected orientation for later use by predictive filtering - QUncorrected = Q; - // Perform tilt correction using the accelerometer data. This enables - // drift errors in pitch and roll to be corrected. Note that yaw cannot be corrected - // because the rotation axis is parallel to the gravity vector. - if (EnableGravity) + if (EnableYawCorrection && MagCalibrated && RunningTime > 2.0f) { - // Correcting for tilt error by using accelerometer data - const float gravityEpsilon = 0.4f; - const float angVelEpsilon = 0.1f; // Relatively slow rotation - const int tiltPeriod = 50; // Required time steps of stability - const float maxTiltError = 0.05f; - const float minTiltError = 0.01f; - - // This condition estimates whether the only measured acceleration is due to gravity - // (the Rift is not linearly accelerating). It is often wrong, but tends to average - // out well over time. - if ((fabs(accLength - 9.81f) < gravityEpsilon) && - (angVelLength < angVelEpsilon)) - TiltCondCount++; - else - TiltCondCount = 0; - - // After stable measurements have been taken over a sufficiently long period, - // estimate the amount of tilt error and calculate the tilt axis for later correction. - if (TiltCondCount >= tiltPeriod) - { // Update TiltErrorEstimate - TiltCondCount = 0; - // Use an average value to reduce noise (could alternatively use an LPF) - Vector3f accWMean = FAccW.Mean(); - // Project the acceleration vector into the XZ plane - Vector3f xzAcc = Vector3f(accWMean.x, 0.0f, accWMean.z); - // The unit normal of xzAcc will be the rotation axis for tilt correction - Vector3f tiltAxis = Vector3f(xzAcc.z, 0.0f, -xzAcc.x).Normalized(); - Vector3f yUp = Vector3f(0.0f, 1.0f, 0.0f); - // This is the amount of rotation - float tiltAngle = yUp.Angle(accWMean); - // Record values if the tilt error is intolerable - if (tiltAngle > maxTiltError) + const float maxMagRefDist = 0.1f; + const float maxTiltError = 0.05f; + float proportionalGain = 0.01f; + float integralGain = 0.0005f; + + // Update the reference point if needed + if (MagRefIdx < 0 || calMag.Distance(MagRefsInBodyFrame[MagRefIdx]) > maxMagRefDist) + { + // Delete a bad point + if (MagRefIdx >= 0 && MagRefScore < 0) { - TiltErrorAngle = tiltAngle; - TiltErrorAxis = tiltAxis; + MagNumReferences--; + MagRefsInBodyFrame[MagRefIdx] = MagRefsInBodyFrame[MagNumReferences]; + MagRefsInWorldFrame[MagRefIdx] = MagRefsInWorldFrame[MagNumReferences]; + } + // Find a new one + MagRefIdx = -1; + MagRefScore = 1000; + float bestDist = maxMagRefDist; + for (int i = 0; i < MagNumReferences; i++) + { + float dist = calMag.Distance(MagRefsInBodyFrame[i]); + if (bestDist > dist) + { + bestDist = dist; + MagRefIdx = i; + } + } + // Create one if needed + if (MagRefIdx < 0 && MagNumReferences < MagMaxReferences) + { + MagRefIdx = MagNumReferences; + MagRefsInBodyFrame[MagRefIdx] = calMag; + MagRefsInWorldFrame[MagRefIdx] = Q.Rotate(calMag).Normalized(); + MagNumReferences++; } } - // This part performs the actual tilt correction as needed - if (TiltErrorAngle > minTiltError) + if (MagRefIdx >= 0) { - if ((TiltErrorAngle > 0.4f)&&(RunningTime < 8.0f)) - { // Tilt completely to correct orientation - Q = Quatf(TiltErrorAxis, -TiltErrorAngle) * Q; - TiltErrorAngle = 0.0f; + Vector3f magEstimated = Qinv.Rotate(MagRefsInWorldFrame[MagRefIdx]); + Vector3f magMeasured = calMag.Normalized(); + + // Correction is computed in the horizontal plane (in the world frame) + Vector3f yawCorrection = SensorFusion_ComputeCorrection(magMeasured.ProjectToPlane(up), + magEstimated.ProjectToPlane(up)); + + if (fabs(up.Dot(magEstimated - magMeasured)) < maxTiltError) + { + MagRefScore += 2; } - else + else // If the vertical angle is wrong, decrease the score { - //LogText("Performing tilt correction - Angle: %f Axis: %f %f %f\n", - // TiltErrorAngle,TiltErrorAxis.x,TiltErrorAxis.y,TiltErrorAxis.z); - //float deltaTiltAngle = -Gain*TiltErrorAngle*0.005f; - // This uses aggressive correction steps while your head is moving fast - float deltaTiltAngle = -Gain*TiltErrorAngle*0.005f*(5.0f*angVelLength+1.0f); - // Incrementally "un-tilt" by a small step size - Q = Quatf(TiltErrorAxis, deltaTiltAngle) * Q; - TiltErrorAngle += deltaTiltAngle; + MagRefScore -= 1; + proportionalGain = integralGain = 0; } + gyroCorrected += (yawCorrection * proportionalGain); + GyroOffset -= (yawCorrection * integralGain * DeltaT); } } - // Yaw drift correction based on magnetometer data. This corrects the part of the drift - // that the accelerometer cannot handle. - // This will only work if the magnetometer has been enabled, calibrated, and a reference - // point has been set. - const float maxAngVelLength = 3.0f; - const int magWindow = 5; - const float yawErrorMax = 0.1f; - const float yawErrorMin = 0.01f; - const int yawErrorCountLimit = 50; - const float yawRotationStep = 0.00002f; - - if (angVelLength < maxAngVelLength) - MagCondCount++; - else - MagCondCount = 0; - - // Find, create, and utilize reference points for the magnetometer - // Need to be careful not to set reference points while there is significant tilt error - if ((EnableYawCorrection && MagCalibrated)&&(RunningTime > 10.0f)&&(TiltErrorAngle < 0.2f)) - { - if (MagNumReferences == 0) - { - setMagReference(); // Use the current direction - } - else if (Q.Distance(MagRefQ) > MagRefDistance) - { - MagHasNearbyReference = false; - float bestDist = 100000.0f; - int bestNdx = 0; - float dist; - for (int i = 0; i < MagNumReferences; i++) - { - dist = Q.Distance(MagRefTableQ[i]); - if (dist < bestDist) - { - bestNdx = i; - bestDist = dist; - } - } - - if (bestDist < MagRefDistance) - { - MagHasNearbyReference = true; - MagRefQ = MagRefTableQ[bestNdx]; - MagRefM = MagRefTableM[bestNdx]; - MagRefYaw = MagRefTableYaw[bestNdx]; - //LogText("Using reference %d\n",bestNdx); - } - else if (MagNumReferences < MagMaxReferences) - setMagReference(); - } - } - - YawCorrectionInProgress = false; - if (EnableYawCorrection && MagCalibrated && (RunningTime > 2.0f) && (MagCondCount >= magWindow) && - MagHasNearbyReference) - { - // Use rotational invariance to bring reference mag value into global frame - Vector3f grefmag = MagRefQ.Rotate(GetCalibratedMagValue(MagRefM)); - // Bring current (averaged) mag reading into global frame - Vector3f gmag = Q.Rotate(GetCalibratedMagValue(FRawMag.Mean())); - // Calculate the reference yaw in the global frame - Anglef gryaw = Anglef(atan2(grefmag.x,grefmag.z)); - // Calculate the current yaw in the global frame - Anglef gyaw = Anglef(atan2(gmag.x,gmag.z)); - // The difference between reference and current yaws is the perceived error - Anglef YawErrorAngle = gyaw - gryaw; - - //LogText("Yaw error estimate: %f\n",YawErrorAngle.Get()); - // If the perceived error is large, keep count - if ((YawErrorAngle.Abs() > yawErrorMax) && (!YawCorrectionActivated)) - YawErrorCount++; - // After enough iterations of high perceived error, start the correction process - if (YawErrorCount > yawErrorCountLimit) - YawCorrectionActivated = true; - // If the perceived error becomes small, turn off the yaw correction - if ((YawErrorAngle.Abs() < yawErrorMin) && YawCorrectionActivated) - { - YawCorrectionActivated = false; - YawErrorCount = 0; - } - - // Perform the actual yaw correction, due to previously detected, large yaw error - if (YawCorrectionActivated) - { - YawCorrectionInProgress = true; - // Incrementally "unyaw" by a small step size - Q = Quatf(Vector3f(0.0f,1.0f,0.0f), -yawRotationStep * YawErrorAngle.Sign()) * Q; - } - } + // Update the orientation quaternion based on the corrected angular velocity vector + Q = Q * Quatf(gyroCorrected, gyroCorrected.Length() * DeltaT); + + // The quaternion magnitude may slowly drift due to numerical error, + // so it is periodically normalized. + if (Stage % 500 == 0) + Q.Normalize(); } - // A predictive filter based on extrapolating the smoothed, current angular velocity Quatf SensorFusion::GetPredictedOrientation(float pdt) { - Lock::Locker lockScope(Handler.GetHandlerLock()); - Quatf qP = QUncorrected; - + Lock::Locker lockScope(Handler.GetHandlerLock()); + Quatf qP = Q; + if (EnablePrediction) { - // This method assumes a constant angular velocity - Vector3f angVelF = FAngV.SavitzkyGolaySmooth8(); + // This method assumes a constant angular velocity + Vector3f angVelF = FAngV.SavitzkyGolaySmooth8(); float angVelFL = angVelF.Length(); - // Force back to raw measurement + // Force back to raw measurement angVelF = AngV; - angVelFL = AngV.Length(); + angVelFL = AngV.Length(); - // Dynamic prediction interval: Based on angular velocity to reduce vibration - const float minPdt = 0.001f; - const float slopePdt = 0.1f; - float newpdt = pdt; - float tpdt = minPdt + slopePdt * angVelFL; - if (tpdt < pdt) - newpdt = tpdt; - //LogText("PredictonDTs: %d\n",(int)(newpdt / PredictionTimeIncrement + 0.5f)); + // Dynamic prediction interval: Based on angular velocity to reduce vibration + const float minPdt = 0.001f; + const float slopePdt = 0.1f; + float newpdt = pdt; + float tpdt = minPdt + slopePdt * angVelFL; + if (tpdt < pdt) + newpdt = tpdt; + //LogText("PredictonDTs: %d\n",(int)(newpdt / PredictionTimeIncrement + 0.5f)); if (angVelFL > 0.001f) { Vector3f rotAxisP = angVelF / angVelFL; float halfRotAngleP = angVelFL * newpdt * 0.5f; float sinaHRAP = sin(halfRotAngleP); - Quatf deltaQP(rotAxisP.x*sinaHRAP, rotAxisP.y*sinaHRAP, + Quatf deltaQP(rotAxisP.x*sinaHRAP, rotAxisP.y*sinaHRAP, rotAxisP.z*sinaHRAP, cos(halfRotAngleP)); - qP = QUncorrected * deltaQP; - } - } + qP = Q * deltaQP; + } + } return qP; } Vector3f SensorFusion::GetCalibratedMagValue(const Vector3f& rawMag) const { - Vector3f mag = rawMag; OVR_ASSERT(HasMagCalibration()); - mag.x += MagCalibrationMatrix.M[0][3]; - mag.y += MagCalibrationMatrix.M[1][3]; - mag.z += MagCalibrationMatrix.M[2][3]; - return mag; -} - - -void SensorFusion::setMagReference(const Quatf& q, const Vector3f& rawMag) -{ - if (MagNumReferences < MagMaxReferences) - { - MagRefTableQ[MagNumReferences] = q; - MagRefTableM[MagNumReferences] = rawMag; //FRawMag.Mean(); - - //LogText("Inserting reference %d\n",MagNumReferences); - - MagRefQ = q; - MagRefM = rawMag; - - float pitch, roll, yaw; - Quatf q2 = q; - q2.GetEulerAngles<Axis_X, Axis_Z, Axis_Y>(&pitch, &roll, &yaw); - MagRefTableYaw[MagNumReferences] = yaw; - MagRefYaw = yaw; - - MagNumReferences++; - - MagHasNearbyReference = true; + return MagCalibrationMatrix.Transform(rawMag); } -} - SensorFusion::BodyFrameHandler::~BodyFrameHandler() { @@ -427,21 +332,18 @@ bool SensorFusion::BodyFrameHandler::SupportsMessageType(MessageType type) const // cal_name - an optional name for the calibration or default if cal_name == NULL bool SensorFusion::SaveMagCalibration(const char* calibrationName) const { - if (pSensor == NULL || !HasMagCalibration()) + if (CachedSensorInfo.SerialNumber[0] == 0 || !HasMagCalibration()) return false; // A named calibration may be specified for calibration in different // environments, otherwise the default calibration is used if (calibrationName == NULL) calibrationName = "default"; - - SensorInfo sinfo; - pSensor->GetDeviceInfo(&sinfo); // Generate a mag calibration event JSON* calibration = JSON::CreateObject(); // (hardcoded for now) the measurement and representation method - calibration->AddStringItem("Version", "1.0"); + calibration->AddStringItem("Version", "2.0"); calibration->AddStringItem("Name", "default"); // time stamp the calibration @@ -460,19 +362,20 @@ bool SensorFusion::SaveMagCalibration(const char* calibrationName) const calibration->AddStringItem("Time", time_str); // write the full calibration matrix + char matrix[256]; Matrix4f calmat = GetMagCalibration(); - char matrix[128]; - int pos = 0; - for (int r=0; r<4; r++) - { - for (int c=0; c<4; c++) - { - pos += (int)OVR_sprintf(matrix+pos, 128, "%g ", calmat.M[r][c]); - } - } + calmat.ToString(matrix, 256); + calibration->AddStringItem("CalibrationMatrix", matrix); + // save just the offset, for backwards compatibility + // this can be removed when we don't want to support 0.2.4 anymore + Vector3f center(calmat.M[0][3], calmat.M[1][3], calmat.M[2][3]); + Matrix4f tmp = calmat; tmp.M[0][3] = tmp.M[1][3] = tmp.M[2][3] = 0; tmp.M[3][3] = 1; + center = tmp.Inverted().Transform(center); + Matrix4f oldcalmat; oldcalmat.M[0][3] = center.x; oldcalmat.M[1][3] = center.y; oldcalmat.M[2][3] = center.z; + oldcalmat.ToString(matrix, 256); calibration->AddStringItem("Calibration", matrix); - + String path = GetBaseOVRPath(true); path += "/Devices.json"; @@ -482,7 +385,14 @@ bool SensorFusion::SaveMagCalibration(const char* calibrationName) const { // Quick sanity check of the file type and format before we parse it JSON* version = root->GetFirstItem(); if (version && version->Name == "Oculus Device Profile Version") - { // In the future I may need to check versioning to determine parse method + { + int major = atoi(version->Value.ToCStr()); + if (major > MAX_DEVICE_PROFILE_MAJOR_VERSION) + { + // don't use the file on unsupported major version number + root->Release(); + root = NULL; + } } else { @@ -502,7 +412,7 @@ bool SensorFusion::SaveMagCalibration(const char* calibrationName) const if (device->Name == "Device") { JSON* item = device->GetItemByName("Serial"); - if (item && item->Value == sinfo.SerialNumber) + if (item && item->Value == CachedSensorInfo.SerialNumber) { // found an entry for this device item = device->GetNextItem(item); while (item) @@ -543,9 +453,9 @@ bool SensorFusion::SaveMagCalibration(const char* calibrationName) const if (device == NULL) { device = JSON::CreateObject(); - device->AddStringItem("Product", sinfo.ProductName); - device->AddNumberItem("ProductID", sinfo.ProductId); - device->AddStringItem("Serial", sinfo.SerialNumber); + device->AddStringItem("Product", CachedSensorInfo.ProductName); + device->AddNumberItem("ProductID", CachedSensorInfo.ProductId); + device->AddStringItem("Serial", CachedSensorInfo.SerialNumber); device->AddBoolItem("EnableYawCorrection", EnableYawCorrection); root->AddItem("Device", device); @@ -562,16 +472,13 @@ bool SensorFusion::SaveMagCalibration(const char* calibrationName) const // cal_name - an optional name for the calibration or the default if cal_name == NULL bool SensorFusion::LoadMagCalibration(const char* calibrationName) { - if (pSensor == NULL) + if (CachedSensorInfo.SerialNumber[0] == 0) return false; // A named calibration may be specified for calibration in different // environments, otherwise the default calibration is used if (calibrationName == NULL) calibrationName = "default"; - - SensorInfo sinfo; - pSensor->GetDeviceInfo(&sinfo); String path = GetBaseOVRPath(true); path += "/Devices.json"; @@ -584,7 +491,10 @@ bool SensorFusion::LoadMagCalibration(const char* calibrationName) // Quick sanity check of the file type and format before we parse it JSON* version = root->GetFirstItem(); if (version && version->Name == "Oculus Device Profile Version") - { // In the future I may need to check versioning to determine parse method + { + int major = atoi(version->Value.ToCStr()); + if (major > MAX_DEVICE_PROFILE_MAJOR_VERSION) + return false; // don't parse the file on unsupported major version number } else { @@ -600,13 +510,14 @@ bool SensorFusion::LoadMagCalibration(const char* calibrationName) if (device->Name == "Device") { JSON* item = device->GetItemByName("Serial"); - if (item && item->Value == sinfo.SerialNumber) + if (item && item->Value == CachedSensorInfo.SerialNumber) { // found an entry for this device JSON* autoyaw = device->GetItemByName("EnableYawCorrection"); if (autoyaw) autoEnableCorrection = (autoyaw->dValue != 0); + int maxCalibrationVersion = 0; item = device->GetNextItem(item); while (item) { @@ -617,69 +528,66 @@ bool SensorFusion::LoadMagCalibration(const char* calibrationName) if (name && name->Value == calibrationName) { // found a calibration with this name - time_t now; - time(&now); + int major = 0; + JSON* version = calibration->GetItemByName("Version"); + if (version) + major = atoi(version->Value.ToCStr()); - // parse the calibration time - time_t calibration_time = now; - JSON* caltime = calibration->GetItemByName("Time"); - if (caltime) + if (major > maxCalibrationVersion && major <= 2) { - const char* caltime_str = caltime->Value.ToCStr(); + time_t now; + time(&now); - tm ct; - memset(&ct, 0, sizeof(tm)); + // parse the calibration time + time_t calibration_time = now; + JSON* caltime = calibration->GetItemByName("Time"); + if (caltime) + { + const char* caltime_str = caltime->Value.ToCStr(); + + tm ct; + memset(&ct, 0, sizeof(tm)); #ifdef OVR_OS_WIN32 - struct tm nowtime; - localtime_s(&nowtime, &now); - ct.tm_isdst = nowtime.tm_isdst; - sscanf_s(caltime_str, "%d-%d-%d %d:%d:%d", - &ct.tm_year, &ct.tm_mon, &ct.tm_mday, - &ct.tm_hour, &ct.tm_min, &ct.tm_sec); + struct tm nowtime; + localtime_s(&nowtime, &now); + ct.tm_isdst = nowtime.tm_isdst; + sscanf_s(caltime_str, "%d-%d-%d %d:%d:%d", + &ct.tm_year, &ct.tm_mon, &ct.tm_mday, + &ct.tm_hour, &ct.tm_min, &ct.tm_sec); #else - struct tm* nowtime = localtime(&now); - ct.tm_isdst = nowtime->tm_isdst; - sscanf(caltime_str, "%d-%d-%d %d:%d:%d", - &ct.tm_year, &ct.tm_mon, &ct.tm_mday, - &ct.tm_hour, &ct.tm_min, &ct.tm_sec); + struct tm* nowtime = localtime(&now); + ct.tm_isdst = nowtime->tm_isdst; + sscanf(caltime_str, "%d-%d-%d %d:%d:%d", + &ct.tm_year, &ct.tm_mon, &ct.tm_mday, + &ct.tm_hour, &ct.tm_min, &ct.tm_sec); #endif - ct.tm_year -= 1900; - ct.tm_mon--; - calibration_time = mktime(&ct); - } + ct.tm_year -= 1900; + ct.tm_mon--; + calibration_time = mktime(&ct); + } - // parse the calibration matrix - JSON* cal = calibration->GetItemByName("Calibration"); - if (cal) - { - const char* data_str = cal->Value.ToCStr(); - Matrix4f calmat; - for (int r=0; r<4; r++) + // parse the calibration matrix + JSON* cal = calibration->GetItemByName("CalibrationMatrix"); + if (cal == NULL) + cal = calibration->GetItemByName("Calibration"); + + if (cal) { - for (int c=0; c<4; c++) - { - calmat.M[r][c] = (float)atof(data_str); - while (data_str && *data_str != ' ') - data_str++; - - if (data_str) - data_str++; - } - } + Matrix4f calmat = Matrix4f::FromString(cal->Value.ToCStr()); + SetMagCalibration(calmat); + MagCalibrationTime = calibration_time; + EnableYawCorrection = autoEnableCorrection; - SetMagCalibration(calmat); - MagCalibrationTime = calibration_time; - EnableYawCorrection = autoEnableCorrection; - - return true; + maxCalibrationVersion = major; + } } } } item = device->GetNextItem(item); } - break; + return (maxCalibrationVersion > 0); } } diff --git a/LibOVR/Src/OVR_SensorFusion.h b/LibOVR/Src/OVR_SensorFusion.h index c660a86..5847f54 100644 --- a/LibOVR/Src/OVR_SensorFusion.h +++ b/LibOVR/Src/OVR_SensorFusion.h @@ -4,7 +4,7 @@ PublicHeader: OVR.h Filename : OVR_SensorFusion.h Content : Methods that determine head orientation from sensor data over time Created : October 9, 2012 -Authors : Michael Antonov, Steve LaValle +Authors : Michael Antonov, Steve LaValle, Max Katsev Copyright : Copyright 2012 Oculus VR, Inc. All Rights reserved. @@ -43,7 +43,7 @@ class SensorFusion : public NewOverrideBase { enum { - MagMaxReferences = 80 + MagMaxReferences = 1000 }; public: @@ -94,11 +94,6 @@ public: void EnableMotionTracking(bool enable = true) { MotionTrackingEnabled = enable; } bool IsMotionTrackingEnabled() const { return MotionTrackingEnabled; } - // Multiplier for yaw rotation (turning); setting this higher than 1 (the default) can allow the game - // to be played without auxillary rotation controls, possibly making it more immersive. - // Whether this is more or less likely to cause motion sickness is unknown. - float GetYawMultiplier() const { return YawMult; } - void SetYawMultiplier(float y) { YawMult = y; } // *** Prediction Control @@ -124,7 +119,6 @@ public: void SetAccelGain(float ag) { Gain = ag; } - // *** Magnetometer and Yaw Drift Correction Control // Methods to load and save a mag calibration. Calibrations can optionally @@ -140,9 +134,6 @@ public: // Determines if yaw correction is enabled. bool IsYawCorrectionEnabled() const { return EnableYawCorrection;} - // Yaw correction is currently working (forcing a corrective yaw rotation) - bool IsYawCorrectionInProgress() const { return YawCorrectionInProgress;} - // Store the calibration matrix for the magnetometer void SetMagCalibration(const Matrix4f& m) { @@ -163,14 +154,10 @@ public: // These refer to reference points that associate mag readings with orientations void ClearMagReferences() { MagNumReferences = 0; } - void SetMagRefDistance(const float d) { MagRefDistance = d; } Vector3f GetCalibratedMagValue(const Vector3f& rawMag) const; - float GetMagRefYaw() const { return MagRefYaw; } - float GetYawErrorAngle() const { return YawErrorAngle; } - // *** Message Handler Logic @@ -220,7 +207,7 @@ private: virtual bool SupportsMessageType(MessageType type) const; }; - Ptr<SensorDevice> pSensor; + SensorInfo CachedSensorInfo; Quatf Q; Quatf QUncorrected; @@ -234,7 +221,6 @@ private: BodyFrameHandler Handler; MessageHandler* pDelegate; float Gain; - float YawMult; volatile bool EnableGravity; bool EnablePrediction; @@ -242,31 +228,21 @@ private: float PredictionTimeIncrement; SensorFilter FRawMag; - SensorFilter FAccW; SensorFilter FAngV; - int TiltCondCount; - float TiltErrorAngle; - Vector3f TiltErrorAxis; + Vector3f GyroOffset; + SensorFilterBase<float> TiltAngleFilter; + bool EnableYawCorrection; + bool MagCalibrated; Matrix4f MagCalibrationMatrix; time_t MagCalibrationTime; - bool MagCalibrated; - int MagCondCount; - float MagRefDistance; - Quatf MagRefQ; - Vector3f MagRefM; - float MagRefYaw; - bool MagHasNearbyReference; - Quatf MagRefTableQ[MagMaxReferences]; - Vector3f MagRefTableM[MagMaxReferences]; - float MagRefTableYaw[MagMaxReferences]; int MagNumReferences; - float YawErrorAngle; - int YawErrorCount; - bool YawCorrectionInProgress; - bool YawCorrectionActivated; + Vector3f MagRefsInBodyFrame[MagMaxReferences]; + Vector3f MagRefsInWorldFrame[MagMaxReferences]; + int MagRefIdx; + int MagRefScore; bool MotionTrackingEnabled; }; diff --git a/LibOVR/Src/OVR_SensorImpl.cpp b/LibOVR/Src/OVR_SensorImpl.cpp index ced6541..c54bccb 100644 --- a/LibOVR/Src/OVR_SensorImpl.cpp +++ b/LibOVR/Src/OVR_SensorImpl.cpp @@ -32,6 +32,8 @@ enum { Sensor_OldVendorId = 0x0483, Sensor_OldProductId = 0x5750, + Sensor_BootLoader = 0x1001, + Sensor_DefaultReportRate = 500, // Hz Sensor_MaxReportRate = 1000 // Hz }; @@ -400,6 +402,16 @@ void SensorDeviceFactory::EnumerateDevices(EnumerateVisitor& visitor) virtual void Visit(HIDDevice& device, const HIDDeviceDesc& desc) { + + if (desc.ProductId == Sensor_BootLoader) + { // If we find a sensor in boot loader mode then notify the app + // about the existence of the device, but don't allow the app + // to create or access the device + BootLoaderDeviceCreateDesc createDesc(pFactory, desc); + ExternalVisitor.Visit(createDesc); + return; + } + SensorDeviceCreateDesc createDesc(pFactory, desc); ExternalVisitor.Visit(createDesc); @@ -433,16 +445,29 @@ void SensorDeviceFactory::EnumerateDevices(EnumerateVisitor& visitor) bool SensorDeviceFactory::MatchVendorProduct(UInt16 vendorId, UInt16 productId) const { + // search for a tracker sensor or a tracker boot loader device return ((vendorId == Sensor_VendorId) && (productId == Sensor_ProductId)) || - ((vendorId == Sensor_OldVendorId) && (productId == Sensor_OldProductId)); + ((vendorId == Sensor_OldVendorId) && (productId == Sensor_OldProductId)) || + ((vendorId == Sensor_VendorId) && (productId == Sensor_BootLoader)); } bool SensorDeviceFactory::DetectHIDDevice(DeviceManager* pdevMgr, const HIDDeviceDesc& desc) { if (MatchVendorProduct(desc.VendorId, desc.ProductId)) { - SensorDeviceCreateDesc createDesc(this, desc); - return pdevMgr->AddDevice_NeedsLock(createDesc).GetPtr() != NULL; + if (desc.ProductId == Sensor_BootLoader) + { // If we find a sensor in boot loader mode then notify the app + // about the existence of the device, but don't allow them + // to create or access the device + BootLoaderDeviceCreateDesc createDesc(this, desc); + pdevMgr->AddDevice_NeedsLock(createDesc); + return false; // return false to allow upstream boot loader factories to catch the device + } + else + { + SensorDeviceCreateDesc createDesc(this, desc); + return pdevMgr->AddDevice_NeedsLock(createDesc).GetPtr() != NULL; + } } return false; } @@ -464,20 +489,19 @@ bool SensorDeviceCreateDesc::GetDeviceInfo(DeviceInfo* info) const OVR_strcpy(info->ProductName, DeviceInfo::MaxNameLength, HIDDesc.Product.ToCStr()); OVR_strcpy(info->Manufacturer, DeviceInfo::MaxNameLength, HIDDesc.Manufacturer.ToCStr()); info->Type = Device_Sensor; - info->Version = 0; if (info->InfoClassType == Device_Sensor) { SensorInfo* sinfo = (SensorInfo*)info; sinfo->VendorId = HIDDesc.VendorId; sinfo->ProductId = HIDDesc.ProductId; + sinfo->Version = HIDDesc.VersionNumber; sinfo->MaxRanges = SensorRangeImpl::GetMaxSensorRange(); OVR_strcpy(sinfo->SerialNumber, sizeof(sinfo->SerialNumber),HIDDesc.SerialNumber.ToCStr()); } return true; } - //------------------------------------------------------------------------------------- // ***** SensorDevice @@ -526,6 +550,9 @@ void SensorDeviceImpl::openDevice() { sr.Unpack(); sr.GetSensorRange(&CurrentRange); + // Increase the magnetometer range, since the default value is not enough in practice + CurrentRange.MaxMagneticField = 2.5f; + setRange(CurrentRange); } diff --git a/LibOVR/Src/OVR_SensorImpl.h b/LibOVR/Src/OVR_SensorImpl.h index 6ad1628..4bdcc4f 100644 --- a/LibOVR/Src/OVR_SensorImpl.h +++ b/LibOVR/Src/OVR_SensorImpl.h @@ -70,12 +70,58 @@ public: { // should paths comparison be case insensitive? return ((HIDDesc.Path.CompareNoCase(hidDesc.Path) == 0) && - (HIDDesc.SerialNumber == hidDesc.SerialNumber)); + (HIDDesc.SerialNumber == hidDesc.SerialNumber) && + (HIDDesc.VersionNumber == hidDesc.VersionNumber)); } virtual bool GetDeviceInfo(DeviceInfo* info) const; }; +// A simple stub for notification of a sensor in Boot Loader mode +// This descriptor does not support the creation of a device, only the detection +// of its existence to warn apps that the sensor device needs firmware. +// The Boot Loader descriptor reuses and is created by the Sensor device factory +// but in the future may use a dedicated factory +class BootLoaderDeviceCreateDesc : public HIDDeviceCreateDesc +{ +public: + BootLoaderDeviceCreateDesc(DeviceFactory* factory, const HIDDeviceDesc& hidDesc) + : HIDDeviceCreateDesc(factory, Device_BootLoader, hidDesc) { } + + virtual DeviceCreateDesc* Clone() const + { + return new BootLoaderDeviceCreateDesc(*this); + } + + // Boot Loader device creation is not allowed + virtual DeviceBase* NewDeviceInstance() { return NULL; }; + + virtual MatchResult MatchDevice(const DeviceCreateDesc& other, + DeviceCreateDesc**) const + { + if ((other.Type == Device_BootLoader) && (pFactory == other.pFactory)) + { + const BootLoaderDeviceCreateDesc& s2 = (const BootLoaderDeviceCreateDesc&) other; + if (MatchHIDDevice(s2.HIDDesc)) + return Match_Found; + } + return Match_None; + } + + virtual bool MatchHIDDevice(const HIDDeviceDesc& hidDesc) const + { + // should paths comparison be case insensitive? + return ((HIDDesc.Path.CompareNoCase(hidDesc.Path) == 0) && + (HIDDesc.SerialNumber == hidDesc.SerialNumber)); + } + + virtual bool GetDeviceInfo(DeviceInfo* info) const + { + OVR_UNUSED(info); + return false; + } +}; + //------------------------------------------------------------------------------------- // ***** OVR::SensorDisplayInfoImpl diff --git a/OculusConfigurationUtility.sh b/OculusConfigurationUtility.sh index aeafec3..a70d7f6 100755 --- a/OculusConfigurationUtility.sh +++ b/OculusConfigurationUtility.sh @@ -20,7 +20,9 @@ MACHINE_TYPE=`uname -m` if [ ${MACHINE_TYPE} == 'x86_64' ]; then ./Tools/OculusConfigUtil/OculusConfigUtil_x86_64 +elif [ ${MACHINE_TYPE} == 'i686' ]; then + ./Tools/OculusConfigUtil/OculusConfigUtil_i386 else - echo "The Oculus Configuration Utility is currently only supported on 64-bit Linux." + echo "The Oculus Configuration Utility does not currently support this platform." fi diff --git a/Samples/CommonSrc/Platform/Win32_Platform.cpp b/Samples/CommonSrc/Platform/Win32_Platform.cpp index 9be7196..eeab429 100644 --- a/Samples/CommonSrc/Platform/Win32_Platform.cpp +++ b/Samples/CommonSrc/Platform/Win32_Platform.cpp @@ -543,6 +543,7 @@ Render::DisplayId PlatformCore::GetDisplay(int screen) }}} +OVR::Platform::Application* g_app; int WINAPI WinMain(HINSTANCE hinst, HINSTANCE prevInst, LPSTR inArgs, int show) { @@ -552,10 +553,10 @@ int WINAPI WinMain(HINSTANCE hinst, HINSTANCE prevInst, LPSTR inArgs, int show) OVR_UNUSED2(prevInst, show); // CreateApplication must be the first call since it does OVR::System::Initialize. - Application* app = Application::CreateApplication(); - Win32::PlatformCore* platform = new Win32::PlatformCore(app, hinst); + g_app = Application::CreateApplication(); + Win32::PlatformCore* platform = new Win32::PlatformCore(g_app, hinst); // The platform attached to an app will be deleted by DestroyApplication. - app->SetPlatformCore(platform); + g_app->SetPlatformCore(platform); int exitCode = 0; @@ -586,14 +587,14 @@ int WINAPI WinMain(HINSTANCE hinst, HINSTANCE prevInst, LPSTR inArgs, int show) for (UPInt i = 0; i < args.GetSize(); i++) argv.PushBack(args[i].ToCStr()); - exitCode = app->OnStartup((int)argv.GetSize(), &argv[0]); + exitCode = g_app->OnStartup((int)argv.GetSize(), &argv[0]); if (!exitCode) exitCode = platform->Run(); } // No OVR functions involving memory are allowed after this. - Application::DestroyApplication(app); - app = 0; + Application::DestroyApplication(g_app); + g_app = 0; OVR_DEBUG_STATEMENT(_CrtDumpMemoryLeaks()); return exitCode; diff --git a/Samples/CommonSrc/Render/Render_XmlSceneLoader.cpp b/Samples/CommonSrc/Render/Render_XmlSceneLoader.cpp index a871f35..b2b1ac1 100644 --- a/Samples/CommonSrc/Render/Render_XmlSceneLoader.cpp +++ b/Samples/CommonSrc/Render/Render_XmlSceneLoader.cpp @@ -239,15 +239,13 @@ bool XmlHandler::ReadFile(const char* fileName, OVR::Render::RenderDevice* pRend FirstChild()->ToText()->Value(); UPInt stringLength = strlen(indexStr); - for(UPInt j = 0; j < stringLength;) + for(UPInt j = 0; j < stringLength; ) { UPInt k = j + 1; for(; k < stringLength; ++k) { - if(indexStr[k] == ' ') - { - break; - } + if (indexStr[k] == ' ') + break; } char text[20]; for(UPInt l = 0; l < k - j; ++l) @@ -255,10 +253,22 @@ bool XmlHandler::ReadFile(const char* fileName, OVR::Render::RenderDevice* pRend text[l] = indexStr[j + l]; } text[k - j] = '\0'; - Models[i]->Indices.InsertAt(0, (unsigned short)atoi(text)); + + Models[i]->Indices.PushBack((unsigned short)atoi(text)); j = k + 1; } + // Reverse index order to match original expected orientation + Array<UInt16>& indices = Models[i]->Indices; + UPInt indexCount = indices.GetSize(); + + for (UPInt revIndex = 0; revIndex < indexCount/2; revIndex++) + { + unsigned short itemp = indices[revIndex]; + indices[revIndex] = indices[indexCount - revIndex - 1]; + indices[indexCount - revIndex - 1] = itemp; + } + delete vertices; delete normals; delete diffuseUVs; diff --git a/Samples/OculusWorldDemo/OculusWorldDemo.cpp b/Samples/OculusWorldDemo/OculusWorldDemo.cpp index 613cdac..1a81c8d 100644 --- a/Samples/OculusWorldDemo/OculusWorldDemo.cpp +++ b/Samples/OculusWorldDemo/OculusWorldDemo.cpp @@ -122,6 +122,9 @@ public: void AdjustDistortionK2(float dt) { AdjustDistortion(dt, 2, "K2"); } void AdjustDistortionK3(float dt) { AdjustDistortion(dt, 3, "K3"); } + void AdjustDistortion(float val, int kIndex); + void AdjustEsd(float val); + // Adds room model to scene. void PopulateScene(const char* fileName); void PopulatePreloadScene(); @@ -137,10 +140,6 @@ protected: int Screen; int FirstScreenInCycle; - // Magnetometer calibration and yaw correction - Util::MagCalibration MagCal; - bool MagAwaitingForwardLook; - // *** Oculus HMD Variables Ptr<DeviceManager> pManager; Ptr<SensorDevice> pSensor; @@ -253,11 +252,6 @@ protected: void RaiseLOD(); void CycleDisplay(); void GamepadStateChanged(const GamepadState& pad); - - // Variable used by UpdateManualCalibration - Anglef FirstMagYaw; - int ManualMagCalStage; - int ManualMagFailures; }; //------------------------------------------------------------------------------------- @@ -679,9 +673,6 @@ void OculusWorldDemoApp::OnKey(OVR::KeyCode key, int chr, bool down, int modifie case Key_R: SFusion.Reset(); - if (MagCal.IsAutoCalibrating() || MagCal.IsManuallyCalibrating()) - MagCal.AbortCalibration(); - SetAdjustMessage("Sensor Fusion Reset"); break; @@ -742,19 +733,11 @@ void OculusWorldDemoApp::OnKey(OVR::KeyCode key, int chr, bool down, int modifie case Key_Escape: if(!down) { - if (MagCal.IsAutoCalibrating() || MagCal.IsManuallyCalibrating()) - { - MagCal.AbortCalibration(); - SetAdjustMessage("Aborting Magnetometer Calibration"); - } - else - { - // switch to primary screen windowed mode - pPlatform->SetFullscreen(RenderParams, Display_Window); - RenderParams.Display = pPlatform->GetDisplay(0); - pRender->SetParams(RenderParams); - Screen = 0; - } + // switch to primary screen windowed mode + pPlatform->SetFullscreen(RenderParams, Display_Window); + RenderParams.Display = pPlatform->GetDisplay(0); + pRender->SetParams(RenderParams); + Screen = 0; } break; @@ -952,27 +935,28 @@ void OculusWorldDemoApp::OnKey(OVR::KeyCode key, int chr, bool down, int modifie DropLOD(); break; */ - // Start calibrating magnetometer - case Key_Z: - if (down) - { - ManualMagCalStage = 0; - if (MagCal.IsManuallyCalibrating()) - MagAwaitingForwardLook = false; - else - { - MagCal.BeginManualCalibration(SFusion); - MagAwaitingForwardLook = true; - } - } - break; - case Key_X: - if (down) - { - MagCal.BeginAutoCalibration(SFusion); - SetAdjustMessage("Starting Auto Mag Calibration"); - } + // Cycle through drift correction options + case Key_Z: + if (down) + { + if (SFusion.IsYawCorrectionEnabled()) + { + SFusion.SetGravityEnabled(false); + SFusion.SetYawCorrectionEnabled(false); + } + else if (SFusion.IsGravityEnabled()) + { + SFusion.SetYawCorrectionEnabled(true); + } + else + { + SFusion.SetGravityEnabled(true); + } + SetAdjustMessage("Tilt Correction %s\nYaw Correction %s", + SFusion.IsGravityEnabled() ? "On" : "Off", + SFusion.IsYawCorrectionEnabled() ? "On" : "Off"); + } break; // Show view of yaw angles (for mag calibration/analysis) @@ -1093,11 +1077,21 @@ void OculusWorldDemoApp::OnIdle() if (!pSensor) { pSensor = *desc.Handle.CreateDeviceTyped<SensorDevice>(); - SFusion.AttachToSensor(pSensor); - - SetAdjustMessage("---------------------------\n" - "SENSOR connected\n" - "---------------------------"); + if (pSensor) + { + SFusion.AttachToSensor(pSensor); + + SetAdjustMessage("---------------------------\n" + "SENSOR connected\n" + "---------------------------"); + } + else + { + SetAdjustMessage("----------------------------\n" + "SENSOR connect failed\n" + "Unplug and reconnect it.\n" + "----------------------------"); + } } else if (!wasAlreadyCreated) { @@ -1200,27 +1194,6 @@ void OculusWorldDemoApp::OnIdle() (this->*pAdjustFunc)(dt * AdjustDirection * (ShiftDown ? 5.0f : 1.0f)); } - // Magnetometer calibration procedure - if (MagCal.IsManuallyCalibrating()) - UpdateManualMagCalibration(); - - if (MagCal.IsAutoCalibrating()) - { - MagCal.UpdateAutoCalibration(SFusion); - int n = MagCal.NumberOfSamples(); - if (n == 1) - SetAdjustMessage(" Magnetometer Calibration Has 1 Sample \n %d Remaining - Please Keep Looking Around ",4-n); - else if (n < 4) - SetAdjustMessage(" Magnetometer Calibration Has %d Samples \n %d Remaining - Please Keep Looking Around ",n,4-n); - if (MagCal.IsCalibrated()) - { - SFusion.SetYawCorrectionEnabled(true); - Vector3f mc = MagCal.GetMagCenter(); - SetAdjustMessage(" Magnetometer Calibration Complete \nCenter: %f %f %f",mc.x,mc.y,mc.z); - } - } - - // Process latency tester results. const char* results = LatencyUtil.GetResultsString(); if (results != NULL) @@ -1341,127 +1314,10 @@ void OculusWorldDemoApp::OnIdle() } -void OculusWorldDemoApp::UpdateManualMagCalibration() -{ - float tyaw, pitch, roll; - Anglef yaw; - Quatf hmdOrient = SFusion.GetOrientation(); - hmdOrient.GetEulerAngles<Axis_Y, Axis_X, Axis_Z>(&tyaw, &pitch, &roll); - Vector3f mag = SFusion.GetMagnetometer(); - float dtr = Math<float>::DegreeToRadFactor; - yaw.Set(tyaw); // Using Angle class to handle angle wraparound arithmetic - - const int timeout = 100; - - switch(ManualMagCalStage) - { - case 0: - if (MagAwaitingForwardLook) - SetAdjustMessage("Magnetometer Calibration\n** Step 1: Please Look Forward **\n** and Press Z When Ready **"); - else - if (fabs(pitch) < 10.0f*dtr) - { - MagCal.InsertIfAcceptable(hmdOrient, mag); - FirstMagYaw = yaw; - MagAwaitingForwardLook = false; - if (MagCal.NumberOfSamples() == 1) - { - ManualMagCalStage = 1; - ManualMagFailures = 0; - } - } - else - MagAwaitingForwardLook = true; - break; - case 1: - SetAdjustMessage("Magnetometer Calibration\n** Step 2: Please Look Up **"); - yaw -= FirstMagYaw; - if ((pitch > 50.0f*dtr) && (yaw.Abs() < 20.0f*dtr)) - { - MagCal.InsertIfAcceptable(hmdOrient, mag); - ManualMagFailures++; - if ((MagCal.NumberOfSamples() == 2)||(ManualMagFailures > timeout)) - { - ManualMagCalStage = 2; - ManualMagFailures = 0; - } - } - break; - case 2: - SetAdjustMessage("Magnetometer Calibration\n** Step 3: Please Look Left **"); - yaw -= FirstMagYaw; - if (yaw.Get() > 60.0f*dtr) - { - MagCal.InsertIfAcceptable(hmdOrient, mag); - ManualMagFailures++; - if ((MagCal.NumberOfSamples() == 3)||(ManualMagFailures > timeout)) - { - ManualMagCalStage = 3; - ManualMagFailures = 0; - } - } - break; - case 3: - SetAdjustMessage("Magnetometer Calibration\n** Step 4: Please Look Right **"); - yaw -= FirstMagYaw; - if (yaw.Get() < -60.0f*dtr) - { - MagCal.InsertIfAcceptable(hmdOrient, mag); - ManualMagFailures++; - if (MagCal.NumberOfSamples() == 4) - ManualMagCalStage = 6; - else - { - if (ManualMagFailures > timeout) - { - ManualMagCalStage = 4; - ManualMagFailures = 0; - } - } - } - break; - case 4: - SetAdjustMessage("Magnetometer Calibration\n** Step 5: Please Look Upper Right **"); - yaw -= FirstMagYaw; - if ((yaw.Get() < -50.0f*dtr) && (pitch > 40.0f*dtr)) - { - MagCal.InsertIfAcceptable(hmdOrient, mag); - if (MagCal.NumberOfSamples() == 4) - ManualMagCalStage = 6; - else - { - if (ManualMagFailures > timeout) - { - ManualMagCalStage = 5; - ManualMagFailures = 0; - } - else - ManualMagFailures++; - } - } - break; - case 5: - SetAdjustMessage("Calibration Failed\n** Try Again From Another Location **"); - MagCal.AbortCalibration(); - break; - case 6: - if (!MagCal.IsCalibrated()) - { - MagCal.SetCalibration(SFusion); - SFusion.SetYawCorrectionEnabled(true); - Vector3f mc = MagCal.GetMagCenter(); - SetAdjustMessage(" Magnetometer Calibration and Activation \nCenter: %f %f %f", - mc.x,mc.y,mc.z); - } - } -} - - - -static const char* HelpText = - "F1 \t100 NoStereo \t420 Z \t520 Manual Mag Calib\n" - "F2 \t100 Stereo \t420 X \t520 Auto Mag Calib\n" - "F3 \t100 StereoHMD \t420 F6 \t520 Yaw Drift Info\n" +static const char* HelpText = + "F1 \t100 NoStereo\n" + "F2 \t100 Stereo \t420 Z \t520 Drift Correction\n" + "F3 \t100 StereoHMD \t420 F6 \t520 Yaw Drift Info\n" "F4 \t100 MSAA \t420 R \t520 Reset SensorFusion\n" "F9 \t100 FullScreen \t420\n" "F11 \t100 Fast FullScreen \t500 - + \t660 Adj EyeHeight\n" @@ -1518,20 +1374,11 @@ void OculusWorldDemoApp::Render(const StereoEyeParams& stereo) if (SceneMode == Scene_YawView) { - Matrix4f calView = Matrix4f(); - float viewYaw = -ThePlayer.LastSensorYaw + SFusion.GetMagRefYaw(); - calView.M[0][0] = calView.M[2][2] = cos(viewYaw); - calView.M[0][2] = sin(viewYaw); - calView.M[2][0] = -sin(viewYaw); - //LogText("yaw: %f\n",SFusion.GetMagRefYaw()); - - if (SFusion.IsYawCorrectionInProgress()) - YawMarkGreenScene.Render(pRender, stereo.ViewAdjust); - else - YawMarkRedScene.Render(pRender, stereo.ViewAdjust); - - if (fabs(ThePlayer.EyePitch) < Math<float>::Pi * 0.33) - YawLinesScene.Render(pRender, stereo.ViewAdjust * calView); + Matrix4f trackerOnlyOrient = Matrix4f::RotationY(ThePlayer.LastSensorYaw) + * Matrix4f::RotationX(ThePlayer.EyePitch) + * Matrix4f::RotationZ(ThePlayer.EyeRoll); + YawLinesScene.Render(pRender, stereo.ViewAdjust * trackerOnlyOrient.Inverted()); + //YawMarkRedScene.Render(pRender, stereo.ViewAdjust); } // *** 2D Text & Grid - Configure Orthographic rendering. @@ -1702,6 +1549,18 @@ void OculusWorldDemoApp::AdjustMotionPrediction(float dt) SetAdjustMessage("MotionPrediction: %6.3fs", motionPred); } +void OculusWorldDemoApp::AdjustDistortion(float val, int kIndex) +{ + SConfig.SetDistortionK(kIndex, val); + SetAdjustMessage("K%d: %6.4f", kIndex, SConfig.GetDistortionK(kIndex)); +} + +void OculusWorldDemoApp::AdjustEsd(float val) +{ + SConfig.SetEyeToScreenDistance(val); + float esd = SConfig.GetEyeToScreenDistance(); + SetAdjustMessage("ESD:%6.3f FOV: %6.3f", esd, SConfig.GetYFOVDegrees()); +} // Loads the scene data void OculusWorldDemoApp::PopulateScene(const char *fileName) @@ -1728,30 +1587,19 @@ void OculusWorldDemoApp::PopulateScene(const char *fileName) Ptr<Model> yawMarkRedModel = *Model::CreateBox(Color(255, 0, 0, 255), Vector3f(0.0f, shifty, -2.0f), Vector3f(0.05f, 0.05f, 0.05f)); YawMarkRedScene.World.Add(yawMarkRedModel); - Ptr<Model> yawLinesModel = *new Model(Prim_Lines); - float r = 2.0f; - float theta0 = Math<float>::PiOver2; - float theta1 = 0.0f; + Ptr<Model> yawLinesModel = *new Model(); Color c = Color(255, 200, 200, 255); - for (int i = 0; i < 35; i++) - { - theta1 = theta0 + Math<float>::Pi / 18.0f; - yawLinesModel->AddLine(yawLinesModel->AddVertex(Vector3f(r*cos(theta0),shifty,-r*sin(theta0)),c), - yawLinesModel->AddVertex(Vector3f(r*cos(theta1),shifty,-r*sin(theta1)),c)); - theta0 = theta1; - yawLinesModel->AddLine(yawLinesModel->AddVertex(Vector3f(r*cos(theta0),shifty,-r*sin(theta0)),c), - yawLinesModel->AddVertex(Vector3f(r*cos(theta0),shifty+0.1f,-r*sin(theta0)),c)); - theta0 = theta1; - } - theta1 = theta0 + Math<float>::Pi / 18.0f; - yawLinesModel->AddLine(yawLinesModel->AddVertex(Vector3f(r*cos(theta0),shifty,-r*sin(theta0)),c), - yawLinesModel->AddVertex(Vector3f(r*cos(theta1),shifty,-r*sin(theta1)),c)); - yawLinesModel->AddLine(yawLinesModel->AddVertex(Vector3f(0.0f,shifty+0.1f,-r),c), - yawLinesModel->AddVertex(Vector3f(r*sin(0.02f),shifty,-r*cos(0.02f)),c)); - yawLinesModel->AddLine(yawLinesModel->AddVertex(Vector3f(0.0f,shifty+0.1f,-r),c), - yawLinesModel->AddVertex(Vector3f(r*sin(-0.02f),shifty,-r*cos(-0.02f)),c)); - yawLinesModel->SetPosition(Vector3f(0.0f,0.0f,0.0f)); - + float r = 1.5f; + yawLinesModel->AddTriangle(yawLinesModel->AddVertex(Vector3f(-0.1f, 0, -r), c), + yawLinesModel->AddVertex(Vector3f(0, 0, -r-0.2f), c), + yawLinesModel->AddVertex(Vector3f(0.1f, 0, -r), c)); + yawLinesModel->AddTriangle(yawLinesModel->AddVertex(Vector3f(-r-0.1f, 0, -r), c), + yawLinesModel->AddVertex(Vector3f(-r, 0, -r-0.2f), c), + yawLinesModel->AddVertex(Vector3f(-r+0.1f, 0, -r), c)); + yawLinesModel->AddTriangle(yawLinesModel->AddVertex(Vector3f(r-0.1f, 0, -r), c), + yawLinesModel->AddVertex(Vector3f(r, 0, -r-0.2f), c), + yawLinesModel->AddVertex(Vector3f(r+0.1f, 0, -r), c)); + yawLinesModel->SetPosition(Vector3f(0.0f,-1.2f,0.0f)); YawLinesScene.World.Add(yawLinesModel); } diff --git a/Samples/OculusWorldDemo/Player.cpp b/Samples/OculusWorldDemo/Player.cpp index 6422920..b910307 100644 --- a/Samples/OculusWorldDemo/Player.cpp +++ b/Samples/OculusWorldDemo/Player.cpp @@ -124,7 +124,7 @@ void Player::HandleCollision(double dt, Array<Ptr<CollisionModel> >* collisionMo { // Project orientationVector onto the plane Vector3f slideVector = orientationVector - collisionPlaneForward.N - * (orientationVector * collisionPlaneForward.N); + * (orientationVector.Dot(collisionPlaneForward.N)); // Make sure we aren't in a corner for(unsigned int j = 0; j < collisionModels->GetSize(); ++j) |