Stabilize OpenSim agent runtime and Mentra integration

crates/libremetaverse/src/agent_manager.rs

@@ -32,7 +32,7 @@ use crate::{
 };
 use libremetaverse_structured_data::{OSD, OSDMap, OSDParser};
 use libremetaverse_types::compat::{EventHandler, Subscription, Uri};
-use libremetaverse_types::{Color4, UUID, Utils, Vector3, Vector3d, Vector4};
+use libremetaverse_types::{Color4, Quaternion, UUID, Utils, Vector3, Vector3d, Vector4};
 use std::collections::{BTreeMap, HashMap};
 use std::fmt;
 use std::panic::{AssertUnwindSafe, catch_unwind};
@@ -1029,7 +1029,16 @@ impl AgentManagerInner {
         }
     }
 
-    pub(crate) fn update_avatar_object_state(&self, local_id: u32, collision_plane: Vector4) {
+    pub(crate) fn update_avatar_object_state(
+        &self,
+        local_id: u32,
+        collision_plane: Vector4,
+        position: Vector3,
+        rotation: Quaternion,
+        velocity: Vector3,
+        acceleration: Vector3,
+        angular_velocity: Vector3,
+    ) {
         *self
             .local_id
             .write()
@@ -1038,6 +1047,15 @@ impl AgentManagerInner {
             .collision_plane
             .write()
             .unwrap_or_else(std::sync::PoisonError::into_inner) = collision_plane;
+        if let Some(movement) = self.movement.get() {
+            movement.update_object_kinematics(
+                position,
+                rotation,
+                velocity,
+                acceleration,
+                angular_velocity,
+            );
+        }
     }
 
     pub(crate) fn avatar_local_id(&self) -> u32 {

crates/libremetaverse/src/agent_movement.rs

@@ -2416,6 +2416,23 @@ impl AgentMovementRuntime {
         write(&self.kinematics).relative_position = value;
     }
 
+    pub(crate) fn update_object_kinematics(
+        &self,
+        position: Vector3,
+        rotation: Quaternion,
+        velocity: Vector3,
+        acceleration: Vector3,
+        angular_velocity: Vector3,
+    ) {
+        let mut state = write(&self.kinematics);
+        state.relative_position = position;
+        state.relative_rotation = rotation;
+        state.velocity = velocity;
+        state.acceleration = acceleration;
+        state.angular_velocity = angular_velocity;
+        state.last_position_update = SystemTime::now();
+    }
+
     pub(crate) fn relative_position_estimate(&self) -> Vector3 {
         let state = *read(&self.kinematics);
         let elapsed = SystemTime::now()

crates/libremetaverse/src/object_manager.rs

@@ -976,10 +976,18 @@ impl ObjectManagerInner {
                     && block.full_id == client.network().native_agent_id()
                     && let Some(agent) = client.cached_agent_manager_inner()
                 {
-                    agent.update_avatar_object_state(block.id, movement.collision_plane);
+                    agent.update_avatar_object_state(
+                        block.id,
+                        movement.collision_plane,
+                        movement.position,
+                        movement.rotation,
+                        movement.velocity,
+                        movement.acceleration,
+                        movement.angular_velocity,
+                    );
                 }
                 let is_new = !read(&simulator.objects_avatars).contains_key(&block.id);
-                let avatar = read(&simulator.objects_avatars)
+                let mut avatar = read(&simulator.objects_avatars)
                     .get(&block.id)
                     .cloned()
                     .unwrap_or(Avatar::new()?);
@@ -998,6 +1006,7 @@ impl ObjectManagerInner {
                 avatar_prim.collision_plane = movement.collision_plane;
                 avatar_prim.textures = Some(movement.textures.clone());
                 avatar_prim.name_values = Some(names.clone());
+                avatar.base = avatar_prim.clone();
                 self.events
                     .object_data_block_update
                     .emit(ObjectDataBlockUpdateEventArgs::new(
@@ -1224,8 +1233,39 @@ impl ObjectManagerInner {
                     && let Some(agent) = client.cached_agent_manager_inner()
                     && local_id == agent.avatar_local_id()
                 {
-                    agent.update_avatar_object_state(local_id, collision_plane);
+                    agent.update_avatar_object_state(
+                        local_id,
+                        collision_plane,
+                        position,
+                        rotation,
+                        velocity,
+                        acceleration,
+                        angular_velocity,
+                    );
                 }
+                let Some(mut found) = read(&simulator.objects_avatars).get(&local_id).cloned()
+                else {
+                    continue;
+                };
+                self.events
+                    .terse_object_update
+                    .emit(TerseObjectUpdateEventArgs::new(
+                        simulator.clone(),
+                        found.base.clone(),
+                        update.clone(),
+                        packet.region_data.time_dilation,
+                    )?);
+                found.position = update.position;
+                found.rotation = update.rotation;
+                found.velocity = update.velocity;
+                found.collision_plane = update.collision_plane;
+                found.acceleration = update.acceleration;
+                found.angular_velocity = update.angular_velocity;
+                found.prim_data.state = update.state;
+                if !block.texture_entry.is_empty() {
+                    found.textures = Some(update.textures);
+                }
+                write(&simulator.objects_avatars).insert(local_id, found);
                 continue;
             }
             let Some(mut prim) = read(&simulator.objects_primitives).get(&local_id).cloned() else {
@@ -2903,6 +2943,171 @@ mod tests {
         );
     }
 
+    #[test]
+    fn full_avatar_update_populates_nearby_identity_and_position() {
+        let (manager, simulator) = context();
+        let avatar_id = UUID::random().expect("uuid");
+        let position = Vector3 {
+            x: 164.0,
+            y: 26.0,
+            z: 67.0,
+        };
+        let mut movement = vec![0; 60];
+        for (offset, value) in [position.x, position.y, position.z].into_iter().enumerate() {
+            movement[offset * 4..offset * 4 + 4].copy_from_slice(&value.to_le_bytes());
+        }
+        let mut packet = crate::packets::ObjectUpdatePacket::new_with_constructor().unwrap();
+        packet.region_data.region_handle = 0x2000;
+        let mut block =
+            crate::packets::ObjectUpdatePacketObjectDataBlock::new_with_constructor().unwrap();
+        block.id = 30;
+        block.full_id = avatar_id;
+        block.p_code = PCode::Avatar as u8;
+        block.object_data = movement;
+        block.scale = Vector3::one();
+        block.name_value = b"FirstName STRING R S Big\nLastName STRING R S Daddy\0".to_vec();
+        packet.object_data = vec![block];
+        let wire = transport_wire(&packet.to_bytes_with_method().expect("wire packet"));
+
+        manager
+            .inner
+            .handle_object_update(&wire, simulator.clone())
+            .expect("avatar update");
+
+        let avatar = read(&simulator.objects_avatars)
+            .get(&30)
+            .cloned()
+            .expect("nearby avatar");
+        assert_eq!(avatar.id, avatar_id);
+        assert_eq!(avatar.local_id, 30);
+        assert_eq!(avatar.position, position);
+        assert_eq!(avatar.name(), "Big Daddy");
+        assert_eq!(
+            read(&simulator.global_to_local_id).get(&avatar_id),
+            Some(&30)
+        );
+    }
+
+    #[test]
+    fn terse_avatar_update_refreshes_nearby_position_and_emits_movement() {
+        let (manager, simulator) = context();
+        let avatar_id = UUID::random().expect("uuid");
+        let mut avatar = Avatar::new().expect("avatar");
+        avatar.id = avatar_id;
+        avatar.local_id = 31;
+        write(&simulator.objects_avatars).insert(31, avatar);
+        let observed = Arc::new(Mutex::new(None));
+        let captured = Arc::clone(&observed);
+        let _subscription = manager.subscribe_terse_object_update(Arc::new(move |event| {
+            *mutex(&captured) = Some(event.update());
+        }));
+        let position = Vector3 {
+            x: 170.0,
+            y: 35.0,
+            z: 68.0,
+        };
+        let mut data = vec![0; 60];
+        data[..4].copy_from_slice(&31_u32.to_le_bytes());
+        data[5] = 1;
+        for (offset, value) in [position.x, position.y, position.z].into_iter().enumerate() {
+            let start = 22 + offset * 4;
+            data[start..start + 4].copy_from_slice(&value.to_le_bytes());
+        }
+        let mut packet =
+            crate::packets::ImprovedTerseObjectUpdatePacket::new_with_constructor().unwrap();
+        let mut block =
+            crate::packets::ImprovedTerseObjectUpdatePacketObjectDataBlock::new_with_constructor()
+                .unwrap();
+        block.data = data;
+        packet.object_data = vec![block];
+        let wire = transport_wire(&packet.to_bytes_with_method().expect("wire packet"));
+
+        manager
+            .inner
+            .handle_terse_update(&wire, simulator.clone())
+            .expect("terse avatar update");
+
+        let avatar = read(&simulator.objects_avatars)
+            .get(&31)
+            .cloned()
+            .expect("nearby avatar");
+        assert_eq!(avatar.id, avatar_id);
+        assert_eq!(avatar.position, position);
+        let movement = mutex(&observed).clone().expect("movement event");
+        assert!(movement.avatar);
+        assert_eq!(movement.position, position);
+    }
+
+    #[test]
+    fn own_avatar_updates_refresh_agent_position_feedback() {
+        let client = GridClient::new().expect("client");
+        let agent =
+            crate::AgentManager::native_new(Some(Arc::new(client.clone()))).expect("agent manager");
+        let manager = ObjectManager::new(Some(Arc::new(client.clone()))).expect("object manager");
+        let simulator = Simulator::new(
+            client,
+            "127.0.0.1:14069".parse().expect("endpoint"),
+            69,
+            None,
+            None,
+        )
+        .expect("simulator");
+        let initial = Vector3 {
+            x: 100.0,
+            y: 101.0,
+            z: 25.0,
+        };
+        let mut movement = vec![0; 60];
+        for (offset, value) in [initial.x, initial.y, initial.z].into_iter().enumerate() {
+            movement[offset * 4..offset * 4 + 4].copy_from_slice(&value.to_le_bytes());
+        }
+        let mut full = crate::packets::ObjectUpdatePacket::new_with_constructor().unwrap();
+        let mut block =
+            crate::packets::ObjectUpdatePacketObjectDataBlock::new_with_constructor().unwrap();
+        block.id = 32;
+        block.full_id = UUID::zero();
+        block.p_code = PCode::Avatar as u8;
+        block.object_data = movement;
+        block.scale = Vector3::one();
+        full.object_data = vec![block];
+        manager
+            .inner
+            .handle_object_update(
+                &transport_wire(&full.to_bytes_with_method().expect("full wire packet")),
+                simulator.clone(),
+            )
+            .expect("full avatar update");
+        assert_eq!(agent.sim_position(), initial);
+
+        let updated = Vector3 {
+            x: 110.0,
+            y: 111.0,
+            z: 25.0,
+        };
+        let mut data = vec![0; 60];
+        data[..4].copy_from_slice(&32_u32.to_le_bytes());
+        data[5] = 1;
+        for (offset, value) in [updated.x, updated.y, updated.z].into_iter().enumerate() {
+            let start = 22 + offset * 4;
+            data[start..start + 4].copy_from_slice(&value.to_le_bytes());
+        }
+        let mut terse =
+            crate::packets::ImprovedTerseObjectUpdatePacket::new_with_constructor().unwrap();
+        let mut block =
+            crate::packets::ImprovedTerseObjectUpdatePacketObjectDataBlock::new_with_constructor()
+                .unwrap();
+        block.data = data;
+        terse.object_data = vec![block];
+        manager
+            .inner
+            .handle_terse_update(
+                &transport_wire(&terse.to_bytes_with_method().expect("terse wire packet")),
+                simulator,
+            )
+            .expect("terse avatar update");
+        assert_eq!(agent.sim_position(), updated);
+    }
+
     #[test]
     fn property_reply_correlates_uuid_and_updates_tracked_primitive() {
         let (manager, simulator) = context();