diff --git a/studio/src/bvh/mod.rs b/studio/src/bvh/mod.rs index a5c3f2d..3a10b2b 100644 --- a/studio/src/bvh/mod.rs +++ b/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, Without)>, ) { 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); } } diff --git a/studio/src/math/matrix.rs b/studio/src/math/matrix.rs index 3ee6086..5219c69 100644 --- a/studio/src/math/matrix.rs +++ b/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 { diff --git a/studio/src/scene.rs b/studio/src/scene.rs index cef4df2..ce2d314 100644 --- a/studio/src/scene.rs +++ b/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); // 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) { // Camera, graphics settings @@ -282,7 +278,6 @@ fn tag_scene_elements( r_scene_spawner: Res, r_active_scene: Res, ra_gltfs: Res>, - ra_inv_bindposes: Res>, // 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()), )); } }