0.4 Win-Beta0.4.0

4 files changed, 587 insertions, 0 deletions

diff --git a/LibOVR/Src/Tracking/Tracking_PoseState.h b/LibOVR/Src/Tracking/Tracking_PoseState.h
new file mode 100644
index 0000000..c22395c
--- /dev/null
+++ b/LibOVR/Src/Tracking/Tracking_PoseState.h
@@ -0,0 +1,125 @@
+/************************************************************************************
+
+Filename    :   Tracking_PoseState.h
+Content     :   Describes the complete pose at a point in time, including derivatives
+Created     :   May 13, 2014
+Authors     :   Dov Katz
+
+Copyright   :   Copyright 2014 Oculus VR, Inc. All Rights reserved.
+
+Licensed under the Oculus VR Rift SDK License Version 3.1 (the "License");
+you may not use the Oculus VR Rift SDK except in compliance with the License,
+which is provided at the time of installation or download, or which
+otherwise accompanies this software in either electronic or hard copy form.
+
+You may obtain a copy of the License at
+
+http://www.oculusvr.com/licenses/LICENSE-3.1
+
+Unless required by applicable law or agreed to in writing, the Oculus VR SDK
+distributed under the License is distributed on an "AS IS" BASIS,
+WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+See the License for the specific language governing permissions and
+limitations under the License.
+
+*************************************************************************************/
+
+#ifndef Tracking_PoseState_h
+#define Tracking_PoseState_h
+
+#include "../Kernel/OVR_Math.h"
+
+namespace OVR {
+
+// PoseState describes the complete pose, or a rigid body configuration, at a
+// point in time, including first and second derivatives. It is used to specify
+// instantaneous location and movement of the headset.
+// SensorState is returned as a part of the sensor state.
+
+template<class T>
+class PoseState
+{
+public:
+	typedef typename CompatibleTypes<Pose<T> >::Type CompatibleType;
+
+	PoseState() : TimeInSeconds(0.0) { }
+    PoseState(Pose<T> pose, double time) : TimeInSeconds(time), ThePose(pose) { }
+
+	// float <-> double conversion constructor.
+	explicit PoseState(const PoseState<typename Math<T>::OtherFloatType> &src)
+		: ThePose(src.ThePose),
+		AngularVelocity(src.AngularVelocity), LinearVelocity(src.LinearVelocity),
+		AngularAcceleration(src.AngularAcceleration), LinearAcceleration(src.LinearAcceleration),
+		TimeInSeconds(src.TimeInSeconds)
+	{ }
+
+	// C-interop support: PoseStatef <-> ovrPoseStatef
+	PoseState(const typename CompatibleTypes<PoseState<T> >::Type& src)
+		: ThePose(src.ThePose),
+		AngularVelocity(src.AngularVelocity), LinearVelocity(src.LinearVelocity),
+		AngularAcceleration(src.AngularAcceleration), LinearAcceleration(src.LinearAcceleration),
+		TimeInSeconds(src.TimeInSeconds)
+	{ }
+
+	operator typename CompatibleTypes<PoseState<T> >::Type() const
+	{
+		typename CompatibleTypes<PoseState<T> >::Type result;
+		result.ThePose = ThePose;
+		result.AngularVelocity = AngularVelocity;
+		result.LinearVelocity = LinearVelocity;
+		result.AngularAcceleration = AngularAcceleration;
+		result.LinearAcceleration = LinearAcceleration;
+		result.TimeInSeconds = TimeInSeconds;
+		return result;
+	}
+
+	Pose<T> ThePose;
+	Vector3<T>  AngularVelocity;
+	Vector3<T>  LinearVelocity;
+	Vector3<T>  AngularAcceleration;
+	Vector3<T>  LinearAcceleration;
+	// Absolute time of this state sample; always a double measured in seconds.
+	double      TimeInSeconds;
+
+	// ***** Helpers for Pose integration
+
+	// Stores and integrates gyro angular velocity reading for a given time step.
+	void StoreAndIntegrateGyro(Vector3d angVel, double dt);
+	// Stores and integrates position/velocity from accelerometer reading for a given time step.
+	void StoreAndIntegrateAccelerometer(Vector3d linearAccel, double dt);
+
+	// Performs integration of state by adding next state delta to it
+	// to produce a combined state change
+	void AdvanceByDelta(const PoseState<T>& delta);
+};
+
+
+template<class T>
+PoseState<T> operator*(const OVR::Pose<T>& trans, const PoseState<T>& poseState)
+{
+	PoseState<T> result;
+	result.ThePose = trans * poseState.ThePose;
+	result.LinearVelocity = trans.Rotate(poseState.LinearVelocity);
+	result.LinearAcceleration = trans.Rotate(poseState.LinearAcceleration);
+	result.AngularVelocity = trans.Rotate(poseState.AngularVelocity);
+	result.AngularAcceleration = trans.Rotate(poseState.AngularAcceleration);
+	return result;
+}
+
+
+// External API returns pose as float, but uses doubles internally for quaternion precision.
+typedef PoseState<float>  PoseStatef;
+typedef PoseState<double> PoseStated;
+
+
+} // namespace OVR::Vision
+
+
+namespace OVR {
+
+	template<> struct CompatibleTypes<OVR::PoseState<float> > { typedef ovrPoseStatef Type; };
+	template<> struct CompatibleTypes<OVR::PoseState<double> > { typedef ovrPoseStated Type; };
+
+}
+
+#endif // Tracking_PoseState_h
diff --git a/LibOVR/Src/Tracking/Tracking_SensorState.h b/LibOVR/Src/Tracking/Tracking_SensorState.h
new file mode 100644
index 0000000..251e612
--- /dev/null
+++ b/LibOVR/Src/Tracking/Tracking_SensorState.h
@@ -0,0 +1,181 @@
+/************************************************************************************
+
+Filename    :   Tracking_SensorState.h
+Content     :   Sensor state information shared by tracking system with games
+Created     :   May 13, 2014
+Authors     :   Dov Katz, Chris Taylor
+
+Copyright   :   Copyright 2014 Oculus VR, Inc. All Rights reserved.
+
+Licensed under the Oculus VR Rift SDK License Version 3.1 (the "License");
+you may not use the Oculus VR Rift SDK except in compliance with the License,
+which is provided at the time of installation or download, or which
+otherwise accompanies this software in either electronic or hard copy form.
+
+You may obtain a copy of the License at
+
+http://www.oculusvr.com/licenses/LICENSE-3.1
+
+Unless required by applicable law or agreed to in writing, the Oculus VR SDK
+distributed under the License is distributed on an "AS IS" BASIS,
+WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+See the License for the specific language governing permissions and
+limitations under the License.
+
+*************************************************************************************/
+
+#ifndef Tracking_SensorState_h
+#define Tracking_SensorState_h
+
+#include "Tracking_PoseState.h"
+#include "../Kernel/OVR_SharedMemory.h"
+#include "../Kernel/OVR_Lockless.h"
+#include "../Kernel/OVR_String.h"
+#include "../Util/Util_LatencyTest2State.h"
+#include "../Sensors/OVR_DeviceConstants.h"
+
+// CAPI forward declarations.
+struct ovrTrackingState_;
+typedef struct ovrTrackingState_ ovrTrackingState;
+struct ovrPoseStatef_;
+typedef struct ovrPoseStatef_ ovrPoseStatef;
+
+namespace OVR { namespace Tracking {
+
+
+//-------------------------------------------------------------------------------------
+// ***** Sensor State
+// These values are reported as compatible with C API.
+
+// Bit flags describing the current status of sensor tracking.
+enum StatusBits
+{
+    // Tracked bits: Toggled by SensorFusion
+	Status_OrientationTracked = 0x0001, // Orientation is currently tracked (connected and in use)
+	Status_PositionTracked    = 0x0002, // Position is currently tracked (false if out of range)
+    Status_CameraPoseTracked  = 0x0004, // Camera pose is currently tracked
+
+    // Connected bits: Toggled by TrackingManager
+    Status_PositionConnected  = 0x0020, // Position tracking HW is connected
+	Status_HMDConnected       = 0x0080, // HMD is available & connected
+
+    // Masks
+    Status_AllMask = 0xffff,
+    Status_TrackingMask = Status_PositionTracked | Status_OrientationTracked | Status_CameraPoseTracked,
+    Status_ConnectedMask = Status_PositionConnected | Status_HMDConnected,
+};
+
+
+// Full state of of the sensor reported by GetSensorState() at a given absolute time.
+class TrackingState
+{
+public:
+	TrackingState() : StatusFlags(0) { }
+
+	// C-interop support
+	TrackingState(const ovrTrackingState& s);
+	operator ovrTrackingState () const;
+
+	// HMD pose information for the requested time.
+	PoseStatef   HeadPose;
+
+    // Orientation and position of the external camera, if present.
+    Posef        CameraPose;
+    // Orientation and position of the camera after alignment with gravity 
+    Posef        LeveledCameraPose;
+
+    // Most recent sensor data received from the HMD
+    SensorDataType RawSensorData;
+
+    // Sensor status described by ovrStatusBits.
+	uint32_t     StatusFlags;
+};
+
+
+// -----------------------------------------------
+
+#pragma pack(push, 8)
+
+struct LocklessSensorStatePadding;
+
+// State version stored in lockless updater "queue" and used for 
+// prediction by GetPoseAtTime/GetSensorStateAtTime
+struct LocklessSensorState
+{
+	PoseState<double> WorldFromImu;
+    SensorDataType    RawSensorData;
+    Pose<double>      WorldFromCamera;
+	uint32_t          StatusFlags;
+
+	// ImuFromCpf for HMD pose tracking
+	Posed             ImuFromCpf;
+
+	// Initialized to invalid state
+	LocklessSensorState() :
+		StatusFlags(0)
+	{
+	}
+
+    LocklessSensorState& operator = (const LocklessSensorStatePadding& rhs);
+};
+
+// Padded out version stored in the updater slots
+// Designed to be a larger fixed size to allow the data to grow in the future
+// without breaking older compiled code.
+struct LocklessSensorStatePadding
+{
+    uint64_t words[64];
+
+    static const int DataWords = (sizeof(LocklessSensorState) + sizeof(uint64_t) - 1) / sizeof(uint64_t);
+
+    // Just copy the low data words
+    inline LocklessSensorStatePadding& operator=(const LocklessSensorState& rhs)
+    {
+        const uint64_t* src = (const uint64_t*)&rhs;
+
+        // if this fires off, then increase words' array size
+        OVR_ASSERT(sizeof(words) > sizeof(LocklessSensorState));
+
+        for (int i = 0; i < DataWords; ++i)
+        {
+            words[i] = src[i];
+        }
+
+        return *this;
+    }
+};
+
+// Just copy the low data words
+inline LocklessSensorState& LocklessSensorState::operator = (const LocklessSensorStatePadding& rhs)
+{
+    uint64_t* dest = (uint64_t*)this;
+
+    for (int i = 0; i < LocklessSensorStatePadding::DataWords; ++i)
+    {
+        dest[i] = rhs.words[i];
+    }
+
+    return *this;
+}
+
+#pragma pack(pop)
+
+// A lockless updater for sensor state
+typedef LocklessUpdater<LocklessSensorState, LocklessSensorStatePadding> SensorStateUpdater;
+
+
+//// Combined state
+
+struct CombinedSharedStateUpdater
+{
+    SensorStateUpdater         SharedSensorState;
+    Util::LockessRecordUpdater SharedLatencyTestState;
+};
+
+typedef SharedObjectWriter< CombinedSharedStateUpdater > CombinedSharedStateWriter;
+typedef SharedObjectReader< CombinedSharedStateUpdater > CombinedSharedStateReader;
+
+
+}} // namespace OVR::Tracking
+
+#endif
diff --git a/LibOVR/Src/Tracking/Tracking_SensorStateReader.cpp b/LibOVR/Src/Tracking/Tracking_SensorStateReader.cpp
new file mode 100644
index 0000000..b290480
--- /dev/null
+++ b/LibOVR/Src/Tracking/Tracking_SensorStateReader.cpp
@@ -0,0 +1,207 @@
+/************************************************************************************
+
+Filename    :   Tracking_SensorStateReader.cpp
+Content     :   Separate reader component that is able to recover sensor pose
+Created     :   June 4, 2014
+Authors     :   Chris Taylor
+
+Copyright   :   Copyright 2014 Oculus VR, Inc. All Rights reserved.
+
+Licensed under the Oculus VR Rift SDK License Version 3.1 (the "License"); 
+you may not use the Oculus VR Rift SDK except in compliance with the License, 
+which is provided at the time of installation or download, or which 
+otherwise accompanies this software in either electronic or hard copy form.
+
+You may obtain a copy of the License at
+
+http://www.oculusvr.com/licenses/LICENSE-3.1 
+
+Unless required by applicable law or agreed to in writing, the Oculus VR SDK 
+distributed under the License is distributed on an "AS IS" BASIS,
+WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+See the License for the specific language governing permissions and
+limitations under the License.
+
+*************************************************************************************/
+
+#include "Tracking_SensorStateReader.h"
+#include "Tracking_PoseState.h"
+
+namespace OVR { namespace Tracking {
+
+
+//-------------------------------------------------------------------------------------
+
+// This is a "perceptually tuned predictive filter", which means that it is optimized
+// for improvements in the VR experience, rather than pure error.  In particular,
+// jitter is more perceptible at lower speeds whereas latency is more perceptible
+// after a high-speed motion.  Therefore, the prediction interval is dynamically
+// adjusted based on speed.  Significant more research is needed to further improve
+// this family of filters.
+static Pose<double> calcPredictedPose(const PoseState<double>& poseState, double predictionDt)
+{
+	Pose<double> pose = poseState.ThePose;
+	const double linearCoef = 1.0;
+	Vector3d angularVelocity = poseState.AngularVelocity;
+	double angularSpeed = angularVelocity.Length();
+
+	// This could be tuned so that linear and angular are combined with different coefficients
+	double speed = angularSpeed + linearCoef * poseState.LinearVelocity.Length();
+
+	const double slope = 0.2; // The rate at which the dynamic prediction interval varies
+	double candidateDt = slope * speed; // TODO: Replace with smoothstep function
+
+	double dynamicDt = predictionDt;
+
+	// Choose the candidate if it is shorter, to improve stability
+	if (candidateDt < predictionDt)
+	{
+		dynamicDt = candidateDt;
+	}
+
+	if (angularSpeed > 0.001)
+	{
+		pose.Rotation = pose.Rotation * Quatd(angularVelocity, angularSpeed * dynamicDt);
+	}
+
+	pose.Translation += poseState.LinearVelocity * dynamicDt;
+
+	return pose;
+}
+
+
+//// SensorStateReader
+
+SensorStateReader::SensorStateReader() :
+	Updater(NULL),
+    LastLatWarnTime(0.)
+{
+}
+
+void SensorStateReader::SetUpdater(const CombinedSharedStateUpdater* updater)
+{
+	Updater = updater;
+}
+
+void SensorStateReader::RecenterPose()
+{
+	if (!Updater)
+	{
+		return;
+	}
+
+	/*
+		This resets position to center in x, y, z, and resets yaw to center.
+		Other rotation components are not affected.
+	*/
+
+	const LocklessSensorState lstate = Updater->SharedSensorState.GetState();
+
+	Posed worldFromCpf = lstate.WorldFromImu.ThePose * lstate.ImuFromCpf;
+
+	double hmdYaw, hmdPitch, hmdRoll;
+	worldFromCpf.Rotation.GetEulerAngles<Axis_Y, Axis_X, Axis_Z>(&hmdYaw, &hmdPitch, &hmdRoll);
+
+	Posed worldFromCentered(Quatd(Axis_Y, hmdYaw), worldFromCpf.Translation);
+
+	CenteredFromWorld = worldFromCentered.Inverted();
+}
+
+bool SensorStateReader::GetSensorStateAtTime(double absoluteTime, TrackingState& ss) const
+{
+	if (!Updater)
+	{
+        ss.StatusFlags = 0;
+        return false;
+	}
+
+	const LocklessSensorState lstate = Updater->SharedSensorState.GetState();
+
+    // Update time
+	ss.HeadPose.TimeInSeconds = absoluteTime;
+
+	// Update the status flags
+	ss.StatusFlags = lstate.StatusFlags;
+	// If no hardware is connected, override the tracking flags
+	if (0 == (ss.StatusFlags & Status_HMDConnected))
+	{
+		ss.StatusFlags &= ~Status_TrackingMask;
+	}
+	if (0 == (ss.StatusFlags & Status_PositionConnected))
+	{
+		ss.StatusFlags &= ~(Status_PositionTracked | Status_CameraPoseTracked);
+	}
+
+	// If tracking info is invalid,
+    if (0 == (ss.StatusFlags & Status_TrackingMask))
+	{
+        return false;
+	}
+
+	// Delta time from the last available data
+	double pdt = absoluteTime - lstate.WorldFromImu.TimeInSeconds;
+	static const double maxPdt = 0.1;
+
+	// If delta went negative due to synchronization problems between processes or just a lag spike,
+	if (pdt < 0.)
+	{
+		pdt = 0.;
+	}
+	else if (pdt > maxPdt)
+	{
+        if (LastLatWarnTime != lstate.WorldFromImu.TimeInSeconds)
+        {
+            LastLatWarnTime = lstate.WorldFromImu.TimeInSeconds;
+            LogText("[SensorStateReader] Prediction interval too high: %f s, clamping at %f s\n", pdt, maxPdt);
+        }
+		pdt = maxPdt;
+	}
+
+	ss.HeadPose = PoseStatef(lstate.WorldFromImu);
+	// Do prediction logic and ImuFromCpf transformation
+	ss.HeadPose.ThePose = Posef(CenteredFromWorld * calcPredictedPose(lstate.WorldFromImu, pdt) * lstate.ImuFromCpf);
+
+    ss.CameraPose = Posef(CenteredFromWorld * lstate.WorldFromCamera);
+
+    Posed worldFromLeveledCamera = Posed(Quatd(), lstate.WorldFromCamera.Translation);
+    ss.LeveledCameraPose = Posef(CenteredFromWorld * worldFromLeveledCamera);
+
+    ss.RawSensorData = lstate.RawSensorData;
+
+	return true;
+}
+
+bool SensorStateReader::GetPoseAtTime(double absoluteTime, Posef& transform) const
+{
+	TrackingState ss;
+	if (!GetSensorStateAtTime(absoluteTime, ss))
+	{
+		return false;
+	}
+
+	transform = ss.HeadPose.ThePose;
+
+	return true;
+}
+
+uint32_t SensorStateReader::GetStatus() const
+{
+	if (!Updater)
+	{
+		return 0;
+	}
+
+	const LocklessSensorState lstate = Updater->SharedSensorState.GetState();
+
+	// If invalid,
+	if (0 == (lstate.StatusFlags & Status_TrackingMask))
+	{
+		// Return 0 indicating no orientation nor position tracking
+		return 0;
+	}
+
+	return lstate.StatusFlags;
+}
+
+
+}} // namespace OVR::Tracking
diff --git a/LibOVR/Src/Tracking/Tracking_SensorStateReader.h b/LibOVR/Src/Tracking/Tracking_SensorStateReader.h
new file mode 100644
index 0000000..f1aa5f9
--- /dev/null
+++ b/LibOVR/Src/Tracking/Tracking_SensorStateReader.h
@@ -0,0 +1,74 @@
+/************************************************************************************
+
+Filename    :   Tracking_SensorStateReader.h
+Content     :   Separate reader component that is able to recover sensor pose
+Created     :   June 4, 2014
+Authors     :   Chris Taylor
+
+Copyright   :   Copyright 2014 Oculus VR, Inc. All Rights reserved.
+
+Licensed under the Oculus VR Rift SDK License Version 3.1 (the "License"); 
+you may not use the Oculus VR Rift SDK except in compliance with the License, 
+which is provided at the time of installation or download, or which 
+otherwise accompanies this software in either electronic or hard copy form.
+
+You may obtain a copy of the License at
+
+http://www.oculusvr.com/licenses/LICENSE-3.1 
+
+Unless required by applicable law or agreed to in writing, the Oculus VR SDK 
+distributed under the License is distributed on an "AS IS" BASIS,
+WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+See the License for the specific language governing permissions and
+limitations under the License.
+
+*************************************************************************************/
+
+#ifndef Tracking_SensorStateReader_h
+#define Tracking_SensorStateReader_h
+
+#include "../Kernel/OVR_Lockless.h"
+#include "Tracking_SensorState.h"
+
+namespace OVR { namespace Tracking {
+
+
+//-----------------------------------------------------------------------------
+// SensorStateReader
+
+// User interface to retrieve pose from the sensor fusion subsystem
+class SensorStateReader : public NewOverrideBase
+{
+protected:
+	const CombinedSharedStateUpdater *Updater;
+
+	// Transform from real-world coordinates to centered coordinates
+	Posed CenteredFromWorld;
+
+    // Last latency warning time
+    mutable double LastLatWarnTime;
+
+public:
+	SensorStateReader();
+
+	// Initialize the updater
+    void         SetUpdater(const CombinedSharedStateUpdater *updater);
+
+	// Re-centers on the current yaw (optionally pitch) and translation
+	void		 RecenterPose();
+
+	// Get the full dynamical system state of the CPF, which includes velocities and accelerations,
+	// predicted at a specified absolute point in time.
+	bool		 GetSensorStateAtTime(double absoluteTime, Tracking::TrackingState& state) const;
+
+	// Get the predicted pose (orientation, position) of the center pupil frame (CPF) at a specific point in time.
+	bool		 GetPoseAtTime(double absoluteTime, Posef& transform) const;
+
+	// Get the sensor status (same as GetSensorStateAtTime(...).Status)
+	uint32_t     GetStatus() const;
+};
+
+
+}} // namespace OVR::Tracking
+
+#endif // Tracking_SensorStateReader_h