1 /************************************************************************************

2

3 Filename : Tracking_PoseState.h

4 Content : Describes the complete pose at a point in time, including derivatives

5 Created : May 13, 2014

6 Authors : Dov Katz

7

8 Copyright : Copyright 2014 Oculus VR, LLC All Rights reserved.

9

10 Licensed under the Oculus VR Rift SDK License Version 3.2 (the "License");

11 you may not use the Oculus VR Rift SDK except in compliance with the License,

12 which is provided at the time of installation or download, or which

13 otherwise accompanies this software in either electronic or hard copy form.

14

15 You may obtain a copy of the License at

16

17 http://www.oculusvr.com/licenses/LICENSE-3.2

18

19 Unless required by applicable law or agreed to in writing, the Oculus VR SDK

20 distributed under the License is distributed on an "AS IS" BASIS,

21 WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.

22 See the License for the specific language governing permissions and

23 limitations under the License.

24

25 *************************************************************************************/

26

27 #ifndef Tracking_PoseState_h

28 #define Tracking_PoseState_h

29

30 #include "../Kernel/OVR_Math.h"

31

32 namespace OVR {

33

34 // PoseState describes the complete pose, or a rigid body configuration, at a

35 // point in time, including first and second derivatives. It is used to specify

36 // instantaneous location and movement of the headset.

37 // SensorState is returned as a part of the sensor state.

38

39 template<class T>

40 class PoseState

41 {

42 public:

43 typedef typename CompatibleTypes<Pose<T> >::Type CompatibleType;

44

45 PoseState() : TimeInSeconds(0.0) { }

46 PoseState(Pose<T> pose, double time) : ThePose(pose), TimeInSeconds(time) { }

47

48 // float <-> double conversion constructor.

49 explicit PoseState(const PoseState<typename Math<T>::OtherFloatType> &src)

50 : ThePose(src.ThePose),

51 AngularVelocity(src.AngularVelocity), LinearVelocity(src.LinearVelocity),

52 AngularAcceleration(src.AngularAcceleration), LinearAcceleration(src.LinearAcceleration),

53 TimeInSeconds(src.TimeInSeconds)

54 { }

55

56 // C-interop support: PoseStatef <-> ovrPoseStatef

57 PoseState(const typename CompatibleTypes<PoseState<T> >::Type& src)

58 : ThePose(src.ThePose),

59 AngularVelocity(src.AngularVelocity), LinearVelocity(src.LinearVelocity),

60 AngularAcceleration(src.AngularAcceleration), LinearAcceleration(src.LinearAcceleration),

61 TimeInSeconds(src.TimeInSeconds)

62 { }

63

64 operator typename CompatibleTypes<PoseState<T> >::Type() const

65 {

66 typename CompatibleTypes<PoseState<T> >::Type result;

67 result.ThePose = ThePose;

68 result.AngularVelocity = AngularVelocity;

69 result.LinearVelocity = LinearVelocity;

70 result.AngularAcceleration = AngularAcceleration;

71 result.LinearAcceleration = LinearAcceleration;

72 result.TimeInSeconds = TimeInSeconds;

73 return result;

74 }

75

76 Pose<T> ThePose;

77 Vector3<T> AngularVelocity;

78 Vector3<T> LinearVelocity;

79 Vector3<T> AngularAcceleration;

80 Vector3<T> LinearAcceleration;

81 // Absolute time of this state sample; always a double measured in seconds.

82 double TimeInSeconds;

83

84 // ***** Helpers for Pose integration

85

86 // Stores and integrates gyro angular velocity reading for a given time step.

87 void StoreAndIntegrateGyro(Vector3d angVel, double dt);

88 // Stores and integrates position/velocity from accelerometer reading for a given time step.

89 void StoreAndIntegrateAccelerometer(Vector3d linearAccel, double dt);

90

91 // Performs integration of state by adding next state delta to it

92 // to produce a combined state change

93 void AdvanceByDelta(const PoseState<T>& delta);

94 };

95

96

97 template<class T>

98 PoseState<T> operator*(const OVR::Pose<T>& trans, const PoseState<T>& poseState)

99 {

100 PoseState<T> result;

101 result.ThePose = trans * poseState.ThePose;

102 result.LinearVelocity = trans.Rotate(poseState.LinearVelocity);

103 result.LinearAcceleration = trans.Rotate(poseState.LinearAcceleration);

104 result.AngularVelocity = trans.Rotate(poseState.AngularVelocity);

105 result.AngularAcceleration = trans.Rotate(poseState.AngularAcceleration);

106 return result;

107 }

108

109

110 // External API returns pose as float, but uses doubles internally for quaternion precision.

111 typedef PoseState<float> PoseStatef;

112 typedef PoseState<double> PoseStated;

113

114

115 } // namespace OVR::Vision

116

117

118 namespace OVR {

119

120 template<> struct CompatibleTypes<OVR::PoseState<float> > { typedef ovrPoseStatef Type; };

121 template<> struct CompatibleTypes<OVR::PoseState<double> > { typedef ovrPoseStated Type; };

122

123 static_assert((sizeof(PoseState<double>) == sizeof(Pose<double>) + 4*sizeof(Vector3<double>) + sizeof(double)), "sizeof(PoseState<double>) failure");

124 #ifdef OVR_CPU_X86_64

125 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.

126 #elif defined(OVR_OS_WIN32) // The Windows 32 bit ABI aligns 64 bit values on 64 bit boundaries

127 static_assert((sizeof(PoseState<float>) == sizeof(Pose<float>) + 4*sizeof(Vector3<float>) + sizeof(uint32_t) + sizeof(double)), "sizeof(PoseState<float>) failure");

128 #else // Else Unix/Apple 32 bit ABI, which aligns 64 bit values on 32 bit boundaries.

129 static_assert((sizeof(PoseState<float>) == sizeof(Pose<float>) + 4*sizeof(Vector3<float>) + sizeof(double)), "sizeof(PoseState<float>) failure");

130 #endif

131 }

132

133 #endif // Tracking_PoseState_h