From 9631299b48dc998dba6f633df4e1f2e2db741705 Mon Sep 17 00:00:00 2001 From: Ben Spiers Date: Sun, 11 Aug 2024 17:34:52 +0100 Subject: [PATCH] Fix locked axes in gyro torque Zeroing out columns of the inertia tensor makes it singular, so the inverse results in all NaNs --- src/dynamics/integrator/semi_implicit_euler.rs | 10 +++------- 1 file changed, 3 insertions(+), 7 deletions(-) diff --git a/src/dynamics/integrator/semi_implicit_euler.rs b/src/dynamics/integrator/semi_implicit_euler.rs index 5c9ffaf7..ee902ff5 100644 --- a/src/dynamics/integrator/semi_implicit_euler.rs +++ b/src/dynamics/integrator/semi_implicit_euler.rs @@ -77,13 +77,9 @@ pub fn integrate_velocity( // However, the basic semi-implicit approach can blow up, as semi-implicit Euler // extrapolates velocity and the gyroscopic torque is quadratic in the angular velocity. // Thus, we use implicit Euler, which is much more accurate and stable, although slightly more expensive. - let effective_inertia = locked_axes.apply_to_rotation(inv_inertia.0).inverse(); - delta_ang_vel += solve_gyroscopic_torque( - *ang_vel, - rotation.0, - Inertia(effective_inertia), - delta_seconds, - ); + let delta_ang_vel_gyro = + solve_gyroscopic_torque(*ang_vel, rotation.0, inv_inertia.inverse(), delta_seconds); + delta_ang_vel += locked_axes.apply_to_angular_velocity(delta_ang_vel_gyro); } if delta_ang_vel != AngularVelocity::ZERO.0 && delta_ang_vel.is_finite() {