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

use bevy::prelude::*;
use de_core::{
    gamestate::GameState,
    objects::{MovableSolid, ObjectTypeComponent, StaticSolid},
    schedule::{Movement, PreMovement},
};
use de_index::SpatialQuery;
use de_objects::SolidObjects;
use de_types::projection::ToFlat;
use parry3d::{bounding_volume::Aabb, math::Point};

use crate::{cache::DecayingCache, disc::Disc};

/// Obstacle avoidance algorithm takes into account only obstacles inside a
/// rectangle of this half-size.
const NEARBY_HALF_EXTENT: f32 = 10.;

pub(crate) struct ObstaclesPlugin;

impl Plugin for ObstaclesPlugin {
    fn build(&self, app: &mut App) {
        app.add_systems(
            PreMovement,
            (setup_discs, update_discs).run_if(in_state(GameState::Playing)),
        )
        .add_systems(
            Movement,
            (
                update_nearby::<StaticObstacles, StaticSolid>,
                update_nearby::<MovableObstacles, MovableSolid>,
            )
                .run_if(in_state(GameState::Playing))
                .in_set(ObstaclesLables::UpdateNearby),
        );
    }
}

#[derive(Copy, Clone, Hash, Debug, PartialEq, Eq, SystemSet)]
pub(crate) enum ObstaclesLables {
    UpdateNearby,
}

pub(crate) struct StaticObstacles;

pub(crate) struct MovableObstacles;

type Uninitialized<'w, 's> = Query<
    'w,
    's,
    (Entity, &'static Transform, &'static ObjectTypeComponent),
    (With<MovableSolid>, Without<Disc>),
>;

fn setup_discs(mut commands: Commands, solids: SolidObjects, objects: Uninitialized) {
    for (entity, transform, &object_type) in objects.iter() {
        let center = transform.translation.to_flat();
        let radius = solids.get(*object_type).ichnography().radius();
        commands.entity(entity).insert((
            Disc::new(center, radius),
            DecayingCache::<StaticObstacles>::default(),
            DecayingCache::<MovableObstacles>::default(),
        ));
    }
}

fn update_discs(mut objects: Query<(&Transform, &mut Disc), Changed<Transform>>) {
    for (transform, mut disc) in objects.iter_mut() {
        disc.set_center(transform.translation.to_flat());
    }
}

fn update_nearby<M: Send + Sync + 'static, T: Component>(
    time: Res<Time>,
    mut objects: Query<(Entity, &Transform, &mut DecayingCache<M>)>,
    space: SpatialQuery<Entity, With<T>>,
) {
    objects
        .par_iter_mut()
        .for_each(|(entity, transform, mut cache)| {
            cache.clear();
            let half_extent = Vec3::splat(NEARBY_HALF_EXTENT);
            let mins = transform.translation - half_extent;
            let maxs = transform.translation + half_extent;
            let region = Aabb::new(Point::from(mins), Point::from(maxs));
            cache.extend(space.query_aabb(&region, Some(entity)));
            cache.decay(time.delta_seconds());
        });
}