Mitigate BVH retargeting jitter (#29)

The initial BVH retargeting solution produced very jittery results. Much
of that jitter was eliminated by using linear interpolation between
frames instead of immediate stepped transitions, but some jittering
still remained, particularly noticeable in the finger motions.

This PR largely fixes remaining jitter issues by making a few changes:

#### Frame-time quantization

Previously, we had been using the `BvhAnim::timer` field incorrectly, by
manually resetting it after `Timer::just_finished` returned `true`, and
returning `0.0` as the interpolation parameter for that frame. This
resulted in a herky-jerky animation due to misalignment between the BVH
animation's frame time and the actual application refresh rate, meaning
we would lose fractions of a frame every time the timer elapsed and the
frame was incremented. This PR addresses that by never calling
`Timer::reset` and always using `Timer::percent` for the interpolation
parameter.

#### Matrix instability

This PR refactors the retargeting system to operate on `Affine3A`
matrices instead of `Mat4`s. `Affine3A` represents a SIMD-optimized 3x3
matrix + a translation vector, so using it provides a small performance
boost in addition to being slightly more numerically stable.
Additionally, we now periodically re-orthonormalize the 3x3 matrices
in-between operations to mitigate drifting due to the accumulation of
floating-point errors.

studio/src/bvh/mod.rs

@@ -13,7 +13,7 @@ mod error;
 mod loader;
 
 use crate::{
-    math::Lerp,
+    math::{Lerp, MatrixExtras},
     scene::{BindPoseGlobal, InvBindPose, SceneState, SkeletalJoint},
 };
 
@@ -73,7 +73,7 @@ fn compute_inverse_bindposes(
     q_bvh_joints: Query<(Entity, &GlobalTransform), (With<BvhJoint>, Without<InvBindPose>)>,
 ) {
     for (ent, world_xform) in q_bvh_joints.iter() {
-        let inv_bind_pose = world_xform.compute_matrix().inverse();
+        let inv_bind_pose = world_xform.affine().inverse();
         cmd.entity(ent).insert(InvBindPose(inv_bind_pose));
     }
 }
@@ -97,11 +97,9 @@ pub fn animate_bvh(
 
             if anim.timer.just_finished() {
                 anim.frame = (anim.frame + 1) % bvh_len;
-                anim.timer.reset();
-                0.
-            } else {
-                anim.timer.percent()
             }
+
+            anim.timer.percent()
         } else {
             0.
         };
@@ -192,7 +190,6 @@ fn retarget_bvh(
     >,
     q_world_xforms: Query<&GlobalTransform>,
 ) {
-    // let retarget_map = RetargetMap::mocap_to_rigify();
     for (source_name, source_inv_bind_pose, source_world_xform) in q_source_bones.iter() {
         // Lookup the target name in the retarget map
         let Some(target_name) = r_retarget_map.source_to_target.get(source_name.as_str()) else {
@@ -234,24 +231,21 @@ fn retarget_bvh(
         // exactly reversed (DirectX uses row-major matrices, while Bevy uses
         // column-major ordering)
 
-        // FIXME: We're seeing some jitteryness due to numerical instability.
-        //        Should refactor the Mat4's to Affine3A's and periodically re-
-        //        orthonormalize the matrices inbetween operations.
-
         // [psuedo] local_mat = source_inv_bind_mat * source_world_mat;
-        let mut local_mat = source_world_xform.compute_matrix() * (**source_inv_bind_pose);
+        let mut local_mat = source_world_xform.affine() * (**source_inv_bind_pose);
+        local_mat.matrix3 = local_mat.matrix3.orthonormalize();
 
         // [psuedo] local_mat = target_world_mat * local_mat * target_inv_parent_mat;
         let parent_world_xform = match q_world_xforms.get(**target_parent_ent) {
             Ok(world_xform) => world_xform,
             Err(_) => &GlobalTransform::IDENTITY,
         };
-        let target_inv_parent_mat = parent_world_xform.compute_matrix().inverse();
+        let target_inv_parent_mat = parent_world_xform.affine().inverse();
         local_mat = target_inv_parent_mat * local_mat * (**target_bind_pose_global);
+        local_mat.matrix3 = local_mat.matrix3.orthonormalize();
 
         // [psuedo] target_xform.rotation = local_mat.rotation;
-        let (_, rotation, _) = local_mat.to_scale_rotation_translation();
-        target_xform.rotation = rotation;
+        target_xform.rotation = Quat::from_affine3(&local_mat);
     }
 }
 

studio/src/math/matrix.rs

@@ -9,6 +9,7 @@ where
 {
     fn decompose_scale(&self) -> Self;
     fn decompose_scale_rotation(&self) -> (Self, Self);
+    fn orthonormalize(&self) -> Self;
 }
 
 pub trait VecExtras
@@ -49,6 +50,23 @@ impl MatrixExtras for Mat3A {
 
         (scale_mat, rotation)
     }
+
+    #[inline]
+    fn orthonormalize(&self) -> Self {
+        let x_axis = self.x_axis.normalize();
+
+        let mut y_axis = self.y_axis;
+        y_axis -= y_axis.dot(x_axis) / x_axis.dot(x_axis) * x_axis;
+        y_axis = y_axis.normalize();
+
+        let z_axis = x_axis.cross(y_axis);
+
+        Self {
+            x_axis,
+            y_axis,
+            z_axis,
+        }
+    }
 }
 
 impl MatrixExtras for Mat3 {
@@ -62,6 +80,11 @@ impl MatrixExtras for Mat3 {
         let (scale, rotation) = Mat3A::from(*self).decompose_scale_rotation();
         (scale.into(), rotation.into())
     }
+
+    #[inline]
+    fn orthonormalize(&self) -> Self {
+        Mat3A::from(*self).orthonormalize().into()
+    }
 }
 
 impl VecExtras for Vec3A {

studio/src/scene.rs

@@ -6,14 +6,10 @@ use bevy::{
         tonemapping::Tonemapping,
     },
     gltf::Gltf,
-    math::{vec2, vec3},
+    math::{vec2, vec3, Affine3A},
     pbr::ShadowFilteringMethod,
     prelude::*,
-    render::{
-        mesh::skinning::{SkinnedMesh, SkinnedMeshInverseBindposes},
-        primitives::Aabb,
-        view::NoFrustumCulling,
-    },
+    render::{mesh::skinning::SkinnedMesh, primitives::Aabb, view::NoFrustumCulling},
     scene::InstanceId,
 };
 
@@ -127,10 +123,10 @@ struct TestBvh(Handle<Bvh>); // TODO
 pub struct BindPoseLocal(pub Transform);
 
 #[derive(Component, Clone, Copy, Debug, Deref, DerefMut)]
-pub struct BindPoseGlobal(pub Mat4);
+pub struct BindPoseGlobal(pub Affine3A);
 
 #[derive(Component, Clone, Copy, Debug, Deref, DerefMut)]
-pub struct InvBindPose(pub Mat4);
+pub struct InvBindPose(pub Affine3A);
 
 fn initialize(mut cmd: Commands, r_assets: Res<AssetServer>) {
     // Camera, graphics settings
@@ -282,7 +278,6 @@ fn tag_scene_elements(
     r_scene_spawner: Res<SceneSpawner>,
     r_active_scene: Res<ActiveScene>,
     ra_gltfs: Res<Assets<Gltf>>,
-    ra_inv_bindposes: Res<Assets<SkinnedMeshInverseBindposes>>,
     // A "glTF mesh" is actually a container for one or more "glTF primitives",
     // which is basically a bundle of vertices paired with the material used to
     // shade them. It's those "sub-meshes" that carry the actual mesh handles,
@@ -317,20 +312,15 @@ fn tag_scene_elements(
     {
         cmd.entity(ent).insert(NoFrustumCulling);
 
-        let inv_bind_poses = ra_inv_bindposes
-            .get(skinned_mesh.inverse_bindposes.clone())
-            .unwrap();
-
-        for (idx, &joint) in skinned_mesh.joints.iter().enumerate() {
-            let inv_bind_pose = inv_bind_poses[idx];
+        for &joint in skinned_mesh.joints.iter() {
             let global_bind_pose = q_xforms.p0().compute_global_transform(joint).unwrap();
             let local_bind_pose = *q_xforms.p1().get(joint).unwrap();
 
             cmd.entity(joint).insert((
                 SkeletalJoint(Color::BLACK),
                 BindPoseLocal(local_bind_pose),
-                BindPoseGlobal(global_bind_pose.compute_matrix()),
-                InvBindPose(inv_bind_pose),
+                BindPoseGlobal(global_bind_pose.affine()),
+                InvBindPose(global_bind_pose.affine().inverse()),
             ));
         }
     }