Complete first release candidate audit (#107)

crates/libremetaverse/src/avatar_physics.rs

@@ -0,0 +1,284 @@
+//! 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]));
+    }
+}