bevy_tnua/
radar_lens.rs

1use std::cell::OnceCell;
2
3use crate::math::{AdjustPrecision, AsF32, Float, Vector3};
4use bevy::{math::InvalidDirectionError, prelude::*};
5use bevy_tnua_physics_integration_layer::{
6    obstacle_radar::TnuaObstacleRadar,
7    spatial_ext::{TnuaPointProjectionResult, TnuaSpatialExt},
8};
9
10pub struct TnuaRadarLens<'a, X: TnuaSpatialExt> {
11    radar: &'a TnuaObstacleRadar,
12    ext: &'a X,
13}
14
15impl<'a, X: TnuaSpatialExt> TnuaRadarLens<'a, X> {
16    pub fn new(radar: &'a TnuaObstacleRadar, ext: &'a X) -> Self {
17        Self { radar, ext }
18    }
19
20    pub fn iter_blips(&'_ self) -> impl Iterator<Item = TnuaRadarBlipLens<'_, X>> {
21        self.radar.iter_blips().filter_map(|entity| {
22            Some(TnuaRadarBlipLens {
23                radar_lens: self,
24                entity,
25                collider_data: self.ext.fetch_collider_data(entity)?,
26                closest_point_cache: OnceCell::new(),
27                closest_point_normal_cache: OnceCell::new(),
28            })
29        })
30    }
31}
32
33pub struct TnuaRadarBlipLens<'a, X: TnuaSpatialExt> {
34    radar_lens: &'a TnuaRadarLens<'a, X>,
35    entity: Entity,
36    pub collider_data: X::ColliderData<'a>,
37    closest_point_cache: OnceCell<TnuaPointProjectionResult>,
38    closest_point_normal_cache: OnceCell<Vector3>,
39}
40
41impl<X: TnuaSpatialExt> TnuaRadarBlipLens<'_, X> {
42    fn radar(&self) -> &TnuaObstacleRadar {
43        self.radar_lens.radar
44    }
45
46    pub fn entity(&self) -> Entity {
47        self.entity
48    }
49
50    pub fn is_interactable(&self) -> bool {
51        self.radar_lens
52            .ext
53            .can_interact(self.radar().tracked_entity(), self.entity)
54    }
55
56    pub fn closest_point(&self) -> TnuaPointProjectionResult {
57        *self.closest_point_cache.get_or_init(|| {
58            self.radar_lens.ext.project_point(
59                self.radar().tracked_position(),
60                false,
61                &self.collider_data,
62            )
63        })
64    }
65
66    pub fn closest_point_from(&self, point: Vector3, solid: bool) -> TnuaPointProjectionResult {
67        self.radar_lens
68            .ext
69            .project_point(point, solid, &self.collider_data)
70    }
71
72    pub fn closest_point_from_offset(
73        &self,
74        offset: Vector3,
75        solid: bool,
76    ) -> TnuaPointProjectionResult {
77        self.closest_point_from(self.radar().tracked_position() + offset, solid)
78    }
79
80    pub fn flat_wall_score(&self, up: Dir3, offsets: &[Float]) -> Float {
81        let Some(closest_point) = self.closest_point().outside() else {
82            return 0.0;
83        };
84        1.0 - offsets
85            .iter()
86            .map(|offset| {
87                if *offset == 0.0 {
88                    return 0.0;
89                }
90                let offset_vec = *offset * up.adjust_precision();
91                let expected = closest_point + offset_vec;
92                let actual = self.closest_point_from_offset(offset_vec, false).get();
93                let dist = expected.distance_squared(actual);
94                dist / offset.powi(2)
95            })
96            .sum::<Float>()
97            / offsets.len() as Float
98    }
99
100    /// Try traversing the geometry from the [`closest_point`](Self::closest_point) along
101    /// `direction` until reaching `probe_at_distance`.
102    ///
103    /// If the geometry reaches that distance (and behind), that distance will be returned.
104    ///
105    /// If the geometry does not reach the desired distance, and it ends in a right angle or acute
106    /// angle, the distance to that point will be returned.
107    ///
108    /// If the geometry does not reach the desired distance, and it "ends" in an obtuse angle, the
109    /// returned value will be between that point and `probe_at_distance`.
110    ///
111    /// This is useful to detect when the character is near the top of a wall or of a climbable
112    /// object.
113    ///
114    /// Maybe have weird results if used on concave colliders, and the distance may not be accurate
115    /// in genral, so always use a threshold
116    pub fn probe_extent_from_closest_point(
117        &self,
118        direction: Dir3,
119        probe_at_distance: Float,
120    ) -> Float {
121        let closest_point = self.closest_point().get();
122        let closest_above = self
123            .closest_point_from_offset(probe_at_distance * direction.adjust_precision(), false)
124            .get();
125        (closest_above - closest_point).dot(direction.adjust_precision())
126    }
127
128    pub fn direction_to_closest_point(&self) -> Result<Dir3, InvalidDirectionError> {
129        match self.closest_point() {
130            TnuaPointProjectionResult::Outside(closest_point) => {
131                Dir3::new((closest_point - self.radar().tracked_position()).f32())
132            }
133            TnuaPointProjectionResult::Inside(closest_point) => {
134                Dir3::new((self.radar().tracked_position() - closest_point).f32())
135            }
136        }
137    }
138
139    pub fn normal_from_closest_point(&self) -> Vector3 {
140        *self.closest_point_normal_cache.get_or_init(|| {
141            let origin = self.radar().tracked_position();
142
143            let get_normal = |closest_point: Vector3| -> Vector3 {
144                let Some(direction) = (closest_point - origin).try_normalize() else {
145                    return Vector3::ZERO;
146                };
147                let Some((_, normal)) = self.radar_lens.ext.cast_ray(
148                    origin,
149                    direction,
150                    Float::INFINITY,
151                    &self.collider_data,
152                ) else {
153                    warn!("Unable to query normal to already-found closest point");
154                    return Vector3::ZERO;
155                };
156                normal
157            };
158
159            match self.closest_point() {
160                TnuaPointProjectionResult::Outside(closest_point) => get_normal(closest_point),
161                TnuaPointProjectionResult::Inside(closest_point) => -get_normal(closest_point),
162            }
163        })
164    }
165
166    pub fn spatial_relation(&self, threshold: Float) -> TnuaBlipSpatialRelation {
167        let Ok(direction) = self.direction_to_closest_point() else {
168            return TnuaBlipSpatialRelation::Invalid;
169        };
170        let dot_up = self
171            .radar()
172            .up_direction()
173            .dot(*direction)
174            .adjust_precision();
175        if threshold < dot_up {
176            TnuaBlipSpatialRelation::Above
177        } else if dot_up < -threshold {
178            TnuaBlipSpatialRelation::Below
179        } else {
180            let planar_direction =
181                Dir3::new(direction.reject_from_normalized(*self.radar().up_direction()))
182                    .expect("since the dot-up is smaller than the threshold, the direction should not be parallel with the direction");
183            TnuaBlipSpatialRelation::Aeside(planar_direction)
184        }
185    }
186}
187
188#[derive(Debug)]
189pub enum TnuaBlipSpatialRelation {
190    Invalid,
191    Above,
192    Below,
193    Aeside(Dir3),
194}
bevy_tnua/radar_lens.rs

bevy_tnua/
radar_lens.rs
