concinnity_core/physics/sim/query/
mod.rs

1// Asking the world a question without stepping it: where does this ray land,
2// and how far does this shape get.
3//
4// Both questions are answered the same way. The broad phase hands back the
5// window of proxies whose bounds the query's own bounds could reach, the
6// window is filtered by layer and by the excluded body, and only what survives
7// is measured exactly. The window is a slice of the sorted sweep order, so the
8// traversal is a walk over an array with no hashing and no set anywhere in it,
9// and two runs of the same query visit the same bodies in the same order.
10//
11// Nothing here allocates. A query keeps one hit, not a list, and every
12// intermediate is a fixed-size array on the stack; the nearest hit is chosen
13// by distance with the body slot breaking ties, so the answer does not even
14// depend on the order the window happened to arrive in.
15//
16// Sensors are left out here, in the one filter every query runs through, so a
17// region that records overlap never stops a ray, a sweep, or a character.
18//
19// The window is only as tight as one sorted axis can make it, which is a real
20// limit rather than a tuning problem. A ray longer than the scene is wide, or
21// a scene holding one proxy that spans it, leaves the window covering nearly
22// every body, and each of those costs a bounds test. A hierarchy over the
23// static set is what makes that case cheap, and it is a structure of its own
24// rather than something this sorted array can be talked into.
25
26pub(super) mod field;
27pub(super) mod gjk;
28mod ray;
29mod simplex;
30pub(super) mod sweep;
31
32use crate::memory::Pool;
33
34use crate::physics::{BodyHandle, ColliderShape, LayerMask, RayHit};
35
36use super::aabb::shape_bounds;
37use super::body::Body;
38use super::collide::Pose;
39use super::math::{Quat, Vec3};
40use super::scene::Scene;
41
42use gjk::Support;
43use ray::{BoundsProbe, Ray};
44
45/// A shape swept through the world along a straight line.
46///
47/// The sweep is a translation: the shape keeps the orientation it starts with
48/// for the whole of `motion`. That is what a character move is, and it is what
49/// makes the time of impact exact rather than bounded.
50#[derive(Debug, Clone, Copy)]
51pub struct ShapeCast {
52    /// What to sweep.
53    pub shape: ColliderShape,
54    /// Where the shape's centre starts, in world space.
55    pub origin: [f32; 3],
56    /// The shape's orientation, held for the whole sweep.
57    pub euler_deg: [f32; 3],
58    /// The whole displacement to sweep along. A zero motion asks only whether
59    /// the shape is already touching something.
60    pub motion: [f32; 3],
61    /// A body to leave out, usually the sweeping character's own.
62    pub exclude: Option<BodyHandle>,
63    /// Layers the sweep interacts with.
64    pub mask: LayerMask,
65}
66
67impl ShapeCast {
68    /// A sweep of `shape` from `origin` along `motion`, unrotated, hitting
69    /// everything.
70    pub fn new(shape: ColliderShape, origin: [f32; 3], motion: [f32; 3]) -> Self {
71        ShapeCast {
72            shape,
73            origin,
74            euler_deg: [0.0; 3],
75            motion,
76            exclude: None,
77            mask: LayerMask::ALL,
78        }
79    }
80}
81
82/// What a [`ShapeCast`] ran into.
83#[derive(Debug, Clone, Copy)]
84pub struct ShapeCastHit {
85    /// The body that was hit.
86    pub body: BodyHandle,
87    /// Fraction of the cast's `motion` covered before the contact, in
88    /// `[0, 1]`. Multiply the motion by it to get the safe displacement.
89    pub toi: f32,
90    /// World-space contact point on the body that was hit.
91    pub point: [f32; 3],
92    /// Unit-length normal on the body that was hit, pointing back toward the
93    /// swept shape. This is the direction to slide along.
94    pub normal: [f32; 3],
95    /// Distance between the two surfaces where the sweep stopped: zero or a
96    /// hair positive for a shape that stopped short of the body, negative for
97    /// one that began inside it. Separating along `normal` by `-gap` is what
98    /// clears the overlap.
99    pub gap: f32,
100    /// Whether the shape was already touching this body before it moved. A
101    /// caller that slides along `normal` has to separate first, or it will be
102    /// handed the same zero-length move again.
103    pub started_touching: bool,
104}
105
106/// What a raycast is asked. One struct rather than five arguments because the
107/// filtering half of it is shared with [`ShapeCast`].
108#[derive(Debug, Clone, Copy)]
109pub(crate) struct RayQuery {
110    pub(crate) origin: [f32; 3],
111    pub(crate) dir: [f32; 3],
112    pub(crate) max_dist: f32,
113    pub(crate) exclude: Option<BodyHandle>,
114    pub(crate) mask: LayerMask,
115}
116
117/// The nearest ray hit, or `None`.
118///
119/// `dir` need not be unit length; a zero direction, a non-finite one, or a
120/// non-positive `max_dist` all miss.
121pub(crate) fn raycast(scene: Scene<'_>, ray_query: &RayQuery) -> Option<RayHit> {
122    let max_dist = ray_query.max_dist;
123    if !(max_dist.is_finite() && max_dist > 0.0) {
124        return None;
125    }
126    let direction = Vec3::from_array(ray_query.dir);
127    let length = direction.length();
128    if !(length.is_finite() && length > 0.0) {
129        return None;
130    }
131    let origin = Vec3::from_array(ray_query.origin);
132    if !origin.is_finite() {
133        return None;
134    }
135    let ray = Ray {
136        origin,
137        direction: direction * (1.0 / length),
138    };
139    let far = origin + ray.direction * max_dist;
140
141    let axis = scene.broadphase.axis();
142    let (low, high) = (
143        origin.get(axis).min(far.get(axis)),
144        origin.get(axis).max(far.get(axis)),
145    );
146
147    let probe = BoundsProbe::new(ray);
148    // Shrinks to the nearest hit found so far: everything past it is out of
149    // the running, and most of a long ray's window is past it.
150    let mut reach = max_dist;
151    let mut best: Option<(u32, RayHit)> = None;
152    for &slot in scene.broadphase.slab_window(low, high) {
153        let proxy = scene.broadphase.proxy(slot);
154        if !ray_query.mask.interacts_with(proxy.mask) || !probe.reaches(proxy.bounds, reach) {
155            continue;
156        }
157        let Some((_, body)) = candidate(scene.bodies, slot, ray_query.exclude) else {
158            continue;
159        };
160        let found = match body.terrain_index() {
161            Some(index) => field::raycast(scene.fields, index, ray, reach),
162            None => body
163                .convex()
164                .and_then(|shape| ray::cast(ray, shape, pose_of(body), reach)),
165        };
166        let Some(impact) = found else {
167            continue;
168        };
169        if nearer(
170            best.map(|(kept, hit)| (kept, hit.distance)),
171            slot,
172            impact.distance,
173        ) {
174            reach = impact.distance;
175            best = Some((
176                slot,
177                RayHit {
178                    point: (origin + ray.direction * impact.distance).to_array(),
179                    normal: impact.normal.to_array(),
180                    distance: impact.distance,
181                },
182            ));
183        }
184    }
185    best.map(|(_, hit)| hit)
186}
187
188/// The nearest body a swept shape runs into, or `None`.
189pub(crate) fn shape_cast(scene: Scene<'_>, cast: &ShapeCast) -> Option<ShapeCastHit> {
190    let origin = Vec3::from_array(cast.origin);
191    let motion = Vec3::from_array(cast.motion);
192    if !origin.is_finite() || !motion.is_finite() {
193        return None;
194    }
195    let rotation = Quat::from_euler_deg(cast.euler_deg);
196    let start = Pose {
197        position: origin,
198        rotation,
199    };
200    let moving = Support::new(&cast.shape, start);
201    let start_bounds = shape_bounds(&cast.shape, origin, rotation);
202    let travel =
203        |toi: f32| start_bounds.union(shape_bounds(&cast.shape, origin + motion * toi, rotation));
204    let swept = travel(1.0);
205
206    let axis = scene.broadphase.axis();
207    // Shrinks to the swept box the nearest contact leaves reachable, so a
208    // sweep that stops early does not measure what it has already passed.
209    let mut reach = swept;
210    let mut best: Option<(u32, ShapeCastHit)> = None;
211    for &slot in scene
212        .broadphase
213        .slab_window(swept.min.get(axis), swept.max.get(axis))
214    {
215        let proxy = scene.broadphase.proxy(slot);
216        if !cast.mask.interacts_with(proxy.mask) || !reach.overlaps(proxy.bounds) {
217            continue;
218        }
219        let Some((handle, body)) = candidate(scene.bodies, slot, cast.exclude) else {
220            continue;
221        };
222        let found = match body.terrain_index() {
223            Some(index) => field::sweep(scene.fields, index, &cast.shape, start, motion, reach),
224            None => body.convex().and_then(|shape| {
225                sweep::sweep(&moving, motion, &Support::new(shape, pose_of(body)))
226            }),
227        };
228        let Some(impact) = found else {
229            continue;
230        };
231        if nearer(best.map(|(kept, hit)| (kept, hit.toi)), slot, impact.toi) {
232            reach = travel(impact.toi);
233            best = Some((
234                slot,
235                ShapeCastHit {
236                    body: handle,
237                    toi: impact.toi,
238                    point: impact.point.to_array(),
239                    normal: impact.normal.to_array(),
240                    gap: impact.gap,
241                    started_touching: impact.started_touching,
242                },
243            ));
244        }
245    }
246    best.map(|(_, hit)| hit)
247}
248
249/// The body at a slot, with its handle, once the cheap filters have let it
250/// through.
251fn candidate(
252    bodies: &Pool<Body>,
253    slot: u32,
254    exclude: Option<BodyHandle>,
255) -> Option<(BodyHandle, &Body)> {
256    let handle = super::world::handle_at(bodies, slot)?;
257    if exclude == Some(handle) {
258        return None;
259    }
260    let body = bodies.get_at(slot as usize)?;
261    // A region records what overlaps it rather than resisting it, so every
262    // query passes straight through one.
263    if body.is_sensor() {
264        return None;
265    }
266    Some((handle, body))
267}
268
269/// Whether a fresh hit beats the one held. Distance decides; the body slot
270/// breaks a tie, so the answer does not depend on traversal order.
271fn nearer(best: Option<(u32, f32)>, slot: u32, measure: f32) -> bool {
272    match best {
273        None => true,
274        Some((kept, held)) => measure < held || (measure == held && slot < kept),
275    }
276}
277
278fn pose_of(body: &Body) -> Pose {
279    Pose {
280        position: body.position,
281        rotation: body.orientation,
282    }
283}
concinnity_core/physics/sim/query/mod.rs

concinnity_core/physics/sim/query/
mod.rs
