The open source OpenXR runtime
h/mercury: Add faster SwingTwistToQuaternion
+70
-11
+70
-11
src/xrt/tracking/hand/mercury/kine_lm/lm_rotations.inl
+70
-11
src/xrt/tracking/hand/mercury/kine_lm/lm_rotations.inl
···
220
220
}
221
221
}
222
222
223
+
224
+
// See
225
+
// https://gitlab.freedesktop.org/slitcch/rotation_visualizer/-/blob/da5021d21600388b07c9c81000e866c4a2d015cb/lm_rotations_story.inl
226
+
// for the derivation
223
227
template <typename T>
224
228
inline void
225
229
SwingTwistToQuaternion(const Vec2<T> swing, const T twist, Quat<T> &result)
226
230
{
227
-
//!@todo
228
-
// Rather than doing compound operations, we should derive it and collapse them.
229
-
Quat<T> swing_quat;
230
-
Quat<T> twist_quat;
231
+
232
+
T swing_x = swing.x;
233
+
T swing_y = swing.y;
234
+
235
+
T theta_squared_swing = swing_x * swing_x + swing_y * swing_y;
236
+
237
+
// So it turns out that we don't get any divisions by zero or nans in the
238
+
// differential part when twist is 0. I'm pretty sure we get lucky wrt. what cancels out
239
+
240
+
if (theta_squared_swing > T(0.0)) {
241
+
// theta_squared_swing is nonzero, so we the regular derived conversion.
242
+
243
+
T theta = sqrt(theta_squared_swing);
244
+
245
+
T half_theta = theta * T(0.5);
246
+
247
+
// the "other" theta
248
+
T half_twist = twist * T(0.5);
249
+
250
+
T cos_half_theta = cos(half_theta);
251
+
T cos_half_twist = cos(half_twist);
252
+
253
+
T sin_half_twist = sin(half_twist);
254
+
255
+
T sin_half_theta_over_theta = sin(half_theta) / theta;
256
+
257
+
result.w = cos_half_theta * cos_half_twist;
258
+
259
+
T x_part_1 = (swing_x * cos_half_twist * sin_half_theta_over_theta);
260
+
T x_part_2 = (swing_y * sin_half_twist * sin_half_theta_over_theta);
261
+
262
+
result.x = x_part_1 + x_part_2;
263
+
264
+
T y_part_1 = (swing_y * cos_half_twist * sin_half_theta_over_theta);
265
+
T y_part_2 = (swing_x * sin_half_twist * sin_half_theta_over_theta);
266
+
267
+
result.y = y_part_1 - y_part_2;
231
268
232
-
Vec3<T> aax_twist;
269
+
result.z = cos_half_theta * sin_half_twist;
233
270
234
-
aax_twist.x = (T)(0);
235
-
aax_twist.y = (T)(0);
236
-
aax_twist.z = twist;
271
+
} else {
272
+
// first: sin_half_theta/theta would be undefined, but
273
+
// the limit approaches 0.5.
237
274
238
-
SwingToQuaternion(swing, swing_quat);
275
+
// second: we only use theta to calculate sin_half_theta/theta
276
+
// and that function's derivative at theta=0 is 0, so this formulation is fine.
239
277
240
-
AngleAxisToQuaternion(aax_twist, twist_quat);
278
+
T half_twist = twist * T(0.5);
279
+
280
+
T cos_half_twist = cos(half_twist);
241
281
242
-
QuaternionProduct(swing_quat, twist_quat, result);
282
+
T sin_half_twist = sin(half_twist);
283
+
284
+
T sin_half_theta_over_theta = T(0.5);
285
+
286
+
// cos(0) is 1 so no cos_half_theta necessary
287
+
result.w = cos_half_twist;
288
+
289
+
T x_part_1 = (swing_x * cos_half_twist * sin_half_theta_over_theta);
290
+
T x_part_2 = (swing_y * sin_half_twist * sin_half_theta_over_theta);
291
+
292
+
result.x = x_part_1 + x_part_2;
293
+
294
+
T y_part_1 = (swing_y * cos_half_twist * sin_half_theta_over_theta);
295
+
T y_part_2 = (swing_x * sin_half_twist * sin_half_theta_over_theta);
296
+
297
+
result.y = y_part_1 - y_part_2;
298
+
299
+
result.z = sin_half_twist;
300
+
}
243
301
}
302
+
244
303
} // namespace xrt::tracking::hand::mercury::lm