MetaCrate/crates/libremetaverse/src/avatar_rig.rs

//! Backend-independent avatar hierarchy and rigged-attachment math.

#![allow(
    clippy::needless_pass_by_value,
    clippy::too_many_arguments,
    clippy::unnecessary_wraps
)] // Mapped signatures and recursive hierarchy state are fixed.

use crate::Error;
use crate::animesh_skinning::MeshSkinData;
use crate::rendering::{BoneTransform, Joint, LindenSkeleton};
use crate::visual_catalog::VisualParams;
use libremetaverse_types::compat::{
    Matrix4x4, Quaternion as NumericsQuaternion, Vector3 as NumericsVector3,
};
use libremetaverse_types::{Matrix4, Quaternion, UUID, Vector3};
use roxmltree::Document;
use std::collections::HashMap;
use std::path::Path;

const MAX_JOINTS: usize = 512;
const MAX_LAD_BYTES: u64 = 8 * 1024 * 1024;
const MAX_ATTACHMENT_POINTS: usize = 256;
const MAX_MESH_DEFINITIONS: usize = 512;

#[derive(Clone, Debug, PartialEq)]
pub struct AvatarAttachmentPoint {
    group: i32,
    id: i32,
    joint: String,
    location: String,
    name: String,
    position: Vector3,
    rotation: Vector3,
    visible_in_first_person: bool,
}

impl AvatarAttachmentPoint {
    pub(crate) fn native_group(&self) -> i32 {
        self.group
    }

    pub(crate) fn native_id(&self) -> i32 {
        self.id
    }

    pub(crate) fn native_joint(&self) -> String {
        self.joint.clone()
    }

    pub(crate) fn native_location(&self) -> String {
        self.location.clone()
    }

    pub(crate) fn native_name(&self) -> String {
        self.name.clone()
    }

    pub(crate) fn native_position(&self) -> Vector3 {
        self.position
    }

    pub(crate) fn native_rotation(&self) -> Vector3 {
        self.rotation
    }

    pub(crate) fn native_visible_in_first_person(&self) -> bool {
        self.visible_in_first_person
    }
}

#[derive(Clone, Debug, PartialEq)]
pub struct AvatarMeshDefinition {
    file_name: String,
    lod_level: i32,
    min_pixel_width: i32,
    type_: String,
}

impl AvatarMeshDefinition {
    pub(crate) fn native_file_name(&self) -> String {
        self.file_name.clone()
    }

    pub(crate) fn native_lod_level(&self) -> i32 {
        self.lod_level
    }

    pub(crate) fn native_min_pixel_width(&self) -> i32 {
        self.min_pixel_width
    }

    pub(crate) fn native_type(&self) -> String {
        self.type_.clone()
    }
}

#[derive(Clone, Debug, PartialEq)]
pub struct LindenAvatarDefinition {
    attachment_points: Vec<AvatarAttachmentPoint>,
    mesh_definitions: Vec<AvatarMeshDefinition>,
    skeleton: LindenSkeleton,
}

impl LindenAvatarDefinition {
    pub(crate) fn native_load(
        lad_file_name: Option<String>,
        skeleton_file_name: Option<String>,
    ) -> Result<Self, Error> {
        let skeleton = LindenSkeleton::load_with_string(skeleton_file_name)?;
        let owned_xml;
        let xml = if let Some(file_name) = lad_file_name {
            owned_xml = read_bounded(&file_name)?;
            owned_xml.as_str()
        } else {
            include_str!("../../../codegen/inputs/avatar_lad.xml")
        };
        let document = Document::parse(xml).map_err(|_| Error::Argument)?;
        let mut attachment_points = Vec::new();
        let mut mesh_definitions = Vec::new();
        for node in document.descendants().filter(roxmltree::Node::is_element) {
            match node.tag_name().name() {
                "attachment_point" => {
                    let Some(id) = parse_i32(node.attribute("id")) else {
                        continue;
                    };
                    if attachment_points.len() >= MAX_ATTACHMENT_POINTS {
                        return Err(Error::Argument);
                    }
                    attachment_points.push(AvatarAttachmentPoint {
                        group: parse_i32(node.attribute("group")).unwrap_or_default(),
                        id,
                        joint: node.attribute("joint").unwrap_or_default().to_owned(),
                        location: node.attribute("location").unwrap_or_default().to_owned(),
                        name: node.attribute("name").unwrap_or_default().to_owned(),
                        position: parse_vector(node.attribute("position")),
                        rotation: parse_vector(node.attribute("rotation")),
                        visible_in_first_person: node
                            .attribute("visible_in_first_person")
                            .is_some_and(|value| value.eq_ignore_ascii_case("true")),
                    });
                }
                "mesh" => {
                    let (Some(type_), Some(lod_level)) =
                        (node.attribute("type"), parse_i32(node.attribute("lod")))
                    else {
                        continue;
                    };
                    if mesh_definitions.len() >= MAX_MESH_DEFINITIONS {
                        return Err(Error::Argument);
                    }
                    mesh_definitions.push(AvatarMeshDefinition {
                        file_name: node.attribute("file_name").unwrap_or_default().to_owned(),
                        lod_level,
                        min_pixel_width: parse_i32(node.attribute("min_pixel_width"))
                            .unwrap_or_default(),
                        type_: type_.to_owned(),
                    });
                }
                _ => {}
            }
        }
        Ok(Self {
            attachment_points,
            mesh_definitions,
            skeleton,
        })
    }

    pub(crate) fn native_compute_bone_transforms(
        &self,
        param_values: &HashMap<i32, f32>,
    ) -> Result<HashMap<String, BoneTransform>, Error> {
        let mut result = HashMap::new();
        let joints = self.skeleton.get_all_joints()?.collect::<Vec<_>>();
        if joints.len() > MAX_JOINTS {
            return Err(Error::Argument);
        }
        for joint in joints {
            seed_transform(&mut result, &joint.base);
            for volume in joint.collision_volume() {
                seed_transform(&mut result, &volume.base);
            }
        }
        for param in VisualParams::params().0.into_values() {
            let raw_value = param_values
                .get(&param.param_id)
                .copied()
                .unwrap_or(param.default_value);
            let weight = raw_value.clamp(param.min_value, param.max_value);
            if !weight.is_finite() {
                return Err(Error::Argument);
            }
            for distortion in param.skeletal_distortions.unwrap_or_default() {
                let Some(transform) = result.get_mut(&distortion.bone_name) else {
                    continue;
                };
                transform.scale = add_scaled(transform.scale, distortion.scale_deformation, weight);
                if distortion.has_position_deformation {
                    transform.position =
                        add_scaled(transform.position, distortion.position_deformation, weight);
                }
            }
            for morph in param.volume_morphs.unwrap_or_default() {
                let Some(transform) = result.get_mut(&morph.bone_name) else {
                    continue;
                };
                if morph.has_scale {
                    transform.scale = add_scaled(transform.scale, morph.scale_delta, weight);
                }
                if morph.has_position {
                    transform.position =
                        add_scaled(transform.position, morph.position_delta, weight);
                }
            }
        }
        if result
            .values()
            .any(|value| !finite(value.position) || !finite(value.scale))
        {
            return Err(Error::Argument);
        }
        Ok(result)
    }

    pub(crate) fn native_get_attachment_point_by_id(
        &self,
        id: i32,
    ) -> Option<AvatarAttachmentPoint> {
        self.attachment_points
            .iter()
            .find(|point| point.id == id)
            .cloned()
    }

    pub(crate) fn native_get_attachment_point_by_name(
        &self,
        name: &str,
    ) -> Option<AvatarAttachmentPoint> {
        self.attachment_points
            .iter()
            .find(|point| point.name.eq_ignore_ascii_case(name))
            .cloned()
    }

    pub(crate) fn native_attachment_points(&self) -> Vec<AvatarAttachmentPoint> {
        self.attachment_points.clone()
    }

    pub(crate) fn native_mesh_definitions(&self) -> Vec<AvatarMeshDefinition> {
        self.mesh_definitions.clone()
    }

    pub(crate) fn native_skeleton(&self) -> LindenSkeleton {
        self.skeleton.clone()
    }
}

#[derive(Clone, Debug, Default)]
pub struct AttachmentRiggedSkin {
    pub inv_bind_matrices: Vec<Matrix4x4>,
    pub joint_names: Vec<String>,
    pub joint_position_overrides: Vec<(String, NumericsVector3)>,
    pub joints: Vec<i32>,
    pub lock_scale_if_joint_position: bool,
    pub mesh_id: UUID,
    pub weights: Vec<f32>,
}

impl AttachmentRiggedSkin {
    pub(crate) fn native_new() -> Result<Self, Error> {
        Ok(Self::default())
    }
}

pub struct AvatarBoneMath;

impl AvatarBoneMath {
    pub(crate) fn native_build_bone_world_matrices(
        skeleton: LindenSkeleton,
        bone_transforms: HashMap<String, BoneTransform>,
    ) -> Result<HashMap<String, Matrix4x4>, Error> {
        let mut output = HashMap::new();
        let mut visited = 0;
        build_joint(
            &skeleton.bone(),
            Quaternion::identity(),
            Vector3::zero(),
            Vector3::one(),
            &bone_transforms,
            None,
            &mut output,
            &mut visited,
        )?;
        Ok(output)
    }

    pub(crate) fn native_compute_animated_bone_world_matrices(
        avatar_definition: &LindenAvatarDefinition,
        bone_transforms: &HashMap<String, BoneTransform>,
        rotation_deltas: &HashMap<String, NumericsQuaternion>,
    ) -> Result<HashMap<String, Matrix4x4>, Error> {
        let mut output = HashMap::new();
        let mut visited = 0;
        build_joint(
            &avatar_definition.skeleton.bone(),
            Quaternion::identity(),
            Vector3::zero(),
            Vector3::one(),
            bone_transforms,
            Some(rotation_deltas),
            &mut output,
            &mut visited,
        )?;
        Ok(output)
    }

    pub(crate) fn native_compute_attachment_bone_world_matrices(
        avatar_definition: &LindenAvatarDefinition,
        bone_transforms: &HashMap<String, BoneTransform>,
        rotation_deltas: &HashMap<String, NumericsQuaternion>,
    ) -> Result<HashMap<String, Matrix4x4>, Error> {
        Self::native_compute_animated_bone_world_matrices(
            avatar_definition,
            bone_transforms,
            rotation_deltas,
        )
    }

    pub(crate) fn native_strip_scale(matrix: Matrix4x4) -> Matrix4x4 {
        let mut value = matrix.0;
        for row in 0..3 {
            let offset = row * 4;
            let length = (value[offset].mul_add(
                value[offset],
                value[offset + 1].mul_add(value[offset + 1], value[offset + 2] * value[offset + 2]),
            ))
            .sqrt();
            if length > 1.0e-5 {
                value[offset] /= length;
                value[offset + 1] /= length;
                value[offset + 2] /= length;
            }
            value[offset + 3] = 0.0;
        }
        Matrix4x4(value)
    }

    pub(crate) fn native_compute_ground_adjustment(
        transforms: Option<&HashMap<String, BoneTransform>>,
    ) -> f32 {
        let Some(transforms) = transforms else {
            return 1.0;
        };
        let names = [
            "mPelvis",
            "mSkull",
            "mNeck",
            "mChest",
            "mHead",
            "mTorso",
            "mHipLeft",
            "mKneeLeft",
            "mAnkleLeft",
            "mFootLeft",
        ];
        let Some(values) = names
            .iter()
            .map(|name| transforms.get(*name))
            .collect::<Option<Vec<_>>>()
        else {
            return 1.0;
        };
        let [
            pelvis,
            skull,
            neck,
            chest,
            head,
            torso,
            hip,
            knee,
            ankle,
            foot,
        ] = values.as_slice()
        else {
            return 1.0;
        };
        let pelvis_to_foot = hip.position.z * pelvis.scale.z
            - knee.position.z * hip.scale.z
            - ankle.position.z * knee.scale.z
            - foot.position.z * ankle.scale.z;
        let height = pelvis_to_foot
            + std::f32::consts::SQRT_2 * skull.position.z * head.scale.z
            + head.position.z * neck.scale.z
            + neck.position.z * chest.scale.z
            + chest.position.z * torso.scale.z
            + torso.position.z * pelvis.scale.z;
        let adjustment = height - pelvis_to_foot;
        if adjustment.is_finite() && adjustment > 0.0 {
            adjustment
        } else {
            1.0
        }
    }
}

fn build_joint(
    joint: &Joint,
    parent_rotation: Quaternion,
    parent_position: Vector3,
    parent_scale: Vector3,
    transforms: &HashMap<String, BoneTransform>,
    rotation_deltas: Option<&HashMap<String, NumericsQuaternion>>,
    output: &mut HashMap<String, Matrix4x4>,
    visited: &mut usize,
) -> Result<(), Error> {
    *visited = visited.checked_add(1).ok_or(Error::Argument)?;
    if *visited > MAX_JOINTS {
        return Err(Error::Argument);
    }
    let name = joint.base.name();
    let transform = transforms.get(&name);
    let position = transform.map_or_else(
        || vector(&joint.base.pos(), Vector3::zero()),
        |v| v.position,
    );
    let scale = transform.map_or_else(|| vector(&joint.base.scale(), Vector3::one()), |v| v.scale);
    if !finite(position) || !finite(scale) {
        return Err(Error::Argument);
    }
    let mut rotation = euler(&joint.base.rot())?;
    if let Some(delta) = rotation_deltas.and_then(|values| values.get(&name)) {
        rotation = Quaternion::multiply_with_quaternion_quaternion(rotation, quaternion(*delta)?)?;
    }
    let world_rotation =
        Quaternion::multiply_with_quaternion_quaternion(parent_rotation, rotation)?;
    let scaled_position = Vector3::multiply_with_vector3_vector3(position, parent_scale)?;
    let rotated_position = Vector3::transform_normal(
        scaled_position,
        Matrix4::create_from_quaternion(parent_rotation)?,
    )?;
    let world_position = Vector3::add_with_vector3_vector3(rotated_position, parent_position)?;
    let world = Matrix4::multiply_with_matrix4_matrix4(
        Matrix4::multiply_with_matrix4_matrix4(
            Matrix4::create_scale(scale)?,
            Matrix4::create_from_quaternion(world_rotation)?,
        )?,
        Matrix4::create_translation(world_position)?,
    )?;
    let world = numerics(world);
    if !name.is_empty() {
        output.insert(name, world);
    }
    for alias in joint.get_aliases_list()? {
        output.entry(alias).or_insert(world);
    }
    for volume in joint.collision_volume() {
        *visited = visited.checked_add(1).ok_or(Error::Argument)?;
        if *visited > MAX_JOINTS {
            return Err(Error::Argument);
        }
        let volume_name = volume.base.name();
        let volume_transform = transforms.get(&volume_name);
        let volume_position = volume_transform.map_or_else(
            || vector(&volume.base.pos(), Vector3::zero()),
            |value| value.position,
        );
        let volume_scale = volume_transform.map_or_else(
            || vector(&volume.base.scale(), Vector3::one()),
            |value| value.scale,
        );
        if !finite(volume_position) || !finite(volume_scale) {
            return Err(Error::Argument);
        }
        let volume_rotation = euler(&volume.base.rot())?;
        let volume_world_rotation =
            Quaternion::multiply_with_quaternion_quaternion(world_rotation, volume_rotation)?;
        let volume_scaled_position =
            Vector3::multiply_with_vector3_vector3(volume_position, scale)?;
        let volume_rotated_position = Vector3::transform_normal(
            volume_scaled_position,
            Matrix4::create_from_quaternion(world_rotation)?,
        )?;
        let volume_world_position =
            Vector3::add_with_vector3_vector3(volume_rotated_position, world_position)?;
        let volume_world = Matrix4::multiply_with_matrix4_matrix4(
            Matrix4::multiply_with_matrix4_matrix4(
                Matrix4::create_scale(volume_scale)?,
                Matrix4::create_from_quaternion(volume_world_rotation)?,
            )?,
            Matrix4::create_translation(volume_world_position)?,
        )?;
        if !volume_name.is_empty() {
            output.entry(volume_name).or_insert(numerics(volume_world));
        }
    }
    for child in joint.bone().unwrap_or_default() {
        build_joint(
            &child,
            world_rotation,
            world_position,
            scale,
            transforms,
            rotation_deltas,
            output,
            visited,
        )?;
    }
    Ok(())
}

fn vector(values: &[f32], fallback: Vector3) -> Vector3 {
    if values.len() < 3 {
        fallback
    } else {
        Vector3 {
            x: values[0],
            y: values[1],
            z: values[2],
        }
    }
}

fn finite(value: Vector3) -> bool {
    value.x.is_finite() && value.y.is_finite() && value.z.is_finite()
}

fn read_bounded(path: impl AsRef<Path>) -> Result<String, Error> {
    let path = path.as_ref();
    if std::fs::metadata(path).map_err(|_| Error::Argument)?.len() > MAX_LAD_BYTES {
        return Err(Error::Argument);
    }
    let contents = std::fs::read_to_string(path).map_err(|_| Error::Argument)?;
    if contents.len() as u64 > MAX_LAD_BYTES {
        return Err(Error::Argument);
    }
    Ok(contents)
}

fn parse_i32(value: Option<&str>) -> Option<i32> {
    value?.parse().ok()
}

fn parse_vector(value: Option<&str>) -> Vector3 {
    let mut values = value.unwrap_or_default().split_ascii_whitespace();
    let mut next = || {
        values
            .next()
            .and_then(|part| part.parse::<f32>().ok())
            .unwrap_or_default()
    };
    Vector3 {
        x: next(),
        y: next(),
        z: next(),
    }
}

fn seed_transform(
    output: &mut HashMap<String, BoneTransform>,
    joint: &crate::rendering::JointBase,
) {
    let name = joint.name();
    if name.is_empty() {
        return;
    }
    output.insert(
        name,
        BoneTransform {
            position: vector(&joint.pos(), Vector3::zero()),
            scale: vector(&joint.scale(), Vector3::one()),
        },
    );
}

fn add_scaled(value: Vector3, delta: Vector3, weight: f32) -> Vector3 {
    Vector3 {
        x: delta.x.mul_add(weight, value.x),
        y: delta.y.mul_add(weight, value.y),
        z: delta.z.mul_add(weight, value.z),
    }
}

fn quaternion(value: NumericsQuaternion) -> Result<Quaternion, Error> {
    if value.0.iter().all(|component| component.is_finite()) {
        Quaternion::new_with_single_single_single_single(
            value.0[0], value.0[1], value.0[2], value.0[3],
        )
    } else {
        Err(Error::Argument)
    }
}

fn euler(values: &[f32]) -> Result<Quaternion, Error> {
    let value = vector(values, Vector3::zero());
    Quaternion::create_from_eulers_with_single_single_single(
        value.x.to_radians(),
        value.y.to_radians(),
        value.z.to_radians(),
    )
}

fn numerics(value: Matrix4) -> Matrix4x4 {
    Matrix4x4([
        value.m11, value.m12, value.m13, value.m14, value.m21, value.m22, value.m23, value.m24,
        value.m31, value.m32, value.m33, value.m34, value.m41, value.m42, value.m43, value.m44,
    ])
}

pub struct RiggedSkinMath;

impl RiggedSkinMath {
    pub(crate) fn native_floats_to_matrix(values: &[f32]) -> Matrix4x4 {
        let Ok(values) = <&[f32; 16]>::try_from(values) else {
            return Matrix4x4::default();
        };
        Matrix4x4(*values)
    }

    pub(crate) fn native_build_inv_bind_matrices(
        skin: &MeshSkinData,
    ) -> Result<Vec<Matrix4x4>, Error> {
        if skin.joint_names.len() > MAX_JOINTS {
            return Err(Error::Argument);
        }
        Ok((0..skin.joint_names.len())
            .map(|index| {
                let start = index * 16;
                skin.inverse_bind_matrices
                    .get(start..start + 16)
                    .map_or_else(identity, Self::native_floats_to_matrix)
            })
            .collect())
    }

    pub(crate) fn native_extract_joint_position_overrides(
        skin: &MeshSkinData,
    ) -> Result<Vec<(String, NumericsVector3)>, Error> {
        let count = skin.joint_names.len();
        if count > MAX_JOINTS {
            return Err(Error::Argument);
        }
        if skin.alt_inverse_bind_matrices.len() / 16 != count
            || !skin.alt_inverse_bind_matrices.len().is_multiple_of(16)
        {
            return Ok(Vec::new());
        }
        Ok(skin
            .joint_names
            .iter()
            .enumerate()
            .filter(|(_, name)| !name.is_empty())
            .map(|(index, name)| {
                let offset = index * 16;
                (
                    name.clone(),
                    NumericsVector3([
                        skin.alt_inverse_bind_matrices[offset + 12],
                        skin.alt_inverse_bind_matrices[offset + 13],
                        skin.alt_inverse_bind_matrices[offset + 14],
                    ]),
                )
            })
            .collect())
    }

    pub(crate) fn native_normalize_skin_weights(
        joint_count: i32,
        joints: [&mut i32; 4],
        weights: [&mut f32; 4],
    ) {
        let mut sum = 0.0;
        for index in 0..4 {
            if *joints[index] < 0 || *joints[index] >= joint_count {
                *weights[index] = 0.0;
            }
            *weights[index] = if weights[index].is_finite() {
                weights[index].clamp(0.0, 1.0)
            } else {
                0.0
            };
            sum += *weights[index];
        }
        if sum > 1.0e-6 {
            for weight in weights {
                *weight /= sum;
            }
        } else {
            for index in 0..4 {
                *joints[index] = 0;
                *weights[index] = if index == 0 && joint_count > 0 {
                    1.0
                } else {
                    0.0
                };
            }
        }
    }
}

fn identity() -> Matrix4x4 {
    Matrix4x4([
        1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0,
    ])
}

#[cfg(test)]
mod tests {
    use super::*;

    #[test]
    fn embedded_avatar_definition_exposes_generated_skeleton_and_lad_records() {
        let definition =
            LindenAvatarDefinition::native_load(None, None).expect("avatar definition");
        assert!(!definition.native_attachment_points().is_empty());
        assert!(!definition.native_mesh_definitions().is_empty());
        assert!(
            definition
                .native_skeleton()
                .get_all_joints()
                .expect("joints")
                .count()
                > 1
        );
        let point = definition.native_attachment_points()[0].clone();
        assert_eq!(
            definition
                .native_get_attachment_point_by_id(point.native_id())
                .expect("point by id"),
            point
        );
        assert_eq!(
            definition
                .native_get_attachment_point_by_name(&point.native_name().to_uppercase())
                .expect("point by name"),
            point
        );
    }

    #[test]
    fn animated_and_attachment_world_matrices_apply_rotation_deltas() {
        let definition =
            LindenAvatarDefinition::native_load(None, None).expect("avatar definition");
        let transforms = definition
            .native_compute_bone_transforms(&HashMap::new())
            .expect("bone transforms");
        let bind = AvatarBoneMath::native_compute_animated_bone_world_matrices(
            &definition,
            &transforms,
            &HashMap::new(),
        )
        .expect("bind matrices");
        let delta = NumericsQuaternion([
            0.0,
            0.0,
            std::f32::consts::FRAC_1_SQRT_2,
            std::f32::consts::FRAC_1_SQRT_2,
        ]);
        let deltas = HashMap::from([("mPelvis".to_owned(), delta)]);
        let animated = AvatarBoneMath::native_compute_animated_bone_world_matrices(
            &definition,
            &transforms,
            &deltas,
        )
        .expect("animated matrices");
        let attachment = AvatarBoneMath::native_compute_attachment_bone_world_matrices(
            &definition,
            &transforms,
            &deltas,
        )
        .expect("attachment matrices");
        assert_ne!(animated["mPelvis"], bind["mPelvis"]);
        assert_eq!(attachment, animated);
    }
}