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
10/// Helper around [`TnuaObstacleRadar`] that adds useful methods for querying it.
11pub struct TnuaRadarLens<'a, X: TnuaSpatialExt> {
12    radar: &'a TnuaObstacleRadar,
13    ext: &'a X,
14}
15
16impl<'a, X: TnuaSpatialExt> TnuaRadarLens<'a, X> {
17    /// Create a radar lens around a [`TnuaObstacleRadar`] component.
18    ///
19    /// The `ext` argument is typically a [`SystemParam`](bevy::ecs::system::SystemParam) - which
20    /// means it can be a direct type of an argument of they system funtion (wrappers like
21    /// [`Query`] or [`Res`] are not needed to obtain it). It is typically called
22    /// `TnuaSpatialExt<Backend>` where `<Backend>` is replaced by the name of the physics backend.
23    pub fn new(radar: &'a TnuaObstacleRadar, ext: &'a X) -> Self {
24        Self { radar, ext }
25    }
26
27    /// Similar to [`TnuaObstacleRadar::iter_blips`], but wraps each blip in a
28    /// [`TnuaRadarBlipLens`] which provides helpers for querying the blip in the physics backend.
29    pub fn iter_blips(&'_ self) -> impl Iterator<Item = TnuaRadarBlipLens<'_, X>> {
30        self.radar.iter_blips().filter_map(|entity| {
31            Some(TnuaRadarBlipLens {
32                radar_lens: self,
33                entity,
34                collider_data: self.ext.fetch_collider_data(entity)?,
35                closest_point_cache: OnceCell::new(),
36                closest_point_normal_cache: OnceCell::new(),
37            })
38        })
39    }
40}
41
42pub struct TnuaRadarBlipLens<'a, X: TnuaSpatialExt> {
43    radar_lens: &'a TnuaRadarLens<'a, X>,
44    entity: Entity,
45    /// Physical properties of the collider from the physics backend.
46    ///
47    /// This is typically a tuple of the collider, the position, and the rotation - but these are
48    /// different types in each physics backend.
49    pub collider_data: X::ColliderData<'a>,
50    closest_point_cache: OnceCell<TnuaPointProjectionResult>,
51    closest_point_normal_cache: OnceCell<Vector3>,
52}
53
54impl<X: TnuaSpatialExt> TnuaRadarBlipLens<'_, X> {
55    fn radar(&self) -> &TnuaObstacleRadar {
56        self.radar_lens.radar
57    }
58
59    /// The entity that generated the blip.
60    pub fn entity(&self) -> Entity {
61        self.entity
62    }
63
64    /// Check if the physics engine is solving interaction between the controller entity and the
65    /// blip entity.
66    pub fn is_interactable(&self) -> bool {
67        self.radar_lens
68            .ext
69            .can_interact(self.radar().tracked_entity(), self.entity)
70    }
71
72    /// Closest point (to the controller entity) on the surface of the collider that generated the
73    /// blip.
74    pub fn closest_point(&self) -> TnuaPointProjectionResult {
75        *self.closest_point_cache.get_or_init(|| {
76            self.radar_lens.ext.project_point(
77                self.radar().tracked_position(),
78                false,
79                &self.collider_data,
80            )
81        })
82    }
83
84    /// Closest point (to some provided point) on the surface of the collider that generated the
85    /// blip.
86    pub fn closest_point_from(&self, point: Vector3, solid: bool) -> TnuaPointProjectionResult {
87        self.radar_lens
88            .ext
89            .project_point(point, solid, &self.collider_data)
90    }
91
92    /// Closest point (to an offset from the controller entity) on the surface of the collider that
93    /// generated the blip.
94    pub fn closest_point_from_offset(
95        &self,
96        offset: Vector3,
97        solid: bool,
98    ) -> TnuaPointProjectionResult {
99        self.closest_point_from(self.radar().tracked_position() + offset, solid)
100    }
101
102    /// A number between 0.0 (floor) and 1.0 (wall) indicating how close the blip is to a perfectly
103    /// vertical wall.
104    pub fn flat_wall_score(&self, up: Dir3, offsets: &[Float]) -> Float {
105        let Some(closest_point) = self.closest_point().outside() else {
106            return 0.0;
107        };
108        1.0 - offsets
109            .iter()
110            .map(|offset| {
111                if *offset == 0.0 {
112                    return 0.0;
113                }
114                let offset_vec = *offset * up.adjust_precision();
115                let expected = closest_point + offset_vec;
116                let actual = self.closest_point_from_offset(offset_vec, false).get();
117                let dist = expected.distance_squared(actual);
118                dist / offset.powi(2)
119            })
120            .sum::<Float>()
121            / offsets.len() as Float
122    }
123
124    /// Try traversing the geometry from the [`closest_point`](Self::closest_point) along
125    /// `direction` until reaching `probe_at_distance`.
126    ///
127    /// If the geometry reaches that distance (and behind), that distance will be returned.
128    ///
129    /// If the geometry does not reach the desired distance, and it ends in a right angle or acute
130    /// angle, the distance to that point will be returned.
131    ///
132    /// If the geometry does not reach the desired distance, and it "ends" in an obtuse angle, the
133    /// returned value will be between that point and `probe_at_distance`.
134    ///
135    /// This is useful to detect when the character is near the top of a wall or of a climbable
136    /// object.
137    ///
138    /// Maybe have weird results if used on concave colliders, and the distance may not be accurate
139    /// in genral, so always use a threshold
140    pub fn probe_extent_from_closest_point(
141        &self,
142        direction: Dir3,
143        probe_at_distance: Float,
144    ) -> Float {
145        let closest_point = self.closest_point().get();
146        let closest_above = self
147            .closest_point_from_offset(probe_at_distance * direction.adjust_precision(), false)
148            .get();
149        (closest_above - closest_point).dot(direction.adjust_precision())
150    }
151
152    /// The direction from the controller entity to the blip's surface.
153    ///
154    /// If the controller entity is _inside_ the blip surface (possible when the physics engine is
155    /// set to not solve contacts between them), this will still point into the insdie of the blip
156    /// entity.
157    pub fn direction_to_closest_point(&self) -> Result<Dir3, InvalidDirectionError> {
158        match self.closest_point() {
159            TnuaPointProjectionResult::Outside(closest_point) => {
160                Dir3::new((closest_point - self.radar().tracked_position()).f32())
161            }
162            TnuaPointProjectionResult::Inside(closest_point) => {
163                Dir3::new((self.radar().tracked_position() - closest_point).f32())
164            }
165        }
166    }
167
168    /// The normal on the surface of the blip collider at the [`closest
169    /// point`](Self::closest_point).
170    pub fn normal_from_closest_point(&self) -> Vector3 {
171        *self.closest_point_normal_cache.get_or_init(|| {
172            let origin = self.radar().tracked_position();
173
174            let get_normal = |closest_point: Vector3| -> Vector3 {
175                let Some(direction) = (closest_point - origin).try_normalize() else {
176                    return Vector3::ZERO;
177                };
178                let Some((_, normal)) = self.radar_lens.ext.cast_ray(
179                    origin,
180                    direction,
181                    Float::INFINITY,
182                    &self.collider_data,
183                ) else {
184                    warn!("Unable to query normal to already-found closest point");
185                    return Vector3::ZERO;
186                };
187                normal
188            };
189
190            match self.closest_point() {
191                TnuaPointProjectionResult::Outside(closest_point) => get_normal(closest_point),
192                TnuaPointProjectionResult::Inside(closest_point) => -get_normal(closest_point),
193            }
194        })
195    }
196
197    /// Where is the blip collider located relative to the controller entity.
198    pub fn spatial_relation(&self, threshold: Float) -> TnuaBlipSpatialRelation {
199        let Ok(direction) = self.direction_to_closest_point() else {
200            return TnuaBlipSpatialRelation::Invalid;
201        };
202        let dot_up = self
203            .radar()
204            .up_direction()
205            .dot(*direction)
206            .adjust_precision();
207        if threshold < dot_up {
208            TnuaBlipSpatialRelation::Above
209        } else if dot_up < -threshold {
210            TnuaBlipSpatialRelation::Below
211        } else {
212            let planar_direction =
213                Dir3::new(direction.reject_from_normalized(*self.radar().up_direction()))
214                    .expect("since the dot-up is smaller than the threshold, the direction should not be parallel with the direction");
215            TnuaBlipSpatialRelation::Aeside(planar_direction)
216        }
217    }
218}
219
220/// Where is the blip collider located relative to the controller entity.
221#[derive(Debug)]
222pub enum TnuaBlipSpatialRelation {
223    Invalid,
224    Above,
225    Below,
226    Aeside(Dir3),
227}
bevy_tnua/radar_lens.rs

bevy_tnua/
radar_lens.rs
