use crate::connectivity_graph::ConnectivityGraph;
use crate::dijkstra::{checked_int_distance_bound, Dist, IntQueue};
use crate::neighborhood::{neighborhood_masks, neighborhood_offsets, DIRECTIONS_26};
use crate::queue::binary_heap::BinaryHeapQueue;
use crate::queue::bucket::BucketQueue;
use crate::voxel::{ToU64Fast, VolumeShape};
use crate::{Connectivity, Dijkstra3dError, Result};
const MAX_BUCKET_RANGE: u64 = 65_536;
enum WsQueue {
Bucket(BucketQueue),
Heap(BinaryHeapQueue<u64, u32>),
}
enum WsDist {
U32(Vec<u32>),
U64(Vec<u64>),
}
pub struct DijkstraWorkspace {
shape: VolumeShape,
distances: WsDist,
parents: Vec<u32>,
gen: Vec<u32>,
generation: u32,
max_weight: u64,
queue: WsQueue,
}
impl DijkstraWorkspace {
pub fn with_capacity(shape: VolumeShape, max_weight: u64) -> Result<Self> {
let total = shape.total();
if total > u32::MAX as u64 {
return Err(Dijkstra3dError::InvalidInput(format!(
"DijkstraWorkspace: volume {total} exceeds u32::MAX voxels; \
use the u64/f32 path"
)));
}
let dist_bound = checked_int_distance_bound(shape, max_weight)?;
let total = total as usize;
let queue = if max_weight < MAX_BUCKET_RANGE {
WsQueue::Bucket(BucketQueue::with_capacity(max_weight as u32))
} else {
WsQueue::Heap(BinaryHeapQueue::<u64, u32>::new())
};
let distances = if dist_bound <= u32::MAX as u64 {
WsDist::U32(vec![u32::MAX; total])
} else {
WsDist::U64(vec![u64::MAX; total])
};
Ok(Self {
shape,
distances,
parents: vec![0u32; total],
gen: vec![0u32; total],
generation: 0,
max_weight,
queue,
})
}
pub fn for_field<T: ToU64Fast>(field: &[T], shape: VolumeShape) -> Result<Self> {
let total = shape.total();
if field.len() as u64 != total {
return Err(Dijkstra3dError::InvalidInput(format!(
"DijkstraWorkspace::for_field: slice length {} != shape.total() {}",
field.len(),
total
)));
}
let max_weight = field.iter().map(|&v| v.to_u64_fast()).max().unwrap_or(0);
Self::with_capacity(shape, max_weight)
}
#[inline]
pub fn shape(&self) -> VolumeShape {
self.shape
}
#[inline]
pub fn max_weight(&self) -> u64 {
self.max_weight
}
#[inline]
fn begin_query(&mut self) {
self.generation = self.generation.wrapping_add(1);
if self.generation == 0 {
for g in self.gen.iter_mut() {
*g = 0;
}
self.generation = 1;
}
match &mut self.queue {
WsQueue::Bucket(q) => q.reset(),
WsQueue::Heap(q) => q.clear(),
}
}
pub fn shortest_path<T: ToU64Fast>(
&mut self,
field: &[T],
source: u64,
target: u64,
connectivity: Connectivity,
graph: Option<ConnectivityGraph<'_>>,
) -> Result<Vec<u64>> {
let total = self.shape.total();
if field.len() as u64 != total {
return Err(Dijkstra3dError::InvalidInput(format!(
"DijkstraWorkspace::shortest_path: slice length {} != shape.total() {}",
field.len(),
total
)));
}
if source >= total {
return Err(Dijkstra3dError::OutOfBounds {
idx: source,
volume_size: total,
});
}
if target >= total {
return Err(Dijkstra3dError::OutOfBounds {
idx: target,
volume_size: total,
});
}
if source == target {
return Ok(vec![source]);
}
self.begin_query();
let generation = self.generation;
let shape = self.shape;
let Self {
distances,
parents,
gen,
queue,
..
} = self;
macro_rules! dispatch {
($dist:expr) => {
match queue {
WsQueue::Bucket(q) => generation_search(
field,
shape,
source,
target,
connectivity,
graph,
$dist,
parents,
gen,
generation,
q,
),
WsQueue::Heap(q) => generation_search(
field,
shape,
source,
target,
connectivity,
graph,
$dist,
parents,
gen,
generation,
q,
),
}
};
}
match distances {
WsDist::U32(d) => dispatch!(d.as_mut_slice()),
WsDist::U64(d) => dispatch!(d.as_mut_slice()),
}
}
}
fn reconstruct(parents: &[u32], source: u64, target: u64) -> Vec<u64> {
let mut path = vec![target];
let mut current = target;
while current != source {
let parent = parents[current as usize];
if parent == 0 {
break;
}
current = (parent - 1) as u64;
path.push(current);
}
path.reverse();
path
}
#[allow(clippy::too_many_arguments)]
fn generation_search<T: ToU64Fast, Q: IntQueue, D: Dist>(
field: &[T],
shape: VolumeShape,
source: u64,
target: u64,
connectivity: Connectivity,
graph: Option<ConnectivityGraph<'_>>,
distances: &mut [D],
parents: &mut [u32],
gen: &mut [u32],
generation: u32,
queue: &mut Q,
) -> Result<Vec<u64>> {
let source_us = source as usize;
distances[source_us] = D::ZERO;
parents[source_us] = 0;
gen[source_us] = generation;
queue.push_i(0, source as u32);
let sx_i = shape.sx as i64;
let sxy_i = (shape.sx * shape.sy) as i64;
let offsets = neighborhood_offsets(sx_i, sxy_i);
let masks = neighborhood_masks();
let neighbor_count = connectivity.count();
let sx_u = shape.sx as i64;
let sy_u = shape.sy as i64;
let sz_u = shape.sz as i64;
while let Some((dist, loc)) = queue.pop_i() {
let loc_us = loc as usize;
if dist > distances[loc_us].to_priority() {
continue;
}
if loc as u64 == target {
return Ok(reconstruct(parents, source, target));
}
let current_dist = distances[loc_us];
let (lx, ly, lz) = shape.coords(loc as u64);
for i in 0..neighbor_count {
let (dx, dy, dz) = DIRECTIONS_26[i];
let nx = lx as i64 + dx as i64;
let ny = ly as i64 + dy as i64;
let nz = lz as i64 + dz as i64;
if nx < 0 || nx >= sx_u || ny < 0 || ny >= sy_u || nz < 0 || nz >= sz_u {
continue;
}
let neighbor = (loc as i64 + offsets[i]) as u64;
let neighbor_us = neighbor as usize;
if let Some(ref g) = graph {
if !g.allows(loc as u64, masks[i]) {
continue;
}
}
let neighbor_cost = field[neighbor_us].to_u64_fast();
let new_dist = current_dist.add_cost(neighbor_cost);
let nb_best = if gen[neighbor_us] == generation {
distances[neighbor_us]
} else {
D::MAX
};
if new_dist < nb_best {
distances[neighbor_us] = new_dist;
parents[neighbor_us] = loc + 1; gen[neighbor_us] = generation;
queue.push_i(new_dist.to_priority(), neighbor as u32);
}
}
}
Err(Dijkstra3dError::Unreachable)
}
#[cfg(test)]
mod tests {
use super::*;
use crate::dijkstra::dijkstra_unidirectional_slice_int;
use rand::rngs::StdRng;
use rand::{Rng, SeedableRng};
#[test]
fn workspace_matches_oneshot_repeated() {
let conns = [
Connectivity::Six,
Connectivity::Eighteen,
Connectivity::TwentySix,
];
let mut rng = StdRng::seed_from_u64(0x5EED_0071);
for trial in 0..200u64 {
let a = rng.random_range(2u64..=14);
let b = rng.random_range(2u64..=14);
let c = rng.random_range(2u64..=14);
let shape = VolumeShape::new(a, b, c);
let total = shape.total() as usize;
let field: Vec<u32> = match trial % 3 {
0 => (0..total).map(|_| rng.random_range(1..=8u32)).collect(),
1 => (0..total).map(|_| rng.random_range(1..=5000u32)).collect(),
_ => {
let mut f: Vec<u32> =
(0..total).map(|_| rng.random_range(1..=100u32)).collect();
f[total / 2] = 5_000_000;
f
}
};
let mut ws = DijkstraWorkspace::for_field(&field, shape).unwrap();
for _ in 0..6 {
let src = rng.random_range(0..total as u64);
let mut tgt = rng.random_range(0..total as u64);
if tgt == src {
tgt = (tgt + 1) % total as u64;
}
let conn = conns[rng.random_range(0..3)];
let ws_path = ws.shortest_path(&field, src, tgt, conn, None);
let one = dijkstra_unidirectional_slice_int(&field, shape, src, tgt, conn, None);
match (ws_path, one) {
(Ok(p), Ok(q)) => assert_eq!(
p, q,
"path mismatch trial={trial} src={src} tgt={tgt} conn={conn:?}"
),
(Err(_), Err(_)) => {}
(a, b) => panic!("Ok/Err disagreement: {a:?} vs {b:?}"),
}
}
}
}
#[test]
fn workspace_source_equals_target() {
let shape = VolumeShape::new(4, 4, 4);
let field = vec![3u32; shape.total() as usize];
let mut ws = DijkstraWorkspace::for_field(&field, shape).unwrap();
let s = shape.linear(1, 1, 1);
assert_eq!(
ws.shortest_path(&field, s, s, Connectivity::Six, None)
.unwrap(),
vec![s]
);
}
#[test]
fn workspace_generation_wrap_is_correct() {
let shape = VolumeShape::new(6, 6, 6);
let total = shape.total() as usize;
let mut rng = StdRng::seed_from_u64(0xC0FFEE);
let field: Vec<u16> = (0..total).map(|_| rng.random_range(1..=20u16)).collect();
let mut ws = DijkstraWorkspace::for_field(&field, shape).unwrap();
ws.generation = u32::MAX - 1;
let src = shape.linear(0, 0, 0);
let tgt = shape.linear(5, 5, 5);
for _ in 0..4 {
let ws_path = ws
.shortest_path(&field, src, tgt, Connectivity::TwentySix, None)
.unwrap();
let one = dijkstra_unidirectional_slice_int(
&field,
shape,
src,
tgt,
Connectivity::TwentySix,
None,
)
.unwrap();
assert_eq!(ws_path, one, "mismatch around generation wrap");
}
assert!(
ws.generation < 8,
"generation should have wrapped, got {}",
ws.generation
);
}
#[test]
fn workspace_input_validation() {
let shape = VolumeShape::new(4, 4, 4);
let field = vec![1u8; shape.total() as usize];
let mut ws = DijkstraWorkspace::for_field(&field, shape).unwrap();
let total = shape.total();
assert!(matches!(
ws.shortest_path(&field, total, 0, Connectivity::Six, None),
Err(Dijkstra3dError::OutOfBounds { .. })
));
assert!(matches!(
ws.shortest_path(&field, 0, total, Connectivity::Six, None),
Err(Dijkstra3dError::OutOfBounds { .. })
));
let short = vec![1u8; 3];
assert!(matches!(
ws.shortest_path(&short, 0, 1, Connectivity::Six, None),
Err(Dijkstra3dError::InvalidInput(_))
));
}
#[test]
fn workspace_rejects_unrepresentable_distance_bound() {
let shape = VolumeShape::new(2, 2, 2);
assert!(matches!(
DijkstraWorkspace::with_capacity(shape, u64::MAX),
Err(Dijkstra3dError::InvalidInput(_))
));
}
}