dymokomi/luce-geocore
Source of release 0.7.9
0.7.9 / src / code / grid.lucb
# A uniform point grid for nearpoint and nearpoints: cells about one point each, the
# points of each cell listed contiguously (compressed rows), built once per input.
## Points bucketed by cell.
struct PointGrid:
var low: Vector3 = Vector3()
var cell: f64
var nx: usize
var ny: usize
var nz: usize
## Cell c's points are order[starts[c]..<starts[c + 1]]; `positions` are their world
## positions in the same (cell) order, so a cell's scan reads contiguous memory.
var starts: u32[]
var order: u32[]
var positions: Vector3[]
var built: bool
func close():
if not self.built: return
free(self.starts) in memory.heap
free(self.order) in memory.heap
free(self.positions) in memory.heap
self.built = false
func build(mesh: const Mesh*) -> !:
let count = (usize)mesh.point_count()
self.positions = try new Vector3[count] --- in memory.heap
errdefer free(self.positions) in memory.heap
# World positions and per-block bounds in parallel, then the box.
let blocks = (count + grid_block - 1) // grid_block
let lows = try new Vector3[blocks] --- in memory.heap
defer free(lows) in memory.heap
let highs = try new Vector3[blocks] --- in memory.heap
defer free(highs) in memory.heap
var gather = GatherWork(mesh.point_span(), mesh.position_origin(), self.positions, lows, highs)
try parallel.for_each_range(count, grid_block, (void*)&gather, gather_positions)
var low = Vector3(1.0e300, 1.0e300, 1.0e300)
var high = Vector3(-1.0e300, -1.0e300, -1.0e300)
for block in 0..<blocks:
low = lower_of(low, lows[block])
high = higher_of(high, highs[block])
if count == 0:
low = Vector3()
high = Vector3()
# Cells of about one point each over the box, a flat box keeping one cell across.
let size = high.subtract(low)
let longest = math.max(size.x, math.max(size.y, size.z))
let floor_size = longest * 0.001 + 1.0e-9
let volume = math.max(size.x, floor_size) * math.max(size.y, floor_size) * math.max(size.z, floor_size)
self.cell = math.max(math.cbrt(volume / (f64)(count if count > 0 else 1)), 1.0e-9)
self.nx = axis_cells(size.x, self.cell)
self.ny = axis_cells(size.y, self.cell)
self.nz = axis_cells(size.z, self.cell)
while self.nx * self.ny * self.nz > 16777216:
self.cell *= 1.5
self.nx = axis_cells(size.x, self.cell)
self.ny = axis_cells(size.y, self.cell)
self.nz = axis_cells(size.z, self.cell)
self.low = low
# Each point's cell in parallel; the counting sort over them stays serial
# (sequential, so the order within a cell is by point number).
let cell_ids = try new u32[count] --- in memory.heap
defer free(cell_ids) in memory.heap
var bucket = BucketWork(&self, cell_ids)
try parallel.for_each_range(count, grid_block, (void*)&bucket, bucket_points)
let cells = self.nx * self.ny * self.nz
self.starts = try new u32[cells + 1] in memory.heap
errdefer free(self.starts) in memory.heap
self.order = try new u32[count] --- in memory.heap
for at in 0..<count: self.starts[(usize)cell_ids[at] + 1] += 1
for c in 0..<cells: self.starts[c + 1] += self.starts[c]
var filled = try new u32[cells] in memory.heap
defer free(filled) in memory.heap
for at in 0..<count:
let c = (usize)cell_ids[at]
self.order[(usize)self.starts[c] + (usize)filled[c]] = (u32)at
filled[c] += 1
# The positions in cell order.
let by_cell = try new Vector3[count] --- in memory.heap
for at in 0..<count: by_cell[at] = self.positions[(usize)self.order[at]]
free(self.positions) in memory.heap
self.positions = by_cell
self.built = true
func axis(value: f64, cells: usize) -> usize:
let at = math.floor(value / self.cell)
if not (at > 0.0): return 0
if at >= (f64)(cells - 1): return cells - 1
return (usize)at
func cell_of(p: Vector3) -> usize:
let x = self.axis(p.x - self.low.x, self.nx)
let y = self.axis(p.y - self.low.y, self.ny)
let z = self.axis(p.z - self.low.z, self.nz)
return (z * self.ny + y) * self.nx + x
## The nearest `most` points within `reach` of `p`, nearest first (ties by number):
## their numbers into `found` and squared distances into `distances` (both at least
## `most` long); how many.
func nearest(p: Vector3, reach: f64, most: usize, found: u32[], distances: f64[]) -> usize:
if not self.built or most == 0 or not (reach >= 0.0) or found.length < most or distances.length < most:
return 0
var count: usize = 0
let limit = reach * reach
let x0 = self.axis(p.x - reach - self.low.x, self.nx)
let x1 = self.axis(p.x + reach - self.low.x, self.nx)
let y0 = self.axis(p.y - reach - self.low.y, self.ny)
let y1 = self.axis(p.y + reach - self.low.y, self.ny)
let z0 = self.axis(p.z - reach - self.low.z, self.nz)
let z1 = self.axis(p.z + reach - self.low.z, self.nz)
for z in z0..<z1 + 1:
for y in y0..<y1 + 1:
for x in x0..<x1 + 1:
let c = (z * self.ny + y) * self.nx + x
for at in (usize)self.starts[c]..<(usize)self.starts[c + 1]:
let index = self.order[at]
let d = self.positions[at].subtract(p)
let distance = d.x * d.x + d.y * d.y + d.z * d.z
if distance > limit:
continue
# Insert into the sorted list, keeping the `most` nearest.
var slot = count
while slot > 0 and (distances[slot - 1] > distance or (distances[slot - 1] == distance and found[slot - 1] > index)):
slot -= 1
if slot >= most:
continue
var move = count if count < most else most - 1
while move > slot:
distances[move] = distances[move - 1]
found[move] = found[move - 1]
move -= 1
distances[slot] = distance
found[slot] = index
if count < most: count += 1
return count
## Points per parallel block of the build.
let grid_block: usize = 65536
struct GatherWork:
let points: const Float3[]
let origin: Vector3
let positions: Vector3[]
let lows: Vector3[]
let highs: Vector3[]
## World positions of one block, and its bounds.
func gather_positions(context: void*, first: usize, end: usize, worker: usize):
let work = (GatherWork*)context
var low = Vector3(1.0e300, 1.0e300, 1.0e300)
var high = Vector3(-1.0e300, -1.0e300, -1.0e300)
for at in first..<end:
let p = work.points[at].vector().add(work.origin)
work.positions[at] = p
low = lower_of(low, p)
high = higher_of(high, p)
work.lows[first // grid_block] = low
work.highs[first // grid_block] = high
struct BucketWork:
let grid: const PointGrid*
let cells: u32[]
## The cell of each point of one block.
func bucket_points(context: void*, first: usize, end: usize, worker: usize):
let work = (BucketWork*)context
for at in first..<end:
work.cells[at] = (u32)work.grid.cell_of(work.grid.positions[at])
func lower_of(a: Vector3, b: Vector3) -> Vector3:
return Vector3(math.min(a.x, b.x), math.min(a.y, b.y), math.min(a.z, b.z))
func higher_of(a: Vector3, b: Vector3) -> Vector3:
return Vector3(math.max(a.x, b.x), math.max(a.y, b.y), math.max(a.z, b.z))
func axis_cells(extent: f64, cell: f64) -> usize:
let cells = math.floor(extent / cell) + 1.0
if not (cells > 1.0): return 1
if cells > 4096.0: return 4096
return (usize)cells