summaryrefslogtreecommitdiffstats
path: root/LibOVR
diff options
context:
space:
mode:
authorBrad Davis <[email protected]>2013-10-10 23:37:45 -0700
committerBrad Davis <[email protected]>2013-10-10 23:37:45 -0700
commit881daef0cb99477da21235c8d0924de15990441b (patch)
tree26b3393d138711244b3b9ea7ae95980c76ce1d9f /LibOVR
parente88dfc0fb802102d27d372457299761b39f05fb9 (diff)
parent5eb50183c96f699557f071878efb6e18df24727d (diff)
Merging in official SDK 0.2.5 changes
Diffstat (limited to 'LibOVR')
-rw-r--r--LibOVR/Include/OVR.h1
-rw-r--r--LibOVR/Include/OVRVersion.h4
-rw-r--r--LibOVR/Projects/Win32/LibOVR_Msvc2010.vcxproj2
-rw-r--r--LibOVR/Projects/Win32/LibOVR_Msvc2010.vcxproj.filters6
-rw-r--r--LibOVR/Src/Kernel/OVR_Atomic.h1
-rw-r--r--LibOVR/Src/Kernel/OVR_FileFILE.cpp2
-rw-r--r--LibOVR/Src/Kernel/OVR_Math.cpp56
-rw-r--r--LibOVR/Src/Kernel/OVR_Math.h523
-rw-r--r--LibOVR/Src/Kernel/OVR_SysFile.h2
-rw-r--r--LibOVR/Src/Kernel/OVR_Timer.h2
-rw-r--r--LibOVR/Src/OVR_DeviceConstants.h1
-rw-r--r--LibOVR/Src/OVR_LatencyTestImpl.cpp2
-rw-r--r--LibOVR/Src/OVR_Linux_HIDDevice.cpp5
-rw-r--r--LibOVR/Src/OVR_OSX_HIDDevice.cpp7
-rw-r--r--LibOVR/Src/OVR_OSX_HMDDevice.cpp2
-rw-r--r--LibOVR/Src/OVR_Profile.cpp293
-rw-r--r--LibOVR/Src/OVR_Profile.h124
-rw-r--r--LibOVR/Src/OVR_SensorFilter.cpp118
-rw-r--r--LibOVR/Src/OVR_SensorFilter.h193
-rw-r--r--LibOVR/Src/OVR_SensorFusion.cpp580
-rw-r--r--LibOVR/Src/OVR_SensorFusion.h46
-rw-r--r--LibOVR/Src/OVR_SensorImpl.cpp37
-rw-r--r--LibOVR/Src/OVR_SensorImpl.h48
23 files changed, 1259 insertions, 796 deletions
diff --git a/LibOVR/Include/OVR.h b/LibOVR/Include/OVR.h
index 0308b9f..c832338 100644
--- a/LibOVR/Include/OVR.h
+++ b/LibOVR/Include/OVR.h
@@ -28,7 +28,6 @@ otherwise accompanies this software in either electronic or hard copy form.
#include "../Src/OVR_Profile.h"
#include "../Src/Util/Util_LatencyTest.h"
#include "../Src/Util/Util_Render_Stereo.h"
-#include "../Src/Util/Util_MagCalibration.h"
#endif
diff --git a/LibOVR/Include/OVRVersion.h b/LibOVR/Include/OVRVersion.h
index 453db75..a1c7ea6 100644
--- a/LibOVR/Include/OVRVersion.h
+++ b/LibOVR/Include/OVRVersion.h
@@ -16,7 +16,7 @@ otherwise accompanies this software in either electronic or hard copy form.
#define OVR_MAJOR_VERSION 0
#define OVR_MINOR_VERSION 2
-#define OVR_BUILD_VERSION 4
-#define OVR_VERSION_STRING "0.2.4"
+#define OVR_BUILD_VERSION 5
+#define OVR_VERSION_STRING "0.2.5"
#endif
diff --git a/LibOVR/Projects/Win32/LibOVR_Msvc2010.vcxproj b/LibOVR/Projects/Win32/LibOVR_Msvc2010.vcxproj
index 6f1539f..e16eaf4 100644
--- a/LibOVR/Projects/Win32/LibOVR_Msvc2010.vcxproj
+++ b/LibOVR/Projects/Win32/LibOVR_Msvc2010.vcxproj
@@ -63,7 +63,6 @@
<ClInclude Include="..\..\Src\OVR_Win32_HIDDevice.h" />
<ClInclude Include="..\..\Src\OVR_Win32_HMDDevice.h" />
<ClInclude Include="..\..\Src\OVR_Win32_SensorDevice.h" />
- <ClInclude Include="..\..\Src\Util\Util_MagCalibration.h" />
<ClInclude Include="..\..\Src\Util\Util_Render_Stereo.h" />
</ItemGroup>
<ItemGroup>
@@ -99,7 +98,6 @@
<ClCompile Include="..\..\Src\OVR_Win32_HMDDevice.cpp" />
<ClCompile Include="..\..\Src\Util\Util_LatencyTest.cpp" />
<ClCompile Include="..\..\Src\OVR_Win32_SensorDevice.cpp" />
- <ClCompile Include="..\..\Src\Util\Util_MagCalibration.cpp" />
<ClCompile Include="..\..\Src\Util\Util_Render_Stereo.cpp" />
</ItemGroup>
<PropertyGroup Label="Globals">
diff --git a/LibOVR/Projects/Win32/LibOVR_Msvc2010.vcxproj.filters b/LibOVR/Projects/Win32/LibOVR_Msvc2010.vcxproj.filters
index 7192960..e6d1dcf 100644
--- a/LibOVR/Projects/Win32/LibOVR_Msvc2010.vcxproj.filters
+++ b/LibOVR/Projects/Win32/LibOVR_Msvc2010.vcxproj.filters
@@ -69,9 +69,6 @@
<ClCompile Include="..\..\Src\OVR_LatencyTestImpl.cpp" />
<ClCompile Include="..\..\Src\OVR_SensorImpl.cpp" />
<ClCompile Include="..\..\Src\OVR_Win32_SensorDevice.cpp" />
- <ClCompile Include="..\..\Src\Util\Util_MagCalibration.cpp">
- <Filter>Util</Filter>
- </ClCompile>
<ClCompile Include="..\..\Src\OVR_SensorFilter.cpp" />
<ClCompile Include="..\..\Src\OVR_JSON.cpp" />
<ClCompile Include="..\..\Src\OVR_Profile.cpp" />
@@ -169,9 +166,6 @@
<ClInclude Include="..\..\Src\OVR_SensorImpl.h" />
<ClInclude Include="..\..\Src\OVR_HIDDeviceBase.h" />
<ClInclude Include="..\..\Src\OVR_Win32_SensorDevice.h" />
- <ClInclude Include="..\..\Src\Util\Util_MagCalibration.h">
- <Filter>Util</Filter>
- </ClInclude>
<ClInclude Include="..\..\Src\OVR_SensorFilter.h" />
<ClInclude Include="..\..\Src\OVR_JSON.h" />
<ClInclude Include="..\..\Src\OVR_Profile.h" />
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