Luce packages
LuceLuce BaseLuciaOS

dymokomi/luce-geocore

Source of release 0.7.7

0.7.7 / src / splats / neighbors.lucb

## Neighbor searches over a cloud's points for the GSplats filters: a hash of
## cubes of one side, whose buckets list their points. Clean GSplats counts
## points within a radius (its isolation test); Reduce GSplats finds each
## splat's nearest few to merge with. Cells hash into a table of a power of
## two buckets at least twice the points, so a bucket can hold several cells;
## a search visiting a cube takes only that cube's points (by their positions), and every candidate is checked by its distance, which
## keeps answers exact.
##
## A search walks rings of cubes outward from the query's own (ring r: the
## cubes r steps away along some axis) and stops once the next ring lies
## beyond the radius, or, for the nearest k, beyond the k-th found. `for_radius`
## picks cubes a fraction of the radius wide where the cloud is dense, so a
## query reads a few cubes' points instead of a radius-wide neighborhood's.

## Points bucketed by the cube of side `cell` they lie in.
pub struct PointHash:
    let points: const Float3[]
    let cell: f64
    let mask: u64
    ## Bucket b's points are order[starts[b]..<starts[b + 1]]; slots[s] is the
    ## position of the point at slot s, so a bucket's points read in a row.
    var starts: u32[]
    var order: u32[]
    var slots: Float3[]

    ## The hash of `points` with cubes of side `cell` (> 0). Each point's
    ## bucket is found on the pool; counting and placing them are sequential
    ## passes.
    pub static func build(points: const Float3[], cell: f64) -> PointHash!:
        let size = bucket_count(points.length)
        let starts = try new u32[size + 1] in memory.heap
        errdefer free(starts) in memory.heap
        let order = try new u32[points.length + 1] --- in memory.heap
        errdefer free(order) in memory.heap
        let slots = try new Float3[points.length + 1] --- in memory.heap
        errdefer free(slots) in memory.heap
        var made = PointHash(points, cell, (u64)(size - 1), starts, order, slots)
        let keys = try made.buckets()
        defer free(keys) in memory.heap
        for key in keys[..<points.length]: starts[(usize)key + 1] += 1
        for at in 0..<size: starts[at + 1] += starts[at]
        let filled = try new u32[size] in memory.heap
        defer free(filled) in memory.heap
        for (index, p) in points.indexed():
            let b = (usize)keys[index]
            let slot = (usize)starts[b] + (usize)filled[b]
            order[slot] = (u32)index
            slots[slot] = p
            filled[b] += 1
        return made

    ## Every point's bucket, found on the pool (one more entry than points;
    ## the caller frees it).
    func buckets() -> u32[]!:
        let keys = try new u32[self.points.length + 1] --- in memory.heap
        var work = BucketWork(&self, keys)
        ranges.each_range(self.points.length, 16384, (void*)&work, bucket_range)
        return keys

    ## A hash for searches within `radius` (> 0): cubes of side radius / m, m
    ## from 1 to 8 chosen so a point's cube holds about `points_per_cube` points
    ## (measured on radius-wide cubes: the mean, over points, of the points
    ## sharing one's cube, which dense regions dominate as they dominate the
    ## searches).
    pub static func for_radius(points: const Float3[], radius: f64) -> PointHash!:
        let size = bucket_count(points.length)
        let counts = try new u32[size] in memory.heap
        defer free(counts) in memory.heap
        let probe = PointHash(points, radius, (u64)(size - 1), no_order(), no_order(), no_points())
        let keys = try probe.buckets()
        defer free(keys) in memory.heap
        for key in keys[..<points.length]: counts[(usize)key] += 1
        var shared: f64 = 0.0
        for count in counts: shared += (f64)count * (f64)count
        let per_cube = shared / (f64)(points.length if points.length > 0 else 1)
        let m = math.clamp(math.round(math.cbrt(per_cube / points_per_cube)), 1.0, 8.0)
        return try PointHash.build(points, radius / m)

    ## The point at slot `slot` (0..<count): visiting points in slot order
    ## visits them cube by cube, so neighboring queries share their cubes in
    ## cache.
    pub func point_at(slot: usize) -> usize:
        return (usize)self.order[slot]

    ## Release the buckets.
    pub func close():
        free(self.starts) in memory.heap
        free(self.order) in memory.heap
        free(self.slots) in memory.heap

    ## The integer cube a point lies in (a non-finite point: cube 0).
    func cube(p: Vector3) -> Cube:
        return Cube(cell_index(p.x / self.cell), cell_index(p.y / self.cell), cell_index(p.z / self.cell))

    func bucket(c: Cube) -> u64:
        let h = ((u64)(c.x + cube_offset) *% 73856093) ^ ((u64)(c.y + cube_offset) *% 19349663) ^ ((u64)(c.z + cube_offset) *% 83492791)
        return (h ^ (h >> 17)) & self.mask

    ## How far `p` is from the faces of its cube `c`, at least.
    func inset(p: Vector3, c: Cube) -> f64:
        let low = Vector3((f64)c.x * self.cell, (f64)c.y * self.cell, (f64)c.z * self.cell)
        let a = math.min(p.x - low.x, low.x + self.cell - p.x)
        let b = math.min(p.y - low.y, low.y + self.cell - p.y)
        let d = math.min(p.z - low.z, low.z + self.cell - p.z)
        return math.max(math.min(a, math.min(b, d)), 0.0)

    ## How many other points lie within `radius` of point `index`, counting no
    ## further than `most`.
    pub func count_within(index: usize, radius: f64, most: usize) -> usize:
        let p = self.points[index].vector()
        let center = self.cube(p)
        let limit = radius * radius
        let inset = self.inset(p, center)
        var found: usize = 0
        var ring: i64 = 0
        while true:
            # Every point of ring r is at least (r - 1) cells and the inset away.
            if ring > 0 and (f64)(ring - 1) * self.cell + inset > radius: break
            var walk = RingWalk.of(center, ring)
            while let c = walk.next():
                let b = (usize)self.bucket(c)
                for slot in (usize)self.starts[b]..<(usize)self.starts[b + 1]:
                    if not (distance2(self.slots[slot], p) <= limit): continue
                    # A bucket can hold other cubes' points: take this cube's.
                    if (usize)self.order[slot] == index or not self.cube(self.slots[slot].vector()).equals(c): continue
                    found += 1
                    if found >= most: return found
            ring += 1
        return found

    ## The up to `k` (at most 16) nearest other points of point `index` within
    ## `radius`, nearest first, into `out`; returns how many.
    pub func nearest(index: usize, k: usize, radius: f64, out: u32[]) -> usize:
        let p = self.points[index].vector()
        let center = self.cube(p)
        let limit = radius * radius
        let inset = self.inset(p, center)
        var distances: f64[16]
        var found: usize = 0
        var ring: i64 = 0
        while true:
            if ring > 0:
                let near = (f64)(ring - 1) * self.cell + inset
                if near > radius: break
                # A full list keeps only nearer points; this ring has none.
                if found == k and near * near >= distances[k - 1]: break
            var walk = RingWalk.of(center, ring)
            while let c = walk.next():
                let b = (usize)self.bucket(c)
                for slot in (usize)self.starts[b]..<(usize)self.starts[b + 1]:
                    let d = distance2(self.slots[slot], p)
                    if not (d <= limit): continue
                    if found == k and d >= distances[k - 1]: continue
                    # A bucket can hold other cubes' points: take this cube's.
                    let other = (usize)self.order[slot]
                    if other == index or not self.cube(self.slots[slot].vector()).equals(c): continue
                    # Insert in order; a full list drops its farthest.
                    var at = found if found < k else k - 1
                    while at > 0 and distances[at - 1] > d:
                        distances[at] = distances[at - 1]
                        out[at] = out[at - 1]
                        at -= 1
                    distances[at] = d
                    out[at] = (u32)other
                    if found < k: found += 1
            ring += 1
        return found

struct BucketWork:
    let hash: const PointHash*
    let keys: u32[]

func bucket_range(context: void*, first: usize, end: usize, worker: usize):
    _ = worker
    let work = (BucketWork*)context
    let hash = work.hash
    for at in first..<end: work.keys[at] = (u32)hash.bucket(hash.cube(hash.points[at].vector()))

## The cubes of ring `ring` around a center: those whose largest axis step
## from it is `ring`, x-major. Ring 0 is the center alone.
struct RingWalk:
    let center: Cube
    let ring: i64
    var dx: i64
    var dy: i64
    var dz: i64
    var done: bool

    static func of(center: Cube, ring: i64) -> RingWalk:
        return RingWalk(center, ring, -ring, -ring, -ring, false)

    ## The next cube of the ring, or none after the last. A column (dx, dy)
    ## on the ring's x or y faces holds every z of -r..r; one inside them only
    ## z = -r and z = r. Every column starts at z = -r.
    func next() -> Cube?:
        if self.done: return none
        let r = self.ring
        let (x, y, z) = (self.dx, self.dy, self.dz)
        let face = x == -r or x == r or y == -r or y == r
        if face and z < r: self.dz = z + 1
        elif not face and z == -r: self.dz = r
        else:
            self.dz = -r
            if y < r: self.dy = y + 1
            elif x < r:
                self.dy = -r
                self.dx = x + 1
            else: self.done = true
        return Cube(self.center.x + x, self.center.y + y, self.center.z + z)

## What `for_radius` aims a point's cube to hold: fewer cubes are slower to
## visit, more points slower to test (measured on a 1.16M-splat capture).
let points_per_cube: f64 = 12.0

## The hash table size for `count` points: a power of two at least twice it.
func bucket_count(count: usize) -> usize:
    var size: usize = 64
    while size < count * 2: size *= 2
    return size

func no_order() -> u32[]:
    return u32[]((u32*)0, 0)

func no_points() -> Float3[]:
    return Float3[]((Float3*)0, 0)

## Cube indices are offset by this before hashing, so they hash as unsigned.
let cube_offset: i64 = 1099511627776

## The cube index of a coordinate in cells, clamped well inside i64 (NaN: 0).
func cell_index(value: f64) -> i64:
    if not (value == value): return 0
    return (i64)math.floor(math.clamp(value, -1.0e12, 1.0e12))

struct Cube:
    let x: i64
    let y: i64
    let z: i64

    func equals(other: Cube) -> bool:
        return self.x == other.x and self.y == other.y and self.z == other.z

## The squared distance from `a` to `p`.
func distance2(a: Float3, p: Vector3) -> f64:
    let dx = (f64)a.x - p.x
    let dy = (f64)a.y - p.y
    let dz = (f64)a.z - p.z
    return dx * dx + dy * dy + dz * dz