wowlab_engine_spatial/

rstar.rs

//! Selected rstar + robust implementation of the spatial-query port.

use robust::{Coord, orient2d};
use rstar::{
    AABB, RTree,
    primitives::{GeomWithData, Line, Rectangle},
};
use wowlab_engine_ports::{
    ConeQuery, RadiusQuery, SpatialActor, SpatialEnvelope, SpatialQuery, SpatialQueryError,
};
use wowlab_types::sim::{
    EnemyIdx, GEOMETRY_EPSILON, Position2, SpatialLayerId, SpatialTransform, StaticObstacle,
    StaticSpatialScene,
};

use crate::{SpatialQueryDiagnostics, SpatialQueryObservation};

const POINT_DIMENSIONS: usize = 2;
const EDGE_POINT_COUNT: usize = 2;
const QUADRANT_COUNT: i32 = 4;

type ActorEntry = GeomWithData<[f64; POINT_DIMENSIONS], EnemyIdx>;
type SegmentEntry = GeomWithData<Line<[f64; POINT_DIMENSIONS]>, usize>;
type PolygonEntry = GeomWithData<Rectangle<[f64; POINT_DIMENSIONS]>, usize>;

#[derive(Clone, Debug)]
struct PolygonBlocker {
    exterior: Vec<Position2>,
    holes: Vec<Box<[Position2]>>,
}

impl PolygonBlocker {
    fn from_rings(exterior: &[Position2], holes: &[Vec<Position2>]) -> Self {
        Self {
            exterior: exterior.to_vec(),
            holes: holes
                .iter()
                .map(|ring| ring.clone().into_boxed_slice())
                .collect(),
        }
    }
}

#[derive(Debug)]
struct LayerIndex {
    actors: RTree<ActorEntry>,
    obstacles: RTree<SegmentEntry>,
    segments: Vec<(Position2, Position2)>,
    polygon_envelopes: RTree<PolygonEntry>,
    polygons: Vec<PolygonBlocker>,
}

impl LayerIndex {
    fn new() -> Self {
        Self {
            actors: RTree::new(),
            obstacles: RTree::new(),
            segments: Vec::new(),
            polygon_envelopes: RTree::new(),
            polygons: Vec::new(),
        }
    }
}

#[derive(Debug)]
pub(crate) struct RstarSpatialQuery {
    layers: Vec<LayerIndex>,
    indexed_transforms: Vec<Option<SpatialTransform>>,
}

impl RstarSpatialQuery {
    pub(crate) fn build(
        scene: &StaticSpatialScene,
        actors: &[SpatialActor],
    ) -> Result<Self, SpatialQueryError> {
        let mut layers: Vec<_> = scene.layers.iter().map(|_| LayerIndex::new()).collect();

        for obstacle in &scene.obstacles {
            match obstacle {
                StaticObstacle::Segment { layer, start, end } => {
                    push_segment(layer_index_mut(&mut layers, *layer)?, *start, *end);
                }
                StaticObstacle::Polygon {
                    layer,
                    exterior,
                    holes,
                } => {
                    let index = layer_index_mut(&mut layers, *layer)?;

                    push_ring_segments(index, exterior);

                    for hole in holes {
                        push_ring_segments(index, hole);
                    }

                    index
                        .polygons
                        .push(PolygonBlocker::from_rings(exterior, holes));
                }
            }
        }

        for layer in &mut layers {
            let entries = layer
                .segments
                .iter()
                .enumerate()
                .map(|(id, segment)| {
                    SegmentEntry::new(Line::new(point(segment.0), point(segment.1)), id)
                })
                .collect();

            layer.obstacles = RTree::bulk_load(entries);
            let polygon_entries = layer
                .polygons
                .iter()
                .enumerate()
                .filter_map(|(id, polygon)| {
                    polygon_envelope(polygon).map(|envelope| PolygonEntry::new(envelope, id))
                })
                .collect();

            layer.polygon_envelopes = RTree::bulk_load(polygon_entries);
        }

        let capacity = actors
            .iter()
            .map(|actor| actor.id.as_usize() + 1)
            .max()
            .unwrap_or(0);
        let mut query = Self {
            layers,
            indexed_transforms: vec![None; capacity],
        };
        let mut actor_entries: Vec<Vec<ActorEntry>> =
            query.layers.iter().map(|_| Vec::new()).collect();

        for actor in actors {
            query.validate_transform(actor.transform)?;
            let indexed_transform = query
                .indexed_transforms
                .get_mut(actor.id.as_usize())
                .ok_or_else(|| SpatialQueryError::missing_actor(actor.id))?;

            if indexed_transform.is_some() {
                return Err(SpatialQueryError::duplicate_actor(actor.id));
            }

            *indexed_transform = Some(actor.transform);
            actor_entries
                .get_mut(usize::from(actor.transform.layer.0))
                .ok_or_else(|| SpatialQueryError::missing_layer(actor.transform.layer))?
                .push(ActorEntry::new(point(actor.transform.position), actor.id));
        }

        for (layer, entries) in query.layers.iter_mut().zip(actor_entries) {
            layer.actors = RTree::bulk_load(entries);
        }

        Ok(query)
    }

    fn layer(&self, layer: SpatialLayerId) -> Option<&LayerIndex> {
        self.layers.get(usize::from(layer.0))
    }

    fn layer_mut(&mut self, layer: SpatialLayerId) -> Option<&mut LayerIndex> {
        self.layers.get_mut(usize::from(layer.0))
    }

    #[cfg(test)]
    fn diagnose_transform_validation(
        &self,
        transform: SpatialTransform,
    ) -> TransformValidationObservation {
        if !transform.position.x.is_finite()
            || !transform.position.y.is_finite()
            || !transform.heading.is_finite()
        {
            return TransformValidationObservation::error(SpatialQueryError::non_finite_transform());
        }

        let Some(index) = self.layer(transform.layer) else {
            return TransformValidationObservation::error(SpatialQueryError::missing_layer(
                transform.layer,
            ));
        };

        let segment_candidates: Vec<_> = transform_segment_candidates(index, transform.position)
            .map(|entry| entry.data)
            .collect();
        let polygon_candidates: Vec<_> = transform_polygon_candidates(index, transform.position)
            .map(|entry| entry.data)
            .collect();
        let mut segment_blocked = false;
        let mut segment_exact_checks = 0;

        for id in &segment_candidates {
            segment_exact_checks += 1;
            segment_blocked |= index.segments.get(*id).is_some_and(|segment| {
                point_segment_distance_sq(transform.position, *segment)
                    <= GEOMETRY_EPSILON * GEOMETRY_EPSILON
            });
        }

        let mut polygon_blocked = false;
        let mut polygon_exact_checks = 0;

        for id in &polygon_candidates {
            polygon_exact_checks += 1;
            polygon_blocked |= index
                .polygons
                .get(*id)
                .is_some_and(|polygon| point_in_blocker(transform.position, polygon));
        }

        TransformValidationObservation {
            result: if segment_blocked || polygon_blocked {
                Err(SpatialQueryError::blocked_transform())
            } else {
                Ok(())
            },
            segment_candidates: segment_candidates.len(),
            polygon_candidates: polygon_candidates.len(),
            segment_exact_checks,
            polygon_exact_checks,
        }
    }
}

#[derive(Debug, Eq, PartialEq)]
#[cfg(test)]
struct TransformValidationObservation {
    result: Result<(), SpatialQueryError>,
    segment_candidates: usize,
    polygon_candidates: usize,
    segment_exact_checks: usize,
    polygon_exact_checks: usize,
}

#[cfg(test)]
impl TransformValidationObservation {
    const fn error(error: SpatialQueryError) -> Self {
        Self {
            result: Err(error),
            segment_candidates: 0,
            polygon_candidates: 0,
            segment_exact_checks: 0,
            polygon_exact_checks: 0,
        }
    }
}

impl SpatialQueryDiagnostics for RstarSpatialQuery {
    fn diagnose_actors_in_radius(
        &self,
        query: RadiusQuery,
        allowed: Option<&[EnemyIdx]>,
    ) -> SpatialQueryObservation<Vec<EnemyIdx>> {
        let RadiusQuery {
            layer,
            center,
            radius,
        } = query;
        let envelope = SpatialEnvelope {
            min: Position2 {
                x: center.x - radius - GEOMETRY_EPSILON,
                y: center.y - radius - GEOMETRY_EPSILON,
            },
            max: Position2 {
                x: center.x + radius + GEOMETRY_EPSILON,
                y: center.y + radius + GEOMETRY_EPSILON,
            },
        };
        let candidates = self.candidates_in_envelope(layer, envelope);
        let broad_phase_candidates = candidates.len();
        let result = candidates
            .into_iter()
            .filter(|enemy| actor_is_allowed(*enemy, allowed))
            .filter(|enemy| {
                self.indexed_transforms
                    .get(enemy.as_usize())
                    .and_then(|transform| *transform)
                    .is_some_and(|transform| {
                        transform.layer == layer
                            && center.distance(transform.position) <= radius + GEOMETRY_EPSILON
                    })
            })
            .collect();

        SpatialQueryObservation {
            result,
            broad_phase_candidates,
        }
    }

    fn diagnose_actors_in_cone(
        &self,
        query: ConeQuery,
        allowed: Option<&[EnemyIdx]>,
    ) -> SpatialQueryObservation<Vec<EnemyIdx>> {
        let ConeQuery {
            layer,
            apex,
            heading,
            half_angle,
            range,
        } = query;
        let candidates =
            self.candidates_in_envelope(layer, cone_envelope(apex, heading, half_angle, range));
        let broad_phase_candidates = candidates.len();
        let result = candidates
            .into_iter()
            .filter(|enemy| actor_is_allowed(*enemy, allowed))
            .filter(|enemy| {
                self.indexed_transforms
                    .get(enemy.as_usize())
                    .and_then(|transform| *transform)
                    .is_some_and(|transform| {
                        transform.layer == layer
                            && in_cone(apex, heading, half_angle, range, transform.position)
                    })
            })
            .collect();

        SpatialQueryObservation {
            result,
            broad_phase_candidates,
        }
    }

    fn diagnose_segment_contacts(
        &self,
        layer: SpatialLayerId,
        start: Position2,
        end: Position2,
    ) -> SpatialQueryObservation<Vec<usize>> {
        let Some(index) = self.layer(layer) else {
            return SpatialQueryObservation {
                result: Vec::new(),
                broad_phase_candidates: 0,
            };
        };
        let envelope = AABB::from_corners(
            [
                start.x.min(end.x) - GEOMETRY_EPSILON,
                start.y.min(end.y) - GEOMETRY_EPSILON,
            ],
            [
                start.x.max(end.x) + GEOMETRY_EPSILON,
                start.y.max(end.y) + GEOMETRY_EPSILON,
            ],
        );
        let mut candidates: Vec<_> = index
            .obstacles
            .locate_in_envelope_intersecting(envelope)
            .map(|entry| entry.data)
            .collect();

        candidates.sort_unstable();
        candidates.dedup();
        let broad_phase_candidates = candidates.len();

        candidates.retain(|id| {
            index
                .segments
                .get(*id)
                .is_some_and(|obstacle| los_contact((start, end), *obstacle))
        });

        SpatialQueryObservation {
            result: candidates,
            broad_phase_candidates,
        }
    }
}

impl SpatialQuery for RstarSpatialQuery {
    fn candidates_on_layer(&self, layer: SpatialLayerId) -> Vec<EnemyIdx> {
        let Some(index) = self.layer(layer) else {
            return Vec::new();
        };
        let mut ids: Vec<_> = index.actors.iter().map(|entry| entry.data).collect();

        ids.sort_unstable();
        ids.dedup();

        ids
    }

    fn candidates_in_envelope(
        &self,
        layer: SpatialLayerId,
        envelope: SpatialEnvelope,
    ) -> Vec<EnemyIdx> {
        let Some(index) = self.layer(layer) else {
            return Vec::new();
        };
        let mut ids: Vec<_> = index
            .actors
            .locate_in_envelope_intersecting(AABB::from_corners(
                point(envelope.min),
                point(envelope.max),
            ))
            .map(|entry| entry.data)
            .collect();

        ids.sort_unstable();
        ids.dedup();

        ids
    }

    fn actors_in_radius(&self, query: RadiusQuery, allowed: Option<&[EnemyIdx]>) -> Vec<EnemyIdx> {
        self.diagnose_actors_in_radius(query, allowed).result
    }

    fn actors_in_cone(&self, query: ConeQuery, allowed: Option<&[EnemyIdx]>) -> Vec<EnemyIdx> {
        self.diagnose_actors_in_cone(query, allowed).result
    }

    fn actors_by_distance(
        &self,
        layer: SpatialLayerId,
        origin: Position2,
        allowed: Option<&[EnemyIdx]>,
    ) -> Vec<EnemyIdx> {
        let Some(index) = self.layer(layer) else {
            return Vec::new();
        };
        let mut actors: Vec<_> = if let Some(allowed) = allowed {
            allowed
                .iter()
                .filter_map(|actor| {
                    let transform = self
                        .indexed_transforms
                        .get(actor.as_usize())
                        .and_then(|transform| *transform)
                        .filter(|transform| transform.layer == layer)?;
                    let dx = transform.position.x - origin.x;
                    let dy = transform.position.y - origin.y;

                    Some((*actor, dx.mul_add(dx, dy * dy)))
                })
                .collect()
        } else {
            index
                .actors
                .nearest_neighbor_iter_with_distance_2(point(origin))
                .map(|(entry, distance)| (entry.data, distance))
                .collect()
        };

        actors.sort_unstable_by(|left, right| {
            left.1
                .total_cmp(&right.1)
                .then_with(|| left.0.cmp(&right.0))
        });

        actors.into_iter().map(|(actor, _)| actor).collect()
    }

    fn nearest_actor_matching(
        &self,
        layer: SpatialLayerId,
        origin: Position2,
        is_eligible: &mut dyn FnMut(EnemyIdx) -> bool,
    ) -> Option<EnemyIdx> {
        let index = self.layer(layer)?;
        let mut nearest = index
            .actors
            .nearest_neighbor_iter_with_distance_2(point(origin))
            .peekable();
        let mut distance_band = Vec::new();

        while let Some((entry, distance)) = nearest.next() {
            distance_band.clear();
            distance_band.push(entry.data);

            while nearest
                .peek()
                .is_some_and(|(_, next_distance)| distance.total_cmp(next_distance).is_eq())
            {
                if let Some((entry, _)) = nearest.next() {
                    distance_band.push(entry.data);
                }
            }

            distance_band.sort_unstable();
            distance_band.dedup();

            if let Some(actor) = distance_band
                .iter()
                .copied()
                .find(|actor| is_eligible(*actor))
            {
                return Some(actor);
            }
        }

        None
    }

    fn segment_visible(&self, layer: SpatialLayerId, start: Position2, end: Position2) -> bool {
        let Some(index) = self.layer(layer) else {
            return false;
        };
        let envelope = AABB::from_corners(
            [
                start.x.min(end.x) - GEOMETRY_EPSILON,
                start.y.min(end.y) - GEOMETRY_EPSILON,
            ],
            [
                start.x.max(end.x) + GEOMETRY_EPSILON,
                start.y.max(end.y) + GEOMETRY_EPSILON,
            ],
        );

        !index
            .obstacles
            .locate_in_envelope_intersecting(envelope)
            .any(|entry| {
                index
                    .segments
                    .get(entry.data)
                    .is_some_and(|obstacle| los_contact((start, end), *obstacle))
            })
    }

    fn validate_transform(&self, transform: SpatialTransform) -> Result<(), SpatialQueryError> {
        if !transform.position.x.is_finite()
            || !transform.position.y.is_finite()
            || !transform.heading.is_finite()
        {
            return Err(SpatialQueryError::non_finite_transform());
        }

        let index = self
            .layer(transform.layer)
            .ok_or_else(|| SpatialQueryError::missing_layer(transform.layer))?;

        if transform_segment_candidates(index, transform.position).any(|entry| {
            index.segments.get(entry.data).is_some_and(|segment| {
                point_segment_distance_sq(transform.position, *segment)
                    <= GEOMETRY_EPSILON * GEOMETRY_EPSILON
            })
        }) || transform_polygon_candidates(index, transform.position).any(|entry| {
            index
                .polygons
                .get(entry.data)
                .is_some_and(|polygon| point_in_blocker(transform.position, polygon))
        }) {
            return Err(SpatialQueryError::blocked_transform());
        }

        Ok(())
    }

    fn insert_actor(&mut self, actor: SpatialActor) -> Result<(), SpatialQueryError> {
        self.validate_transform(actor.transform)?;

        if self
            .indexed_transforms
            .get(actor.id.as_usize())
            .is_some_and(Option::is_some)
        {
            return Err(SpatialQueryError::duplicate_actor(actor.id));
        }

        if actor.id.as_usize() >= self.indexed_transforms.len() {
            self.indexed_transforms
                .resize(actor.id.as_usize() + 1, None);
        }

        self.layer_mut(actor.transform.layer)
            .ok_or_else(|| SpatialQueryError::missing_layer(actor.transform.layer))?
            .actors
            .insert(ActorEntry::new(point(actor.transform.position), actor.id));
        let Some(indexed_transform) = self.indexed_transforms.get_mut(actor.id.as_usize()) else {
            return Err(SpatialQueryError::missing_actor(actor.id));
        };

        *indexed_transform = Some(actor.transform);

        Ok(())
    }

    fn remove_actor(&mut self, actor: SpatialActor) -> Result<(), SpatialQueryError> {
        let indexed = self
            .indexed_transforms
            .get(actor.id.as_usize())
            .and_then(|transform| *transform)
            .ok_or_else(|| SpatialQueryError::missing_actor(actor.id))?;

        if indexed != actor.transform {
            return Err(SpatialQueryError::transform_mismatch(actor.id));
        }

        let removed = self
            .layer_mut(indexed.layer)
            .ok_or_else(|| SpatialQueryError::missing_layer(indexed.layer))?
            .actors
            .remove(&ActorEntry::new(point(indexed.position), actor.id));

        if removed.is_none() {
            return Err(SpatialQueryError::missing_actor(actor.id));
        }

        let Some(indexed_transform) = self.indexed_transforms.get_mut(actor.id.as_usize()) else {
            return Err(SpatialQueryError::missing_actor(actor.id));
        };

        *indexed_transform = None;

        Ok(())
    }

    fn relocate_actor(
        &mut self,
        actor: EnemyIdx,
        from: SpatialTransform,
        to: SpatialTransform,
    ) -> Result<(), SpatialQueryError> {
        self.validate_transform(to)?;
        let indexed = self
            .indexed_transforms
            .get(actor.as_usize())
            .and_then(|transform| *transform)
            .ok_or_else(|| SpatialQueryError::missing_actor(actor))?;

        if indexed != from {
            return Err(SpatialQueryError::transform_mismatch(actor));
        }

        let from_entry = ActorEntry::new(point(from.position), actor);

        if self
            .layer_mut(from.layer)
            .ok_or_else(|| SpatialQueryError::missing_layer(from.layer))?
            .actors
            .remove(&from_entry)
            .is_none()
        {
            return Err(SpatialQueryError::missing_actor(actor));
        }

        self.layer_mut(to.layer)
            .ok_or_else(|| SpatialQueryError::missing_layer(to.layer))?
            .actors
            .insert(ActorEntry::new(point(to.position), actor));
        let Some(indexed_transform) = self.indexed_transforms.get_mut(actor.as_usize()) else {
            return Err(SpatialQueryError::missing_actor(actor));
        };

        *indexed_transform = Some(to);

        Ok(())
    }
}

fn actor_is_allowed(actor: EnemyIdx, allowed: Option<&[EnemyIdx]>) -> bool {
    allowed.is_none_or(|ids| ids.binary_search(&actor).is_ok())
}

fn layer_index_mut(
    layers: &mut [LayerIndex],
    layer: SpatialLayerId,
) -> Result<&mut LayerIndex, SpatialQueryError> {
    layers
        .get_mut(usize::from(layer.0))
        .ok_or_else(|| SpatialQueryError::missing_layer(layer))
}

fn push_segment(index: &mut LayerIndex, start: Position2, end: Position2) {
    index.segments.push((start, end));
}

fn push_ring_segments(index: &mut LayerIndex, ring: &[Position2]) {
    for edge in ring.windows(EDGE_POINT_COUNT) {
        if let [start, end] = edge {
            push_segment(index, *start, *end);
        }
    }
}

fn point_epsilon_envelope(position: Position2) -> AABB<[f64; POINT_DIMENSIONS]> {
    AABB::from_corners(
        [position.x - GEOMETRY_EPSILON, position.y - GEOMETRY_EPSILON],
        [position.x + GEOMETRY_EPSILON, position.y + GEOMETRY_EPSILON],
    )
}

fn transform_segment_candidates(
    index: &LayerIndex,
    position: Position2,
) -> impl Iterator<Item = &SegmentEntry> {
    index
        .obstacles
        .locate_in_envelope_intersecting(point_epsilon_envelope(position))
}

fn transform_polygon_candidates(
    index: &LayerIndex,
    position: Position2,
) -> impl Iterator<Item = &PolygonEntry> {
    index
        .polygon_envelopes
        .locate_in_envelope_intersecting(point_epsilon_envelope(position))
}

fn polygon_envelope(polygon: &PolygonBlocker) -> Option<Rectangle<[f64; POINT_DIMENSIONS]>> {
    let first = polygon.exterior.first()?;
    let mut min = *first;
    let mut max = *first;

    for position in polygon.exterior.iter().skip(1) {
        min.x = min.x.min(position.x);
        min.y = min.y.min(position.y);
        max.x = max.x.max(position.x);
        max.y = max.y.max(position.y);
    }

    Some(Rectangle::from_corners(point(min), point(max)))
}

const fn point(position: Position2) -> [f64; POINT_DIMENSIONS] {
    [position.x, position.y]
}

const fn coord(position: Position2) -> Coord<f64> {
    Coord {
        x: position.x,
        y: position.y,
    }
}

fn exact_intersection(a: (Position2, Position2), b: (Position2, Position2)) -> bool {
    let o1 = orient2d(coord(a.0), coord(a.1), coord(b.0));
    let o2 = orient2d(coord(a.0), coord(a.1), coord(b.1));
    let o3 = orient2d(coord(b.0), coord(b.1), coord(a.0));
    let o4 = orient2d(coord(b.0), coord(b.1), coord(a.1));

    if opposite_signs(o1, o2) && opposite_signs(o3, o4) {
        return true;
    }

    exact_zero(o1) && on_closed_segment(b.0, a.0, a.1)
        || exact_zero(o2) && on_closed_segment(b.1, a.0, a.1)
        || exact_zero(o3) && on_closed_segment(a.0, b.0, b.1)
        || exact_zero(o4) && on_closed_segment(a.1, b.0, b.1)
}

const fn opposite_signs(left: f64, right: f64) -> bool {
    (left > 0.0 && right < 0.0) || (left < 0.0 && right > 0.0)
}

fn exact_zero(value: f64) -> bool {
    value.total_cmp(&0.0).is_eq() || value.total_cmp(&-0.0).is_eq()
}

fn on_closed_segment(point: Position2, start: Position2, end: Position2) -> bool {
    point.x >= start.x.min(end.x)
        && point.x <= start.x.max(end.x)
        && point.y >= start.y.min(end.y)
        && point.y <= start.y.max(end.y)
}

fn point_segment_distance_sq(point: Position2, segment: (Position2, Position2)) -> f64 {
    let dx = segment.1.x - segment.0.x;
    let dy = segment.1.y - segment.0.y;
    let length_sq = dx.mul_add(dx, dy * dy);

    if exact_zero(length_sq) {
        let ex = point.x - segment.0.x;
        let ey = point.y - segment.0.y;

        return ex.mul_add(ex, ey * ey);
    }

    let t =
        (((point.x - segment.0.x) * dx + (point.y - segment.0.y) * dy) / length_sq).clamp(0.0, 1.0);
    let qx = segment.0.x + t * dx;
    let qy = segment.0.y + t * dy;
    let ex = point.x - qx;
    let ey = point.y - qy;

    ex.mul_add(ex, ey * ey)
}

fn los_contact(a: (Position2, Position2), b: (Position2, Position2)) -> bool {
    exact_intersection(a, b)
        || point_segment_distance_sq(a.0, b) <= GEOMETRY_EPSILON * GEOMETRY_EPSILON
        || point_segment_distance_sq(a.1, b) <= GEOMETRY_EPSILON * GEOMETRY_EPSILON
        || point_segment_distance_sq(b.0, a) <= GEOMETRY_EPSILON * GEOMETRY_EPSILON
        || point_segment_distance_sq(b.1, a) <= GEOMETRY_EPSILON * GEOMETRY_EPSILON
}

fn angle_diff(left: f64, right: f64) -> f64 {
    let mut delta = (left - right) % std::f64::consts::TAU;

    if delta > std::f64::consts::PI {
        delta -= std::f64::consts::TAU;
    }

    if delta < -std::f64::consts::PI {
        delta += std::f64::consts::TAU;
    }

    delta.abs()
}

fn in_cone(apex: Position2, heading: f64, half_angle: f64, range: f64, point: Position2) -> bool {
    let distance = apex.distance(point);

    if distance > range + GEOMETRY_EPSILON {
        return false;
    }

    if distance <= GEOMETRY_EPSILON {
        return true;
    }

    let angle = (point.y - apex.y).atan2(point.x - apex.x);

    angle_diff(angle, heading) <= half_angle + GEOMETRY_EPSILON
}

fn cone_envelope(apex: Position2, heading: f64, half_angle: f64, range: f64) -> SpatialEnvelope {
    if half_angle >= std::f64::consts::PI - GEOMETRY_EPSILON {
        return SpatialEnvelope {
            min: Position2 {
                x: apex.x - range - GEOMETRY_EPSILON,
                y: apex.y - range - GEOMETRY_EPSILON,
            },
            max: Position2 {
                x: apex.x + range + GEOMETRY_EPSILON,
                y: apex.y + range + GEOMETRY_EPSILON,
            },
        };
    }

    let mut min = apex;
    let mut max = apex;
    let mut include = |angle: f64| {
        let position = Position2 {
            x: apex.x + range * angle.cos(),
            y: apex.y + range * angle.sin(),
        };

        min.x = min.x.min(position.x);
        min.y = min.y.min(position.y);
        max.x = max.x.max(position.x);
        max.y = max.y.max(position.y);
    };

    include(heading - half_angle);
    include(heading + half_angle);

    for quadrant in 0..QUADRANT_COUNT {
        let angle = f64::from(quadrant) * std::f64::consts::FRAC_PI_2;

        if angle_diff(angle, heading) <= half_angle + GEOMETRY_EPSILON {
            include(angle);
        }
    }

    SpatialEnvelope {
        min: Position2 {
            x: min.x - GEOMETRY_EPSILON,
            y: min.y - GEOMETRY_EPSILON,
        },
        max: Position2 {
            x: max.x + GEOMETRY_EPSILON,
            y: max.y + GEOMETRY_EPSILON,
        },
    }
}

fn point_in_blocker(point: Position2, polygon: &PolygonBlocker) -> bool {
    ring_contains(point, &polygon.exterior)
        && !polygon
            .holes
            .iter()
            .any(|hole| ring_contains_strict(point, hole))
}

fn ring_contains(point: Position2, ring: &[Position2]) -> bool {
    ring.windows(EDGE_POINT_COUNT).any(|edge| {
        let [start, end] = edge else {
            return false;
        };

        point_segment_distance_sq(point, (*start, *end)) <= GEOMETRY_EPSILON * GEOMETRY_EPSILON
    }) || ring_contains_strict(point, ring)
}

fn ring_contains_strict(point: Position2, ring: &[Position2]) -> bool {
    let mut inside = false;

    for edge in ring.windows(EDGE_POINT_COUNT) {
        let [a, b] = edge else {
            continue;
        };

        if (a.y > point.y) != (b.y > point.y) {
            let crossing_x = (b.x - a.x) * (point.y - a.y) / (b.y - a.y) + a.x;

            if point.x < crossing_x {
                inside = !inside;
            }
        }
    }

    inside
}

#[cfg(test)]
#[path = "../benches/support/mod.rs"]
mod frozen_oracle;

#[cfg(test)]
#[path = "rstar/tests.rs"]
mod tests;