specs-physics

sync_bodies_to_physics.rs (7566B)

---

 use std::marker::PhantomData;
 
 use specs::{
     storage::ComponentEvent,
     world::Index,
     BitSet,
     Join,
     ReadStorage,
     ReaderId,
     System,
     SystemData,
     World,
     WriteExpect,
     WriteStorage,
 };
 
 use crate::{
     bodies::{PhysicsBody, Position},
     nalgebra::RealField,
     Physics,
 };
 
 use super::iterate_component_events;
 
 /// The `SyncBodiesToPhysicsSystem` handles the synchronisation of `PhysicsBody`
 /// `Component`s into the physics `World`.
 pub struct SyncBodiesToPhysicsSystem<N, P> {
     positions_reader_id: Option<ReaderId<ComponentEvent>>,
     physics_bodies_reader_id: Option<ReaderId<ComponentEvent>>,
 
     n_marker: PhantomData<N>,
     p_marker: PhantomData<P>,
 }
 
 impl<'s, N, P> System<'s> for SyncBodiesToPhysicsSystem<N, P>
 where
     N: RealField,
     P: Position<N>,
 {
     type SystemData = (
         ReadStorage<'s, P>,
         WriteExpect<'s, Physics<N>>,
         WriteStorage<'s, PhysicsBody<N>>,
     );
 
     fn run(&mut self, data: Self::SystemData) {
         let (positions, mut physics, mut physics_bodies) = data;
 
         // collect all ComponentEvents for the Position storage
         let (inserted_positions, modified_positions, removed_positions) =
             iterate_component_events(&positions, self.positions_reader_id.as_mut().unwrap());
 
         // collect all ComponentEvents for the PhysicsBody storage
         let (inserted_physics_bodies, modified_physics_bodies, removed_physics_bodies) =
             iterate_component_events(
                 &physics_bodies,
                 self.physics_bodies_reader_id.as_mut().unwrap(),
             );
 
         // handle removed events
         for id in &removed_physics_bodies | &removed_positions {
             remove_rigid_body::<N, P>(id, &mut physics);
         }
 
         // iterate over PhysicsBody and Position components with an id/Index that
         // exists in either of the collected ComponentEvent BitSets
         for (position, mut physics_body, id) in (
             &positions,
             &mut physics_bodies,
             &inserted_positions
                 | &modified_positions
                 | &removed_positions
                 | &inserted_physics_bodies
                 | &modified_physics_bodies
                 | &removed_physics_bodies,
         )
             .join()
         {
             // handle inserted events
             if inserted_positions.contains(id) || inserted_physics_bodies.contains(id) {
                 debug!("Inserted PhysicsBody with id: {}", id);
                 add_rigid_body::<N, P>(id, &position, &mut physics, &mut physics_body);
             }
 
             // handle modified events
             if modified_positions.contains(id) || modified_physics_bodies.contains(id) {
                 debug!("Modified PhysicsBody with id: {}", id);
                 update_rigid_body::<N, P>(
                     id,
                     &position,
                     &mut physics,
                     &mut physics_body,
                     &modified_positions,
                     &modified_physics_bodies,
                 );
             }
         }
     }
 
     fn setup(&mut self, res: &mut World) {
         info!("SyncBodiesToPhysicsSystem.setup");
         Self::SystemData::setup(res);
 
         // initialise required resources
         res.entry::<Physics<N>>().or_insert_with(Physics::default);
 
         // register reader id for the Position storage
         let mut position_storage: WriteStorage<P> = SystemData::fetch(&res);
         self.positions_reader_id = Some(position_storage.register_reader());
 
         // register reader id for the PhysicsBody storage
         let mut physics_body_storage: WriteStorage<PhysicsBody<N>> = SystemData::fetch(&res);
         self.physics_bodies_reader_id = Some(physics_body_storage.register_reader());
     }
 }
 
 impl<N, P> Default for SyncBodiesToPhysicsSystem<N, P>
 where
     N: RealField,
     P: Position<N>,
 {
     fn default() -> Self {
         Self {
             positions_reader_id: None,
             physics_bodies_reader_id: None,
             n_marker: PhantomData,
             p_marker: PhantomData,
         }
     }
 }
 
 fn add_rigid_body<N, P>(
     id: Index,
     position: &P,
     physics: &mut Physics<N>,
     physics_body: &mut PhysicsBody<N>,
 ) where
     N: RealField,
     P: Position<N>,
 {
     // remove already existing bodies for this inserted component;
     // this technically should never happen but we need to keep the list of body
     // handles clean
     if let Some(body_handle) = physics.body_handles.remove(&id) {
         warn!("Removing orphaned body handle: {:?}", body_handle);
         physics.bodies.remove(body_handle);
     }
 
     // create a new RigidBody in the PhysicsWorld and store its
     // handle for later usage
     let handle = physics.bodies.insert(
         physics_body
             .to_rigid_body_desc()
             .position(*position.isometry())
             .user_data(id)
             .build(),
     );
 
     physics_body.handle = Some(handle);
     physics.body_handles.insert(id, handle);
 
     info!(
         "Inserted rigid body to world with values: {:?}",
         physics_body
     );
 }
 
 fn update_rigid_body<N, P>(
     id: Index,
     position: &P,
     physics: &mut Physics<N>,
     physics_body: &mut PhysicsBody<N>,
     modified_positions: &BitSet,
     modified_physics_bodies: &BitSet,
 ) where
     N: RealField,
     P: Position<N>,
 {
     if let Some(rigid_body) = physics.bodies.rigid_body_mut(physics_body.handle.unwrap()) {
         // the PhysicsBody was modified, update everything but the position
         if modified_physics_bodies.contains(id) {
             physics_body.apply_to_physics_world(rigid_body);
         }
 
         // the Position was modified, update the position directly
         if modified_positions.contains(id) {
             rigid_body.set_position(*position.isometry());
         }
 
         trace!(
             "Updated rigid body in world with values: {:?}",
             physics_body
         );
     }
 }
 
 fn remove_rigid_body<N, P>(id: Index, physics: &mut Physics<N>)
 where
     N: RealField,
     P: Position<N>,
 {
     if let Some(handle) = physics.body_handles.remove(&id) {
         // remove body if it still exists in the PhysicsWorld
         physics.bodies.remove(handle);
         info!("Removed rigid body from world with id: {}", id);
     }
 }
 
 #[cfg(test)]
 mod tests {
     use crate::{
         nalgebra::Isometry3,
         nphysics::object::BodyStatus,
         systems::SyncBodiesToPhysicsSystem,
         Physics,
         PhysicsBodyBuilder,
         SimplePosition,
     };
 
     use specs::prelude::*;
 
     #[test]
     fn add_rigid_body() {
         let mut world = World::new();
         let mut dispatcher = DispatcherBuilder::new()
             .with(
                 SyncBodiesToPhysicsSystem::<f32, SimplePosition<f32>>::default(),
                 "sync_bodies_to_physics_system",
                 &[],
             )
             .build();
         dispatcher.setup(&mut world);
 
         // create an Entity with the PhysicsBody component and execute the dispatcher
         world
             .create_entity()
             .with(SimplePosition::<f32>(Isometry3::<f32>::translation(
                 1.0, 1.0, 1.0,
             )))
             .with(PhysicsBodyBuilder::<f32>::from(BodyStatus::Dynamic).build())
             .build();
         dispatcher.dispatch(&world);
 
         // fetch the Physics instance and check for new bodies
         let physics = world.read_resource::<Physics<f32>>();
         assert_eq!(physics.body_handles.len(), 1);
         assert_eq!(physics.bodies.iter().count(), 2);
     }
 }