perigee/physics/
mod.rs

1use std::collections::HashMap;
2
3use crate::config::PhysicsConfig;
4use crate::math::Transform3;
5use crate::perigee_gltf::extras::{GltfBodyType, GltfExtras, GltfOptimizedShape};
6use crate::perigee_gltf::util::access_gltf_bytes;
7use crate::physics::contact_event_mgmt::ContactEventManager;
8use crate::physics::handle_map::{NamedColliderHandleMap, NamedRigidBodyHandleMap};
9use crate::traits::{physics::ColliderEventListener, FromConfig};
10pub use collider_event_listener::*;
11use gltf::Node;
12use gltf::{accessor::DataType as GltfDataType, Gltf, Semantic as PrimitiveSemantic};
13use log::warn;
14use rapier3d::{
15    na::{Point3, Quaternion, Translation3, UnitQuaternion, Vector3},
16    prelude::*,
17};
18use serde::{Deserialize, Serialize};
19use thiserror::Error;
20
21mod collider_event_listener;
22mod contact_event_mgmt;
23mod handle_map;
24
25#[derive(Error, Debug)]
26pub enum PhysicsWorldInitError {
27    /// The binary payload for the glTF couldn't be found.
28    #[error("can't access the provided glTF's binary payload")]
29    CantAccessBlob,
30    /// The Perigee-specific glTF extras for a glTF node couldn't be found.
31    #[error("glTF must have Perigee extras to load physics world")]
32    PerigeeExtrasUndetected,
33    /// The Perigee-specific glTF extras didn't follow the expected schema.
34    #[error("invalid JSON stored in glTF node extras")]
35    InvalidPerigeeExtrasData,
36    /// A glTF mesh didn't have a name.
37    #[error("glTF mesh must have a name")]
38    UnnamedMesh,
39    /// A glTF node didn't have a name.
40    #[error("glTF node must have a name")]
41    UnnamedNode,
42    /// A glTF meshes was defined as a sensor type.
43    #[error("glTF mesh cannot be imported as sensor")]
44    MeshCantBeSensor,
45    /// The accessor for the primitive indices of a trimesh couldn't be found.
46    #[error("no primitive accessor for trimesh")]
47    NoPrimitiveAccessorForTrimesh,
48    /// An accessor for a trimesh's vertex positions couldn't be found.
49    #[error("no vertex positions accessor found for mesh")]
50    NoVertexPositionsAccessor,
51    /// Mesh indices accessor data type was neither u16 nor u32.
52    #[error("indices accessor data type was neither U16 nor U32")]
53    InvalidIndicesDataType,
54    /// No mesh indices were found for a mesh.
55    #[error("no indices found for mesh")]
56    NoIndicesFound,
57    /// No vertices were found for a mesh.
58    #[error("no vertices found for mesh")]
59    NoVerticesFound,
60    #[error("could not get accessor bytes")]
61    CouldntAccessBytes,
62    #[error("mesh defined as convex is not convex")]
63    MeshNotConvex,
64}
65
66/// The physics management structure. This is a
67/// thin wrapper around [the Rapier physics engine](https://rapier.rs)
68/// with additional utilities.
69#[derive(Serialize, Deserialize)]
70pub struct PhysicsWorld {
71    pub gravity: Vector3<f32>,
72    pub rigid_body_set: RigidBodySet,
73    pub collider_set: ColliderSet,
74    pub integration_parameters: IntegrationParameters,
75    pub island_manager: IslandManager,
76    pub broad_phase: BroadPhase,
77    pub narrow_phase: NarrowPhase,
78    pub impulse_joint_set: ImpulseJointSet,
79    pub multibody_joint_set: MultibodyJointSet,
80    pub ccd_solver: CCDSolver,
81    pub query_pipeline: QueryPipeline,
82    pub named_rigid_bodies: NamedRigidBodyHandleMap,
83    pub named_sensors: NamedColliderHandleMap,
84    #[serde(skip)]
85    collider_event_handlers: HashMap<ColliderHandle, Vec<Box<dyn ColliderEventListener>>>,
86    #[serde(skip)]
87    pub pipeline: PhysicsPipeline,
88    #[serde(skip)]
89    contact_event_manager: ContactEventManager,
90}
91
92impl FromConfig for PhysicsWorld {
93    type Config<'a> = &'a PhysicsConfig;
94    fn from_config<'a>(config: Self::Config<'a>) -> Self {
95        Self {
96            gravity: config.gravity().into(),
97            rigid_body_set: RigidBodySet::new(),
98            collider_set: ColliderSet::new(),
99            integration_parameters: IntegrationParameters::default(),
100            island_manager: IslandManager::new(),
101            broad_phase: BroadPhase::new(),
102            narrow_phase: NarrowPhase::new(),
103            impulse_joint_set: ImpulseJointSet::new(),
104            multibody_joint_set: MultibodyJointSet::new(),
105            ccd_solver: CCDSolver::new(),
106            query_pipeline: QueryPipeline::new(),
107            pipeline: PhysicsPipeline::new(),
108            contact_event_manager: ContactEventManager::with_capacity(
109                config.event_queue_capacity(),
110            ),
111            named_rigid_bodies: NamedRigidBodyHandleMap::default(),
112            named_sensors: NamedColliderHandleMap::default(),
113            collider_event_handlers: HashMap::default(),
114        }
115    }
116
117    fn set_config<'a>(&mut self, _config: Self::Config<'a>) {
118        warn!("Perigee PhysicsWorld doesn't allow resetting configuration");
119    }
120}
121
122impl Default for PhysicsWorld {
123    fn default() -> Self {
124        Self::from_config(&PhysicsConfig::default())
125    }
126}
127
128impl PhysicsWorld {
129    /// Remove a RigidBody from the physics world using its handle.
130    pub fn remove_body(&mut self, body_handle: RigidBodyHandle) -> Option<RigidBody> {
131        self.rigid_body_set.remove(
132            body_handle,
133            &mut self.island_manager,
134            &mut self.collider_set,
135            &mut self.impulse_joint_set,
136            &mut self.multibody_joint_set,
137            true,
138        )
139    }
140
141    fn visit_gltf_node(
142        &mut self,
143        node: &Node,
144        gltf_blob: Option<&Vec<u8>>,
145        parent_transform: &Transform3<f32>,
146        visited_nodes: &mut HashMap<usize, ()>,
147    ) -> Result<(), PhysicsWorldInitError> {
148        let gltf_bytes = match gltf_blob {
149            Some(bytes) => bytes,
150            None => {
151                return Err(PhysicsWorldInitError::CantAccessBlob);
152            }
153        };
154        let node_extra_data = match node.extras().as_ref() {
155            Some(extra_data) => extra_data,
156            None => return Err(PhysicsWorldInitError::PerigeeExtrasUndetected),
157        };
158        let node_extras_json = node_extra_data.get();
159        let node_extras: GltfExtras = match serde_json::from_str(node_extras_json) {
160            Ok(extras) => extras,
161            Err(_) => return Err(PhysicsWorldInitError::InvalidPerigeeExtrasData),
162        };
163
164        let body_type = node_extras.sim_settings.physics.body_type;
165
166        let (translation, quaternion, scale) = node.transform().decomposed();
167        let scale = Vector3::new(scale[0], scale[1], scale[2]);
168        let object_isometry = Isometry::from_parts(
169            Translation3::new(translation[0], translation[1], translation[2]),
170            UnitQuaternion::from_quaternion(Quaternion::new(
171                quaternion[3],
172                quaternion[0],
173                quaternion[1],
174                quaternion[2],
175            )),
176        );
177        let global_transform = parent_transform
178            * Transform3 {
179                isometry: object_isometry,
180                scale,
181            };
182        let global_isometry = *global_transform.isometry();
183        let global_scale = global_transform.scale();
184
185        for child_node in node.children() {
186            self.visit_gltf_node(&child_node, gltf_blob, &global_transform, visited_nodes)?;
187        }
188        if !node_extras.sim_settings.physics.enabled || visited_nodes.contains_key(&node.index()) {
189            return Ok(());
190        }
191
192        // Create a rigid body
193        if let Some(mesh) = node.mesh() {
194            let mesh_name = match node.name() {
195                Some(name) => name,
196                None => return Err(PhysicsWorldInitError::UnnamedMesh),
197            };
198            let mesh_name = String::from(mesh_name);
199            let mut rigid_body_builder = match body_type {
200                GltfBodyType::Static => RigidBodyBuilder::fixed().sleeping(true),
201                GltfBodyType::Kinematic => {
202                    RigidBodyBuilder::kinematic_position_based().sleeping(true)
203                }
204                GltfBodyType::Dynamic => RigidBodyBuilder::dynamic(),
205                GltfBodyType::Sensor => return Err(PhysicsWorldInitError::MeshCantBeSensor),
206            };
207            rigid_body_builder = rigid_body_builder.position(global_isometry);
208
209            let base_scale = node_extras.sim_settings.physics.base_scale;
210            let collider_silhouette = match node_extras.sim_settings.physics.optimized_shape {
211                GltfOptimizedShape::Cuboid => {
212                    let cuboid_half_dimensions = base_scale.component_mul(&global_scale) / 2.0;
213                    SharedShape::cuboid(
214                        cuboid_half_dimensions.x,
215                        cuboid_half_dimensions.y,
216                        cuboid_half_dimensions.z,
217                    )
218                }
219                GltfOptimizedShape::Sphere => {
220                    let ball_dimensions = base_scale.component_mul(global_scale);
221                    SharedShape::ball(ball_dimensions.x / 2.0)
222                }
223                GltfOptimizedShape::ConvexMesh => {
224                    let mut maybe_indices: Option<Vec<[u32; 3]>> = None;
225                    let mut maybe_vertices: Option<Vec<Point3<f32>>> = None;
226                    for primitive in mesh.primitives() {
227                        let indices_accesor = match primitive.indices() {
228                            Some(accessor) => accessor,
229                            None => {
230                                return Err(PhysicsWorldInitError::NoPrimitiveAccessorForTrimesh)
231                            }
232                        };
233
234                        let indices_bytes = if let Ok(indices_bytes) =
235                            access_gltf_bytes(gltf_bytes, &indices_accesor)
236                        {
237                            indices_bytes
238                        } else {
239                            return Err(PhysicsWorldInitError::CouldntAccessBytes);
240                        };
241                        let mut indices: Vec<[u32; 3]> =
242                            Vec::with_capacity(indices_accesor.count() / 3);
243
244                        match indices_accesor.data_type() {
245                            GltfDataType::U16 => {
246                                let flattened_indices: Vec<u16> = indices_bytes
247                                    .chunks_exact(2)
248                                    .map(|uint_bytes| {
249                                        let uint_byte_array: [u8; 2] = uint_bytes[0..2]
250                                            .try_into()
251                                            .expect(
252                                            "Could not convert u16 byte slice into u16 byte array",
253                                        );
254                                        u16::from_le_bytes(uint_byte_array)
255                                    })
256                                    .collect();
257                                let chunked_indices: Vec<&[u16]> =
258                                    flattened_indices.chunks(3).collect();
259                                for face_u16 in chunked_indices {
260                                    indices.push([
261                                        u32::from(face_u16[0]),
262                                        u32::from(face_u16[1]),
263                                        u32::from(face_u16[2]),
264                                    ]);
265                                }
266                                maybe_indices = Some(indices);
267                            }
268                            GltfDataType::U32 => {
269                                let flattened_indices: Vec<u32> = indices_bytes
270                                    .chunks_exact(4)
271                                    .map(|uint_bytes| {
272                                        let uint_byte_array: [u8; 4] = uint_bytes[0..4]
273                                            .try_into()
274                                            .expect(
275                                            "Could not convert u32 byte slice into u32 byte array",
276                                        );
277                                        u32::from_le_bytes(uint_byte_array)
278                                    })
279                                    .collect();
280                                let chunked_indices: Vec<&[u32]> =
281                                    flattened_indices.chunks(3).collect();
282                                for face_u32 in chunked_indices {
283                                    indices.push([face_u32[0], face_u32[1], face_u32[2]]);
284                                }
285                                maybe_indices = Some(indices);
286                            }
287                            _ => {
288                                return Err(PhysicsWorldInitError::InvalidIndicesDataType);
289                            }
290                        };
291                        match primitive.get(&PrimitiveSemantic::Positions) {
292                            None => {
293                                return Err(PhysicsWorldInitError::NoVertexPositionsAccessor);
294                            }
295                            Some(vertex_positions_accessor) => {
296                                let positions_bytes = if let Ok(positions_bytes) =
297                                    access_gltf_bytes(gltf_bytes, &vertex_positions_accessor)
298                                {
299                                    positions_bytes
300                                } else {
301                                    return Err(PhysicsWorldInitError::CouldntAccessBytes);
302                                };
303
304                                let mut floats: Vec<f32> =
305                                    Vec::with_capacity(positions_bytes.len() / 4);
306                                for float_bytes in positions_bytes.chunks_exact(4) {
307                                    let float_byte_array: [u8; 4] = float_bytes[0..4]
308                                        .try_into()
309                                        .expect(
310                                        "Could not convert float byte slice into float byte array",
311                                    );
312                                    floats.push(f32::from_le_bytes(float_byte_array));
313                                }
314                                let mut vertices: Vec<Point3<f32>> =
315                                    Vec::with_capacity(floats.len() / 3);
316                                for float_chunk in floats.chunks(3) {
317                                    vertices.push(Point3::new(
318                                        float_chunk[0],
319                                        float_chunk[1],
320                                        float_chunk[2],
321                                    ));
322                                }
323                                maybe_vertices = Some(vertices);
324                            }
325                        };
326                    }
327                    if maybe_indices.is_none() {
328                        return Err(PhysicsWorldInitError::NoIndicesFound);
329                    }
330                    if maybe_vertices.is_none() {
331                        return Err(PhysicsWorldInitError::NoVerticesFound);
332                    }
333                    let scaled_trimesh: TriMesh = TriMesh::new(
334                        maybe_vertices
335                            .expect("Trimesh vertices were None despite asserting they weren't!"),
336                        maybe_indices
337                            .expect("Trimesh indices were None despite asserting they weren't!"),
338                    )
339                    .scaled(&global_scale);
340                    if let Some(shape) =
341                        SharedShape::convex_hull(scaled_trimesh.vertices().to_vec().as_ref())
342                    {
343                        shape
344                    } else {
345                        return Err(PhysicsWorldInitError::MeshNotConvex);
346                    }
347                }
348                GltfOptimizedShape::None => {
349                    let mut maybe_indices: Option<Vec<[u32; 3]>> = None;
350                    let mut maybe_vertices: Option<Vec<Point3<f32>>> = None;
351                    for primitive in mesh.primitives() {
352                        let indices_accesor = match primitive.indices() {
353                            Some(accessor) => accessor,
354                            None => {
355                                return Err(PhysicsWorldInitError::NoPrimitiveAccessorForTrimesh)
356                            }
357                        };
358
359                        let indices_bytes = if let Ok(indices_bytes) =
360                            access_gltf_bytes(gltf_bytes, &indices_accesor)
361                        {
362                            indices_bytes
363                        } else {
364                            return Err(PhysicsWorldInitError::CouldntAccessBytes);
365                        };
366                        let mut indices: Vec<[u32; 3]> =
367                            Vec::with_capacity(indices_accesor.count() / 3);
368
369                        match indices_accesor.data_type() {
370                            GltfDataType::U16 => {
371                                let flattened_indices: Vec<u16> = indices_bytes
372                                    .chunks_exact(2)
373                                    .map(|uint_bytes| {
374                                        let uint_byte_array: [u8; 2] = uint_bytes[0..2]
375                                            .try_into()
376                                            .expect(
377                                            "Could not convert u16 byte slice into u16 byte array",
378                                        );
379                                        u16::from_le_bytes(uint_byte_array)
380                                    })
381                                    .collect();
382                                let chunked_indices: Vec<&[u16]> =
383                                    flattened_indices.chunks(3).collect();
384                                for face_u16 in chunked_indices {
385                                    indices.push([
386                                        u32::from(face_u16[0]),
387                                        u32::from(face_u16[1]),
388                                        u32::from(face_u16[2]),
389                                    ]);
390                                }
391                                maybe_indices = Some(indices);
392                            }
393                            GltfDataType::U32 => {
394                                let flattened_indices: Vec<u32> = indices_bytes
395                                    .chunks_exact(4)
396                                    .map(|uint_bytes| {
397                                        let uint_byte_array: [u8; 4] = uint_bytes[0..4]
398                                            .try_into()
399                                            .expect(
400                                            "Could not convert u32 byte slice into u32 byte array",
401                                        );
402                                        u32::from_le_bytes(uint_byte_array)
403                                    })
404                                    .collect();
405                                let chunked_indices: Vec<&[u32]> =
406                                    flattened_indices.chunks(3).collect();
407                                for face_u32 in chunked_indices {
408                                    indices.push([face_u32[0], face_u32[1], face_u32[2]]);
409                                }
410                                maybe_indices = Some(indices);
411                            }
412                            _ => {
413                                return Err(PhysicsWorldInitError::InvalidIndicesDataType);
414                            }
415                        };
416                        match primitive.get(&PrimitiveSemantic::Positions) {
417                            None => {
418                                return Err(PhysicsWorldInitError::NoVertexPositionsAccessor);
419                            }
420                            Some(vertex_positions_accessor) => {
421                                let positions_bytes = if let Ok(positions_bytes) =
422                                    access_gltf_bytes(gltf_bytes, &vertex_positions_accessor)
423                                {
424                                    positions_bytes
425                                } else {
426                                    return Err(PhysicsWorldInitError::CouldntAccessBytes);
427                                };
428
429                                let mut floats: Vec<f32> =
430                                    Vec::with_capacity(positions_bytes.len() / 4);
431                                for float_bytes in positions_bytes.chunks_exact(4) {
432                                    let float_byte_array: [u8; 4] = float_bytes[0..4]
433                                        .try_into()
434                                        .expect(
435                                        "Could not convert float byte slice into float byte array",
436                                    );
437                                    floats.push(f32::from_le_bytes(float_byte_array));
438                                }
439                                let mut vertices: Vec<Point3<f32>> =
440                                    Vec::with_capacity(floats.len() / 3);
441                                for float_chunk in floats.chunks(3) {
442                                    vertices.push(Point3::new(
443                                        float_chunk[0],
444                                        float_chunk[1],
445                                        float_chunk[2],
446                                    ));
447                                }
448                                maybe_vertices = Some(vertices);
449                            }
450                        };
451                    }
452                    if maybe_indices.is_none() {
453                        return Err(PhysicsWorldInitError::NoIndicesFound);
454                    }
455                    if maybe_vertices.is_none() {
456                        return Err(PhysicsWorldInitError::NoVerticesFound);
457                    }
458                    let scaled_trimesh: TriMesh = TriMesh::new(
459                        maybe_vertices
460                            .expect("Trimesh vertices were None despite asserting they weren't!"),
461                        maybe_indices
462                            .expect("Trimesh indices were None despite asserting they weren't!"),
463                    )
464                    .scaled(&global_scale);
465                    SharedShape::trimesh(
466                        scaled_trimesh.vertices().to_vec(),
467                        scaled_trimesh.indices().to_vec(),
468                    )
469                }
470            };
471
472            let mut collider_builder = ColliderBuilder::new(collider_silhouette);
473            if matches!(body_type, GltfBodyType::Dynamic) {
474                collider_builder = collider_builder.mass(node_extras.sim_settings.physics.mass);
475            }
476
477            let rb_handle = self.rigid_body_set.insert(rigid_body_builder.build());
478            let _col_handle = self.collider_set.insert_with_parent(
479                collider_builder.build(),
480                rb_handle,
481                &mut self.rigid_body_set,
482            );
483            if !node_extras.sim_settings.physics.is_anonymous {
484                self.named_rigid_bodies.insert(mesh_name.clone(), rb_handle);
485            }
486        } else {
487            // Create a sensor
488            let sensor_name = match node.name() {
489                Some(name) => name,
490                None => return Err(PhysicsWorldInitError::UnnamedNode),
491            };
492            let sensor_name = String::from(sensor_name);
493
494            let base_scale = node_extras.sim_settings.physics.base_scale;
495            let sensor_silhouette = match node_extras.sim_settings.physics.optimized_shape {
496                GltfOptimizedShape::Cuboid => {
497                    let cuboid_half_dimensions = base_scale.component_mul(global_scale) / 2.0;
498                    SharedShape::cuboid(
499                        cuboid_half_dimensions.x,
500                        cuboid_half_dimensions.y,
501                        cuboid_half_dimensions.z,
502                    )
503                }
504                GltfOptimizedShape::Sphere => {
505                    let ball_dimensions = base_scale.component_mul(global_scale);
506                    SharedShape::ball(ball_dimensions.x / 2.0)
507                }
508                _ => return Err(PhysicsWorldInitError::CantAccessBlob),
509            };
510            let collider_builder = ColliderBuilder::new(sensor_silhouette)
511                .position(global_isometry)
512                .sensor(true);
513
514            let sensor_handle = self.collider_set.insert(collider_builder.build());
515            self.named_sensors.insert(sensor_name, sensor_handle);
516        }
517
518        visited_nodes.insert(node.index(), ());
519        Ok(())
520    }
521
522    /// Load physics-enabled objects from a Perigee-enabled
523    /// glTF into the physics world.
524    ///
525    /// Note: Nodes that are children of others will be ignored.
526    pub fn load_from_gltf(
527        &mut self,
528        gltf: &Gltf,
529        parent_transform: Option<Transform3<f32>>,
530    ) -> Result<(), PhysicsWorldInitError> {
531        let mut visited_nodes: HashMap<usize, ()> = HashMap::new();
532        // Only loads the first scene
533        if let Some(scene) = gltf.scenes().next() {
534            for node in scene.nodes() {
535                self.visit_gltf_node(
536                    &node,
537                    gltf.blob.as_ref(),
538                    &parent_transform.unwrap_or(Transform3::identity()),
539                    &mut visited_nodes,
540                )?;
541            }
542        }
543
544        return Ok(());
545    }
546
547    pub fn listen_to_collider<L: ColliderEventListener + 'static>(
548        &mut self,
549        handle: ColliderHandle,
550        listener: L,
551    ) {
552        let wrapped_listener = Box::new(listener);
553        if let Some(handlers) = self.collider_event_handlers.get_mut(&handle) {
554            handlers.push(wrapped_listener);
555        } else {
556            self.collider_event_handlers
557                .insert(handle, vec![wrapped_listener]);
558        }
559    }
560
561    pub fn rekey_listeners(&mut self, old_handle: ColliderHandle, new_handle: ColliderHandle) {
562        if let Some(listeners) = self.collider_event_handlers.remove(&old_handle) {
563            self.collider_event_handlers.insert(new_handle, listeners);
564        }
565    }
566
567    /// Step the physics simulation by the provided number of seconds.
568    pub fn step(&mut self, delta_seconds: f32) {
569        self.integration_parameters.dt = delta_seconds;
570
571        self.pipeline.step(
572            &self.gravity,
573            &self.integration_parameters,
574            &mut self.island_manager,
575            &mut self.broad_phase,
576            &mut self.narrow_phase,
577            &mut self.rigid_body_set,
578            &mut self.collider_set,
579            &mut self.impulse_joint_set,
580            &mut self.multibody_joint_set,
581            &mut self.ccd_solver,
582            Some(&mut self.query_pipeline),
583            &(),
584            self.contact_event_manager.event_collector(),
585        );
586
587        while let Ok(collision_event) = self.contact_event_manager.get_collider_event() {
588            match collision_event {
589                CollisionEvent::Started(collider_a, collider_b, collision_type) => {
590                    if collision_type != CollisionEventFlags::SENSOR {
591                        if let Some(handlers) = self.collider_event_handlers.get_mut(&collider_a) {
592                            for handler in handlers {
593                                handler.on_collision_start(&collider_b);
594                            }
595                        };
596                        if let Some(handlers) = self.collider_event_handlers.get_mut(&collider_b) {
597                            for handler in handlers {
598                                handler.on_collision_start(&collider_a);
599                            }
600                        }
601                    } else {
602                        if let Some(handlers) = self.collider_event_handlers.get_mut(&collider_a) {
603                            for handler in handlers {
604                                handler.on_intersection_start(&collider_b);
605                            }
606                        };
607                        if let Some(handlers) = self.collider_event_handlers.get_mut(&collider_b) {
608                            for handler in handlers {
609                                handler.on_intersection_start(&collider_a);
610                            }
611                        }
612                    }
613                }
614                CollisionEvent::Stopped(collider_a, collider_b, collision_type) => {
615                    if collision_type != CollisionEventFlags::SENSOR {
616                        if let Some(handlers) = self.collider_event_handlers.get_mut(&collider_a) {
617                            for handler in handlers {
618                                handler.on_collision_end(&collider_b);
619                            }
620                        };
621                        if let Some(handlers) = self.collider_event_handlers.get_mut(&collider_b) {
622                            for handler in handlers {
623                                handler.on_collision_end(&collider_a);
624                            }
625                        }
626                    } else {
627                        if let Some(handlers) = self.collider_event_handlers.get_mut(&collider_a) {
628                            for handler in handlers {
629                                handler.on_intersection_end(&collider_b);
630                            }
631                        };
632                        if let Some(handlers) = self.collider_event_handlers.get_mut(&collider_b) {
633                            for handler in handlers {
634                                handler.on_intersection_end(&collider_a);
635                            }
636                        }
637                    }
638                }
639            }
640        }
641        while let Ok(contact_force_event) = self.contact_event_manager.get_contact_force_event() {
642            if let Some(handlers) = self
643                .collider_event_handlers
644                .get_mut(&contact_force_event.collider1)
645            {
646                for handler in handlers {
647                    handler.on_contact_force_event(
648                        &contact_force_event.collider2,
649                        contact_force_event,
650                    );
651                }
652            };
653            if let Some(handlers) = self
654                .collider_event_handlers
655                .get_mut(&contact_force_event.collider2)
656            {
657                for handler in handlers {
658                    handler.on_contact_force_event(
659                        &contact_force_event.collider1,
660                        contact_force_event,
661                    );
662                }
663            }
664        }
665    }
666
667    /// Recover the handle from a RigidBody using its `user_data` field.
668    pub unsafe fn get_body_handle(body: &RigidBody) -> RigidBodyHandle {
669        let lower_32_bits_mask = 0xffffffff_u128;
670        let body_user_data = body.user_data;
671        let handle_generation_u128 = body_user_data & lower_32_bits_mask;
672        let handle_index_u128 = body_user_data.rotate_right(32) & lower_32_bits_mask;
673        let handle_generation = u32::try_from(handle_generation_u128)
674            .expect("Could not downcast rigid handle generation part from u128 to u32!");
675        let handle_index = u32::try_from(handle_index_u128)
676            .expect("Could not downcast rigid handle index part from u128 to u32!");
677        RigidBodyHandle::from_raw_parts(handle_index, handle_generation)
678    }
679
680    /// Store the parts of the RigidBody's handle in its `user_data`  field.
681    pub unsafe fn store_handle_in_body(handle: &RigidBodyHandle, body: &mut RigidBody) {
682        let handle_parts = handle.into_raw_parts();
683        let handle_index = handle_parts.0;
684        let handle_generation = handle_parts.1;
685        body.user_data = u128::from(handle_index).rotate_left(32) | u128::from(handle_generation);
686    }
687}
688
689#[cfg(test)]
690mod tests {
691    use super::*;
692
693    #[test]
694    fn store_and_recover_rigid_body_handles() {
695        let mut world = PhysicsWorld::default();
696        for _ in 0..10 {
697            let body = RigidBodyBuilder::dynamic().build();
698            let handle = world.rigid_body_set.insert(body);
699            // Get the body again since it was moved into the simulation
700            let body = world.rigid_body_set.get_mut(handle).unwrap();
701
702            unsafe {
703                PhysicsWorld::store_handle_in_body(&handle, body);
704
705                let recovered_handle = PhysicsWorld::get_body_handle(body);
706                assert_eq!(handle, recovered_handle);
707            }
708        }
709    }
710}
perigee/physics/mod.rs

perigee/physics/
mod.rs
