MetaCrate/crates/libremetaverse/src/avatar_physics.rs

//! Native spring-mass-damper simulation for avatar physics wearables.

use crate::Error;
use libremetaverse_types::compat::Vector3;
use std::collections::HashMap;
use std::sync::{Arc, Mutex};

type BoneProvider = Arc<dyn Fn(String) -> Vector3 + Send + Sync>;

#[derive(Clone)]
struct Channel {
    driver: i32,
    driven: &'static [i32],
    direction: [f32; 3],
    params: [i32; 7],
    position: f32,
    velocity: f32,
    joint_velocity: f32,
    joint_acceleration: f32,
    last_time: f32,
    world_position: [f32; 3],
}

impl Channel {
    fn new(driver: i32, driven: &'static [i32], direction: [f32; 3], params: [i32; 7]) -> Self {
        Self {
            driver,
            driven,
            direction,
            params,
            position: 0.5,
            velocity: 0.0,
            joint_velocity: 0.0,
            joint_acceleration: 0.0,
            last_time: -1.0,
            world_position: [0.0; 3],
        }
    }
    fn reset(&mut self) {
        self.position = 0.5;
        self.velocity = 0.0;
        self.joint_velocity = 0.0;
        self.joint_acceleration = 0.0;
        self.last_time = -1.0;
        self.world_position = [0.0; 3];
    }
}

struct State {
    channels: Vec<Channel>,
    weights: HashMap<i32, f32>,
    provider: Option<BoneProvider>,
}

/// Stateful simulator matching the viewer's six LLPhysicsMotion channels.
pub struct AvatarPhysicsSimulator {
    state: Mutex<State>,
}

impl AvatarPhysicsSimulator {
    pub(crate) fn native_new() -> Result<Self, Error> {
        Ok(Self {
            state: Mutex::new(State {
                channels: vec![
                    Channel::new(
                        1100,
                        &[1200],
                        [0.0, 0.0, -1.0],
                        [10000, 10001, 10002, 10006, 10003, 10004, 10005],
                    ),
                    Channel::new(
                        1101,
                        &[1201],
                        [-1.0, 0.0, 0.0],
                        [10000, 10001, 10002, 10010, 10007, 10008, 10009],
                    ),
                    Channel::new(
                        1105,
                        &[1207],
                        [0.0, -1.0, 0.0],
                        [10000, 10001, 10002, 10032, 10029, 10030, 10031],
                    ),
                    Channel::new(
                        1103,
                        &[1205],
                        [0.0, 0.0, -1.0],
                        [10018, 10019, 10020, 10024, 10021, 10022, 10023],
                    ),
                    Channel::new(
                        1104,
                        &[1206],
                        [0.0, -1.0, 0.0],
                        [10018, 10019, 10020, 10028, 10025, 10026, 10027],
                    ),
                    Channel::new(
                        1102,
                        &[1202, 1203, 1204],
                        [0.0, 0.0, -1.0],
                        [10011, 10012, 10013, 10017, 10014, 10015, 10016],
                    ),
                ],
                weights: HashMap::new(),
                provider: None,
            }),
        })
    }

    pub(crate) fn native_set_wearable_params(
        &self,
        values: HashMap<i32, f32>,
    ) -> Result<(), Error> {
        let mut state = self
            .state
            .lock()
            .unwrap_or_else(std::sync::PoisonError::into_inner);
        state.weights = values;
        state.channels.iter_mut().for_each(Channel::reset);
        Ok(())
    }

    pub(crate) fn native_set_bone_position_provider(
        &self,
        provider: Box<dyn Fn(String) -> Vector3 + Send + Sync>,
    ) -> Result<(), Error> {
        self.state
            .lock()
            .unwrap_or_else(std::sync::PoisonError::into_inner)
            .provider = Some(provider.into());
        Ok(())
    }

    pub(crate) fn native_tick(&self, time: f32) -> Result<HashMap<i32, f32>, Error> {
        if !time.is_finite() {
            return Err(Error::Argument);
        }
        let mut state = self
            .state
            .lock()
            .unwrap_or_else(std::sync::PoisonError::into_inner);
        let weights = state.weights.clone();
        let provider = state.provider.clone();
        let mut output = HashMap::new();
        for channel in &mut state.channels {
            let position = step(channel, time, &weights, provider.as_ref());
            for id in channel.driven {
                output.insert(*id, position);
            }
        }
        Ok(output)
    }
}

fn param(weights: &HashMap<i32, f32>, id: i32, fallback: f32) -> f32 {
    weights
        .get(&id)
        .copied()
        .or_else(|| crate::visual_catalog::param_by_id(id).map(|p| p.default_value))
        .unwrap_or(fallback)
}

fn driver_position(channel: &Channel, weights: &HashMap<i32, f32>) -> f32 {
    let Some(definition) = crate::visual_catalog::param_by_id(channel.driver) else {
        return 0.5;
    };
    let range = definition.max_value - definition.min_value;
    if range.abs() < 1.0e-6 {
        return 0.0;
    }
    ((weights
        .get(&channel.driver)
        .copied()
        .unwrap_or(definition.default_value)
        - definition.min_value)
        / range)
        .clamp(0.0, 1.0)
}

fn dot(left: [f32; 3], right: [f32; 3]) -> f32 {
    left[0] * right[0] + left[1] * right[1] + left[2] * right[2]
}
fn subtract(left: [f32; 3], right: [f32; 3]) -> [f32; 3] {
    [left[0] - right[0], left[1] - right[1], left[2] - right[2]]
}
fn vector(value: Vector3) -> [f32; 3] {
    value.0
}

fn step(
    channel: &mut Channel,
    time: f32,
    weights: &HashMap<i32, f32>,
    provider: Option<&BoneProvider>,
) -> f32 {
    let joint_name = if matches!(channel.driver, 1100 | 1101 | 1105) {
        "mChest"
    } else {
        "mPelvis"
    };
    let joint = provider.map_or(channel.world_position, |get| vector(get(joint_name.into())));
    let target = driver_position(channel, weights);
    if channel.last_time < 0.0 {
        channel.last_time = time;
        channel.position = target;
        channel.world_position = joint;
        return target;
    }
    let dt = time - channel.last_time;
    if dt <= 0.0 || dt > 1.0 {
        channel.last_time = time;
        return channel.position.clamp(0.0, 1.0);
    }
    let [
        mass_id,
        gravity_id,
        drag_id,
        damping_id,
        max_id,
        spring_id,
        gain_id,
    ] = channel.params;
    let mass = param(weights, mass_id, 0.1).max(1.0e-6);
    let gravity = param(weights, gravity_id, 0.0);
    let drag = param(weights, drag_id, 1.0);
    let damping = param(weights, damping_id, 0.2);
    let max_effect = param(weights, max_id, 0.0);
    let spring = param(weights, spring_id, 10.0);
    let gain = param(weights, gain_id, 10.0);
    let scaled_dt = dt * 30.0;
    let joint_velocity =
        dot(subtract(joint, channel.world_position), channel.direction) * 100.0 / scaled_dt;
    let acceleration = ((joint_velocity - channel.joint_velocity) / scaled_dt) / 3.0
        + channel.joint_acceleration * (2.0 / 3.0);
    if max_effect == 0.0 && channel.position.clamp(0.0, 1.0) == target {
        channel.last_time = time;
        channel.world_position = joint;
        channel.joint_velocity = joint_velocity;
        channel.joint_acceleration = acceleration;
        return target;
    }
    let steps = (dt / 0.05) as u32 + 1;
    let sub_dt = dt / steps as f32;
    let mut position = channel.position.clamp(0.0, 1.0);
    let mut velocity = channel.velocity;
    for _ in 0..steps {
        let force = gain * acceleration * mass
            + dot([0.0, 0.0, 1.0], channel.direction) * gravity * mass
            - (position - target) * spring
            - damping * velocity
            + 0.5 * drag * joint_velocity * joint_velocity * joint_velocity.signum();
        velocity = (velocity + force / mass * sub_dt).clamp(-100.0, 100.0);
        position += velocity * sub_dt;
        if (position < 0.0 && velocity < 0.0) || (position > 1.0 && velocity > 0.0) {
            velocity = 0.0;
        }
    }
    if !position.is_finite() || !velocity.is_finite() {
        position = 0.0;
        velocity = 0.0;
        channel.joint_velocity = 0.0;
        channel.joint_acceleration = 0.0;
    }
    channel.last_time = time;
    channel.position = position;
    channel.velocity = velocity;
    channel.joint_velocity = joint_velocity;
    channel.joint_acceleration = acceleration;
    channel.world_position = joint;
    position.clamp(0.0, 1.0)
}

#[cfg(test)]
mod tests {
    use super::*;
    #[test]
    fn exposes_all_driven_channels_and_resets_to_driver_weights() {
        let simulator = AvatarPhysicsSimulator::native_new().unwrap();
        simulator
            .native_set_wearable_params(HashMap::from([(1100, 0.25)]))
            .unwrap();
        let output = simulator.native_tick(1.0).unwrap();
        assert_eq!(output.len(), 8);
        assert!((0.0..=1.0).contains(&output[&1200]));
    }
}