@@ -31,7 +31,7 @@ inline std::wstring StringToWString(const std::string& str)
3131{
3232 const int count = MultiByteToWideChar (CP_UTF8, 0 , str.c_str (), str.length (), nullptr , 0 );
3333 std::wstring w_str (count, 0 );
34- MultiByteToWideChar (CP_UTF8, 0 , str.c_str (), str.length (), & w_str[ 0 ] , count);
34+ MultiByteToWideChar (CP_UTF8, 0 , str.c_str (), str.length (), w_str. data () , count);
3535 return w_str;
3636}
3737
@@ -40,14 +40,14 @@ inline std::string WStringToString(const std::wstring& w_str)
4040{
4141 const int count = WideCharToMultiByte (CP_UTF8, 0 , w_str.c_str (), w_str.length (), nullptr , 0 , nullptr , nullptr );
4242 std::string str (count, 0 );
43- WideCharToMultiByte (CP_UTF8, 0 , w_str.c_str (), -1 , & str[ 0 ] , count, nullptr , nullptr );
43+ WideCharToMultiByte (CP_UTF8, 0 , w_str.c_str (), -1 , str. data () , count, nullptr , nullptr );
4444 return str;
4545}
4646
4747namespace ktvr
4848{
4949 // Interface Version
50- static const char * IAME_API_Devices_Version = " IAME_API_Version_018 " ;
50+ static const char * IAME_API_Devices_Version = " IAME_API_Version_020 " ;
5151
5252 // Return messaging types
5353 enum K2InitErrorType
@@ -135,6 +135,9 @@ namespace ktvr
135135 [[nodiscard]] Eigen::Vector3d getJointPosition () const { return jointPosition; }
136136 [[nodiscard]] Eigen::Quaterniond getJointOrientation () const { return jointOrientation; }
137137
138+ [[nodiscard]] Eigen::Vector3d getPreviousJointPosition () const { return previousJointPosition; }
139+ [[nodiscard]] Eigen::Quaterniond getPreviousJointOrientation () const { return previousJointOrientation; }
140+
138141 [[nodiscard]] Eigen::Vector3d getJointVelocity () const { return jointVelocity; }
139142 [[nodiscard]] Eigen::Vector3d getJointAcceleration () const { return jointAcceleration; }
140143
@@ -151,8 +154,12 @@ namespace ktvr
151154 Eigen::Quaterniond orientation,
152155 const ITrackedJointState state)
153156 {
157+ previousJointPosition = jointPosition;
158+ previousJointOrientation = jointOrientation;
159+
154160 jointPosition = std::move (position);
155161 jointOrientation = std::move (orientation);
162+
156163 trackingState = state;
157164
158165 // Update pose timestamp
@@ -169,6 +176,9 @@ namespace ktvr
169176 Eigen::Vector3d angularAcceleration,
170177 const ITrackedJointState state)
171178 {
179+ previousJointPosition = jointPosition;
180+ previousJointOrientation = jointOrientation;
181+
172182 jointPosition = std::move (position);
173183 jointOrientation = std::move (orientation);
174184
@@ -187,6 +197,7 @@ namespace ktvr
187197 // For servers!
188198 void update_position (Eigen::Vector3d position)
189199 {
200+ previousJointPosition = jointPosition;
190201 jointPosition = std::move (position);
191202
192203 // Update pose timestamp
@@ -197,6 +208,7 @@ namespace ktvr
197208 // For servers!
198209 void update_orientation (Eigen::Quaterniond orientation)
199210 {
211+ previousJointOrientation = jointOrientation;
200212 jointOrientation = std::move (orientation);
201213 }
202214
@@ -235,13 +247,17 @@ namespace ktvr
235247 Eigen::Vector3d jointPosition = Eigen::Vector3d(0 ., 0 ., 0 .);
236248 Eigen::Quaterniond jointOrientation = Eigen::Quaterniond(1 ., 0 ., 0 ., 0 .);
237249
250+ Eigen::Vector3d previousJointPosition = Eigen::Vector3d(0 ., 0 ., 0 .);
251+ Eigen::Quaterniond previousJointOrientation = Eigen::Quaterniond(1 ., 0 ., 0 ., 0 .);
252+
238253 Eigen::Vector3d jointVelocity = Eigen::Vector3d(0 ., 0 ., 0 .);
239254 Eigen::Vector3d jointAcceleration = Eigen::Vector3d(0 ., 0 ., 0 .);
240255
241256 Eigen::Vector3d jointAngularVelocity = Eigen::Vector3d(0 ., 0 ., 0 .);
242257 Eigen::Vector3d jointAngularAcceleration = Eigen::Vector3d(0 ., 0 ., 0 .);
243258
244259 ITrackedJointState trackingState = State_NotTracked;
260+
245261 long long poseTimestamp = 0 ;
246262 long long previousPoseTimestamp = 0 ;
247263 };
0 commit comments