pfx_physics/rigid/
query.rs

1use rapier3d::parry::query::ShapeCastOptions;
2use rapier3d::prelude::{Collider, ColliderHandle, Group, QueryFilter, QueryPipeline, Ray};
3
4use super::convert::{body_handle, body_id, collider_id, iso, vec};
5use super::{BodyId, ColliderId, Pose, Shape, World};
6
7#[derive(Clone, Copy, Debug, PartialEq, Eq)]
8pub struct Filter {
9    pub mask: u32,
10    pub sensors: bool,
11    pub exclude: Option<BodyId>,
12}
13
14impl Filter {
15    pub const ALL: Self = Self {
16        mask: u32::MAX,
17        sensors: false,
18        exclude: None,
19    };
20
21    pub fn layers(mask: u32) -> Self {
22        Self { mask, ..Self::ALL }
23    }
24
25    pub fn with_sensors(mut self) -> Self {
26        self.sensors = true;
27        self
28    }
29
30    pub fn excluding(mut self, body: BodyId) -> Self {
31        self.exclude = Some(body);
32        self
33    }
34
35    fn with_pipeline<R>(self, world: &World, run: impl FnOnce(&QueryPipeline<'_>) -> R) -> R {
36        let mask = Group::from_bits_retain(self.mask);
37        let by_layer = move |_: ColliderHandle, collider: &Collider| {
38            collider.collision_groups().memberships.intersects(mask)
39        };
40        let mut filter = QueryFilter::new().predicate(&by_layer);
41        if !self.sensors {
42            filter = filter.exclude_sensors();
43        }
44        if let Some(body) = self.exclude {
45            filter = filter.exclude_rigid_body(body_handle(body));
46        }
47        let physics = &world.physics;
48        let pipeline = world.queries.as_query_pipeline(
49            physics.narrow_phase.query_dispatcher(),
50            &physics.bodies,
51            &physics.colliders,
52            filter,
53        );
54        run(&pipeline)
55    }
56}
57
58impl Default for Filter {
59    fn default() -> Self {
60        Self::ALL
61    }
62}
63
64#[derive(Clone, Copy, Debug, PartialEq)]
65pub struct Hit {
66    pub collider: ColliderId,
67    pub body: Option<BodyId>,
68    pub distance: f32,
69    pub point: [f32; 3],
70    pub normal: [f32; 3],
71}
72
73impl World {
74    pub fn raycast(
75        &self,
76        origin: [f32; 3],
77        direction: [f32; 3],
78        max_distance: f32,
79        filter: Filter,
80    ) -> Option<Hit> {
81        let direction = vec(direction).normalize_or_zero();
82        if direction == super::convert::Vec3::ZERO {
83            return None;
84        }
85        let ray = Ray::new(vec(origin), direction);
86        let physics = &self.physics;
87        let (handle, hit) = filter.with_pipeline(self, |pipeline| {
88            pipeline.cast_ray_and_get_normal(&ray, max_distance, true)
89        })?;
90        Some(Hit {
91            collider: collider_id(handle),
92            body: physics
93                .colliders
94                .get(handle)
95                .and_then(|c| c.parent())
96                .map(body_id),
97            distance: hit.time_of_impact,
98            point: ray.point_at(hit.time_of_impact).to_array(),
99            normal: hit.normal.to_array(),
100        })
101    }
102
103    pub fn shape_cast(
104        &self,
105        shape: &Shape,
106        pose: Pose,
107        direction: [f32; 3],
108        max_distance: f32,
109        filter: Filter,
110    ) -> Option<Hit> {
111        let shared = shape.shared()?;
112        let direction = vec(direction).normalize_or_zero();
113        if direction == super::convert::Vec3::ZERO {
114            return None;
115        }
116        let start = iso(pose);
117        let physics = &self.physics;
118        let (handle, hit) = filter.with_pipeline(self, |pipeline| {
119            pipeline.cast_shape(
120                &start,
121                direction,
122                shared.as_ref(),
123                ShapeCastOptions::with_max_time_of_impact(max_distance),
124            )
125        })?;
126        let mut at = start;
127        at.translation += direction * hit.time_of_impact;
128        Some(Hit {
129            collider: collider_id(handle),
130            body: physics
131                .colliders
132                .get(handle)
133                .and_then(|c| c.parent())
134                .map(body_id),
135            distance: hit.time_of_impact,
136            point: at.transform_point(hit.witness2).to_array(),
137            normal: hit.normal1.to_array(),
138        })
139    }
140
141    pub fn point_overlaps(&self, point: [f32; 3], filter: Filter) -> Vec<ColliderId> {
142        let mut found: Vec<ColliderId> = filter.with_pipeline(self, |pipeline| {
143            pipeline
144                .intersect_point(vec(point))
145                .map(|(handle, _)| collider_id(handle))
146                .collect()
147        });
148        found.sort();
149        found
150    }
151
152    pub fn shape_overlaps(&self, shape: &Shape, pose: Pose, filter: Filter) -> Vec<ColliderId> {
153        let Some(shared) = shape.shared() else {
154            return Vec::new();
155        };
156        let mut found: Vec<ColliderId> = filter.with_pipeline(self, |pipeline| {
157            pipeline
158                .intersect_shape(iso(pose), shared.as_ref())
159                .map(|(handle, _)| collider_id(handle))
160                .collect()
161        });
162        found.sort();
163        found
164    }
165}
pfx_physics/rigid/query.rs

pfx_physics/rigid/
query.rs
