Luce packages
LuceLuce BaseLuciaOS

dymokomi/luce-geocore

Source of release 0.7.8

0.7.8 / src / geometries / distance.lucb

## Squared Euclidean distances, including degenerate reference triangles.
from luce_std import math
from math.vector import Vector3

pub func bounds_distance(p: Vector3, lo: Vector3, hi: Vector3) -> f64:
    let x = math.max(math.max(lo.x - p.x, p.x - hi.x), 0.0)
    let y = math.max(math.max(lo.y - p.y, p.y - hi.y), 0.0)
    let z = math.max(math.max(lo.z - p.z, p.z - hi.z), 0.0)
    return x * x + y * y + z * z

## `triangle_distance` when it is below `best`, otherwise any value at least `best`:
## the triangle's box and then its plane (both lower bounds, the plane's with a margin
## for rounding) skip the exact test for most triangles a nearest search meets.
pub func triangle_distance_below(p: Vector3, a: Vector3, b: Vector3, c: Vector3, best: f64) -> f64:
    # The box test written out: called per candidate, a call costs more than the test.
    let x = math.max(math.max(math.min(a.x, math.min(b.x, c.x)) - p.x, p.x - math.max(a.x, math.max(b.x, c.x))), 0.0)
    let y = math.max(math.max(math.min(a.y, math.min(b.y, c.y)) - p.y, p.y - math.max(a.y, math.max(b.y, c.y))), 0.0)
    let z = math.max(math.max(math.min(a.z, math.min(b.z, c.z)) - p.z, p.z - math.max(a.z, math.max(b.z, c.z))), 0.0)
    if x * x + y * y + z * z >= best: return best
    let normal = b.subtract(a).cross(c.subtract(a))
    let area2 = normal.dot(normal)
    if area2 > 0.0:
        let height = p.subtract(a).dot(normal)
        if height * height / area2 * 0.999999 >= best: return best
    return triangle_distance(p, a, b, c)

func segment(p: Vector3, a: Vector3, b: Vector3) -> f64:
    let ab = b.subtract(a)
    let length = ab.dot(ab)
    let t = math.clamp(p.subtract(a).dot(ab) / length, 0.0, 1.0) if length > 0.0 else 0.0
    let d = p.subtract(a.add(ab.multiply_scalar(t)))
    return d.dot(d)

## Squared distance using signed areas instead of a cancellation-prone Gram
## solve. Degenerate triangles reduce to their point/segment boundary.
pub func triangle_distance(p: Vector3, a: Vector3, b: Vector3, c: Vector3) -> f64:
    let ab = b.subtract(a)
    let ac = c.subtract(a)
    let bc = c.subtract(b)
    let boundary = math.min(segment(p,a,b),math.min(segment(p,b,c),segment(p,c,a)))
    # The Gram determinant |ab|²|ac|²-(ab.ac)² subtracts nearly equal
    # numbers on thin ears. Its barycentrics can return a distant interior
    # point even when p lies on an edge. Use oriented areas instead, anchored
    # opposite the longest side (the two shortest incident vectors).
    var origin = a
    var first = ab
    var second = ac
    if ab.dot(ab) >= ac.dot(ac) and ab.dot(ab) >= bc.dot(bc):
        origin = c
        first = ac.multiply_scalar(-1.0)
        second = bc.multiply_scalar(-1.0)
    elif ac.dot(ac) >= bc.dot(bc):
        origin = b
        first = bc
        second = ab.multiply_scalar(-1.0)
    let relative = p.subtract(origin)
    let normal = first.cross(second)
    let area2 = normal.dot(normal)
    if area2 > 0.0:
        let s = relative.cross(second).dot(normal)
        let t = first.cross(relative).dot(normal)
        # Test all three signed subareas without subtracting s+t from area2.
        let remaining = first.subtract(relative).cross(second.subtract(relative)).dot(normal)
        if s >= 0.0 and t >= 0.0 and remaining >= 0.0:
            let height = relative.dot(normal)
            # The boundary is always a valid candidate, including when the
            # projection is numerically ambiguous beside a very short side.
            return math.min(boundary,height*height/area2)
    return boundary

## The point of triangle (a, b, c) nearest `p` (Ericson's regions: a
## corner, a side, or inside), for a degenerate triangle the nearest point
## of its sides.
pub func triangle_nearest_point(p: Vector3, a: Vector3, b: Vector3, c: Vector3) -> Vector3:
    let ab = b.subtract(a)
    let ac = c.subtract(a)
    let ap = p.subtract(a)
    let d1 = ab.dot(ap)
    let d2 = ac.dot(ap)
    if d1 <= 0.0 and d2 <= 0.0: return a
    let bp = p.subtract(b)
    let d3 = ab.dot(bp)
    let d4 = ac.dot(bp)
    if d3 >= 0.0 and d4 <= d3: return b
    let vc = d1 * d4 - d3 * d2
    if vc <= 0.0 and d1 >= 0.0 and d3 <= 0.0 and d1 - d3 > 0.0: return a.add(ab.multiply_scalar(d1 / (d1 - d3)))
    let cp = p.subtract(c)
    let d5 = ab.dot(cp)
    let d6 = ac.dot(cp)
    if d6 >= 0.0 and d5 <= d6: return c
    let vb = d5 * d2 - d1 * d6
    if vb <= 0.0 and d2 >= 0.0 and d6 <= 0.0 and d2 - d6 > 0.0: return a.add(ac.multiply_scalar(d2 / (d2 - d6)))
    let va = d3 * d6 - d5 * d4
    if va <= 0.0 and d4 - d3 >= 0.0 and d5 - d6 >= 0.0 and (d4 - d3) + (d5 - d6) > 0.0:
        return b.add(c.subtract(b).multiply_scalar((d4 - d3) / ((d4 - d3) + (d5 - d6))))
    let total = va + vb + vc
    if total > 0.0 and math.is_finite(total):
        return a.add(ab.multiply_scalar(vb / total)).add(ac.multiply_scalar(vc / total))
    # Degenerate: the nearest of its sides' points.
    var best = nearest_on_segment(p, a, b)
    for other in [nearest_on_segment(p, b, c), nearest_on_segment(p, c, a)]:
        if other.subtract(p).length() < best.subtract(p).length(): best = other
    return best

func nearest_on_segment(p: Vector3, a: Vector3, b: Vector3) -> Vector3:
    let ab = b.subtract(a)
    let length = ab.dot(ab)
    let t = math.clamp(p.subtract(a).dot(ab) / length, 0.0, 1.0) if length > 0.0 else 0.0
    return a.add(ab.multiply_scalar(t))