Implement avatar animation and skinning (#67)

crates/libremetaverse/src/animesh_skinning.rs

@@ -0,0 +1,238 @@
+//! CPU-side forward kinematics and bounded linear-blend skinning.
+
+#![allow(clippy::needless_pass_by_value, clippy::unnecessary_wraps)] // Mapped signatures are fixed.
+
+use crate::animesh_runtime::JointPose;
+use crate::rendering::{Face, Joint, LindenSkeleton};
+use libremetaverse_types::{Error, Matrix4, Quaternion, Vector3};
+use std::collections::HashMap;
+
+const MAX_SKIN_JOINTS: usize = 512;
+const MAX_VERTICES: usize = 1_000_000;
+
+#[derive(Clone, Debug, Default)]
+pub struct MeshSkinData {
+    pub alt_inverse_bind_matrices: Vec<f32>,
+    pub bind_shape_matrix: Vec<f32>,
+    pub inverse_bind_matrices: Vec<f32>,
+    pub joint_names: Vec<String>,
+    pub lock_scale_if_joint_position: bool,
+    pub pelvis_offset: f32,
+}
+
+impl MeshSkinData {
+    pub(crate) fn native_new() -> Result<Self, Error> {
+        Ok(Self::default())
+    }
+}
+
+pub struct AnimeshSkinning;
+
+impl AnimeshSkinning {
+    pub(crate) fn native_compute_skinning_matrices(
+        pose: Option<HashMap<String, JointPose>>,
+        skeleton: Option<LindenSkeleton>,
+        skin_data: Option<MeshSkinData>,
+    ) -> Result<Vec<Matrix4>, Error> {
+        let pose = pose.ok_or(Error::ArgumentNull)?;
+        let skeleton = skeleton.ok_or(Error::ArgumentNull)?;
+        let skin_data = skin_data.ok_or(Error::ArgumentNull)?;
+        if skin_data.joint_names.len() > MAX_SKIN_JOINTS {
+            return Err(Error::Argument);
+        }
+        let mut world = HashMap::new();
+        let mut visited = 0_usize;
+        compute_world(
+            &skeleton.bone(),
+            Matrix4::identity(),
+            &pose,
+            &mut world,
+            &mut visited,
+        )?;
+        skin_data
+            .joint_names
+            .iter()
+            .enumerate()
+            .map(|(index, name)| {
+                let Some(world) = world.get(name).copied() else {
+                    return Ok(Matrix4::identity());
+                };
+                let inverse = extract_matrix(&skin_data.inverse_bind_matrices, index);
+                Matrix4::multiply_with_matrix4_matrix4(inverse, world)
+            })
+            .collect()
+    }
+
+    pub(crate) fn native_deform_vertices(
+        face: Face,
+        matrices: Vec<Matrix4>,
+        bind_shape: Matrix4,
+        positions: &mut [Vector3],
+        mut normals: Option<&mut [Vector3]>,
+    ) -> Result<(), Error> {
+        let count = face.vertices.len();
+        if count > MAX_VERTICES || positions.len() < count {
+            return Err(Error::Argument);
+        }
+        if normals.as_ref().is_some_and(|values| values.len() < count) {
+            return Err(Error::Argument);
+        }
+        for (index, vertex) in face.vertices.iter().enumerate() {
+            let Some(weights) = face.weights.as_ref().and_then(|values| values.get(index)) else {
+                positions[index] = vertex.position;
+                if let Some(output) = normals.as_deref_mut() {
+                    output[index] = vertex.normal;
+                }
+                continue;
+            };
+            if matrices.is_empty() {
+                positions[index] = vertex.position;
+                if let Some(output) = normals.as_deref_mut() {
+                    output[index] = vertex.normal;
+                }
+                continue;
+            }
+            let position = Vector3::transform(vertex.position, bind_shape)?;
+            let influences = [
+                (weights.joint0, weights.weight0),
+                (weights.joint1, weights.weight1),
+                (weights.joint2, weights.weight2),
+                (weights.joint3, weights.weight3),
+            ];
+            let mut result = Vector3::zero();
+            for (joint, weight) in influences {
+                if weight > 0.0 && weight.is_finite() {
+                    let joint = usize::try_from(joint).map_err(|_| Error::Argument)?;
+                    let matrix = matrices.get(joint).ok_or(Error::Argument)?;
+                    result = Vector3::add_with_vector3_vector3(
+                        result,
+                        Vector3::multiply_with_vector3_single(
+                            Vector3::transform(position, *matrix)?,
+                            weight,
+                        )?,
+                    )?;
+                }
+            }
+            positions[index] = result;
+            if let Some(output) = normals.as_deref_mut() {
+                let mut result = Vector3::zero();
+                for (joint, weight) in influences {
+                    if weight > 0.0 && weight.is_finite() {
+                        let joint = usize::try_from(joint).map_err(|_| Error::Argument)?;
+                        let matrix = matrices.get(joint).ok_or(Error::Argument)?;
+                        result = Vector3::add_with_vector3_vector3(
+                            result,
+                            Vector3::multiply_with_vector3_single(
+                                Vector3::transform_normal(vertex.normal, *matrix)?,
+                                weight,
+                            )?,
+                        )?;
+                    }
+                }
+                output[index] = if result == Vector3::zero() {
+                    result
+                } else {
+                    Vector3::normalize(result)?
+                };
+            }
+        }
+        Ok(())
+    }
+}
+
+fn compute_world(
+    joint: &Joint,
+    parent: Matrix4,
+    pose: &HashMap<String, JointPose>,
+    output: &mut HashMap<String, Matrix4>,
+    visited: &mut usize,
+) -> Result<(), Error> {
+    *visited = visited.checked_add(1).ok_or(Error::Argument)?;
+    if *visited > MAX_SKIN_JOINTS {
+        return Err(Error::Argument);
+    }
+    let local = local_transform(joint, pose)?;
+    let world = Matrix4::multiply_with_matrix4_matrix4(local, parent)?;
+    output.insert(joint.base.name(), world);
+    for alias in joint.get_aliases_list()? {
+        output.insert(alias, world);
+    }
+    for child in joint.bone().unwrap_or_default() {
+        compute_world(&child, world, pose, output, visited)?;
+    }
+    Ok(())
+}
+
+fn local_transform(joint: &Joint, pose: &HashMap<String, JointPose>) -> Result<Matrix4, Error> {
+    let mut translation = vector3(&joint.base.pos());
+    let mut rotation = rotation(&joint.base.rot())?;
+    let override_pose = pose.get(&joint.base.name()).copied().or_else(|| {
+        joint
+            .get_aliases_list()
+            .ok()?
+            .into_iter()
+            .find_map(|alias| pose.get(&alias).copied())
+    });
+    if let Some(value) = override_pose {
+        if value.has_position {
+            translation = value.position;
+        }
+        if value.has_rotation {
+            rotation = value.rotation;
+        }
+    }
+    Matrix4::multiply_with_matrix4_matrix4(
+        Matrix4::create_from_quaternion(rotation)?,
+        Matrix4::create_translation(translation)?,
+    )
+}
+
+fn vector3(values: &[f32]) -> Vector3 {
+    if values.len() < 3 {
+        Vector3::zero()
+    } else {
+        Vector3 {
+            x: values[0],
+            y: values[1],
+            z: values[2],
+        }
+    }
+}
+
+fn rotation(values: &[f32]) -> Result<Quaternion, Error> {
+    if values.len() < 3 {
+        return Ok(Quaternion::identity());
+    }
+    Quaternion::create_from_eulers_with_single_single_single(
+        values[0].to_radians(),
+        values[1].to_radians(),
+        values[2].to_radians(),
+    )
+}
+
+fn extract_matrix(values: &[f32], index: usize) -> Matrix4 {
+    let Some(offset) = index.checked_mul(16) else {
+        return Matrix4::identity();
+    };
+    let Some(values) = values.get(offset..offset.saturating_add(16)) else {
+        return Matrix4::identity();
+    };
+    Matrix4 {
+        m11: values[0],
+        m12: values[1],
+        m13: values[2],
+        m14: values[3],
+        m21: values[4],
+        m22: values[5],
+        m23: values[6],
+        m24: values[7],
+        m31: values[8],
+        m32: values[9],
+        m33: values[10],
+        m34: values[11],
+        m41: values[12],
+        m42: values[13],
+        m43: values[14],
+        m44: values[15],
+    }
+}