use crate::error::{Dijkstra3dError, Result};
use crate::voxel::{VolumeShape, VoxelSource};
use ndarray::{ArrayView3, ArrayViewMut3};
use num_traits::ToPrimitive;
#[derive(Clone, Copy, Debug)]
pub struct InvalidationAnisotropy {
pub wx: f32,
pub wy: f32,
pub wz: f32,
}
impl InvalidationAnisotropy {
pub fn new(wx: f32, wy: f32, wz: f32) -> Self {
Self { wx, wy, wz }
}
}
#[inline]
fn ball_axis_bounds(coord: i64, half_extent: f32, axis_size: u64) -> (i64, i64) {
let lo = (0.5 + (coord as f32 - half_extent)) as i64;
let hi = (0.5 + (coord as f32 + half_extent)) as i64;
let lo = lo.max(0);
let hi = hi.min(axis_size as i64);
(lo, hi)
}
#[inline]
fn cube_axis_bounds(coord: i64, half_extent: f32, axis_size: u64) -> (i64, i64) {
let lo = (coord as f32 - half_extent) as i64;
let hi = (0.5 + (coord as f32 + half_extent)) as i64;
let lo = lo.max(0);
let hi = hi.min(axis_size as i64 - 1);
(lo, hi)
}
pub fn roll_invalidation_ball<D>(
labels: &mut ArrayViewMut3<'_, u8>,
dbf: &D,
path: &[u64],
scale: f32,
constant: f32,
anisotropy: InvalidationAnisotropy,
) -> Result<usize>
where
D: VoxelSource,
D::Item: ToPrimitive,
{
let shape = dbf.shape();
let labels_shape = labels.shape();
if labels_shape[0] as u64 != shape.sx
|| labels_shape[1] as u64 != shape.sy
|| labels_shape[2] as u64 != shape.sz
{
return Err(Dijkstra3dError::InvalidInput(format!(
"labels and dbf shape mismatch: labels={:?}, dbf=({}, {}, {})",
labels_shape, shape.sx, shape.sy, shape.sz
)));
}
let (sx, sy, sz) = (shape.sx, shape.sy, shape.sz);
let total = shape.total();
let InvalidationAnisotropy { wx, wy, wz } = anisotropy;
if wx <= 0.0 || wy <= 0.0 || wz <= 0.0 {
return Err(Dijkstra3dError::InvalidInput(format!(
"anisotropy must be positive: ({}, {}, {})",
wx, wy, wz
)));
}
let mut invalidated: usize = 0;
for &loc in path {
if loc >= total {
return Err(Dijkstra3dError::OutOfBounds {
idx: loc,
volume_size: total,
});
}
let (x0, y0, z0) = shape.coords(loc);
let (x0i, y0i, z0i) = (x0 as i64, y0 as i64, z0 as i64);
let dbf_val = dbf.get(loc)?.to_f32().ok_or(Dijkstra3dError::OutOfBounds {
idx: loc,
volume_size: total,
})?;
let radius = scale * dbf_val + constant;
if radius <= 0.0 {
continue;
}
let radius_sq = radius * radius;
let (xmin, xmax) = ball_axis_bounds(x0i, radius / wx, sx);
let (ymin, ymax) = ball_axis_bounds(y0i, radius / wy, sy);
let (zmin, zmax) = ball_axis_bounds(z0i, radius / wz, sz);
for z in zmin..zmax {
let dz = wz * (z - z0i) as f32;
let dz_sq = dz * dz;
if dz_sq > radius_sq {
continue;
}
for y in ymin..ymax {
let dy = wy * (y - y0i) as f32;
let dy_sq = dy * dy;
if dz_sq + dy_sq > radius_sq {
continue;
}
for x in xmin..xmax {
let dx = wx * (x - x0i) as f32;
let dist_sq = dz_sq + dy_sq + dx * dx;
if dist_sq > radius_sq {
continue;
}
let cell = &mut labels[(x as usize, y as usize, z as usize)];
if *cell != 0 {
invalidated += 1;
*cell = 0;
}
}
}
}
}
Ok(invalidated)
}
pub fn roll_invalidation_cube<D>(
labels: &mut ArrayViewMut3<'_, u8>,
dbf: &D,
path: &[u64],
scale: f32,
constant: f32,
anisotropy: InvalidationAnisotropy,
) -> Result<usize>
where
D: VoxelSource,
D::Item: ToPrimitive,
{
let shape = dbf.shape();
let labels_shape = labels.shape();
if labels_shape[0] as u64 != shape.sx
|| labels_shape[1] as u64 != shape.sy
|| labels_shape[2] as u64 != shape.sz
{
return Err(Dijkstra3dError::InvalidInput(format!(
"labels and dbf shape mismatch: labels={:?}, dbf=({}, {}, {})",
labels_shape, shape.sx, shape.sy, shape.sz
)));
}
if path.is_empty() {
return Ok(0);
}
let (sx, sy, sz) = (shape.sx, shape.sy, shape.sz);
let total = shape.total();
let InvalidationAnisotropy { wx, wy, wz } = anisotropy;
if wx <= 0.0 || wy <= 0.0 || wz <= 0.0 {
return Err(Dijkstra3dError::InvalidInput(format!(
"anisotropy must be positive: ({}, {}, {})",
wx, wy, wz
)));
}
let mut topology: Vec<i32> = vec![0; total as usize];
let (mut gx_min, mut gx_max) = (sx as i64, -1i64);
let (mut gy_min, mut gy_max) = (sy as i64, -1i64);
let (mut gz_min, mut gz_max) = (sz as i64, -1i64);
for &loc in path {
if loc >= total {
return Err(Dijkstra3dError::OutOfBounds {
idx: loc,
volume_size: total,
});
}
let (x0, y0, z0) = shape.coords(loc);
let (x0i, y0i, z0i) = (x0 as i64, y0 as i64, z0 as i64);
let dbf_val = dbf.get(loc)?.to_f32().ok_or(Dijkstra3dError::OutOfBounds {
idx: loc,
volume_size: total,
})?;
let radius = scale * dbf_val + constant;
if radius <= 0.0 {
continue;
}
let (xmin, xmax) = cube_axis_bounds(x0i, radius / wx, sx);
let (ymin, ymax) = cube_axis_bounds(y0i, radius / wy, sy);
let (zmin, zmax) = cube_axis_bounds(z0i, radius / wz, sz);
if xmin > xmax || ymin > ymax || zmin > zmax {
continue;
}
gx_min = gx_min.min(xmin);
gx_max = gx_max.max(xmax);
gy_min = gy_min.min(ymin);
gy_max = gy_max.max(ymax);
gz_min = gz_min.min(zmin);
gz_max = gz_max.max(zmax);
let stride_y = sx as i64;
let stride_z = (sx * sy) as i64;
for z in zmin..=zmax {
for y in ymin..=ymax {
let yz_offset = (y * stride_y + z * stride_z) as usize;
topology[xmin as usize + yz_offset] += 1;
topology[xmax as usize + yz_offset] -= 1;
}
}
}
if gx_max < 0 {
return Ok(0);
}
let stride_y = sx as i64;
let stride_z = (sx * sy) as i64;
let mut invalidated: usize = 0;
for z in gz_min..=gz_max {
for y in gy_min..=gy_max {
let yz_offset = (y * stride_y + z * stride_z) as usize;
let mut coloring: i32 = 0;
for x in gx_min..=gx_max {
let idx = x as usize + yz_offset;
coloring += topology[idx];
if coloring > 0 || topology[idx] != 0 {
let cell = &mut labels[(x as usize, y as usize, z as usize)];
if *cell != 0 {
invalidated += 1;
*cell = 0;
}
}
}
}
}
Ok(invalidated)
}
pub struct DbfView<'a> {
view: ArrayView3<'a, f32>,
shape: VolumeShape,
}
impl<'a> DbfView<'a> {
pub fn new(view: ArrayView3<'a, f32>) -> Self {
let s = view.shape();
let shape = VolumeShape::new(s[0] as u64, s[1] as u64, s[2] as u64);
Self { view, shape }
}
}
impl<'a> VoxelSource for DbfView<'a> {
type Item = f32;
fn shape(&self) -> VolumeShape {
self.shape
}
fn get(&self, idx: u64) -> Result<Self::Item> {
let total = self.shape.total();
if idx >= total {
return Err(Dijkstra3dError::OutOfBounds {
idx,
volume_size: total,
});
}
let (x, y, z) = self.shape.coords(idx);
Ok(self.view[(x as usize, y as usize, z as usize)])
}
}