physics_stepper.rs (6406B)
1 use std::marker::PhantomData; 2 3 use specs::{world::Index, Entities, Entity, Read, System, SystemData, World, Write, WriteExpect}; 4 5 use crate::{ 6 events::{ContactEvent, ContactEvents, ContactType, ProximityEvent, ProximityEvents}, 7 nalgebra::RealField, 8 ncollide::pipeline::{CollisionObjectSet, ContactEvent as NContactEvent}, 9 nphysics::object::{DefaultColliderHandle, DefaultColliderSet}, 10 parameters::TimeStep, 11 Physics, 12 }; 13 14 /// The `PhysicsStepperSystem` progresses the nphysics `World`. 15 pub struct PhysicsStepperSystem<N> { 16 n_marker: PhantomData<N>, 17 } 18 19 impl<'s, N: RealField> System<'s> for PhysicsStepperSystem<N> { 20 type SystemData = ( 21 Entities<'s>, 22 Option<Read<'s, TimeStep<N>>>, 23 Write<'s, ContactEvents>, 24 Write<'s, ProximityEvents>, 25 WriteExpect<'s, Physics<N>>, 26 ); 27 28 fn run(&mut self, data: Self::SystemData) { 29 let (entities, time_step, mut contact_events, mut proximity_events, mut physics) = data; 30 31 // Convert physics from Write to &mut pointer so rustc can correctly reason 32 // about independence of &mut borrows to struct components 33 let physics: &mut Physics<N> = &mut *physics; 34 35 // if a TimeStep resource exits, set the timestep for the nphysics integration 36 // accordingly; this should not be required if the Systems are executed in a 37 // fixed interval 38 if let Some(time_step) = time_step { 39 // only update timestep if it actually differs from the current nphysics World 40 // one; keep in mind that changing the Resource will destabilize the simulation 41 if physics.mechanical_world.timestep() != time_step.0 { 42 warn!( 43 "TimeStep and world.timestep() differ, changing worlds timestep from {} to: {:?}", 44 physics.mechanical_world.timestep(), 45 time_step.0 46 ); 47 physics.mechanical_world.set_timestep(time_step.0); 48 } 49 } 50 51 physics.mechanical_world.step( 52 &mut physics.geometrical_world, 53 &mut physics.bodies, 54 &mut physics.colliders, 55 &mut physics.joint_constraints, 56 &mut physics.force_generators, 57 ); 58 59 // map occurred ncollide ContactEvents to a custom ContactEvent type; this 60 // custom type contains data that is more relevant for Specs users than 61 // CollisionObjectHandles, such as the Entities that took part in the collision 62 contact_events.iter_write(physics.geometrical_world.contact_events().iter().map( 63 |contact_event| { 64 debug!("Got ContactEvent: {:?}", contact_event); 65 // retrieve CollisionObjectHandles from ContactEvent and map the ContactEvent 66 // type to our own custom ContactType 67 let (handle1, handle2, contact_type) = match contact_event { 68 NContactEvent::Started(handle1, handle2) => { 69 (*handle1, *handle2, ContactType::Started) 70 } 71 NContactEvent::Stopped(handle1, handle2) => { 72 (*handle1, *handle2, ContactType::Stopped) 73 } 74 }; 75 76 // create our own ContactEvent from the extracted data; mapping the 77 // CollisionObjectHandles to Entities is error prone but should work as intended 78 // as long as we're the only ones working directly with the nphysics World 79 ContactEvent { 80 collider1: entity_from_collision_object_handle( 81 &entities, 82 handle1, 83 &physics.colliders, 84 ), 85 collider2: entity_from_collision_object_handle( 86 &entities, 87 handle2, 88 &physics.colliders, 89 ), 90 contact_type, 91 } 92 }, 93 )); 94 95 // map occurred ncollide ProximityEvents to a custom ProximityEvent type; see 96 // ContactEvents for reasoning 97 proximity_events.iter_write(physics.geometrical_world.proximity_events().iter().map( 98 |proximity_event| { 99 debug!("Got ProximityEvent: {:?}", proximity_event); 100 // retrieve CollisionObjectHandles and Proximity statuses from the ncollide 101 // ProximityEvent 102 let (handle1, handle2, prev_status, new_status) = ( 103 proximity_event.collider1, 104 proximity_event.collider2, 105 proximity_event.prev_status, 106 proximity_event.new_status, 107 ); 108 109 // create our own ProximityEvent from the extracted data; mapping 110 // CollisionObjectHandles to Entities is once again error prone, but yeah... 111 // ncollides Proximity types are mapped to our own types 112 ProximityEvent { 113 collider1: entity_from_collision_object_handle( 114 &entities, 115 handle1, 116 &physics.colliders, 117 ), 118 collider2: entity_from_collision_object_handle( 119 &entities, 120 handle2, 121 &physics.colliders, 122 ), 123 prev_status, 124 new_status, 125 } 126 }, 127 )); 128 } 129 130 fn setup(&mut self, res: &mut World) { 131 info!("PhysicsStepperSystem.setup"); 132 Self::SystemData::setup(res); 133 134 // initialise required resources 135 res.entry::<Physics<N>>().or_insert_with(Physics::default); 136 } 137 } 138 139 impl<N> Default for PhysicsStepperSystem<N> 140 where 141 N: RealField, 142 { 143 fn default() -> Self { 144 Self { 145 n_marker: PhantomData, 146 } 147 } 148 } 149 150 fn entity_from_collision_object_handle<N: RealField>( 151 entities: &Entities, 152 collision_object_handle: DefaultColliderHandle, 153 collider_set: &DefaultColliderSet<N>, 154 ) -> Entity { 155 entities.entity( 156 *collider_set 157 .collision_object(collision_object_handle) 158 .unwrap() 159 .user_data() 160 .unwrap() 161 .downcast_ref::<Index>() 162 .unwrap(), 163 ) 164 }