The open source OpenXR runtime
0
fork

Configure Feed

Select the types of activity you want to include in your feed.

h/mercury: Add faster SwingTwistToQuaternion

+70 -11
+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