1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
//! Functionality for performing ray casts, shape casts, and other spatial queries.
//!
//! Spatial queries query the world for geometric information about [`Collider`s](Collider)
//! and various types of intersections. Currently, four types of spatial queries are supported:
//!
//! - [Raycasts](#raycasting)
//! - [Shapecasts](#shapecasting),
//! - [Point projection](#point-projection)
//! - [Intersection tests](#intersection-tests)
//!
//! All spatial queries can be done using the various methods provided by the [`SpatialQuery`] system parameter.
//!
//! Raycasting and shapecasting can also be done with a component-based approach using the [`RayCaster`] and
//! [`ShapeCaster`] components. They enable performing casts every frame in a way that is often more convenient
//! than the normal [`SpatialQuery`] methods. See their documentation for more information.
//!
//! ## Raycasting
//!
//! **Raycasting** is a spatial query that finds intersections between colliders and a half-line. This can be used for
//! a variety of things like getting information about the environment for character controllers and AI,
//! and even rendering using ray tracing.
//!
//! For each hit during raycasting, the hit entity, a *time of impact* and a normal will be stored in [`RayHitData`].
//! The time of impact refers to how long the ray travelled, which is essentially the distance from the ray origin to
//! the point of intersection.
//!
//! There are two ways to perform raycasts.
//!
//! 1. For simple raycasts, use the [`RayCaster`] component. It returns the results of the raycast
//!    in the [`RayHits`] component every frame. It uses local coordinates, so it will automatically follow the entity
//!    it's attached to or its parent.
//! 2. When you need more control or don't want to cast every frame, use the raycasting methods provided by
//!    [`SpatialQuery`], like [`cast_ray`](SpatialQuery::cast_ray), [`ray_hits`](SpatialQuery::ray_hits) or
//!    [`ray_hits_callback`](SpatialQuery::ray_hits_callback).
//!
//! See the documentation of the components and methods for more information.
//!
//! A simple example using the component-based method looks like this:
//!
//! ```
//! # #[cfg(feature = "2d")]
//! # use avian2d::prelude::*;
//! # #[cfg(feature = "3d")]
//! use avian3d::prelude::*;
//! use bevy::prelude::*;
//!
//! # #[cfg(all(feature = "3d", feature = "f32"))]
//! fn setup(mut commands: Commands) {
//!     // Spawn a ray caster at the center with the rays travelling right
//!     commands.spawn(RayCaster::new(Vec3::ZERO, Dir3::X));
//!     // ...spawn colliders and other things
//! }
//!
//! # #[cfg(all(feature = "3d", feature = "f32"))]
//! fn print_hits(query: Query<(&RayCaster, &RayHits)>) {
//!     for (ray, hits) in &query {
//!         // For the faster iterator that isn't sorted, use `.iter()`
//!         for hit in hits.iter_sorted() {
//!             println!(
//!                 "Hit entity {:?} at {} with normal {}",
//!                 hit.entity,
//!                 ray.origin + *ray.direction * hit.time_of_impact,
//!                 hit.normal,
//!             );
//!         }
//!     }
//! }
//! ```
//!
//! To specify which colliders should be considered in the query, use a [spatial query filter](`SpatialQueryFilter`).
//!
//! ## Shapecasting
//!
//! **Shapecasting** or **sweep testing** is a spatial query that finds intersections between colliders and a shape
//! that is travelling along a half-line. It is very similar to [raycasting](#raycasting), but instead of a "point"
//! we have an entire shape travelling along a half-line. One use case is determining how far an object can move
//! before it hits the environment.
//!
//! For each hit during shapecasting, the hit entity, the *time of impact*, two local points of intersection and two local
//! normals will be stored in [`ShapeHitData`]. The time of impact refers to how long the shape travelled before the initial
//! hit, which is essentially the distance from the shape origin to the global point of intersection.
//!
//! There are two ways to perform shapecasts.
//!
//! 1. For simple shapecasts, use the [`ShapeCaster`] component. It returns the results of the shapecast
//!    in the [`ShapeHits`] component every frame. It uses local coordinates, so it will automatically follow the entity
//!    it's attached to or its parent.
//! 2. When you need more control or don't want to cast every frame, use the shapecasting methods provided by
//!    [`SpatialQuery`], like [`cast_shape`](SpatialQuery::cast_shape), [`shape_hits`](SpatialQuery::shape_hits) or
//!    [`shape_hits_callback`](SpatialQuery::shape_hits_callback).
//!
//! See the documentation of the components and methods for more information.
//!
//! A simple example using the component-based method looks like this:
//!
//! ```
//! # #[cfg(feature = "2d")]
//! # use avian2d::prelude::*;
//! # #[cfg(feature = "3d")]
//! use avian3d::prelude::*;
//! use bevy::prelude::*;
//!
//! # #[cfg(all(feature = "3d", feature = "f32"))]
//! fn setup(mut commands: Commands) {
//!     // Spawn a shape caster with a sphere shape at the center travelling right
//!     commands.spawn(ShapeCaster::new(
//!         Collider::sphere(0.5), // Shape
//!         Vec3::ZERO,            // Origin
//!         Quat::default(),       // Shape rotation
//!         Dir3::X         // Direction
//!     ));
//!     // ...spawn colliders and other things
//! }
//!
//! fn print_hits(query: Query<(&ShapeCaster, &ShapeHits)>) {
//!     for (shape_caster, hits) in &query {
//!         for hit in hits.iter() {
//!             println!("Hit entity {:?}", hit.entity);
//!         }
//!     }
//! }
//! ```
//!
//! To specify which colliders should be considered in the query, use a [spatial query filter](`SpatialQueryFilter`).
//!
//! ## Point projection
//!
//! **Point projection** is a spatial query that projects a point on the closest collider. It returns the collider's
//! entity, the projected point, and whether the point is inside of the collider.
//!
//! Point projection can be done with the [`project_point`](SpatialQuery::project_point) method of the [`SpatialQuery`]
//! system parameter. See its documentation for more information.
//!
//! To specify which colliders should be considered in the query, use a [spatial query filter](`SpatialQueryFilter`).
//!
//! ## Intersection tests
//!
//! **Intersection tests** are spatial queries that return the entities of colliders that are intersecting a given
//! shape or area.
//!
//! There are three types of intersection tests. They are all methods of the [`SpatialQuery`] system parameter,
//! and they all have callback variants that call a given callback on each intersection.
//!
//! - [`point_intersections`](SpatialQuery::point_intersections): Finds all entities with a collider that contains
//!   the given point.
//! - [`aabb_intersections_with_aabb`](SpatialQuery::aabb_intersections_with_aabb):
//!   Finds all entities with a [`ColliderAabb`] that is intersecting the given [`ColliderAabb`].
//! - [`shape_intersections`](SpatialQuery::shape_intersections): Finds all entities with a [collider](Collider)
//!   that is intersecting the given shape.
//!
//! See the documentation of the components and methods for more information.
//!
//! To specify which colliders should be considered in the query, use a [spatial query filter](`SpatialQueryFilter`).

#[cfg(any(feature = "parry-f32", feature = "parry-f64"))]
mod pipeline;
mod query_filter;
mod ray_caster;
#[cfg(any(feature = "parry-f32", feature = "parry-f64"))]
mod shape_caster;
#[cfg(any(feature = "parry-f32", feature = "parry-f64"))]
mod system_param;

#[cfg(any(feature = "parry-f32", feature = "parry-f64"))]
pub use pipeline::*;
pub use query_filter::*;
pub use ray_caster::*;
#[cfg(any(feature = "parry-f32", feature = "parry-f64"))]
pub use shape_caster::*;
#[cfg(any(feature = "parry-f32", feature = "parry-f64"))]
pub use system_param::*;

use crate::{prelude::*, prepare::PrepareSet};
use bevy::{
    ecs::{intern::Interned, schedule::ScheduleLabel},
    prelude::*,
};

/// Initializes the [`SpatialQueryPipeline`] resource and handles component-based [spatial queries](spatial_query)
/// like [raycasting](spatial_query#raycasting) and [shapecasting](spatial_query#shapecasting) with
/// [`RayCaster`] and [`ShapeCaster`].
pub struct SpatialQueryPlugin {
    schedule: Interned<dyn ScheduleLabel>,
}

impl SpatialQueryPlugin {
    /// Creates a [`SpatialQueryPlugin`] with the schedule that is used for running the [`PhysicsSchedule`].
    ///
    /// The default schedule is `PostUpdate`.
    pub fn new(schedule: impl ScheduleLabel) -> Self {
        Self {
            schedule: schedule.intern(),
        }
    }
}

impl Default for SpatialQueryPlugin {
    fn default() -> Self {
        Self::new(PostUpdate)
    }
}

impl Plugin for SpatialQueryPlugin {
    fn build(&self, app: &mut App) {
        #[cfg(all(
            feature = "default-collider",
            any(feature = "parry-f32", feature = "parry-f64")
        ))]
        app.init_resource::<SpatialQueryPipeline>();

        app.add_systems(self.schedule, init_ray_hits.in_set(PrepareSet::PreInit));

        #[cfg(all(
            feature = "default-collider",
            any(feature = "parry-f32", feature = "parry-f64")
        ))]
        app.add_systems(self.schedule, init_shape_hits.in_set(PrepareSet::PreInit));

        let physics_schedule = app
            .get_schedule_mut(PhysicsSchedule)
            .expect("add PhysicsSchedule first");

        physics_schedule.add_systems(
            (
                update_ray_caster_positions,
                #[cfg(all(
                    feature = "default-collider",
                    any(feature = "parry-f32", feature = "parry-f64")
                ))]
                (
                    update_shape_caster_positions,
                    |mut spatial_query: SpatialQuery| spatial_query.update_pipeline(),
                    raycast,
                    shapecast,
                )
                    .chain(),
            )
                .chain()
                .in_set(PhysicsStepSet::SpatialQuery),
        );
    }
}

fn init_ray_hits(mut commands: Commands, rays: Query<(Entity, &RayCaster), Added<RayCaster>>) {
    for (entity, ray) in &rays {
        let max_hits = if ray.max_hits == u32::MAX {
            10
        } else {
            ray.max_hits as usize
        };
        commands.entity(entity).try_insert(RayHits {
            vector: Vec::with_capacity(max_hits),
            count: 0,
        });
    }
}

#[cfg(any(feature = "parry-f32", feature = "parry-f64"))]
fn init_shape_hits(
    mut commands: Commands,
    shape_casters: Query<(Entity, &ShapeCaster), Added<ShapeCaster>>,
) {
    for (entity, shape_caster) in &shape_casters {
        commands.entity(entity).try_insert(ShapeHits {
            vector: Vec::with_capacity(shape_caster.max_hits.min(100_000) as usize),
            count: 0,
        });
    }
}

type RayCasterPositionQueryComponents = (
    &'static mut RayCaster,
    Option<&'static Position>,
    Option<&'static Rotation>,
    Option<&'static Parent>,
    Option<&'static GlobalTransform>,
);

#[allow(clippy::type_complexity)]
fn update_ray_caster_positions(
    mut rays: Query<RayCasterPositionQueryComponents>,
    parents: Query<
        (
            Option<&Position>,
            Option<&Rotation>,
            Option<&GlobalTransform>,
        ),
        With<Children>,
    >,
) {
    for (mut ray, position, rotation, parent, transform) in &mut rays {
        let origin = ray.origin;
        let direction = ray.direction;

        let global_position = position.copied().or(transform.map(Position::from));
        let global_rotation = rotation.copied().or(transform.map(Rotation::from));

        if let Some(global_position) = global_position {
            ray.set_global_origin(global_position.0 + rotation.map_or(origin, |rot| rot * origin));
        } else if parent.is_none() {
            ray.set_global_origin(origin);
        }

        if let Some(global_rotation) = global_rotation {
            let global_direction = global_rotation * ray.direction;
            ray.set_global_direction(global_direction);
        } else if parent.is_none() {
            ray.set_global_direction(direction);
        }

        if let Some(Ok((parent_position, parent_rotation, parent_transform))) =
            parent.map(|p| parents.get(p.get()))
        {
            let parent_position = parent_position
                .copied()
                .or(parent_transform.map(Position::from));
            let parent_rotation = parent_rotation
                .copied()
                .or(parent_transform.map(Rotation::from));

            // Apply parent transformations
            if global_position.is_none() {
                if let Some(position) = parent_position {
                    let rotation = global_rotation.unwrap_or(parent_rotation.unwrap_or_default());
                    ray.set_global_origin(position.0 + rotation * origin);
                }
            }
            if global_rotation.is_none() {
                if let Some(rotation) = parent_rotation {
                    let global_direction = rotation * ray.direction;
                    ray.set_global_direction(global_direction);
                }
            }
        }
    }
}

#[cfg(any(feature = "parry-f32", feature = "parry-f64"))]
type ShapeCasterPositionQueryComponents = (
    &'static mut ShapeCaster,
    Option<&'static Position>,
    Option<&'static Rotation>,
    Option<&'static Parent>,
    Option<&'static GlobalTransform>,
);

#[cfg(any(feature = "parry-f32", feature = "parry-f64"))]
#[allow(clippy::type_complexity)]
fn update_shape_caster_positions(
    mut shape_casters: Query<ShapeCasterPositionQueryComponents>,
    parents: Query<
        (
            Option<&Position>,
            Option<&Rotation>,
            Option<&GlobalTransform>,
        ),
        With<Children>,
    >,
) {
    for (mut shape_caster, position, rotation, parent, transform) in &mut shape_casters {
        let origin = shape_caster.origin;
        let shape_rotation = shape_caster.shape_rotation;
        let direction = shape_caster.direction;

        let global_position = position.copied().or(transform.map(Position::from));
        let global_rotation = rotation.copied().or(transform.map(Rotation::from));

        if let Some(global_position) = global_position {
            shape_caster
                .set_global_origin(global_position.0 + rotation.map_or(origin, |rot| rot * origin));
        } else if parent.is_none() {
            shape_caster.set_global_origin(origin);
        }

        if let Some(global_rotation) = global_rotation {
            let global_direction = global_rotation * shape_caster.direction;
            shape_caster.set_global_direction(global_direction);
            #[cfg(feature = "2d")]
            {
                shape_caster
                    .set_global_shape_rotation(shape_rotation + global_rotation.as_radians());
            }
            #[cfg(feature = "3d")]
            {
                shape_caster.set_global_shape_rotation(shape_rotation * global_rotation.0);
            }
        } else if parent.is_none() {
            shape_caster.set_global_direction(direction);
            #[cfg(feature = "2d")]
            {
                shape_caster.set_global_shape_rotation(shape_rotation);
            }
            #[cfg(feature = "3d")]
            {
                shape_caster.set_global_shape_rotation(shape_rotation);
            }
        }

        if let Some(Ok((parent_position, parent_rotation, parent_transform))) =
            parent.map(|p| parents.get(p.get()))
        {
            let parent_position = parent_position
                .copied()
                .or(parent_transform.map(Position::from));
            let parent_rotation = parent_rotation
                .copied()
                .or(parent_transform.map(Rotation::from));

            // Apply parent transformations
            if global_position.is_none() {
                if let Some(position) = parent_position {
                    let rotation = global_rotation.unwrap_or(parent_rotation.unwrap_or_default());
                    shape_caster.set_global_origin(position.0 + rotation * origin);
                }
            }
            if global_rotation.is_none() {
                if let Some(rotation) = parent_rotation {
                    let global_direction = rotation * shape_caster.direction;
                    shape_caster.set_global_direction(global_direction);
                    #[cfg(feature = "2d")]
                    {
                        shape_caster
                            .set_global_shape_rotation(shape_rotation + rotation.as_radians());
                    }
                    #[cfg(feature = "3d")]
                    {
                        shape_caster.set_global_shape_rotation(shape_rotation * rotation.0);
                    }
                }
            }
        }
    }
}

#[cfg(any(feature = "parry-f32", feature = "parry-f64"))]
fn raycast(mut rays: Query<(Entity, &RayCaster, &mut RayHits)>, spatial_query: SpatialQuery) {
    for (entity, ray, mut hits) in &mut rays {
        if ray.enabled {
            ray.cast(entity, &mut hits, &spatial_query.query_pipeline);
        } else if !hits.is_empty() {
            hits.clear();
        }
    }
}

#[cfg(any(feature = "parry-f32", feature = "parry-f64"))]
fn shapecast(
    mut shape_casters: Query<(Entity, &ShapeCaster, &mut ShapeHits)>,
    spatial_query: SpatialQuery,
) {
    for (entity, shape_caster, mut hits) in &mut shape_casters {
        if shape_caster.enabled {
            shape_caster.cast(entity, &mut hits, &spatial_query.query_pipeline);
        } else if !hits.is_empty() {
            hits.clear();
        }
    }
}