#![allow(clippy::too_many_arguments)]
use crate::processing::{finite, point, radix, Context};
use crate::{Error, Positions, Workspace};
use alloc::vec::Vec;
fn part(mut x: u64) -> u64 {
x &= 0xffff;
x = (x ^ (x << 16)) & 0x000f0000ff0000ff;
x = (x ^ (x << 8)) & 0x000f00f00f00f00f;
x = (x ^ (x << 4)) & 0x00c30c30c30c30c3;
(x ^ (x << 2)) & 0x0249249249249249
}
fn keys(p: Positions<'_>, morton: bool, ctx: &mut Context<'_>) -> Result<Vec<u64>, Error> {
let mut keys = ctx.alloc(p.len())?;
if p.is_empty() {
return Ok(keys);
}
let mut lo = [f32::MAX; 3];
let mut hi = [-f32::MAX; 3];
ctx.tick(p.len() * 2)?;
p.for_each(|v| {
for j in 0..3 {
if lo[j] > v[j] {
lo[j] = v[j];
}
if hi[j] < v[j] {
hi[j] = v[j];
}
}
Ok(())
})?;
let mut extent = 0.0;
for j in 0..3 {
let e = finite(hi[j] - lo[j])?;
if e >= extent {
extent = e;
}
}
let scale = if extent == 0.0 {
0.0
} else {
finite(65535.0 / extent)?
};
let mut destination = keys.iter_mut();
p.for_each(|v| {
let x = ((v[0] - lo[0]) * scale + 0.5) as u32 as u64;
let y = ((v[1] - lo[1]) * scale + 0.5) as u32 as u64;
let z = ((v[2] - lo[2]) * scale + 0.5) as u32 as u64;
let key = destination.next().expect("validated position count");
*key = if morton {
part(x) | (part(y) << 1) | (part(z) << 2)
} else {
x | (y << 20) | (z << 40)
};
Ok(())
})?;
Ok(keys)
}
fn remap_into(out: &mut [u32], p: Positions<'_>, ctx: &mut Context<'_>) -> Result<(), Error> {
let keys = keys(p, true, ctx)?;
let mut order = ctx.alloc::<u32>(p.len())?;
let mut temp = ctx.alloc::<u32>(p.len())?;
for (i, v) in order.iter_mut().enumerate() {
*v = i as u32;
}
radix::<5, u64>(&mut order, &mut temp, &keys, ctx)?;
ctx.tick(order.len())?;
for (i, &v) in order.iter().enumerate() {
out[v as usize] = i as u32;
}
Ok(())
}
pub fn spatial_sort_remap_generic<I: crate::Index>(
p: Positions<'_>,
workspace: &mut Workspace,
) -> Result<Vec<I>, Error> {
spatial_sort_remap_u32(p, workspace).and_then(|value| I::from_vec(value, workspace))
}
fn spatial_sort_remap_u32(p: Positions<'_>, workspace: &mut Workspace) -> Result<Vec<u32>, Error> {
let mut ctx = Context::new(workspace)?;
ctx.positions(p)?;
let mut out = ctx.alloc(p.len())?;
remap_into(&mut out, p, &mut ctx)?;
Ok(out)
}
#[inline]
pub fn spatial_sort_remap_into_generic<I: crate::Index>(
out: &mut [I],
p: Positions<'_>,
workspace: &mut Workspace,
) -> Result<(), Error> {
I::with_mut(out, workspace, |out, workspace| {
spatial_sort_remap_into_u32(out, p, workspace)
})
}
fn spatial_sort_remap_into_u32(
out: &mut [u32],
p: Positions<'_>,
workspace: &mut Workspace,
) -> Result<(), Error> {
let mut ctx = Context::new(workspace)?;
if out.len() < p.len() {
return Err(Error::BufferTooSmall);
}
ctx.positions(p)?;
remap_into(&mut out[..p.len()], p, &mut ctx)
}
fn fill_centers<const CHECK: bool>(
centers: &mut [[f32; 3]],
triangles: &[[u32; 3]],
read: impl Fn(u32) -> [f32; 3],
) -> Result<(), Error> {
for (i, t) in triangles.iter().enumerate() {
let a = read(t[0]);
let b = read(t[1]);
let c = read(t[2]);
for j in 0..3 {
let v = (a[j] + b[j] + c[j]) / 3.0;
centers[i][j] = if CHECK { finite(v)? } else { v };
}
}
Ok(())
}
fn triangles_into(
out: &mut [u32],
indices: &[u32],
p: Positions<'_>,
ctx: &mut Context<'_>,
) -> Result<(), Error> {
let mut centers = ctx.alloc::<[f32; 3]>(indices.len() / 3)?;
ctx.tick(centers.len())?;
let triangles = indices.as_chunks::<3>().0;
match (ctx.moderate, p.packed_values()) {
(true, Some(values)) => {
fill_centers::<false>(&mut centers, triangles, |i| values[i as usize])?
}
(false, Some(values)) => {
fill_centers::<true>(&mut centers, triangles, |i| values[i as usize])?
}
(true, None) => fill_centers::<false>(&mut centers, triangles, |i| point(p, i))?,
(false, None) => fill_centers::<true>(&mut centers, triangles, |i| point(p, i))?,
}
let mut remap = ctx.alloc(centers.len())?;
remap_into(&mut remap, Positions::from_packed(¢ers), ctx)?;
ctx.tick(centers.len())?;
for (i, t) in indices.as_chunks::<3>().0.iter().enumerate() {
out[remap[i] as usize * 3..][..3].copy_from_slice(t);
}
Ok(())
}
#[inline]
pub fn spatial_sort_triangles_generic<I: crate::Index>(
indices: &[I],
p: Positions<'_>,
workspace: &mut Workspace,
) -> Result<Vec<I>, Error> {
I::with_input(indices, workspace, |indices, workspace| {
spatial_sort_triangles_u32(indices, p, workspace)
.and_then(|value| crate::index::Convert::<I>::convert(value, workspace))
})
}
fn spatial_sort_triangles_u32(
indices: &[u32],
p: Positions<'_>,
workspace: &mut Workspace,
) -> Result<Vec<u32>, Error> {
let mut ctx = Context::new(workspace)?;
ctx.topology(indices, p.len())?;
ctx.positions(p)?;
let mut out = ctx.alloc(indices.len())?;
triangles_into(&mut out, indices, p, &mut ctx)?;
Ok(out)
}
#[inline]
pub fn spatial_sort_triangles_into_generic<I: crate::Index>(
out: &mut [I],
indices: &[I],
p: Positions<'_>,
workspace: &mut Workspace,
) -> Result<(), Error> {
I::with_input(indices, workspace, |indices, workspace| {
I::with_mut(out, workspace, |out, workspace| {
spatial_sort_triangles_into_u32(out, indices, p, workspace)
})
})
}
fn spatial_sort_triangles_into_u32(
out: &mut [u32],
indices: &[u32],
p: Positions<'_>,
workspace: &mut Workspace,
) -> Result<(), Error> {
let mut ctx = Context::new(workspace)?;
if out.len() < indices.len() {
return Err(Error::BufferTooSmall);
}
ctx.topology(indices, p.len())?;
ctx.positions(p)?;
triangles_into(&mut out[..indices.len()], indices, p, &mut ctx)
}
#[inline]
pub fn spatial_sort_triangles_in_place_generic<I: crate::Index>(
indices: &mut [I],
p: Positions<'_>,
workspace: &mut Workspace,
) -> Result<(), Error> {
I::with_mut(indices, workspace, |indices, workspace| {
spatial_sort_triangles_in_place_u32(indices, p, workspace)
})
}
fn spatial_sort_triangles_in_place_u32(
indices: &mut [u32],
p: Positions<'_>,
workspace: &mut Workspace,
) -> Result<(), Error> {
let result = spatial_sort_triangles_generic(indices, p, workspace)?;
indices.copy_from_slice(&result);
Ok(())
}
fn split(
out: &mut [u32],
axes: &mut [Vec<u32>; 3],
start: usize,
n: usize,
keys: &[u64],
temp: &mut [u32],
sides: &mut [u8],
size: usize,
ctx: &mut Context<'_>,
) -> Result<(), Error> {
if n <= size {
out.copy_from_slice(&axes[0][start..start + n]);
return Ok(());
}
ctx.tick(n * 3)?;
let mut best = 0;
let mut dim = 0;
for (k, axis) in axes.iter().enumerate() {
let d = ((keys[axis[start + n - 1] as usize] >> (k * 20)) & 0xfffff)
- ((keys[axis[start] as usize] >> (k * 20)) & 0xfffff);
if d >= dim {
best = k;
dim = d;
}
}
let s = (n / 2).div_ceil(size) * size;
for i in 0..n {
sides[axes[best][start + i] as usize] = u8::from(i >= s);
}
for (k, axis) in axes.iter_mut().enumerate() {
if k == best {
continue;
}
let (mut l, mut r) = (0, s);
for &v in &axis[start..start + n] {
let side = sides[v as usize] as usize;
temp[if side != 0 { r } else { l }] = v;
l += 1 - side;
r += side;
}
axis[start..start + n].copy_from_slice(&temp[..n]);
}
let (left, right) = out.split_at_mut(s);
split(left, axes, start, s, keys, temp, sides, size, ctx)?;
split(right, axes, start + s, n - s, keys, temp, sides, size, ctx)
}
fn cluster_into(
out: &mut [u32],
p: Positions<'_>,
size: usize,
ctx: &mut Context<'_>,
) -> Result<(), Error> {
let keys = keys(p, false, ctx)?;
let n = p.len();
let mut axes = [ctx.alloc(n)?, ctx.alloc(n)?, ctx.alloc(n)?];
let mut temp = ctx.alloc(n)?;
let mut sides = ctx.alloc(n)?;
let mut key = ctx.alloc::<u16>(n)?;
for (k, axis) in axes.iter_mut().enumerate() {
for i in 0..n {
axis[i] = i as u32;
key[i] = (keys[i] >> (k * 20)) as u16;
}
ctx.tick(n * 4)?;
let mut hist = [[0u32; 2]; 256];
for &v in &key {
hist[(v & 255) as usize][0] += 1;
hist[(v >> 8) as usize][1] += 1;
}
let mut sums = [0; 2];
for h in &mut hist {
let counts = *h;
*h = sums;
sums[0] += counts[0];
sums[1] += counts[1];
}
for &v in axis.iter() {
let h = &mut hist[(key[v as usize] & 255) as usize][0];
temp[*h as usize] = v;
*h += 1;
}
for &v in &temp {
let h = &mut hist[(key[v as usize] >> 8) as usize][1];
axis[*h as usize] = v;
*h += 1;
}
}
split(
out, &mut axes, 0, n, &keys, &mut temp, &mut sides, size, ctx,
)
}
pub fn spatial_cluster_points_generic<I: crate::Index>(
p: Positions<'_>,
size: usize,
workspace: &mut Workspace,
) -> Result<Vec<I>, Error> {
spatial_cluster_points_u32(p, size, workspace).and_then(|value| I::from_vec(value, workspace))
}
fn spatial_cluster_points_u32(
p: Positions<'_>,
size: usize,
workspace: &mut Workspace,
) -> Result<Vec<u32>, Error> {
let mut ctx = Context::new(workspace)?;
if size == 0 {
return Err(Error::InvalidParameter);
}
ctx.positions(p)?;
let mut out = ctx.alloc(p.len())?;
cluster_into(&mut out, p, size, &mut ctx)?;
Ok(out)
}
#[inline]
pub fn spatial_cluster_points_into_generic<I: crate::Index>(
out: &mut [I],
p: Positions<'_>,
size: usize,
workspace: &mut Workspace,
) -> Result<(), Error> {
I::with_mut(out, workspace, |out, workspace| {
spatial_cluster_points_into_u32(out, p, size, workspace)
})
}
fn spatial_cluster_points_into_u32(
out: &mut [u32],
p: Positions<'_>,
size: usize,
workspace: &mut Workspace,
) -> Result<(), Error> {
let mut ctx = Context::new(workspace)?;
if size == 0 {
return Err(Error::InvalidParameter);
}
if out.len() < p.len() {
return Err(Error::BufferTooSmall);
}
ctx.positions(p)?;
cluster_into(&mut out[..p.len()], p, size, &mut ctx)
}
#[inline]
pub fn spatial_sort_remap(p: Positions<'_>, workspace: &mut Workspace) -> Result<Vec<u32>, Error> {
spatial_sort_remap_generic::<u32>(p, workspace)
}
#[inline]
pub fn spatial_sort_remap_into(
out: &mut [u32],
p: Positions<'_>,
workspace: &mut Workspace,
) -> Result<(), Error> {
spatial_sort_remap_into_generic::<u32>(out, p, workspace)
}
#[inline]
pub fn spatial_sort_triangles(
indices: &[u32],
p: Positions<'_>,
workspace: &mut Workspace,
) -> Result<Vec<u32>, Error> {
spatial_sort_triangles_generic::<u32>(indices, p, workspace)
}
#[inline]
pub fn spatial_sort_triangles_into(
out: &mut [u32],
indices: &[u32],
p: Positions<'_>,
workspace: &mut Workspace,
) -> Result<(), Error> {
spatial_sort_triangles_into_generic::<u32>(out, indices, p, workspace)
}
#[inline]
pub fn spatial_sort_triangles_in_place(
indices: &mut [u32],
p: Positions<'_>,
workspace: &mut Workspace,
) -> Result<(), Error> {
spatial_sort_triangles_in_place_generic::<u32>(indices, p, workspace)
}
#[inline]
pub fn spatial_cluster_points(
p: Positions<'_>,
size: usize,
workspace: &mut Workspace,
) -> Result<Vec<u32>, Error> {
spatial_cluster_points_generic::<u32>(p, size, workspace)
}
#[inline]
pub fn spatial_cluster_points_into(
out: &mut [u32],
p: Positions<'_>,
size: usize,
workspace: &mut Workspace,
) -> Result<(), Error> {
spatial_cluster_points_into_generic::<u32>(out, p, size, workspace)
}