concinnity_physics/sim/query/
mod.rs

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

concinnity_physics/sim/query/
mod.rs
