dijkstra3d-core 0.1.0

Voxel-grid Dijkstra and rolling-ball invalidation, ported from seung-lab/dijkstra3d and seung-lab/kimimaro
Documentation
//! Shortest last-mile path to a zero-cost rail network.
//!
//! Upstream reference: `dijkstra3d.hpp::value_target_dijkstra3d`.

use crate::parental::{
    is_neighbor_in_bounds, path_from_parents, LibstdcxxCompatHeap, UpstreamCompatEntry,
    UPSTREAM_DIRECTIONS_26,
};
use crate::voxel::VolumeShape;
use crate::{Connectivity, Dijkstra3dError, Result};
use num_traits::ToPrimitive;

/// Find the upstream-compatible shortest path from `source` to a zero-cost voxel.
///
/// `field` must use x-fastest linear order. The search deliberately mirrors
/// upstream's direction order, `f32` accumulation, strict relaxation, early
/// stop on the first successful zero-cost relaxation, and pinned libstdc++ heap
/// tie behavior. The returned path includes both endpoints in source-to-rail
/// order.
pub fn railroad_upstream_compat_slice<T>(
    field: &[T],
    shape: VolumeShape,
    source: u64,
    connectivity: Connectivity,
) -> Result<Vec<u64>>
where
    T: ToPrimitive,
{
    let total = shape.total();
    if field.len() as u64 != total {
        return Err(Dijkstra3dError::InvalidInput(format!(
            "railroad_upstream_compat_slice: slice length {} != shape.total() {total}",
            field.len()
        )));
    }
    if total > u32::MAX as u64 {
        return Err(Dijkstra3dError::InvalidInput(format!(
            "railroad_upstream_compat_slice: volume {total} exceeds u32::MAX voxels"
        )));
    }
    if source >= total {
        return Err(Dijkstra3dError::OutOfBounds {
            idx: source,
            volume_size: total,
        });
    }

    let source_cost = field[source as usize].to_f32().ok_or_else(|| {
        Dijkstra3dError::InvalidInput("source cost is not representable as f32".into())
    })?;
    if source_cost == 0.0 {
        return Ok(vec![source]);
    }

    let mut distances = vec![f32::INFINITY; total as usize];
    let mut parents = vec![0u32; total as usize];
    let mut queue = LibstdcxxCompatHeap::<u32>::default();
    let stride_y = shape.sx as i64;
    let stride_z = (shape.sx * shape.sy) as i64;
    let offsets = UPSTREAM_DIRECTIONS_26
        .map(|(dx, dy, dz)| dx as i64 + dy as i64 * stride_y + dz as i64 * stride_z);
    let neighbor_count = connectivity.count();

    distances[source as usize] = 0.0;
    queue.push(UpstreamCompatEntry {
        priority: 0.0,
        payload: source as u32,
    });

    while let Some(entry) = queue.pop() {
        let loc = entry.payload as usize;
        if distances[loc].is_sign_negative() {
            continue;
        }

        let (x, y, z) = shape.coords(loc as u64);
        let interior =
            x > 0 && x + 1 < shape.sx && y > 0 && y + 1 < shape.sy && z > 0 && z + 1 < shape.sz;
        let current_dist = distances[loc];

        for (direction, &offset) in UPSTREAM_DIRECTIONS_26[..neighbor_count]
            .iter()
            .zip(&offsets[..neighbor_count])
        {
            let (dx, dy, dz) = *direction;
            if !interior
                && !is_neighbor_in_bounds(
                    x,
                    y,
                    z,
                    dx,
                    dy,
                    dz,
                    shape.sx as i64,
                    shape.sy as i64,
                    shape.sz as i64,
                )
            {
                continue;
            }

            let neighbor = (loc as i64 + offset) as usize;
            let delta = field[neighbor].to_f32().ok_or_else(|| {
                Dijkstra3dError::InvalidInput(format!(
                    "cost at voxel {neighbor} is not representable as f32"
                ))
            })?;
            let new_dist = current_dist + delta;
            if new_dist < distances[neighbor] {
                distances[neighbor] = new_dist;
                parents[neighbor] = (loc + 1) as u32;
                if delta == 0.0 {
                    return path_from_parents(&parents, neighbor as u64);
                }
                queue.push(UpstreamCompatEntry {
                    priority: new_dist,
                    payload: neighbor as u32,
                });
            }
        }

        distances[loc] *= -1.0;
    }

    Ok(Vec::new())
}

#[cfg(test)]
mod tests {
    use super::*;

    #[test]
    fn source_on_rail_is_singleton() {
        let shape = VolumeShape::new(2, 2, 1);
        let path =
            railroad_upstream_compat_slice(&[0u8, 1, 1, 1], shape, 0, Connectivity::Six).unwrap();
        assert_eq!(path, [0]);
    }

    #[test]
    fn includes_source_and_first_relaxed_rail_endpoint() {
        let shape = VolumeShape::new(4, 1, 1);
        let path =
            railroad_upstream_compat_slice(&[0u8, 3, 2, 1], shape, 3, Connectivity::Six).unwrap();
        assert_eq!(path, [3, 2, 1, 0]);
    }

    #[test]
    fn no_rail_returns_empty_path() {
        let shape = VolumeShape::new(2, 2, 1);
        let path =
            railroad_upstream_compat_slice(&[1u8, 1, 1, 1], shape, 3, Connectivity::Eighteen)
                .unwrap();
        assert!(path.is_empty());
    }
}