use core::ops::RangeBounds;
use crate::cover::{Masks, Quadrants, Rect};
use crate::word::Word;
#[derive(Clone, Copy, PartialEq, Eq, PartialOrd, Ord, Debug, Hash)]
pub struct Dilated<W: Word, const D: u32>(W);
impl<W: Word, const D: u32> Default for Dilated<W, D> {
#[inline]
fn default() -> Self {
Self(W::ZERO)
}
}
impl<W: Word, const D: u32> core::fmt::Binary for Dilated<W, D> {
#[allow(clippy::missing_inline_in_public_items)]
fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
core::fmt::Binary::fmt(&self.0, f)
}
}
impl<W: Word, const D: u32> Dilated<W, D> {
#[inline]
#[must_use]
pub fn mask() -> W {
let mut m = W::ZERO;
let mut i = 0;
while i < W::BITS {
m = m.or(W::ONE.shl(i));
i += D;
}
m
}
#[inline]
#[must_use]
pub const fn width() -> u32 {
W::BITS.div_ceil(D)
}
#[inline]
#[must_use]
pub fn from_int(x: W) -> Self {
Self(x.pdep(Self::mask()))
}
#[inline]
#[must_use]
pub fn to_int(self) -> W {
self.0.pext(Self::mask())
}
#[inline]
#[must_use]
pub fn from_bits(bits: W) -> Self {
debug_assert!(
bits.and(Self::mask().not()).is_zero(),
"bits outside dilation mask"
);
Self(bits)
}
#[inline]
#[must_use]
pub const fn bits(self) -> W {
self.0
}
#[inline]
#[must_use]
pub fn incr(self) -> Self {
let m = Self::mask();
Self(self.0.or(m.not()).wrapping_add(W::ONE).and(m))
}
#[inline]
#[must_use]
pub fn decr(self) -> Self {
Self(self.0.wrapping_sub(W::ONE).and(Self::mask()))
}
#[inline]
#[must_use]
pub fn wrapping_add(self, other: Self) -> Self {
let m = Self::mask();
Self(self.0.or(m.not()).wrapping_add(other.0).and(m))
}
}
#[derive(Clone, Copy, PartialEq, Eq, Debug, Hash, PartialOrd, Ord)]
#[doc(alias("z-order", "zorder", "morton code"))]
pub struct Morton2<W: Word>(W);
impl<W: Word> Morton2<W> {
pub const LEVELS: u32 = W::BITS / 2;
#[inline]
#[must_use]
pub fn encode(x: W, y: W) -> Self {
Self(x.zip(y))
}
#[inline]
#[must_use]
pub fn decode(self) -> (W, W) {
self.0.unzip()
}
#[inline]
#[must_use]
pub const fn from_code(code: W) -> Self {
Self(code)
}
#[inline]
#[must_use]
pub const fn code(self) -> W {
self.0
}
#[inline]
#[must_use]
pub fn x(self) -> Dilated<W, 2> {
Dilated::from_bits(self.0.and(Dilated::<W, 2>::mask()))
}
#[inline]
#[must_use]
pub fn y(self) -> Dilated<W, 2> {
Dilated::from_bits(self.0.shr(1).and(Dilated::<W, 2>::mask()))
}
#[inline]
#[must_use]
pub fn with_x(self, x: Dilated<W, 2>) -> Self {
Self(self.0.and(Dilated::<W, 2>::mask().not()).or(x.bits()))
}
#[inline]
#[must_use]
pub fn with_y(self, y: Dilated<W, 2>) -> Self {
Self(self.0.and(Dilated::<W, 2>::mask()).or(y.bits().shl(1)))
}
#[inline]
#[must_use]
pub fn step_x(self) -> Self {
self.with_x(self.x().incr())
}
#[inline]
#[must_use]
pub fn step_y(self) -> Self {
self.with_y(self.y().incr())
}
}
#[derive(Clone, Copy, PartialEq, Eq, Debug, Hash, PartialOrd, Ord)]
pub struct Morton3<W: Word>(W);
impl<W: Word> Morton3<W> {
pub const LEVELS: u32 = W::BITS / 3;
#[inline]
#[must_use]
pub fn encode(x: W, y: W, z: W) -> Self {
let side = W::low_ones(Self::LEVELS);
let d = |v: W| Dilated::<W, 3>::from_int(v.and(side)).bits();
Self(d(x).or(d(y).shl(1)).or(d(z).shl(2)))
}
#[inline]
#[must_use]
pub fn decode(self) -> (W, W, W) {
let lane = Dilated::<W, 3>::mask();
let u = |v: W| Dilated::<W, 3>::from_bits(v.and(lane)).to_int();
(u(self.0), u(self.0.shr(1)), u(self.0.shr(2)))
}
#[inline]
#[must_use]
pub const fn from_code(code: W) -> Self {
Self(code)
}
#[inline]
#[must_use]
pub const fn code(self) -> W {
self.0
}
}
macro_rules! morton2_columns {
($($w:ty => $encode:ident, $decode:ident),* $(,)?) => {$(
impl Morton2<$w> {
#[allow(clippy::missing_inline_in_public_items)]
pub fn encode_columns(xs: &[$w], ys: &[$w], out: &mut [$w]) {
let n = out.len();
assert!(
xs.len() == n && ys.len() == n,
"columns of {} and {} for {n} codes",
xs.len(),
ys.len()
);
let done = batch::$encode(xs, ys, out);
for ((o, &x), &y) in out[done..].iter_mut().zip(&xs[done..]).zip(&ys[done..]) {
*o = Self::encode(x, y).code();
}
}
#[allow(clippy::missing_inline_in_public_items)]
pub fn decode_columns(codes: &[$w], xs: &mut [$w], ys: &mut [$w]) {
let n = codes.len();
assert!(
xs.len() == n && ys.len() == n,
"columns of {} and {} for {n} codes",
xs.len(),
ys.len()
);
let done = batch::$decode(codes, xs, ys);
for ((&c, x), y) in codes[done..].iter().zip(&mut xs[done..]).zip(&mut ys[done..]) {
(*x, *y) = Self::from_code(c).decode();
}
}
}
)*};
}
pub(crate) struct ZQuadrants;
static Z_MASKS: Masks<1> = Masks::build([[(0, 0), (1, 0), (2, 0), (3, 0)]]);
impl Quadrants for ZQuadrants {
type Context<W> = ();
#[inline]
fn digit(_: u8, dx: u8, dy: u8) -> (u8, u8) {
(dx | dy << 1, 0)
}
#[inline]
fn child(k: u32, _: u8, p: u32) -> (u8, u8, u8) {
Z_MASKS.child(k, 0, p)
}
#[inline]
fn cells(k: u32, _: u8, cols: usize, rows: usize) -> u64 {
Z_MASKS.cells(k, 0, cols, rows)
}
fn context<W: Word>(_: &Rect<W>) {}
fn runs<W: Word>(levels: u32, r: &Rect<W>, s: u32, (): &()) -> W {
z_runs(
(r.x0.shr(s), r.x1.shr(s)),
(r.y0.shr(s), r.y1.shr(s)),
levels - s,
)
}
}
fn z_runs<W: Word>((x0, x1): (W, W), (y0, y1): (W, W), levels: u32) -> W {
let progression = |(a, b): (W, W), r: W, m: u32| {
let upto = |v: W| {
if v < r {
W::ZERO
} else {
v.wrapping_sub(r).shr(m).wrapping_add(W::ONE)
}
};
let before = if a.is_zero() {
W::ZERO
} else {
upto(a.wrapping_sub(W::ONE))
};
upto(b).wrapping_sub(before)
};
let spills = |(a, b): (W, W), m: u32| {
let low = W::low_ones(m);
b.and(low.not()) >= a && b.and(low) != low
};
let one = |c: bool| if c { W::ONE } else { W::ZERO };
let mut n = one(x0.is_zero() && y0.is_zero());
for t in 0..levels {
let half = W::ONE.shl(t);
let near = x0.trailing_zeros() == t;
let cols = progression((x0, x1), half, t + 1).wrapping_sub(one(near));
let rows = progression((y0, y1), W::ZERO, t);
n = n.wrapping_add(if near { rows } else { W::ZERO });
n = n.wrapping_add(if spills((y0, y1), t) { cols } else { W::ZERO });
let near = y0.trailing_zeros() == t;
let rows = progression((y0, y1), half, t + 1).wrapping_sub(one(near));
let cols = progression((x0, x1), W::ZERO, t + 1);
n = n.wrapping_add(if near { cols } else { W::ZERO });
n = n.wrapping_add(if spills((x0, x1), t + 1) {
rows
} else {
W::ZERO
});
}
n
}
impl<W: Word> Morton2<W> {
#[inline]
pub fn cover(x: impl RangeBounds<W>, y: impl RangeBounds<W>, out: &mut [(W, W)]) -> &[(W, W)] {
crate::cover::cover_ranges::<W, ZQuadrants>(Self::LEVELS, x, y, out)
}
#[must_use]
#[inline]
pub fn intersects(
keys: impl RangeBounds<W>,
x: impl RangeBounds<W>,
y: impl RangeBounds<W>,
) -> bool {
crate::cover::intersects_ranges::<W, ZQuadrants>(Self::LEVELS, keys, x, y)
}
}
morton2_columns!(
u32 => encode_u32, decode_u32,
u64 => encode_u64, decode_u64,
);
mod batch {
#[cfg(all(
target_arch = "x86_64",
target_feature = "sse2",
not(feature = "portable")
))]
#[allow(unsafe_code, clippy::inline_always)]
mod avx2 {
use core::arch::x86_64::{
__m256i, _mm_loadu_si128, _mm256_and_si256, _mm256_broadcastsi128_si256,
_mm256_loadu_si256, _mm256_or_si256, _mm256_set1_epi8, _mm256_shuffle_epi8,
_mm256_slli_epi64, _mm256_srli_epi16, _mm256_srli_epi64, _mm256_storeu_si256,
};
const Z: u8 = 0x80;
const fn spread(odd: bool) -> [u8; 16] {
let mut t = [0u8; 16];
let mut n = 0;
while n < 16 {
let s = (n & 1) | (n & 2) << 1 | (n & 4) << 2 | (n & 8) << 3;
t[n as usize] = if odd { s << 1 } else { s };
n += 1;
}
t
}
const fn compact(odd: bool, high: bool) -> [u8; 16] {
let mut t = [0u8; 16];
let mut n = 0u8;
while n < 16 {
let m = if odd { n >> 1 } else { n };
let c = (m & 1) | (m >> 1 & 2);
t[n as usize] = if high { c << 2 } else { c };
n += 1;
}
t
}
const SPREAD_X: [u8; 16] = spread(false);
const SPREAD_Y: [u8; 16] = spread(true);
const X_LO: [u8; 16] = compact(false, false);
const X_HI: [u8; 16] = compact(false, true);
const Y_LO: [u8; 16] = compact(true, false);
const Y_HI: [u8; 16] = compact(true, true);
#[inline(always)]
unsafe fn table(t: &[u8; 16]) -> __m256i {
unsafe { _mm256_broadcastsi128_si256(_mm_loadu_si128(t.as_ptr().cast())) }
}
macro_rules! kernels {
($encode:ident, $decode:ident, $w:ty, $widen:expr, $gather:expr) => {
#[target_feature(enable = "avx2")]
pub(in crate::dilated) fn $encode(xs: &[$w], ys: &[$w], out: &mut [$w]) -> usize {
const PER: usize = 32 / size_of::<$w>();
let (out, _) = out.as_chunks_mut::<{ 4 * PER }>();
let (xs, _) = xs.as_chunks::<{ 4 * PER }>();
let (ys, _) = ys.as_chunks::<{ 4 * PER }>();
let done = out.len() * 4 * PER;
unsafe {
let widen = table(&$widen);
let (sx, sy) = (table(&SPREAD_X), table(&SPREAD_Y));
let low = _mm256_set1_epi8(0x0F);
let nibbles = |v: __m256i| {
let w = _mm256_shuffle_epi8(v, widen);
_mm256_and_si256(_mm256_or_si256(w, _mm256_slli_epi64::<4>(w)), low)
};
for ((o, x), y) in out.iter_mut().zip(xs).zip(ys) {
for r in 0..4 {
let xv = _mm256_loadu_si256(x.as_ptr().add(PER * r).cast());
let yv = _mm256_loadu_si256(y.as_ptr().add(PER * r).cast());
let code = _mm256_or_si256(
_mm256_shuffle_epi8(sx, nibbles(xv)),
_mm256_shuffle_epi8(sy, nibbles(yv)),
);
_mm256_storeu_si256(o.as_mut_ptr().add(PER * r).cast(), code);
}
}
}
done
}
#[target_feature(enable = "avx2")]
pub(in crate::dilated) fn $decode(
codes: &[$w],
xs: &mut [$w],
ys: &mut [$w],
) -> usize {
const PER: usize = 32 / size_of::<$w>();
let (codes, _) = codes.as_chunks::<{ 4 * PER }>();
let (xs, _) = xs.as_chunks_mut::<{ 4 * PER }>();
let (ys, _) = ys.as_chunks_mut::<{ 4 * PER }>();
let done = codes.len() * 4 * PER;
unsafe {
let gather = table(&$gather);
let (xl, xh) = (table(&X_LO), table(&X_HI));
let (yl, yh) = (table(&Y_LO), table(&Y_HI));
let low = _mm256_set1_epi8(0x0F);
let pack = |n: __m256i| {
_mm256_shuffle_epi8(
_mm256_or_si256(n, _mm256_srli_epi16::<4>(n)),
gather,
)
};
for ((c, x), y) in codes.iter().zip(xs).zip(ys) {
for r in 0..4 {
let v = _mm256_loadu_si256(c.as_ptr().add(PER * r).cast());
let lo = _mm256_and_si256(v, low);
let hi = _mm256_and_si256(_mm256_srli_epi64::<4>(v), low);
let xn = _mm256_or_si256(
_mm256_shuffle_epi8(xl, lo),
_mm256_shuffle_epi8(xh, hi),
);
let yn = _mm256_or_si256(
_mm256_shuffle_epi8(yl, lo),
_mm256_shuffle_epi8(yh, hi),
);
_mm256_storeu_si256(x.as_mut_ptr().add(PER * r).cast(), pack(xn));
_mm256_storeu_si256(y.as_mut_ptr().add(PER * r).cast(), pack(yn));
}
}
}
done
}
};
}
kernels!(
encode_u64,
decode_u64,
u64,
[0, Z, 1, Z, 2, Z, 3, Z, 8, Z, 9, Z, 10, Z, 11, Z],
[0, 2, 4, 6, Z, Z, Z, Z, 8, 10, 12, 14, Z, Z, Z, Z]
);
kernels!(
encode_u32,
decode_u32,
u32,
[0, Z, 1, Z, 4, Z, 5, Z, 8, Z, 9, Z, 12, Z, 13, Z],
[0, 2, Z, Z, 4, 6, Z, Z, 8, 10, Z, Z, 12, 14, Z, Z]
);
}
#[cfg(all(
target_arch = "x86_64",
target_feature = "sse2",
not(feature = "portable")
))]
#[allow(unsafe_code)]
mod dispatch {
use super::avx2;
macro_rules! pick {
($($name:ident($a:ident: $ta:ty, $b:ident: $tb:ty, $c:ident: $tc:ty);)*) => {$(
pub(in crate::dilated) fn $name($a: $ta, $b: $tb, $c: $tc) -> usize {
if crate::isa::batch().avx2 {
unsafe { avx2::$name($a, $b, $c) }
} else {
0
}
}
)*};
}
pick! {
encode_u32(xs: &[u32], ys: &[u32], out: &mut [u32]);
encode_u64(xs: &[u64], ys: &[u64], out: &mut [u64]);
decode_u32(codes: &[u32], xs: &mut [u32], ys: &mut [u32]);
decode_u64(codes: &[u64], xs: &mut [u64], ys: &mut [u64]);
}
}
#[cfg(all(
target_arch = "x86_64",
target_feature = "sse2",
not(feature = "portable")
))]
pub(super) use dispatch::{decode_u32, decode_u64, encode_u32, encode_u64};
#[cfg(not(all(
target_arch = "x86_64",
target_feature = "sse2",
not(feature = "portable")
)))]
mod none {
pub(in crate::dilated) const fn encode_u32(_: &[u32], _: &[u32], _: &mut [u32]) -> usize {
0
}
pub(in crate::dilated) const fn encode_u64(_: &[u64], _: &[u64], _: &mut [u64]) -> usize {
0
}
pub(in crate::dilated) const fn decode_u32(
_: &[u32],
_: &mut [u32],
_: &mut [u32],
) -> usize {
0
}
pub(in crate::dilated) const fn decode_u64(
_: &[u64],
_: &mut [u64],
_: &mut [u64],
) -> usize {
0
}
}
#[cfg(not(all(
target_arch = "x86_64",
target_feature = "sse2",
not(feature = "portable")
)))]
pub(super) use none::{decode_u32, decode_u64, encode_u32, encode_u64};
}