1.1 --- /dev/null	Thu Jan 01 00:00:00 1970 +0000
     1.2 +++ b/LibOVR/Src/Tracking/Tracking_PoseState.h	Wed Jan 14 06:51:16 2015 +0200
     1.3 @@ -0,0 +1,133 @@
     1.4 +/************************************************************************************
     1.5 +
     1.6 +Filename    :   Tracking_PoseState.h
     1.7 +Content     :   Describes the complete pose at a point in time, including derivatives
     1.8 +Created     :   May 13, 2014
     1.9 +Authors     :   Dov Katz
    1.10 +
    1.11 +Copyright   :   Copyright 2014 Oculus VR, LLC All Rights reserved.
    1.12 +
    1.13 +Licensed under the Oculus VR Rift SDK License Version 3.2 (the "License");
    1.14 +you may not use the Oculus VR Rift SDK except in compliance with the License,
    1.15 +which is provided at the time of installation or download, or which
    1.16 +otherwise accompanies this software in either electronic or hard copy form.
    1.17 +
    1.18 +You may obtain a copy of the License at
    1.19 +
    1.20 +http://www.oculusvr.com/licenses/LICENSE-3.2
    1.21 +
    1.22 +Unless required by applicable law or agreed to in writing, the Oculus VR SDK
    1.23 +distributed under the License is distributed on an "AS IS" BASIS,
    1.24 +WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
    1.25 +See the License for the specific language governing permissions and
    1.26 +limitations under the License.
    1.27 +
    1.28 +*************************************************************************************/
    1.29 +
    1.30 +#ifndef Tracking_PoseState_h
    1.31 +#define Tracking_PoseState_h
    1.32 +
    1.33 +#include "../Kernel/OVR_Math.h"
    1.34 +
    1.35 +namespace OVR {
    1.36 +
    1.37 +// PoseState describes the complete pose, or a rigid body configuration, at a
    1.38 +// point in time, including first and second derivatives. It is used to specify
    1.39 +// instantaneous location and movement of the headset.
    1.40 +// SensorState is returned as a part of the sensor state.
    1.41 +
    1.42 +template<class T>
    1.43 +class PoseState
    1.44 +{
    1.45 +public:
    1.46 +	typedef typename CompatibleTypes<Pose<T> >::Type CompatibleType;
    1.47 +
    1.48 +	PoseState() : TimeInSeconds(0.0) { }
    1.49 +    PoseState(Pose<T> pose, double time) : ThePose(pose), TimeInSeconds(time) { }
    1.50 +
    1.51 +	// float <-> double conversion constructor.
    1.52 +	explicit PoseState(const PoseState<typename Math<T>::OtherFloatType> &src)
    1.53 +		: ThePose(src.ThePose),
    1.54 +		AngularVelocity(src.AngularVelocity), LinearVelocity(src.LinearVelocity),
    1.55 +		AngularAcceleration(src.AngularAcceleration), LinearAcceleration(src.LinearAcceleration),
    1.56 +		TimeInSeconds(src.TimeInSeconds)
    1.57 +	{ }
    1.58 +
    1.59 +	// C-interop support: PoseStatef <-> ovrPoseStatef
    1.60 +	PoseState(const typename CompatibleTypes<PoseState<T> >::Type& src)
    1.61 +		: ThePose(src.ThePose),
    1.62 +		AngularVelocity(src.AngularVelocity), LinearVelocity(src.LinearVelocity),
    1.63 +		AngularAcceleration(src.AngularAcceleration), LinearAcceleration(src.LinearAcceleration),
    1.64 +		TimeInSeconds(src.TimeInSeconds)
    1.65 +	{ }
    1.66 +
    1.67 +	operator typename CompatibleTypes<PoseState<T> >::Type() const
    1.68 +	{
    1.69 +		typename CompatibleTypes<PoseState<T> >::Type result;
    1.70 +		result.ThePose = ThePose;
    1.71 +		result.AngularVelocity = AngularVelocity;
    1.72 +		result.LinearVelocity = LinearVelocity;
    1.73 +		result.AngularAcceleration = AngularAcceleration;
    1.74 +		result.LinearAcceleration = LinearAcceleration;
    1.75 +		result.TimeInSeconds = TimeInSeconds;
    1.76 +		return result;
    1.77 +	}
    1.78 +
    1.79 +	Pose<T> ThePose;
    1.80 +	Vector3<T>  AngularVelocity;
    1.81 +	Vector3<T>  LinearVelocity;
    1.82 +	Vector3<T>  AngularAcceleration;
    1.83 +	Vector3<T>  LinearAcceleration;
    1.84 +	// Absolute time of this state sample; always a double measured in seconds.
    1.85 +	double      TimeInSeconds;
    1.86 +
    1.87 +	// ***** Helpers for Pose integration
    1.88 +
    1.89 +	// Stores and integrates gyro angular velocity reading for a given time step.
    1.90 +	void StoreAndIntegrateGyro(Vector3d angVel, double dt);
    1.91 +	// Stores and integrates position/velocity from accelerometer reading for a given time step.
    1.92 +	void StoreAndIntegrateAccelerometer(Vector3d linearAccel, double dt);
    1.93 +
    1.94 +	// Performs integration of state by adding next state delta to it
    1.95 +	// to produce a combined state change
    1.96 +	void AdvanceByDelta(const PoseState<T>& delta);
    1.97 +};
    1.98 +
    1.99 +
   1.100 +template<class T>
   1.101 +PoseState<T> operator*(const OVR::Pose<T>& trans, const PoseState<T>& poseState)
   1.102 +{
   1.103 +	PoseState<T> result;
   1.104 +	result.ThePose = trans * poseState.ThePose;
   1.105 +	result.LinearVelocity = trans.Rotate(poseState.LinearVelocity);
   1.106 +	result.LinearAcceleration = trans.Rotate(poseState.LinearAcceleration);
   1.107 +	result.AngularVelocity = trans.Rotate(poseState.AngularVelocity);
   1.108 +	result.AngularAcceleration = trans.Rotate(poseState.AngularAcceleration);
   1.109 +	return result;
   1.110 +}
   1.111 +
   1.112 +
   1.113 +// External API returns pose as float, but uses doubles internally for quaternion precision.
   1.114 +typedef PoseState<float>  PoseStatef;
   1.115 +typedef PoseState<double> PoseStated;
   1.116 +
   1.117 +
   1.118 +} // namespace OVR::Vision
   1.119 +
   1.120 +
   1.121 +namespace OVR {
   1.122 +
   1.123 +	template<> struct CompatibleTypes<OVR::PoseState<float> > { typedef ovrPoseStatef Type; };
   1.124 +	template<> struct CompatibleTypes<OVR::PoseState<double> > { typedef ovrPoseStated Type; };
   1.125 +
   1.126 +    static_assert((sizeof(PoseState<double>) == sizeof(Pose<double>) + 4*sizeof(Vector3<double>) + sizeof(double)), "sizeof(PoseState<double>) failure");
   1.127 +#ifdef OVR_CPU_X86_64
   1.128 +    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.
   1.129 +#elif defined(OVR_OS_WIN32) // The Windows 32 bit ABI aligns 64 bit values on 64 bit boundaries
   1.130 +    static_assert((sizeof(PoseState<float>) == sizeof(Pose<float>) + 4*sizeof(Vector3<float>) + sizeof(uint32_t) + sizeof(double)), "sizeof(PoseState<float>) failure");
   1.131 +#else // Else Unix/Apple 32 bit ABI, which aligns 64 bit values on 32 bit boundaries.
   1.132 +    static_assert((sizeof(PoseState<float>) == sizeof(Pose<float>) + 4*sizeof(Vector3<float>) +                    sizeof(double)), "sizeof(PoseState<float>) failure");
   1.133 +#endif
   1.134 +}
   1.135 +
   1.136 +#endif // Tracking_PoseState_h