events.rs

use crate::math::{Real, Vect};
use bevy::prelude::{Entity, Event};
use rapier::dynamics::RigidBodySet;
use rapier::geometry::{
    ColliderHandle, ColliderSet, CollisionEvent as RapierCollisionEvent, CollisionEventFlags,
    ContactForceEvent as RapierContactForceEvent, ContactPair,
};
use rapier::pipeline::EventHandler;
use std::collections::HashMap;
use std::sync::RwLock;

/// Events occurring when two colliders start or stop colliding
///
/// This will only get triggered if the entity has the
/// [`ActiveEvent::COLLISION_EVENTS`] flag enabled.
#[derive(Event, Copy, Clone, Debug, PartialEq, Eq)]
pub enum CollisionEvent {
    /// Event occurring when two colliders start colliding
    Started(Entity, Entity, CollisionEventFlags),
    /// Event occurring when two colliders stop colliding
    Stopped(Entity, Entity, CollisionEventFlags),
}

/// Event occurring when the sum of the magnitudes of the contact forces
/// between two colliders exceed a threshold ([`ContactForceEventThreshold`]).
///
/// This will only get triggered if the entity has the
/// [`ActiveEvent::CONTACT_FORCE_EVENTS`] flag enabled.
#[derive(Event, Copy, Clone, Debug, PartialEq)]
pub struct ContactForceEvent {
    /// The first collider involved in the contact.
    pub collider1: Entity,
    /// The second collider involved in the contact.
    pub collider2: Entity,
    /// The sum of all the forces between the two colliders.
    pub total_force: Vect,
    /// The sum of the magnitudes of each force between the two colliders.
    ///
    /// Note that this is **not** the same as the magnitude of `self.total_force`.
    /// Here we are summing the magnitude of all the forces, instead of taking
    /// the magnitude of their sum.
    pub total_force_magnitude: Real,
    /// The world-space (unit) direction of the force with strongest magnitude.
    pub max_force_direction: Vect,
    /// The magnitude of the largest force at a contact point of this contact pair.
    pub max_force_magnitude: Real,
}

// TODO: it may be more efficient to use crossbeam channel.
// However crossbeam channels cause a Segfault (I have not
// investigated how to reproduce this exactly to open an
// issue).
/// A set of queues collecting events emitted by the physics engine.
pub(crate) struct EventQueue<'a> {
    // Used ot retrieve the entity of colliders that have been removed from the simulation
    // since the last physics step.
    pub deleted_colliders: &'a HashMap<ColliderHandle, Entity>,
    pub collision_events: RwLock<Vec<CollisionEvent>>,
    pub contact_force_events: RwLock<Vec<ContactForceEvent>>,
}

impl<'a> EventQueue<'a> {
    fn collider2entity(&self, colliders: &ColliderSet, handle: ColliderHandle) -> Entity {
        colliders
            .get(handle)
            .map(|co| Entity::from_bits(co.user_data as u64))
            .or_else(|| self.deleted_colliders.get(&handle).copied())
            .expect("Internal error: entity not found for collision event.")
    }
}

impl<'a> EventHandler for EventQueue<'a> {
    fn handle_collision_event(
        &self,
        _bodies: &RigidBodySet,
        colliders: &ColliderSet,
        event: RapierCollisionEvent,
        _: Option<&ContactPair>,
    ) {
        let event = match event {
            RapierCollisionEvent::Started(h1, h2, flags) => {
                let e1 = self.collider2entity(colliders, h1);
                let e2 = self.collider2entity(colliders, h2);
                CollisionEvent::Started(e1, e2, flags)
            }
            RapierCollisionEvent::Stopped(h1, h2, flags) => {
                let e1 = self.collider2entity(colliders, h1);
                let e2 = self.collider2entity(colliders, h2);
                CollisionEvent::Stopped(e1, e2, flags)
            }
        };

        if let Ok(mut events) = self.collision_events.write() {
            events.push(event);
        }
    }

    fn handle_contact_force_event(
        &self,
        dt: Real,
        _bodies: &RigidBodySet,
        colliders: &ColliderSet,
        contact_pair: &ContactPair,
        total_force_magnitude: Real,
    ) {
        let rapier_event =
            RapierContactForceEvent::from_contact_pair(dt, contact_pair, total_force_magnitude);
        let event = ContactForceEvent {
            collider1: self.collider2entity(colliders, rapier_event.collider1),
            collider2: self.collider2entity(colliders, rapier_event.collider2),
            total_force: rapier_event.total_force.into(),
            total_force_magnitude: rapier_event.total_force_magnitude,
            max_force_direction: rapier_event.max_force_direction.into(),
            max_force_magnitude: rapier_event.max_force_magnitude,
        };

        if let Ok(mut events) = self.contact_force_events.write() {
            events.push(event);
        }
    }
}

#[cfg(test)]
mod test {
    use bevy::time::TimePlugin;
    use systems::tests::HeadlessRenderPlugin;

    use crate::{plugin::*, prelude::*};

    #[cfg(feature = "dim3")]
    fn cuboid(hx: Real, hy: Real, hz: Real) -> Collider {
        Collider::cuboid(hx, hy, hz)
    }
    #[cfg(feature = "dim2")]
    fn cuboid(hx: Real, hy: Real, _hz: Real) -> Collider {
        Collider::cuboid(hx, hy)
    }

    #[test]
    pub fn events_received() {
        return main();

        use bevy::prelude::*;

        #[derive(Resource, Reflect)]
        pub struct EventsSaver<E: Event> {
            pub events: Vec<E>,
        }
        impl<E: Event> Default for EventsSaver<E> {
            fn default() -> Self {
                Self {
                    events: Default::default(),
                }
            }
        }
        pub fn save_events<E: Event + Clone>(
            mut events: EventReader<E>,
            mut saver: ResMut<EventsSaver<E>>,
        ) {
            for event in events.read() {
                saver.events.push(event.clone());
            }
        }
        fn run_test(app: &mut App) {
            app.add_systems(PostUpdate, save_events::<CollisionEvent>)
                .add_systems(PostUpdate, save_events::<ContactForceEvent>)
                .init_resource::<EventsSaver<CollisionEvent>>()
                .init_resource::<EventsSaver<ContactForceEvent>>();
            // while app.plugins_state() == bevy::app::PluginsState::Adding {
            //     #[cfg(not(target_arch = "wasm32"))]
            //     bevy::tasks::tick_global_task_pools_on_main_thread();
            // }
            // app.finish();
            // app.cleanup();
            let mut time = app.world_mut().get_resource_mut::<Time<Virtual>>().unwrap();
            time.set_relative_speed(1000f32);
            for _ in 0..300 {
                // FIXME: advance by set durations to avoid being at the mercy of the CPU.
                app.update();
            }
            let saved_collisions = app
                .world()
                .get_resource::<EventsSaver<CollisionEvent>>()
                .unwrap();
            assert!(saved_collisions.events.len() > 0);
            let saved_contact_forces = app
                .world()
                .get_resource::<EventsSaver<CollisionEvent>>()
                .unwrap();
            assert!(saved_contact_forces.events.len() > 0);
        }

        /// Adapted from events example
        fn main() {
            let mut app = App::new();
            app.add_plugins((
                HeadlessRenderPlugin,
                TransformPlugin,
                TimePlugin,
                RapierPhysicsPlugin::<NoUserData>::default(),
            ))
            .add_systems(Startup, setup_physics);
            run_test(&mut app);
        }

        pub fn setup_physics(mut commands: Commands) {
            /*
             * Ground
             */
            commands.spawn((
                TransformBundle::from(Transform::from_xyz(0.0, -1.2, 0.0)),
                cuboid(4.0, 1.0, 1.0),
            ));

            commands.spawn((
                TransformBundle::from(Transform::from_xyz(0.0, 5.0, 0.0)),
                cuboid(4.0, 1.5, 1.0),
                Sensor,
            ));

            commands.spawn((
                TransformBundle::from(Transform::from_xyz(0.0, 13.0, 0.0)),
                RigidBody::Dynamic,
                cuboid(0.5, 0.5, 0.5),
                ActiveEvents::COLLISION_EVENTS,
                ContactForceEventThreshold(30.0),
            ));
        }
    }
}