Luce packages
LuceLuce BaseLuciaOS

dymokomi/luce-geocore

Source of release 0.7.5

0.7.5 / 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