dymokomi/luce-geocore
Source of release 0.7.8
0.7.8 / 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, triangle_nearest_point
## 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
## The surface point nearest a query: its face (-1 for an empty tree), how
## far it is and where it is (in the tree's positions' frame).
pub struct SurfacePoint:
pub let face: i64 = -1
pub let distance: f64 = -1.0
pub let position: Vector3 = Vector3()
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)
## Every triangle whose box meets the box `lo`..`hi` (in the tree's
## frame), into `out` (cleared first): a self-intersection check's
## candidates.
pub func overlapping(lo: Vector3, hi: Vector3, out: Growing[u32]*) -> !:
out.clear()
if self.nodes.length == 0: return
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 (f64)node.lo.x > hi.x or (f64)node.lo.y > hi.y or (f64)node.lo.z > hi.z or (f64)node.hi.x < lo.x or (f64)node.hi.y < lo.y or (f64)node.hi.z < lo.z:
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:
try out.push(self.order[(usize)at])
## Mark in `hits` (one byte per triangle, zeroed by the caller) every
## triangle that crosses another of a different face sharing no point
## with it: a self-intersection check. Each leaf's box finds its
## candidates once for its few triangles, the leaves in parallel.
pub func self_crossings(hits: u8[]) -> !:
if hits.length != self.triangle_count(): trap("one hit byte per triangle")
var leaves = Growing[u32]()
defer leaves.close()
for id in 0..<self.nodes.length:
if self.nodes[id].count > 0: try leaves.push((u32)id)
var work = CrossWork(&self, leaves.view(), hits)
work.failed.store(0, .relaxed)
try parallel.for_each_range(leaves.count, 64, (void*)&work, cross_leaves)
if work.failed.load(.acquire) != 0: error(memory.exhausted, "no memory for the intersection check")
## How many triangles the tree holds; triangle `triangle`'s corner `k`
## (0..2) as a point index and as a position; and its face.
pub func triangle_count() -> usize:
return self.triangles.length // 3
## Triangle `triangle`'s three points.
func triangle_corners(triangle: usize) -> (usize, usize, usize):
let base = triangle * 3
return ((usize)self.corners[(usize)self.triangles[base]], (usize)self.corners[(usize)self.triangles[base + 1]], (usize)self.corners[(usize)self.triangles[base + 2]])
pub func corner_point(triangle: usize, k: usize) -> usize:
return (usize)self.corners[(usize)self.triangles[triangle * 3 + k]]
pub func corner_position(triangle: usize, k: usize) -> Vector3:
return self.point(triangle, k)
pub func triangle_face(triangle: usize) -> usize:
return (usize)self.face_of(triangle)
## 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:
let found = self.nearest_triangle(point, bound)
if found.0 < 0: return RayHit()
return RayHit(self.face_of((usize)found.0), math.sqrt(found.1))
## The surface point nearest `point`: Shrinkwrap's and Relax's
## projection onto a reference surface.
pub func nearest_point(point: Vector3) -> SurfacePoint:
return self.nearest_point_within(point, 1.0e150)
## The surface point nearest `point`, searched closer than `bound`
## first (a neighboring query's distance plus how far apart they are:
## most of the tree pruned), then everywhere when nothing is that close.
pub func nearest_point_within(point: Vector3, bound: f64) -> SurfacePoint:
var found = self.nearest_triangle(point, bound)
if found.0 < 0 and bound < 1.0e150: found = self.nearest_triangle(point, 1.0e150)
if found.0 < 0: return SurfacePoint()
let triangle = (usize)found.0
return SurfacePoint(self.face_of(triangle), math.sqrt(found.1), triangle_nearest_point(point, self.point(triangle, 0), self.point(triangle, 1), self.point(triangle, 2)))
## The triangle nearest `point` closer than `bound` (-1: none) and its
## squared distance.
func nearest_triangle(point: Vector3, bound: f64) -> (i64, f64):
if self.nodes.length == 0: return (-1, 0.0)
var best: f64 = bound * bound
var nearest: 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
nearest = (i64)triangle
return (nearest, best)
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
## What the crossing pass reads.
struct CrossWork:
let index: const TriangleIndex*
let leaves: const u32[]
let hits: u8[]
var failed: @u32
func cross_leaves(context: void*, first: usize, end: usize, worker: usize):
let work = (CrossWork*)context
let index = work.index
var candidates = Growing[u32]()
defer candidates.close()
# Each candidate's box and points, gathered once for the leaf's few
# triangles.
var boxes = Growing[Float3]()
defer boxes.close()
var points = Growing[u32]()
defer points.close()
for at in first..<end:
let node = &index.nodes[(usize)work.leaves[at]]
index.overlapping(node.lo.vector(), node.hi.vector(), &candidates) catch failure:
_ = work.failed.add(1, .relaxed)
return
let count = candidates.count
boxes.fill(count * 2, Float3(0.0, 0.0, 0.0)) catch failure:
_ = work.failed.add(1, .relaxed)
return
points.fill(count * 3, 0) catch failure:
_ = work.failed.add(1, .relaxed)
return
for k in 0..<count:
let u = (usize)candidates.items[k]
let corners = index.triangle_corners(u)
let d = index.positions[corners.0]
let e = index.positions[corners.1]
let f = index.positions[corners.2]
boxes.items[k * 2] = Float3(fmin(d.x, fmin(e.x, f.x)), fmin(d.y, fmin(e.y, f.y)), fmin(d.z, fmin(e.z, f.z)))
boxes.items[k * 2 + 1] = Float3(fmax(d.x, fmax(e.x, f.x)), fmax(d.y, fmax(e.y, f.y)), fmax(d.z, fmax(e.z, f.z)))
points.items[k * 3] = (u32)corners.0
points.items[k * 3 + 1] = (u32)corners.1
points.items[k * 3 + 2] = (u32)corners.2
for slot in node.first..<node.first + node.count:
let triangle = (usize)index.order[(usize)slot]
let own = index.triangle_corners(triangle)
let p0 = (u32)own.0
let p1 = (u32)own.1
let p2 = (u32)own.2
let a3 = index.positions[own.0]
let b3 = index.positions[own.1]
let c3 = index.positions[own.2]
let lo = Float3(fmin(a3.x, fmin(b3.x, c3.x)), fmin(a3.y, fmin(b3.y, c3.y)), fmin(a3.z, fmin(b3.z, c3.z)))
let hi = Float3(fmax(a3.x, fmax(b3.x, c3.x)), fmax(a3.y, fmax(b3.y, c3.y)), fmax(a3.z, fmax(b3.z, c3.z)))
let face = index.face_of(triangle)
for k in 0..<count:
let near = boxes.items[k * 2]
let far = boxes.items[k * 2 + 1]
if far.x < lo.x or near.x > hi.x or far.y < lo.y or near.y > hi.y or far.z < lo.z or near.z > hi.z: continue
# Triangles sharing a point meet there; they are not counted
# (a triangle shares all three with itself).
let q0 = points.items[k * 3]
let q1 = points.items[k * 3 + 1]
let q2 = points.items[k * 3 + 2]
if q0 == p0 or q0 == p1 or q0 == p2 or q1 == p0 or q1 == p1 or q1 == p2 or q2 == p0 or q2 == p1 or q2 == p2: continue
let u = (usize)candidates.items[k]
if index.face_of(u) == face: continue
if triangles_cross(a3.vector(), b3.vector(), c3.vector(), index.positions[(usize)q0].vector(), index.positions[(usize)q1].vector(), index.positions[(usize)q2].vector()):
work.hits[triangle] = 1
break
func fmin(a: f32, b: f32) -> f32:
return a if a < b else b
func fmax(a: f32, b: f32) -> f32:
return a if a > b else b
## Whether triangles abc and def cross: an edge of one passes through the
## other (coplanar overlaps are not counted).
func triangles_cross(a: Vector3, b: Vector3, c: Vector3, d: Vector3, e: Vector3, f: Vector3) -> bool:
return segment_crosses(a, b, d, e, f) or segment_crosses(b, c, d, e, f) or segment_crosses(c, a, d, e, f) or segment_crosses(d, e, a, b, c) or segment_crosses(e, f, a, b, c) or segment_crosses(f, d, a, b, c)
## Whether segment pq passes through the inside of triangle abc
## (Möller and Trumbore's test, the ray cut to the segment).
func segment_crosses(p: Vector3, q: Vector3, a: Vector3, b: Vector3, c: Vector3) -> bool:
let along = q.subtract(p)
let ab = b.subtract(a)
let ac = c.subtract(a)
let h = along.cross(ac)
let det = ab.dot(h)
let scale = ab.length() * ac.length() * along.length()
if math.abs(det) <= 1.0e-12 * scale: return false
let inverse = 1.0 / det
let s = p.subtract(a)
let u = s.dot(h) * inverse
if u <= 0.0 or u >= 1.0: return false
let qv = s.cross(ab)
let v = along.dot(qv) * inverse
if v <= 0.0 or u + v >= 1.0: return false
let t = ac.dot(qv) * inverse
return t > 0.0 and t < 1.0