MetaCrate/crates/libremetaverse/src/animesh_skinning.rs

//! 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],
    }
}