1 files changed, 133 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..19e2ee2
--- /dev/null
+++ b/LibOVR/Src/Tracking/Tracking_PoseState.h
@@ -0,0 +1,133 @@
+/************************************************************************************
+
+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, LLC All Rights reserved.
+
+Licensed under the Oculus VR Rift SDK License Version 3.2 (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.2
+
+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) : ThePose(pose), TimeInSeconds(time) { }
+
+	// 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; };
+
+    static_assert((sizeof(PoseState<double>) == sizeof(Pose<double>) + 4*sizeof(Vector3<double>) + sizeof(double)), "sizeof(PoseState<double>) failure");
+#ifdef OVR_CPU_X86_64
+    static_assert((sizeof(PoseState<float>) == sizeof(Pose<float>) + 4*sizeof(Vector3<float>) + sizeof(uint32_t) + sizeof(double)), "sizeof(PoseState<float>) failure"); //TODO: Manually pad template.
+#elif defined(OVR_OS_WIN32) // The Windows 32 bit ABI aligns 64 bit values on 64 bit boundaries
+    static_assert((sizeof(PoseState<float>) == sizeof(Pose<float>) + 4*sizeof(Vector3<float>) + sizeof(uint32_t) + sizeof(double)), "sizeof(PoseState<float>) failure");
+#else // Else Unix/Apple 32 bit ABI, which aligns 64 bit values on 32 bit boundaries.
+    static_assert((sizeof(PoseState<float>) == sizeof(Pose<float>) + 4*sizeof(Vector3<float>) +                    sizeof(double)), "sizeof(PoseState<float>) failure");
+#endif
+}
+
+#endif // Tracking_PoseState_h