dymokomi/luce-geocore
Source of release 0.7.6
0.7.6 / src / geometries / triangle_index.lucb
## An immutable triangle BVH over f32 positions, for CPU queries: rays,
## nearest points, snapping. Nodes are in preorder (an inner node's left child
## follows it), 32 bytes each. The top levels split on this thread and the
## subtrees build in parallel; a mesh whose positions moved over the same
## triangles refits the tree (new bounds, same structure) instead of
## rebuilding it. The index borrows the mesh arrays it was built over.
from luce_std import math
import memory
from luce_std import parallel
from core.float3 import Float3
from core.growing import Growing
from math.vector import Vector3
from geometries.distance import bounds_distance, triangle_distance, triangle_distance_below
## Leaves hold at most this many triangles.
let leaf_size: usize = 8
struct Node:
var lo: Float3
var hi: Float3
## Leaf: the first triangle in `order`. Inner: the right child.
var first: u32
## Leaf: its triangles (at least one). Inner: 0.
var count: u32
pub struct RayHit:
pub let face: i64 = -1
pub let distance: f64 = -1.0
pub struct TriangleIndex:
var positions: const Float3[]
var corners: const i32[]
var triangles: const i32[]
var faces: const i32[]
var order: u32[]
var nodes: Node[]
## Build over triangles `triangles` (corner ids) of `positions`.
pub func init(positions: const Float3[], corners: const i32[], triangles: const i32[], faces: const i32[]) -> !:
self.positions = positions
self.corners = corners
self.triangles = triangles
self.faces = faces
let count = triangles.length // 3
let order = try new u32[count] --- in memory.heap
errdefer free(order) in memory.heap
let lo = try new Float3[count] --- in memory.heap
defer free(lo) in memory.heap
let hi = try new Float3[count] --- in memory.heap
defer free(hi) in memory.heap
var boxes = BoxWork(positions, corners, triangles, lo, hi, order)
try parallel.for_each_range(count, 16384, (void*)&boxes, fill_boxes)
var builder = Builder(lo, hi, order)
defer builder.close()
let nodes = try builder.build()
self.order = order
self.nodes = nodes
## A tree over the same triangles for moved `positions`: this tree's
## structure with every bounds recomputed. O(triangles), in parallel.
## This tree over moved `positions` and `triangles` with the same slots
## (a folded face may have re-triangulated): only the boxes change.
pub func refit(positions: const Float3[], triangles: const i32[]) -> TriangleIndex!:
let order = try new u32[self.order.length] --- in memory.heap
errdefer free(order) in memory.heap
memory.copy(order, self.order, self.order.length)
let nodes = try new Node[self.nodes.length] --- in memory.heap
errdefer free(nodes) in memory.heap
memory.copy(nodes, self.nodes, self.nodes.length)
var work = RefitWork(positions, self.corners, triangles, order, nodes)
try parallel.for_each_range(nodes.length, 8192, (void*)&work, refit_leaves)
# Children follow their parent, so a reverse pass sees them first.
var at = nodes.length
while at > 0:
at -= 1
let node = &nodes[at]
if node.count > 0: continue
let left = nodes[at + 1]
let right = nodes[(usize)node.first]
node.lo = low(left.lo, right.lo)
node.hi = high(left.hi, right.hi)
var result = self
result.positions = positions
result.triangles = triangles
result.order = order
result.nodes = nodes
return result
func point(triangle: usize, corner: usize) -> Vector3:
return self.positions[(usize)self.corners[(usize)self.triangles[triangle * 3 + corner]]].vector()
func face_of(triangle: usize) -> i64:
return (i64)self.faces[(usize)self.triangles[triangle * 3]]
pub func ray(origin: Vector3, direction: Vector3) -> RayHit:
if self.nodes.length == 0 or not origin.is_finite() or not direction.is_finite():
return RayHit()
var nearest = 1.0e30
var face: i64 = -1
var stack: u32[64]
var count: usize = 1
stack[0] = 0
while count > 0:
count -= 1
let id = (usize)stack[count]
let node = &self.nodes[id]
if not hits_bounds(origin, direction, node.lo.vector(), node.hi.vector(), nearest):
continue
if node.count == 0:
stack[count] = node.first
stack[count + 1] = (u32)(id + 1)
count += 2
continue
for at in node.first..<node.first + node.count:
let triangle = (usize)self.order[(usize)at]
let distance = hit_triangle(origin, direction, self.point(triangle, 0), self.point(triangle, 1), self.point(triangle, 2))
let candidate = self.face_of(triangle)
if distance >= 0.0 and (distance < nearest or (distance == nearest and candidate < face)):
nearest = distance
face = candidate
return RayHit(face, nearest if face >= 0 else -1.0)
## Exact nearest distance to the triangulated mesh, using AABB pruning.
pub func distance(point: Vector3) -> f64:
return self.nearest(point).distance
pub func nearest(point: Vector3) -> RayHit:
return self.nearest_within(point, 1.0e150)
## The nearest face closer than `bound`, or face -1: a caller that knows
## a face at `bound` (a neighboring query's) prunes most of the tree.
pub func nearest_within(point: Vector3, bound: f64) -> RayHit:
if self.nodes.length == 0: return RayHit()
var best: f64 = bound * bound
var face: i64 = -1
# Nodes to visit with their box distances, the nearer child on top: each box
# is measured once, and a node is dropped when the best has passed it since.
var stack: u32[64]
var reach: f64[64]
var count: usize = 1
stack[0] = 0
reach[0] = 0.0
while count > 0:
count -= 1
if reach[count] >= best: continue
let id = (usize)stack[count]
let node = &self.nodes[id]
if node.count == 0:
let a = &self.nodes[id + 1]
let b = &self.nodes[(usize)node.first]
let near_a = box_distance(point, a)
let near_b = box_distance(point, b)
let closer = near_a < near_b
stack[count] = node.first if closer else (u32)(id + 1)
reach[count] = near_b if closer else near_a
stack[count + 1] = (u32)(id + 1) if closer else node.first
reach[count + 1] = near_a if closer else near_b
count += 2
else:
for at in node.first..<node.first + node.count:
let triangle = (usize)self.order[(usize)at]
let distance = triangle_distance_below(point, self.point(triangle, 0), self.point(triangle, 1), self.point(triangle, 2), best)
if distance < best:
best = distance
face = self.face_of(triangle)
return RayHit(face, math.sqrt(best) if face >= 0 else -1.0)
pub func close():
free(self.order) in memory.heap
if self.nodes.length > 0: free(self.nodes) in memory.heap
## `bounds_distance` to a node's box, written out for the nearest search's inner loop.
func box_distance(p: Vector3, node: const Node*) -> f64:
let x = math.max(math.max((f64)node.lo.x - p.x, p.x - (f64)node.hi.x), 0.0)
let y = math.max(math.max((f64)node.lo.y - p.y, p.y - (f64)node.hi.y), 0.0)
let z = math.max(math.max((f64)node.lo.z - p.z, p.z - (f64)node.hi.z), 0.0)
return x * x + y * y + z * z
# mark: Building ==============================================================================
## A range of `order` to build a subtree over.
struct Task:
var first: usize
var end: usize
var nodes: Growing[Node]
## Top-down median-of-extent splits: the top levels here, subtrees of at most
## `task_size` triangles on the pool, spliced back in preorder.
struct Builder:
let lo: const Float3[]
let hi: const Float3[]
let order: u32[]
var top: Growing[Node]
## Top-level leaves standing for tasks: their `first` is the task index.
var placeholders: Growing[bool]
var tasks: Growing[Task]
func init(lo: const Float3[], hi: const Float3[], order: u32[]):
self.lo = lo
self.hi = hi
self.order = order
self.top = Growing[Node]()
self.placeholders = Growing[bool]()
self.tasks = Growing[Task]()
func build() -> Node[]!:
let count = self.order.length
var empty: Node[]
if count == 0: return empty
let task_size = count // 64 + leaf_size
_ = try self.split_top(0, count, 0, task_size)
var work = TaskWork(self.lo, self.hi, self.order, self.tasks.items[..<self.tasks.count])
try parallel.for_each_range(self.tasks.count, 1, (void*)&work, build_tasks)
var total: usize = 0
for at in 0..<self.top.count:
total += self.tasks.items[(usize)self.top.items[at].first].nodes.count if self.placeholders.items[at] else 1
let nodes = try new Node[total] --- in memory.heap
var written: usize = 0
self.splice(0, nodes, &written)
return nodes
## Returns the node index in `top`.
func split_top(first: usize, end: usize, depth: usize, task_size: usize) -> usize!:
let id = self.top.count
let bounds = extent(self.lo, self.hi, self.order, first, end)
if end - first <= task_size or depth >= 12:
try self.top.push(Node(bounds.0, bounds.1, (u32)self.tasks.count, 0))
try self.placeholders.push(true)
try self.tasks.push(Task(first, end, Growing[Node]()))
return id
try self.top.push(Node(bounds.0, bounds.1, 0, 0))
try self.placeholders.push(false)
let middle = partition(self.lo, self.hi, self.order, first, end, bounds.0, bounds.1)
_ = try self.split_top(first, middle, depth + 1, task_size)
let right = try self.split_top(middle, end, depth + 1, task_size)
self.top.items[id].first = (u32)right
return id
## Write top node `id`'s subtree into `output` in preorder.
func splice(id: usize, output: Node[], written: usize*):
if self.placeholders.items[id]:
let task = &self.tasks.items[(usize)self.top.items[id].first]
let base = *written
for at in 0..<task.nodes.count:
var node = task.nodes.items[at]
if node.count == 0: node.first += (u32)base
output[base + at] = node
*written = base + task.nodes.count
return
let here = *written
output[here] = self.top.items[id]
*written = here + 1
self.splice(id + 1, output, written)
output[here].first = (u32)*written
self.splice((usize)self.top.items[id].first, output, written)
func close():
for at in 0..<self.tasks.count: self.tasks.items[at].nodes.close()
self.tasks.close()
self.top.close()
self.placeholders.close()
struct TaskWork:
let lo: const Float3[]
let hi: const Float3[]
let order: u32[]
let tasks: Task[]
func build_tasks(context: void*, first: usize, end: usize, worker: usize) -> !:
let work = (TaskWork*)context
for index in first..<end:
let task = &work.tasks[index]
_ = try build_subtree(work.lo, work.hi, work.order, task.first, task.end, 0, &task.nodes)
## Preorder subtree over order[first..<end] appended to `nodes`; returns its index.
func build_subtree(lo: const Float3[], hi: const Float3[], order: u32[], first: usize, end: usize, depth: usize, nodes: Growing[Node]*) -> usize!:
let id = nodes.count
let bounds = extent(lo, hi, order, first, end)
if end - first <= leaf_size or depth >= 48:
try nodes.push(Node(bounds.0, bounds.1, (u32)first, (u32)(end - first)))
return id
try nodes.push(Node(bounds.0, bounds.1, 0, 0))
let middle = partition(lo, hi, order, first, end, bounds.0, bounds.1)
_ = try build_subtree(lo, hi, order, first, middle, depth + 1, nodes)
let right = try build_subtree(lo, hi, order, middle, end, depth + 1, nodes)
nodes.items[id].first = (u32)right
return id
## Split at the middle of the longest axis of the range's bounds, by box
## centers; an empty side falls back to halving the range.
func partition(lo: const Float3[], hi: const Float3[], order: u32[], first: usize, end: usize, low_bound: Float3, high_bound: Float3) -> usize:
let dx = high_bound.x - low_bound.x
let dy = high_bound.y - low_bound.y
let dz = high_bound.z - low_bound.z
let axis: usize = 0 if dx >= dy and dx >= dz else (1 if dy >= dz else 2)
let split = (component(low_bound, axis) + component(high_bound, axis)) * 0.5
var middle = first
for at in first..<end:
let triangle = (usize)order[at]
if (component(lo[triangle], axis) + component(hi[triangle], axis)) * 0.5 < split:
let swap = order[middle]
order[middle] = order[at]
order[at] = swap
middle += 1
if middle == first or middle == end:
middle = first + (end - first) // 2
return middle
func extent(lo: const Float3[], hi: const Float3[], order: const u32[], first: usize, end: usize) -> (Float3, Float3):
var a = lo[(usize)order[first]]
var b = hi[(usize)order[first]]
for at in first + 1..<end:
let triangle = (usize)order[at]
a = low(a, lo[triangle])
b = high(b, hi[triangle])
return (a, b)
struct BoxWork:
let positions: const Float3[]
let corners: const i32[]
let triangles: const i32[]
let lo: Float3[]
let hi: Float3[]
let order: u32[]
func fill_boxes(context: void*, first: usize, end: usize, worker: usize):
let work = (BoxWork*)context
for triangle in first..<end:
let a = work.positions[(usize)work.corners[(usize)work.triangles[triangle * 3]]]
let b = work.positions[(usize)work.corners[(usize)work.triangles[triangle * 3 + 1]]]
let c = work.positions[(usize)work.corners[(usize)work.triangles[triangle * 3 + 2]]]
work.lo[triangle] = low(a, low(b, c))
work.hi[triangle] = high(a, high(b, c))
work.order[triangle] = (u32)triangle
struct RefitWork:
let positions: const Float3[]
let corners: const i32[]
let triangles: const i32[]
let order: const u32[]
let nodes: Node[]
func refit_leaves(context: void*, first: usize, end: usize, worker: usize):
let work = (RefitWork*)context
for id in first..<end:
let node = &work.nodes[id]
if node.count == 0: continue
var a = Float3(1.0e30, 1.0e30, 1.0e30)
var b = Float3(-1.0e30, -1.0e30, -1.0e30)
for at in node.first..<node.first + node.count:
let triangle = (usize)work.order[(usize)at]
for corner in 0..<(usize)3:
let p = work.positions[(usize)work.corners[(usize)work.triangles[triangle * 3 + corner]]]
a = low(a, p)
b = high(b, p)
node.lo = a
node.hi = b
func low(a: Float3, b: Float3) -> Float3:
return Float3(a.x if a.x < b.x else b.x, a.y if a.y < b.y else b.y, a.z if a.z < b.z else b.z)
func high(a: Float3, b: Float3) -> Float3:
return Float3(a.x if a.x > b.x else b.x, a.y if a.y > b.y else b.y, a.z if a.z > b.z else b.z)
func component(v: Float3, axis: usize) -> f32:
return v.x if axis == 0 else (v.y if axis == 1 else v.z)
func hits_bounds(origin: Vector3, direction: Vector3, minimum: Vector3, maximum: Vector3, nearest: f64) -> bool:
var near = 0.0
var far = nearest
for axis in 0..<(usize)3:
let o = origin.coordinate(axis)
let d = direction.coordinate(axis)
let lo = minimum.coordinate(axis) - 0.000001
let hi = maximum.coordinate(axis) + 0.000001
if math.abs(d) < 1.0e-15:
if o < lo or o > hi:
return false
else:
let a = (lo - o) / d
let b = (hi - o) / d
near = math.max(near, math.min(a, b))
far = math.min(far, math.max(a, b))
if far < near:
return false
return true
func hit_triangle(origin: Vector3, direction: Vector3, a: Vector3, b: Vector3, c: Vector3) -> f64:
let ab = b.subtract(a)
let ac = c.subtract(a)
let p = direction.cross(ac)
let determinant = ab.dot(p)
if math.abs(determinant) < 0.000000001:
return -1.0
let inverse = 1.0 / determinant
let offset = origin.subtract(a)
let u = offset.dot(p) * inverse
if u < -0.000001 or u > 1.000001:
return -1.0
let q = offset.cross(ab)
let v = direction.dot(q) * inverse
if v < -0.000001 or u + v > 1.000001:
return -1.0
let distance = ac.dot(q) * inverse
return distance if distance >= 0.0 else -1.0