1use pfx_load::scene::BodyShape;
2use pfx_physics::rigid::{self, BodyId, ColliderId, Filter, Pose};
3
4use super::{ObjectId, World, shape};
5
6#[derive(Clone, Debug, Default, PartialEq)]
7pub struct Query {
8 layers: Option<Vec<String>>,
9 mask: Option<u32>,
10 triggers: bool,
11 exclude: Option<String>,
12}
13
14impl Query {
15 pub fn all() -> Self {
16 Self::default()
17 }
18
19 pub fn layers<S: AsRef<str>>(names: &[S]) -> Self {
20 Self::all().on(names)
21 }
22
23 pub fn on<S: AsRef<str>>(mut self, names: &[S]) -> Self {
24 self.layers = Some(names.iter().map(|name| name.as_ref().to_string()).collect());
25 self.mask = None;
26 self
27 }
28
29 pub fn mask(mut self, mask: u32) -> Self {
30 self.mask = Some(mask);
31 self.layers = None;
32 self
33 }
34
35 pub fn with_triggers(mut self) -> Self {
36 self.triggers = true;
37 self
38 }
39
40 pub fn excluding(mut self, object: &str) -> Self {
41 self.exclude = Some(object.to_string());
42 self
43 }
44}
45
46#[derive(Clone, Debug, PartialEq, Eq)]
47pub enum QueryError {
48 Layer(String),
49 Object(String),
50 Shape,
51}
52
53impl std::fmt::Display for QueryError {
54 fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
55 match self {
56 Self::Layer(message) => f.write_str(message),
57 Self::Object(name) => write!(f, "query: {name} is not an object of the scene"),
58 Self::Shape => f.write_str("query: the shape needs sizes above 0"),
59 }
60 }
61}
62
63impl std::error::Error for QueryError {}
64
65#[derive(Clone, Debug, PartialEq)]
66pub struct Hit {
67 pub object: Option<ObjectId>,
68 pub name: Option<String>,
69 pub trigger: bool,
70 pub distance: f32,
71 pub point: [f32; 3],
72 pub normal: [f32; 3],
73}
74
75#[derive(Clone, Debug, PartialEq, Eq)]
76pub struct Overlap {
77 pub object: ObjectId,
78 pub name: String,
79 pub trigger: bool,
80}
81
82fn usable(body: BodyShape) -> Result<rigid::Shape, QueryError> {
83 let ok = |size: f32| size.is_finite() && size > 0.0;
84 let fits = match body {
85 BodyShape::Box { half } => half.into_iter().all(ok),
86 BodyShape::Sphere { radius } => ok(radius),
87 BodyShape::Capsule {
88 half_height,
89 radius,
90 }
91 | BodyShape::Cylinder {
92 half_height,
93 radius,
94 } => ok(half_height) && ok(radius),
95 };
96 if fits {
97 Ok(shape(body))
98 } else {
99 Err(QueryError::Shape)
100 }
101}
102
103impl World {
104 fn filter(&self, query: &Query) -> Result<Filter, QueryError> {
105 let mask = match (&query.layers, query.mask) {
106 (Some(names), _) => self.layers.mask(names).map_err(QueryError::Layer)?,
107 (None, Some(mask)) => mask,
108 (None, None) => u32::MAX,
109 };
110 let mut filter = Filter::layers(mask);
111 if query.triggers {
112 filter = filter.with_sensors();
113 }
114 if let Some(name) = &query.exclude {
115 let place = *self
116 .index
117 .get(name)
118 .ok_or_else(|| QueryError::Object(name.clone()))?;
119 if let Some(body) = self.body_of_place(place) {
120 filter = filter.excluding(body);
121 }
122 }
123 Ok(filter)
124 }
125
126 fn body_of_place(&self, place: usize) -> Option<BodyId> {
127 self.bodies
128 .get(&place)
129 .map(|physical| physical.id)
130 .or_else(|| self.sensors.get(&place).map(|sensor| sensor.body))
131 }
132
133 fn owner(&self, collider: ColliderId) -> Option<(usize, bool)> {
134 self.owners
135 .get(&collider)
136 .map(|place| (*place, false))
137 .or_else(|| {
138 self.sensor_owners
139 .get(&collider)
140 .map(|place| (*place, true))
141 })
142 }
143
144 fn hit(&self, hit: rigid::Hit) -> Hit {
145 let owner = self.owner(hit.collider);
146 Hit {
147 object: owner.map(|(place, _)| ObjectId(place)),
148 name: owner.map(|(place, _)| self.objects[place].name.clone()),
149 trigger: owner.is_some_and(|(_, trigger)| trigger),
150 distance: hit.distance,
151 point: hit.point,
152 normal: hit.normal,
153 }
154 }
155
156 pub fn raycast(
157 &self,
158 origin: [f32; 3],
159 direction: [f32; 3],
160 max_distance: f32,
161 query: &Query,
162 ) -> Result<Option<Hit>, QueryError> {
163 let filter = self.filter(query)?;
164 Ok(self
165 .physics
166 .raycast(origin, direction, max_distance, filter)
167 .map(|hit| self.hit(hit)))
168 }
169
170 pub fn shape_cast(
171 &self,
172 shape_of: BodyShape,
173 at: [f32; 3],
174 rotation: [f32; 4],
175 direction: [f32; 3],
176 max_distance: f32,
177 query: &Query,
178 ) -> Result<Option<Hit>, QueryError> {
179 let filter = self.filter(query)?;
180 let shape = usable(shape_of)?;
181 Ok(self
182 .physics
183 .shape_cast(
184 &shape,
185 Pose::new(at, rotation),
186 direction,
187 max_distance,
188 filter,
189 )
190 .map(|hit| self.hit(hit)))
191 }
192
193 pub fn overlaps(
194 &self,
195 shape_of: BodyShape,
196 at: [f32; 3],
197 rotation: [f32; 4],
198 query: &Query,
199 ) -> Result<Vec<Overlap>, QueryError> {
200 let filter = self.filter(query)?;
201 let shape = usable(shape_of)?;
202 Ok(self.overlaps_of(
203 self.physics
204 .shape_overlaps(&shape, Pose::new(at, rotation), filter),
205 ))
206 }
207
208 pub fn overlaps_point(
209 &self,
210 point: [f32; 3],
211 query: &Query,
212 ) -> Result<Vec<Overlap>, QueryError> {
213 let filter = self.filter(query)?;
214 Ok(self.overlaps_of(self.physics.point_overlaps(point, filter)))
215 }
216
217 fn overlaps_of(&self, colliders: Vec<ColliderId>) -> Vec<Overlap> {
218 let mut found: Vec<(usize, bool)> = colliders
219 .into_iter()
220 .filter_map(|collider| self.owner(collider))
221 .collect();
222 found.sort();
223 found.dedup();
224 found
225 .into_iter()
226 .map(|(place, trigger)| Overlap {
227 object: ObjectId(place),
228 name: self.objects[place].name.clone(),
229 trigger,
230 })
231 .collect()
232 }
233}