condor-pathfinding-navmesh 0.4.0

//! Converts a Condor [`Navmesh`] into a Polyanya triangle mesh.
//!
//! # Role
//!
//! Feature-gated boundary (`polyanya`) between Condor's convex-cell substrate
//! and the external Polyanya solver. Each cell is fan-triangulated from its
//! centroid; portal endpoints are welded so adjacent cells share vertices;
//! [`PolyanyaMeshAdapter::triangle_to_cell`] maps each output triangle back to
//! its source cell for corridor recovery after the external path returns.
//!
//! This module does **not** pathfind. The [`crate::Polyanya`] pathfinder builds
//! the adapter, converts to an external mesh, runs Polyanya, then maps the
//! triangle corridor back onto navmesh cells and funnels.

use glam::Vec2;
use polyanya::{Mesh as ExternalMesh, Trimesh};
use std::collections::{BTreeMap, BTreeSet};

use crate::{Navmesh, Point2};

const EPSILON: f64 = 1e-9;
const KEY_SCALE: f64 = 1_000_000_000.0;

/// Failure while adapting or validating a navmesh for Polyanya.
///
/// Distinct from [`super::NavmeshValidationError`]: the source mesh may already
/// validate under Condor rules yet still collapse under `f32` welding or Polyanya's
/// mesh construction constraints.
#[derive(Debug, Clone, Copy, PartialEq, thiserror::Error)]
#[non_exhaustive]
pub enum PolyanyaMeshAdapterError {
    /// A Condor cell collapsed under `f32` welding / fan triangulation.
    #[error("navmesh cell {cell_index} degenerates during mesh adaptation")]
    DegenerateCell {
        /// Index of the cell that degenerated during adaptation.
        cell_index: usize,
    },
    /// External Polyanya mesh construction rejected the adapted geometry.
    #[error("{0}")]
    ExternalMesh(
        #[from]
        #[source]
        polyanya::MeshError,
    ),
}

/// Intermediate triangle mesh plus per-triangle source-cell mapping.
///
/// Hold this before calling [`Self::into_external_mesh`] if the caller needs
/// [`Self::triangle_to_cell`] for post-search cell corridor reconstruction.
#[derive(Debug)]
pub struct PolyanyaMeshAdapter {
    vertices: Vec<Vec2>,
    triangles: Vec<[usize; 3]>,
    triangle_to_cell: Vec<usize>,
}

impl PolyanyaMeshAdapter {
    /// Fan-triangulates every navmesh cell while retaining its source-cell mapping.
    ///
    /// Portal endpoints shared across cells are welded to a single vertex index so
    /// Polyanya sees a connected trimesh. Non-portal boundary vertices stay local
    /// to their cell.
    ///
    /// # Errors
    ///
    /// Returns [`PolyanyaMeshAdapterError::DegenerateCell`] when a cell
    /// collapses during conversion to Polyanya's `f32` geometry.
    pub fn from_navmesh(navmesh: &Navmesh) -> Result<Self, PolyanyaMeshAdapterError> {
        let mut vertices = Vec::new();
        let mut triangles = Vec::new();
        let mut triangle_to_cell = Vec::new();
        let mut shared_vertex_indices = BTreeMap::new();
        let shared_portal_endpoints = navmesh
            .portals()
            .iter()
            .flat_map(|portal| [point_key(portal.start), point_key(portal.end)])
            .collect::<BTreeSet<_>>();

        for (cell_index, cell) in navmesh.cells().iter().enumerate() {
            let ring = build_cell_boundary_ring(
                navmesh,
                cell_index,
                &normalized_ccw(cell.vertices()),
                &shared_portal_endpoints,
                &mut vertices,
                &mut shared_vertex_indices,
            );
            if ring.len() < 3 {
                return Err(PolyanyaMeshAdapterError::DegenerateCell { cell_index });
            }

            let centroid_index = push_vertex(&mut vertices, polygon_centroid(cell.vertices()));
            let new_triangles = triangulate_convex_ring(&vertices, centroid_index, &ring);
            if new_triangles.is_empty() {
                return Err(PolyanyaMeshAdapterError::DegenerateCell { cell_index });
            }
            for _ in 0..new_triangles.len() {
                triangle_to_cell.push(cell_index);
            }
            triangles.extend(new_triangles);
        }

        Ok(Self {
            vertices,
            triangles,
            triangle_to_cell,
        })
    }

    /// Consumes the adapter and builds a Polyanya [`ExternalMesh`] from the triangles.
    ///
    /// Drops access to [`Self::triangle_to_cell`]; copy that mapping first if needed.
    ///
    /// # Errors
    ///
    /// Returns [`PolyanyaMeshAdapterError::ExternalMesh`] with the original
    /// [`polyanya::MeshError`] as its source when Polyanya rejects the mesh.
    pub fn into_external_mesh(self) -> Result<ExternalMesh, PolyanyaMeshAdapterError> {
        Trimesh {
            vertices: self.vertices,
            triangles: self.triangles,
        }
        .try_into()
        .map_err(Into::into)
    }

    /// Per-triangle source cell index: `triangle_to_cell[t]` is the navmesh cell that produced triangle `t`.
    #[must_use]
    pub fn triangle_to_cell(&self) -> &[usize] {
        &self.triangle_to_cell
    }
}

fn build_cell_boundary_ring(
    navmesh: &Navmesh,
    cell_index: usize,
    vertices: &[Point2],
    shared_portal_endpoints: &BTreeSet<PointKey>,
    global_vertices: &mut Vec<Vec2>,
    shared_vertex_indices: &mut BTreeMap<PointKey, usize>,
) -> Vec<usize> {
    let mut ring = Vec::new();
    let mut local_vertex_indices = BTreeMap::new();

    for (start, end) in polygon_edges(vertices) {
        let mut edge_points = vec![(0.0, start), (1.0, end)];
        for portal in navmesh.portals_from(cell_index) {
            for endpoint in [portal.start, portal.end] {
                if point_on_segment(endpoint, start, end) {
                    edge_points.push((segment_parameter(endpoint, start, end), endpoint));
                }
            }
        }

        edge_points.sort_by(|left, right| left.0.total_cmp(&right.0));
        edge_points.dedup_by(|left, right| points_equal(left.1, right.1));

        for (position, (_, point)) in edge_points.iter().enumerate() {
            if position + 1 == edge_points.len() {
                continue;
            }

            let key = point_key(*point);
            let index = if shared_portal_endpoints.contains(&key) {
                *shared_vertex_indices
                    .entry(key)
                    .or_insert_with(|| push_vertex(global_vertices, *point))
            } else {
                *local_vertex_indices
                    .entry(key)
                    .or_insert_with(|| push_vertex(global_vertices, *point))
            };
            if ring.last().copied() != Some(index) {
                ring.push(index);
            }
        }
    }

    if ring.first() == ring.last() {
        ring.pop();
    }

    ring
}

fn triangulate_convex_ring(
    vertices: &[Vec2],
    centroid_index: usize,
    ring: &[usize],
) -> Vec<[usize; 3]> {
    let mut triangles = Vec::with_capacity(ring.len());
    for (left, right) in ring
        .iter()
        .copied()
        .zip(ring.iter().copied().cycle().skip(1))
        .take(ring.len())
    {
        let area = triangle_area(vertices[centroid_index], vertices[left], vertices[right]);
        if area.abs() <= EPSILON {
            continue;
        }

        if area > 0.0 {
            triangles.push([centroid_index, left, right]);
        } else {
            triangles.push([centroid_index, right, left]);
        }
    }
    triangles
}

fn normalized_ccw(vertices: &[Point2]) -> Vec<Point2> {
    if signed_area(vertices) >= 0.0 {
        vertices.to_vec()
    } else {
        let mut reversed = vertices.to_vec();
        reversed.reverse();
        reversed
    }
}

fn polygon_centroid(vertices: &[Point2]) -> Point2 {
    let (x_sum, y_sum) = vertices.iter().fold((0.0, 0.0), |(x_sum, y_sum), vertex| {
        (x_sum + vertex.x, y_sum + vertex.y)
    });
    let count = vertices.len() as f64;
    Point2::new(x_sum / count, y_sum / count)
}

fn polygon_edges(vertices: &[Point2]) -> impl Iterator<Item = (Point2, Point2)> + '_ {
    vertices
        .iter()
        .copied()
        .zip(vertices.iter().copied().cycle().skip(1))
        .take(vertices.len())
}

fn push_vertex(vertices: &mut Vec<Vec2>, point: Point2) -> usize {
    let index = vertices.len();
    vertices.push(to_external_vec2(point));
    index
}

fn segment_parameter(point: Point2, start: Point2, end: Point2) -> f64 {
    let dx = end.x - start.x;
    let dy = end.y - start.y;
    if dx.abs() >= dy.abs() && dx.abs() > EPSILON {
        ((point.x - start.x) / dx).clamp(0.0, 1.0)
    } else if dy.abs() > EPSILON {
        ((point.y - start.y) / dy).clamp(0.0, 1.0)
    } else {
        0.0
    }
}

fn signed_area(vertices: &[Point2]) -> f64 {
    polygon_edges(vertices)
        .map(|(left, right)| (left.x * right.y) - (right.x * left.y))
        .sum::<f64>()
        / 2.0
}

fn triangle_area(a: Vec2, b: Vec2, c: Vec2) -> f64 {
    f64::from(((b.x - a.x) * (c.y - a.y)) - ((b.y - a.y) * (c.x - a.x)))
}

fn point_on_segment(point: Point2, start: Point2, end: Point2) -> bool {
    let cross =
        ((point.y - start.y) * (end.x - start.x)) - ((point.x - start.x) * (end.y - start.y));
    if cross.abs() > EPSILON {
        return false;
    }

    let dot = ((point.x - start.x) * (end.x - start.x)) + ((point.y - start.y) * (end.y - start.y));
    if dot < -EPSILON {
        return false;
    }

    let length_sq =
        ((end.x - start.x) * (end.x - start.x)) + ((end.y - start.y) * (end.y - start.y));
    dot <= length_sq + EPSILON
}

/// Equality within the adapter float epsilon used for portal welding.
pub fn points_equal(left: Point2, right: Point2) -> bool {
    (left.x - right.x).abs() <= EPSILON && (left.y - right.y).abs() <= EPSILON
}

fn point_key(point: Point2) -> PointKey {
    PointKey {
        x: (point.x * KEY_SCALE).round() as i64,
        y: (point.y * KEY_SCALE).round() as i64,
    }
}

/// Converts a Condor [`Point2`] into glam's `Vec2` for the Polyanya mesh API.
pub fn to_external_vec2(point: Point2) -> Vec2 {
    Vec2::new(point.x as f32, point.y as f32)
}

#[derive(Debug, Clone, Copy, PartialEq, Eq, PartialOrd, Ord)]
struct PointKey {
    x: i64,
    y: i64,
}