Parallelize forward kinematics animation systems (#6785)

# Objective
Speed up animation by leveraging all threads in `ComputeTaskPool`.

## Solution
This PR parallelizes animation sampling across all threads. 

To ensure that this is safely done, all animation is predicated with an ancestor query to ensure that there is no conflicting `AnimationPlayer` above each animated hierarchy that may cause this to alias.

Unlike the RFC, this does not add support for reflect based "animate anything", but only extends the existing `AnimationPlayer` to support high numbers of animated characters on screen at once.

## Performance
This cuts `many_foxes`'s frame time on my machine by a full millisecond, from 7.49ms to 6.5ms. (yellow is this PR, red is main).
![image](https://user-images.githubusercontent.com/3137680/204219698-ffe0136c-5e9b-436f-b8d9-b23f0b8d7d36.png)

---

## Changelog
Changed: Animation sampling now runs fully multi-threaded using threads from `ComputeTaskPool`.
Changed: `AnimationPlayer` that are on a child or descendant of another entity with another player will no longer be run.

Author: james7132

crates/bevy_animation/src/lib.rs

@@ -11,11 +11,12 @@ use bevy_ecs::{
     change_detection::DetectChanges,
     entity::Entity,
     prelude::Component,
+    query::With,
     reflect::ReflectComponent,
     schedule::IntoSystemDescriptor,
     system::{Query, Res},
 };
-use bevy_hierarchy::Children;
+use bevy_hierarchy::{Children, Parent};
 use bevy_math::{Quat, Vec3};
 use bevy_reflect::{FromReflect, Reflect, TypeUuid};
 use bevy_time::Time;
@@ -63,17 +64,34 @@ pub struct EntityPath {
 #[derive(Reflect, FromReflect, Clone, TypeUuid, Debug, Default)]
 #[uuid = "d81b7179-0448-4eb0-89fe-c067222725bf"]
 pub struct AnimationClip {
-    curves: HashMap<EntityPath, Vec<VariableCurve>>,
+    curves: Vec<Vec<VariableCurve>>,
+    paths: HashMap<EntityPath, usize>,
     duration: f32,
 }
 
 impl AnimationClip {
     #[inline]
-    /// Hashmap of the [`VariableCurve`]s per [`EntityPath`].
-    pub fn curves(&self) -> &HashMap<EntityPath, Vec<VariableCurve>> {
+    /// [`VariableCurve`]s for each bone. Indexed by the bone ID.
+    pub fn curves(&self) -> &Vec<Vec<VariableCurve>> {
         &self.curves
     }
 
+    /// Gets the curves for a bone.
+    ///
+    /// Returns `None` if the bone is invalid.
+    #[inline]
+    pub fn get_curves(&self, bone_id: usize) -> Option<&'_ Vec<VariableCurve>> {
+        self.curves.get(bone_id)
+    }
+
+    /// Gets the curves by it's [`EntityPath`].
+    ///
+    /// Returns `None` if the bone is invalid.
+    #[inline]
+    pub fn get_curves_by_path(&self, path: &EntityPath) -> Option<&'_ Vec<VariableCurve>> {
+        self.paths.get(path).and_then(|id| self.curves.get(*id))
+    }
+
     /// Duration of the clip, represented in seconds
     #[inline]
     pub fn duration(&self) -> f32 {
@@ -86,7 +104,13 @@ impl AnimationClip {
         self.duration = self
             .duration
             .max(*curve.keyframe_timestamps.last().unwrap_or(&0.0));
-        self.curves.entry(path).or_default().push(curve);
+        if let Some(bone_id) = self.paths.get(&path) {
+            self.curves[*bone_id].push(curve);
+        } else {
+            let idx = self.curves.len();
+            self.curves.push(vec![curve]);
+            self.paths.insert(path, idx);
+        }
     }
 }
 
@@ -99,6 +123,7 @@ pub struct AnimationPlayer {
     speed: f32,
     elapsed: f32,
     animation_clip: Handle<AnimationClip>,
+    path_cache: Vec<Vec<Option<Entity>>>,
 }
 
 impl Default for AnimationPlayer {
@@ -109,6 +134,7 @@ impl Default for AnimationPlayer {
             speed: 1.0,
             elapsed: 0.0,
             animation_clip: Default::default(),
+            path_cache: Vec::new(),
         }
     }
 }
@@ -181,117 +207,180 @@ impl AnimationPlayer {
     }
 }
 
+fn find_bone(
+    root: Entity,
+    path: &EntityPath,
+    children: &Query<&Children>,
+    names: &Query<&Name>,
+    path_cache: &mut Vec<Option<Entity>>,
+) -> Option<Entity> {
+    // PERF: finding the target entity can be optimised
+    let mut current_entity = root;
+    path_cache.resize(path.parts.len(), None);
+    // Ignore the first name, it is the root node which we already have
+    for (idx, part) in path.parts.iter().enumerate().skip(1) {
+        let mut found = false;
+        let children = children.get(current_entity).ok()?;
+        if let Some(cached) = path_cache[idx] {
+            if children.contains(&cached) {
+                if let Ok(name) = names.get(cached) {
+                    if name == part {
+                        current_entity = cached;
+                        found = true;
+                    }
+                }
+            }
+        }
+        if !found {
+            for child in children.deref() {
+                if let Ok(name) = names.get(*child) {
+                    if name == part {
+                        // Found a children with the right name, continue to the next part
+                        current_entity = *child;
+                        path_cache[idx] = Some(*child);
+                        found = true;
+                        break;
+                    }
+                }
+            }
+        }
+        if !found {
+            warn!("Entity not found for path {:?} on part {:?}", path, part);
+            return None;
+        }
+    }
+    Some(current_entity)
+}
+
+/// Verify that there are no ancestors of a given entity that have an `AnimationPlayer`.
+fn verify_no_ancestor_player(
+    player_parent: Option<&Parent>,
+    parents: &Query<(Option<With<AnimationPlayer>>, Option<&Parent>)>,
+) -> bool {
+    let Some(mut current) = player_parent.map(Parent::get) else { return true };
+    loop {
+        let Ok((maybe_player, parent)) = parents.get(current) else { return true };
+        if maybe_player.is_some() {
+            return false;
+        }
+        if let Some(parent) = parent {
+            current = parent.get();
+        } else {
+            return true;
+        }
+    }
+}
+
 /// System that will play all animations, using any entity with a [`AnimationPlayer`]
 /// and a [`Handle<AnimationClip>`] as an animation root
 pub fn animation_player(
     time: Res<Time>,
     animations: Res<Assets<AnimationClip>>,
-    mut animation_players: Query<(Entity, &mut AnimationPlayer)>,
-    names: Query<&Name>,
-    mut transforms: Query<&mut Transform>,
     children: Query<&Children>,
+    names: Query<&Name>,
+    transforms: Query<&mut Transform>,
+    parents: Query<(Option<With<AnimationPlayer>>, Option<&Parent>)>,
+    mut animation_players: Query<(Entity, Option<&Parent>, &mut AnimationPlayer)>,
 ) {
-    for (entity, mut player) in &mut animation_players {
-        if let Some(animation_clip) = animations.get(&player.animation_clip) {
-            // Continue if paused unless the `AnimationPlayer` was changed
-            // This allow the animation to still be updated if the player.elapsed field was manually updated in pause
-            if player.paused && !player.is_changed() {
-                continue;
-            }
-            if !player.paused {
-                player.elapsed += time.delta_seconds() * player.speed;
-            }
-            let mut elapsed = player.elapsed;
-            if player.repeat {
-                elapsed %= animation_clip.duration;
-            }
-            if elapsed < 0.0 {
-                elapsed += animation_clip.duration;
-            }
-            'entity: for (path, curves) in &animation_clip.curves {
-                // PERF: finding the target entity can be optimised
-                let mut current_entity = entity;
-                // Ignore the first name, it is the root node which we already have
-                for part in path.parts.iter().skip(1) {
-                    let mut found = false;
-                    if let Ok(children) = children.get(current_entity) {
-                        for child in children.deref() {
-                            if let Ok(name) = names.get(*child) {
-                                if name == part {
-                                    // Found a children with the right name, continue to the next part
-                                    current_entity = *child;
-                                    found = true;
-                                    break;
-                                }
-                            }
+    animation_players.par_for_each_mut(10, |(root, maybe_parent, mut player)| {
+        let Some(animation_clip) = animations.get(&player.animation_clip) else { return };
+        // Continue if paused unless the `AnimationPlayer` was changed
+        // This allow the animation to still be updated if the player.elapsed field was manually updated in pause
+        if player.paused && !player.is_changed() {
+            return;
+        }
+        if !player.paused {
+            player.elapsed += time.delta_seconds() * player.speed;
+        }
+        let mut elapsed = player.elapsed;
+        if player.repeat {
+            elapsed %= animation_clip.duration;
+        }
+        if elapsed < 0.0 {
+            elapsed += animation_clip.duration;
+        }
+        if player.path_cache.len() != animation_clip.paths.len() {
+            player.path_cache = vec![Vec::new(); animation_clip.paths.len()];
+        }
+        if !verify_no_ancestor_player(maybe_parent, &parents) {
+            warn!("Animation player on {:?} has a conflicting animation player on an ancestor. Cannot safely animate.", root);
+            return;
+        }
+        for (path, bone_id) in &animation_clip.paths {
+            let cached_path = &mut player.path_cache[*bone_id];
+            let curves = animation_clip.get_curves(*bone_id).unwrap();
+            let Some(target) = find_bone(root, path, &children, &names, cached_path) else { continue };
+            // SAFETY: The verify_no_ancestor_player check above ensures that two animation players cannot alias
+            // any of their descendant Transforms.
+            // 
+            // The system scheduler prevents any other system from mutating Transforms at the same time, 
+            // so the only way this fetch can alias is if two AnimationPlayers are targetting the same bone.
+            // This can only happen if there are two or more AnimationPlayers are ancestors to the same
+            // entities. By verifying that there is no other AnimationPlayer in the ancestors of a
+            // running AnimationPlayer before animating any entity, this fetch cannot alias.
+            //
+            // This means only the AnimationPlayers closest to the root of the hierarchy will be able
+            // to run their animation. Any players in the children or descendants will log a warning
+            // and do nothing.
+            let Ok(mut transform) = (unsafe { transforms.get_unchecked(target) }) else { continue };
+            for curve in curves {
+                // Some curves have only one keyframe used to set a transform
+                if curve.keyframe_timestamps.len() == 1 {
+                    match &curve.keyframes {
+                        Keyframes::Rotation(keyframes) => transform.rotation = keyframes[0],
+                        Keyframes::Translation(keyframes) => {
+                            transform.translation = keyframes[0];
                         }
+                        Keyframes::Scale(keyframes) => transform.scale = keyframes[0],
                     }
-                    if !found {
-                        warn!("Entity not found for path {:?} on part {:?}", path, part);
-                        continue 'entity;
-                    }
+                    continue;
                 }
-                if let Ok(mut transform) = transforms.get_mut(current_entity) {
-                    for curve in curves {
-                        // Some curves have only one keyframe used to set a transform
-                        if curve.keyframe_timestamps.len() == 1 {
-                            match &curve.keyframes {
-                                Keyframes::Rotation(keyframes) => transform.rotation = keyframes[0],
-                                Keyframes::Translation(keyframes) => {
-                                    transform.translation = keyframes[0];
-                                }
-                                Keyframes::Scale(keyframes) => transform.scale = keyframes[0],
-                            }
-                            continue;
-                        }
 
-                        // Find the current keyframe
-                        // PERF: finding the current keyframe can be optimised
-                        let step_start = match curve
-                            .keyframe_timestamps
-                            .binary_search_by(|probe| probe.partial_cmp(&elapsed).unwrap())
-                        {
-                            Ok(n) if n >= curve.keyframe_timestamps.len() - 1 => continue, // this curve is finished
-                            Ok(i) => i,
-                            Err(0) => continue, // this curve isn't started yet
-                            Err(n) if n > curve.keyframe_timestamps.len() - 1 => continue, // this curve is finished
-                            Err(i) => i - 1,
-                        };
-                        let ts_start = curve.keyframe_timestamps[step_start];
-                        let ts_end = curve.keyframe_timestamps[step_start + 1];
-                        let lerp = (elapsed - ts_start) / (ts_end - ts_start);
-
-                        // Apply the keyframe
-                        match &curve.keyframes {
-                            Keyframes::Rotation(keyframes) => {
-                                let rot_start = keyframes[step_start];
-                                let mut rot_end = keyframes[step_start + 1];
-                                // Choose the smallest angle for the rotation
-                                if rot_end.dot(rot_start) < 0.0 {
-                                    rot_end = -rot_end;
-                                }
-                                // Rotations are using a spherical linear interpolation
-                                transform.rotation =
-                                    rot_start.normalize().slerp(rot_end.normalize(), lerp);
-                            }
-                            Keyframes::Translation(keyframes) => {
-                                let translation_start = keyframes[step_start];
-                                let translation_end = keyframes[step_start + 1];
-                                let result = translation_start.lerp(translation_end, lerp);
-                                transform.translation = result;
-                            }
-                            Keyframes::Scale(keyframes) => {
-                                let scale_start = keyframes[step_start];
-                                let scale_end = keyframes[step_start + 1];
-                                let result = scale_start.lerp(scale_end, lerp);
-                                transform.scale = result;
-                            }
+                // Find the current keyframe
+                // PERF: finding the current keyframe can be optimised
+                let step_start = match curve
+                    .keyframe_timestamps
+                    .binary_search_by(|probe| probe.partial_cmp(&elapsed).unwrap())
+                {
+                    Ok(n) if n >= curve.keyframe_timestamps.len() - 1 => continue, // this curve is finished
+                    Ok(i) => i,
+                    Err(0) => continue, // this curve isn't started yet
+                    Err(n) if n > curve.keyframe_timestamps.len() - 1 => continue, // this curve is finished
+                    Err(i) => i - 1,
+                };
+                let ts_start = curve.keyframe_timestamps[step_start];
+                let ts_end = curve.keyframe_timestamps[step_start + 1];
+                let lerp = (elapsed - ts_start) / (ts_end - ts_start);
+
+                // Apply the keyframe
+                match &curve.keyframes {
+                    Keyframes::Rotation(keyframes) => {
+                        let rot_start = keyframes[step_start];
+                        let mut rot_end = keyframes[step_start + 1];
+                        // Choose the smallest angle for the rotation
+                        if rot_end.dot(rot_start) < 0.0 {
+                            rot_end = -rot_end;
                         }
+                        // Rotations are using a spherical linear interpolation
+                        transform.rotation =
+                            rot_start.normalize().slerp(rot_end.normalize(), lerp);
+                    }
+                    Keyframes::Translation(keyframes) => {
+                        let translation_start = keyframes[step_start];
+                        let translation_end = keyframes[step_start + 1];
+                        let result = translation_start.lerp(translation_end, lerp);
+                        transform.translation = result;
+                    }
+                    Keyframes::Scale(keyframes) => {
+                        let scale_start = keyframes[step_start];
+                        let scale_end = keyframes[step_start + 1];
+                        let result = scale_start.lerp(scale_end, lerp);
+                        transform.scale = result;
                     }
                 }
             }
         }
-    }
+    });
 }
 
 /// Adds animation support to an app