Adding Linux 0.2.5 changes

19 files changed, 1257 insertions, 785 deletions

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