dymokomi/luce-geocore
Source of release 0.7.3
0.7.3 / src / fields / mesh_distance.lucb
## A closed mesh's signed distance as a narrow-band level set.
##
## Distances are rasterized per leaf, not queried per voxel. Each triangle
## is binned into the leaves its band-grown box touches. A leaf then walks
## its triangles over the voxels of each grown box, keeping the nearest
## point and which feature (a vertex, an edge or the face) it lies on. Every
## voxel within the band has its true nearest triangle in its leaf's list:
## a triangle whose grown box misses the leaf is farther than the band from
## all of it.
##
## The sign is the side of the nearest feature's angle-weighted
## pseudo-normal (Bærentzen and Aanæs 2005): a vertex's is its faces'
## normals weighted by their angles there, an edge's is its two faces'
## normals summed, a face's is its normal. This is exact for closed,
## consistently oriented meshes, including at saddle vertices and concave
## edges, where the nearest face's own normal is not.
##
## Voxels of a touched leaf beyond the band take the sign of a neighbor
## (a flood inside the leaf; no surface lies between them), and a region
## no band voxel reaches asks the mesh's BVH once.
##
## A voxel visit first tests the triangle's box against the voxel's best
## distance so far. A mesh with more than `dense_triangles` triangles per
## band voxel (triangles far smaller than voxels) is cheaper by a BVH query
## per voxel instead. Leaves, then their 4×4×4 blocks, beyond the band are
## filled from one query at their center, and a voxel next to one that is
## more than a voxel beyond the band needs no query of its own (the field
## is 1-Lipschitz).
## Triangles per band voxel above which the BVH answers each voxel. The
## 700k-quad plane at 256 voxels has about 3 (queries: 0.46 s, rasterizing
## 1.6 s); camera.step's tessellation about 1.2 and a 64-segment sphere 0.01
## at 0.01 voxels (rasterizing: 2.8 s and 57 ms).
let dense_triangles: f64 = 2.0
## The signed distance of `mesh` as a level set of voxels `voxel` apart,
## `band` voxels each side.
pub func mesh_level_set(mesh: const Mesh*, voxel: f64, band: f64, name: str) -> SparseGrid*!:
try check_voxel(voxel, band)
if mesh.face_count() == 0: error(errors.invalid, "the mesh has no faces")
let dense = (f64)(mesh.offset_span()[mesh.face_count()] - (i32)(mesh.face_count() * 2)) > dense_triangles * (try band_voxels(mesh, voxel, band))
# A dense mesh is answered by BVH queries: its index is built first, in
# parallel (built inside a pool item it would build serially).
if dense: try mesh.prepare_queries()
var shape = try MeshShape.of(mesh)
defer shape.close()
if dense: return try queried_level_set(mesh, &shape, voxel, band, name)
let reach = band * voxel
let triangles = shape.corners.count // 3
# Leaves each triangle's grown box touches: count, then list.
let counts = try new usize[triangles + 1] --- in memory.heap
defer free(counts) in memory.heap
var count_work = BinWork(&shape, voxel, reach, counts, nothing[u64](), nothing[u64](), 0)
try parallel.for_each_range(triangles, 1024, (void*)&count_work, bin_triangles)
var total: usize = 0
for triangle in 0..<triangles:
let count = counts[triangle]
counts[triangle] = total
total += count
counts[triangles] = total
if total > 400000000: error(errors.limit, "the voxels are too small for this mesh")
let pair_keys = try new u64[total] --- in memory.heap
defer free(pair_keys) in memory.heap
let pair_triangles = try new u64[total] --- in memory.heap
defer free(pair_triangles) in memory.heap
var list_work = BinWork(&shape, voxel, reach, counts, pair_keys, pair_triangles, 1)
try parallel.for_each_range(triangles, 1024, (void*)&list_work, bin_triangles)
# Pairs sorted by leaf: each leaf's triangles are one run.
try sort_pairs(pair_keys, pair_triangles)
var leaves = Growing[u64]()
defer leaves.close()
var starts = Growing[usize]()
defer starts.close()
for at in 0..<total:
let key = pair_keys[at]
if leaves.count == 0 or leaves.items[leaves.count - 1] != key:
try leaves.push(key)
try starts.push(at)
try starts.push(total)
var fill = RasterWork(mesh, &shape, voxel, band, pair_triangles, starts.view())
return try grid_of_leaves(name, GridClass.level_set, (f32)reach, voxel_transform(voxel), leaves.view(), fill_raster_leaf, (void*)&fill)
## About how many voxels the band holds: the surface's area in voxel faces
## times the band's thickness (face areas summed in parallel chunks).
func band_voxels(mesh: const Mesh*, voxel: f64, band: f64) -> f64!:
let faces = (usize)mesh.face_count()
var parts: f64[64] = ---
for at in 0..<(usize)64: parts[at] = 0.0
var work = AreaWork(mesh.face_normal_span(), mesh.offset_span(), mesh.corner_span(), mesh.point_span(), (faces + 63) // 64, parts)
try parallel.for_each(64, (void*)&work, sum_areas)
var area = 0.0
for part in work.parts: area += part
return area / (voxel * voxel) * (2.0 * band + 1.0)
struct AreaWork:
let normals: const Float3[]
let offsets: const i32[]
let corners: const i32[]
let points: const Float3[]
let chunk: usize
var parts: f64[64]
## Polygon areas (Newell's vector along the face normal), a chunk each.
func sum_areas(context: void*, index: usize, worker: usize):
let work = (AreaWork*)context
let faces = work.offsets.length - 1
let first = index * work.chunk
let end = first + work.chunk if first + work.chunk < faces else faces
var area = 0.0
for face in first..<end:
let start = (usize)work.offsets[face]
let size = (usize)work.offsets[face + 1] - start
var sum = Vector3()
for at in 0..<size:
sum = sum.add(work.points[(usize)work.corners[start + at]].vector().cross(work.points[(usize)work.corners[start + (at + 1) % size]].vector()))
area += math.abs(sum.dot(work.normals[face].vector())) * 0.5
work.parts[index] = area
## The level set by a BVH query per voxel of the leaves the triangles'
## grown boxes touch (for triangles far smaller than voxels).
func queried_level_set(mesh: const Mesh*, shape: const MeshShape*, voxel: f64, band: f64, name: str) -> SparseGrid*!:
let reach = band * voxel
let size = voxel * 8.0
let triangles = shape.corners.count // 3
# The leaves the triangles' grown boxes touch. Neighboring triangles
# of a dense mesh touch the same leaves: a triangle whose leaf box is its
# predecessor's lists none.
let counts = try new usize[triangles + 1] --- in memory.heap
defer free(counts) in memory.heap
var count_work = BinWork(shape, voxel, reach, counts, nothing[u64](), nothing[u64](), 3)
try parallel.for_each_range(triangles, 4096, (void*)&count_work, bin_triangles)
var total: usize = 0
for triangle in 0..<triangles:
let count = counts[triangle]
counts[triangle] = total
total += count
counts[triangles] = total
if total > 400000000: error(errors.limit, "the voxels are too small for this mesh")
let listed = try new u64[total] --- in memory.heap
defer free(listed) in memory.heap
var list_work = BinWork(shape, voxel, reach, counts, listed, nothing[u64](), 4)
try parallel.for_each_range(triangles, 4096, (void*)&list_work, bin_triangles)
try sort_keys(listed)
var keys = Growing[u64]()
defer keys.close()
try keys.assign(listed[..<unique_keys(listed)])
var fill = RasterWork(mesh, shape, voxel, band, nothing[u64](), nothing[usize]())
return try grid_of_leaves(name, GridClass.level_set, (f32)reach, voxel_transform(voxel), keys.view(), fill_query_leaf, (void*)&fill)
func fill_query_leaf(context: void*?, leaf: usize, key: u64, values: f32[], mask: u64[]):
let work = (RasterWork*)(context else return)
_ = leaf
let (lx, ly, lz) = key_leaf(key)
query_leaf(work, lx, ly, lz, work.band * work.voxel, values, mask)
## An empty slice, for work fields a pass does not use.
func nothing[T]() -> T[]:
var empty: T[]
return empty
## A mesh's triangles in world coordinates, with the pseudo-normals of
## their features: per triangle its three point ids and the normals of its
## edges a-b, b-c, c-a (a polygon's diagonals take the face normal); per
## point its angle-weighted normal; per triangle its face.
struct MeshShape:
var points: Growing[Vector3]
var corners: Growing[i32]
var faces: Growing[i32]
var edge_normals: Growing[Vector3]
var point_normals: Growing[Vector3]
var face_normals: Growing[Vector3]
## Built in parallel: points and face normals, then point and edge
## normals (each gathered from its faces), then triangles.
static func of(mesh: const Mesh*) -> MeshShape!:
var shape = MeshShape(Growing[Vector3](), Growing[i32](), Growing[i32](), Growing[Vector3](), Growing[Vector3](), Growing[Vector3]())
errdefer shape.close()
let positions = mesh.point_span()
let offsets = mesh.offset_span()
let faces = offsets.length - 1
let triangles = (usize)offsets[faces] - faces * 2
try shape.points.fill(positions.length, Vector3())
try shape.face_normals.fill(faces, Vector3())
try shape.point_normals.fill(positions.length, Vector3())
try shape.corners.fill(triangles * 3, 0)
try shape.faces.fill(triangles, 0)
try shape.edge_normals.fill(triangles * 3, Vector3())
let (point_offsets, point_faces) = try mesh.point_face_span()
let (edge_offsets, edge_corners) = try mesh.edge_corner_span()
var edge_sums = Growing[Vector3]()
defer edge_sums.close()
try edge_sums.fill(edge_offsets.length - 1, Vector3())
var work = ShapeWork(&shape, positions, mesh.position_origin(), offsets, mesh.corner_span(), mesh.triangle_span(), try mesh.corner_face_span(), try mesh.corner_edge_span(),
point_offsets, point_faces, edge_offsets, edge_corners, edge_sums.items[..<edge_sums.count])
try parallel.for_each_range(positions.length, 65536, (void*)&work, shape_points)
try parallel.for_each_range(faces, 4096, (void*)&work, shape_faces)
try parallel.for_each_range(positions.length, 16384, (void*)&work, shape_point_normals)
try parallel.for_each_range(edge_sums.count, 16384, (void*)&work, shape_edge_normals)
try parallel.for_each_range(faces, 4096, (void*)&work, shape_triangles)
return shape
func close():
self.points.close()
self.corners.close()
self.faces.close()
self.edge_normals.close()
self.point_normals.close()
self.face_normals.close()
func vertex(triangle: usize, side: usize) -> Vector3:
return self.points.items[(usize)self.corners.items[triangle * 3 + side]]
struct ShapeWork:
let shape: MeshShape*
let positions: const Float3[]
let origin: Vector3
let offsets: const i32[]
let corner_points: const i32[]
let triangles: const i32[]
let corner_faces: const i32[]
let corner_edges: const i32[]
let point_offsets: const i32[]
let point_faces: const i32[]
let edge_offsets: const i32[]
let edge_corners: const i32[]
let edge_sums: Vector3[]
func shape_points(context: void*, first: usize, end: usize, worker: usize):
let work = (ShapeWork*)context
for point in first..<end: work.shape.points.items[point] = work.positions[point].vector().add(work.origin)
## Face normals from the polygons themselves (Newell), unit length.
func shape_faces(context: void*, first: usize, end: usize, worker: usize):
let work = (ShapeWork*)context
let points = work.shape.points.items
for face in first..<end:
var normal = Vector3()
let start = (usize)work.offsets[face]
let size = (usize)work.offsets[face + 1] - start
for at in 0..<size:
normal = normal.add(points[(usize)work.corner_points[start + at]].cross(points[(usize)work.corner_points[start + (at + 1) % size]]))
work.shape.face_normals.items[face] = normal.normalized() catch failure: recover Vector3()
## A point's faces' normals, each weighted by the face's angle there.
func shape_point_normals(context: void*, first: usize, end: usize, worker: usize):
let work = (ShapeWork*)context
let points = work.shape.points.items
for point in first..<end:
var sum = Vector3()
for at in (usize)work.point_offsets[point]..<(usize)work.point_offsets[point + 1]:
let face = (usize)work.point_faces[at]
let start = (usize)work.offsets[face]
let size = (usize)work.offsets[face + 1] - start
for corner in 0..<size:
if (usize)work.corner_points[start + corner] != point: continue
let here = points[point]
let before = points[(usize)work.corner_points[start + (corner + size - 1) % size]]
let after = points[(usize)work.corner_points[start + (corner + 1) % size]]
sum = sum.add(work.shape.face_normals.items[face].multiply_scalar(corner_angle(before.subtract(here), after.subtract(here))))
break
work.shape.point_normals.items[point] = sum
## An edge's faces' normals, summed.
func shape_edge_normals(context: void*, first: usize, end: usize, worker: usize):
let work = (ShapeWork*)context
for edge in first..<end:
var sum = Vector3()
for at in (usize)work.edge_offsets[edge]..<(usize)work.edge_offsets[edge + 1]:
sum = sum.add(work.shape.face_normals.items[(usize)work.corner_faces[(usize)work.edge_corners[at]]])
work.edge_sums[edge] = sum
## A face's triangles (after the faces before it), each edge's normal
## found by its corners: a polygon edge's, or a diagonal's face normal.
func shape_triangles(context: void*, first: usize, end: usize, worker: usize):
let work = (ShapeWork*)context
for face in first..<end:
let start = (usize)work.offsets[face]
let size = (usize)work.offsets[face + 1] - start
let base = (start - face * 2) * 3
for triangle in 0..<size - 2:
let index = start - face * 2 + triangle
var sides: usize[3] = [0, 0, 0]
for side in 0..<(usize)3:
let corner = (usize)work.triangles[base + triangle * 3 + side]
sides[side] = corner - start
work.shape.corners.items[index * 3 + side] = work.corner_points[corner]
work.shape.faces.items[index] = (i32)face
for side in 0..<(usize)3:
let a = sides[side]
let b = sides[(side + 1) % 3]
var normal = work.shape.face_normals.items[face]
if (a + 1) % size == b: normal = work.edge_sums[(usize)work.corner_edges[start + a]]
elif (b + 1) % size == a: normal = work.edge_sums[(usize)work.corner_edges[start + b]]
work.shape.edge_normals.items[index * 3 + side] = normal
## The angle between two directions.
func corner_angle(a: Vector3, b: Vector3) -> f64:
let la = a.length()
let lb = b.length()
if la <= 0.0 or lb <= 0.0: return 0.0
return math.acos(math.clamp(a.dot(b) / (la * lb), -1.0, 1.0))
## Binning triangles into the leaves their band-grown boxes touch, in
## parallel passes over the triangles: count, prefix-sum (the caller),
## then list into each triangle's range.
struct BinWork:
let shape: const MeshShape*
let voxel: f64
let reach: f64
let counts: usize[]
let keys: u64[]
let triangles: u64[]
## 0 count, 1 list keys and triangles; 3 count and 4 list keys only,
## none for a triangle whose leaf box is its predecessor's.
let pass: u8
## Triangle `triangle`'s grown box in leaf coordinates.
func leaf_box(work: const BinWork*, triangle: usize) -> (i64, i64, i64, i64, i64, i64):
let size = work.voxel * 8.0
let (low, high) = triangle_box(work.shape, triangle)
return ((i64)math.floor((low.x - work.reach) / size), (i64)math.floor((low.y - work.reach) / size), (i64)math.floor((low.z - work.reach) / size),
(i64)math.floor((high.x + work.reach) / size), (i64)math.floor((high.y + work.reach) / size), (i64)math.floor((high.z + work.reach) / size))
## One pass of BinWork over triangles first..<end.
func bin_triangles(context: void*, first: usize, end: usize, worker: usize):
let work = (BinWork*)context
for triangle in first..<end:
let (lx, ly, lz, hx, hy, hz) = leaf_box(work, triangle)
if work.pass >= 3 and triangle > 0:
let (px, py, pz, qx, qy, qz) = leaf_box(work, triangle - 1)
if px == lx and py == ly and pz == lz and qx == hx and qy == hy and qz == hz:
if work.pass == 3: work.counts[triangle] = 0
continue
if work.pass == 0 or work.pass == 3:
work.counts[triangle] = (usize)((hx - lx + 1) * (hy - ly + 1) * (hz - lz + 1))
continue
var at = work.counts[triangle]
for z in lz..=hz:
for y in ly..=hy:
for x in lx..=hx:
work.keys[at] = leaf_key(x, y, z)
if work.pass == 1: work.triangles[at] = (u64)triangle
at += 1
## The world box around triangle `triangle`.
func triangle_box(shape: const MeshShape*, triangle: usize) -> (Vector3, Vector3):
let a = shape.vertex(triangle, 0)
let b = shape.vertex(triangle, 1)
let c = shape.vertex(triangle, 2)
return (Vector3(math.min(a.x, math.min(b.x, c.x)), math.min(a.y, math.min(b.y, c.y)), math.min(a.z, math.min(b.z, c.z))),
Vector3(math.max(a.x, math.max(b.x, c.x)), math.max(a.y, math.max(b.y, c.y)), math.max(a.z, math.max(b.z, c.z))))
struct RasterWork:
let mesh: const Mesh*
let shape: const MeshShape*
let voxel: f64
let band: f64
let pair_triangles: const u64[]
let starts: const usize[]
func fill_raster_leaf(context: void*?, leaf: usize, key: u64, values: f32[], mask: u64[]):
let work = (RasterWork*)(context else return)
let (lx, ly, lz) = key_leaf(key)
let limit = work.band * work.voxel
var best: f64[512] = ---
# 0 unknown, 1 outside, 2 inside.
var side: u8[512] = ---
for at in 0..<leaf_voxels:
best[at] = limit
side[at] = 0
for pair in work.starts[leaf]..<work.starts[leaf + 1]:
let triangle = (usize)work.pair_triangles[pair]
let (low, high) = triangle_box(work.shape, triangle)
let x0 = math.imax(0, (i64)math.ceil((low.x - limit) / work.voxel) - lx * 8)
let x1 = math.imin(7, (i64)math.floor((high.x + limit) / work.voxel) - lx * 8)
let y0 = math.imax(0, (i64)math.ceil((low.y - limit) / work.voxel) - ly * 8)
let y1 = math.imin(7, (i64)math.floor((high.y + limit) / work.voxel) - ly * 8)
let z0 = math.imax(0, (i64)math.ceil((low.z - limit) / work.voxel) - lz * 8)
let z1 = math.imin(7, (i64)math.floor((high.z + limit) / work.voxel) - lz * 8)
let a = work.shape.vertex(triangle, 0)
let b = work.shape.vertex(triangle, 1)
let c = work.shape.vertex(triangle, 2)
for z in z0..=z1:
for y in y0..=y1:
for x in x0..=x1:
let at = (usize)(x + y * 8 + z * 64)
let p = Vector3((f64)(lx * 8 + x) * work.voxel, (f64)(ly * 8 + y) * work.voxel, (f64)(lz * 8 + z) * work.voxel)
# The box's distance bounds the triangle's: most dense
# triangles lose here, before the exact test.
if math.sqrt(bounds_distance(p, low, high)) >= best[at]: continue
let found = closest_feature(p, a, b, c)
let d = found.point.subtract(p).length()
if d >= best[at]: continue
best[at] = d
let normal = feature_normal(work.shape, triangle, found.feature)
side[at] = 2 if p.subtract(found.point).dot(normal) < 0.0 else 1
# Beyond the band: a neighbor's side; a region no band voxel reaches
# asks the BVH.
flood_sides(&side)
for at in 0..<leaf_voxels:
if side[at] != 0: continue
let p = Vector3((f64)(lx * 8 + (i64)(at & 7)) * work.voxel, (f64)(ly * 8 + (i64)((at >> 3) & 7)) * work.voxel, (f64)(lz * 8 + (i64)(at >> 6)) * work.voxel)
side[at] = 2 if exact_inside(work, p) else 1
flood_sides(&side)
for at in 0..<mask_words: mask[at] = 0
for at in 0..<leaf_voxels:
let d = best[at]
values[at] = (f32)(-d if side[at] == 2 else d)
if d < limit: mask[at >> 6] = mask[at >> 6] | ((u64)1 << (u64)(at & 63))
## Every voxel of a leaf by a BVH query: the exact signed distance. The
## leaf, then each of its eight 4×4×4 blocks, is first asked at its center:
## one farther from the surface than its half diagonal plus the band holds
## no band voxel and takes that one side and the clamped value. A voxel
## beside one at least a voxel beyond the band is beyond it too, on the same
## side (the distance changes by at most a voxel), and needs no query.
func query_leaf(work: const RasterWork*, lx: i64, ly: i64, lz: i64, limit: f64, values: f32[], mask: u64[]):
for at in 0..<mask_words: mask[at] = 0
let root3 = 1.7320508075688772
let middle = Vector3(((f64)(lx * 8) + 3.5) * work.voxel, ((f64)(ly * 8) + 3.5) * work.voxel, ((f64)(lz * 8) + 3.5) * work.voxel)
let middle_distance = exact_signed(work, middle)
if math.abs(middle_distance) > 3.5 * root3 * work.voxel + limit:
for at in 0..<leaf_voxels: values[at] = (f32)(limit if middle_distance > 0.0 else -limit)
return
# A lower bound of each voxel's distance, with its sign.
var raw: f64[512] = ---
var far: bool[8] = ---
var sides: f64[8] = ---
for block in 0..<(usize)8:
let bx = (i64)(block & 1) * 4
let by = (i64)((block >> 1) & 1) * 4
let bz = (i64)(block >> 2) * 4
let block_center = Vector3(((f64)(lx * 8 + bx) + 1.5) * work.voxel, ((f64)(ly * 8 + by) + 1.5) * work.voxel, ((f64)(lz * 8 + bz) + 1.5) * work.voxel)
let center_distance = exact_signed(work, block_center)
let bound = math.abs(center_distance) - 1.5 * root3 * work.voxel
far[block] = bound > limit
sides[block] = bound if center_distance > 0.0 else -bound
let beyond = limit + work.voxel
for at in 0..<leaf_voxels:
let x = (i64)(at & 7)
let y = (i64)((at >> 3) & 7)
let z = (i64)(at >> 6)
let block = (usize)(x >> 2) + (usize)(y >> 2) * 2 + (usize)(z >> 2) * 4
if far[block]:
raw[at] = sides[block]
continue
var known = 0.0
if x > 0 and math.abs(raw[at - 1]) >= beyond: known = raw[at - 1]
elif y > 0 and math.abs(raw[at - 8]) >= beyond: known = raw[at - 8]
elif z > 0 and math.abs(raw[at - 64]) >= beyond: known = raw[at - 64]
if known != 0.0:
raw[at] = known - work.voxel if known > 0.0 else known + work.voxel
continue
let p = Vector3((f64)(lx * 8 + x) * work.voxel, (f64)(ly * 8 + y) * work.voxel, (f64)(lz * 8 + z) * work.voxel)
raw[at] = exact_signed(work, p)
for at in 0..<leaf_voxels:
let d = raw[at]
values[at] = (f32)math.clamp(d, -limit, limit)
if math.abs(d) < limit: mask[at >> 6] = mask[at >> 6] | ((u64)1 << (u64)(at & 63))
## Spread known sides to unknown 6-neighbors until nothing changes.
func flood_sides(side: u8[512]*):
var changed = true
while changed:
changed = false
for at in 0..<leaf_voxels:
if (*side)[at] != 0: continue
let x = at & 7
let y = (at >> 3) & 7
let z = at >> 6
var found: u8 = 0
if x > 0 and (*side)[at - 1] != 0: found = (*side)[at - 1]
elif x < 7 and (*side)[at + 1] != 0: found = (*side)[at + 1]
elif y > 0 and (*side)[at - 8] != 0: found = (*side)[at - 8]
elif y < 7 and (*side)[at + 8] != 0: found = (*side)[at + 8]
elif z > 0 and (*side)[at - 64] != 0: found = (*side)[at - 64]
elif z < 7 and (*side)[at + 64] != 0: found = (*side)[at + 64]
if found != 0:
(*side)[at] = found
changed = true
## Whether `p` is inside, from its nearest face on the whole mesh (BVH).
func exact_inside(work: const RasterWork*, p: Vector3) -> bool:
return exact_signed(work, p) < 0.0
## The signed distance from `p` to the whole mesh: the nearest face by the
## BVH, the nearest point over its triangles, the side of that point's
## feature's pseudo-normal.
func exact_signed(work: const RasterWork*, p: Vector3) -> f64:
let face = work.mesh.nearest_face_within(p, 1.0e150) catch failure: return 1.0e30
if face < 0: return 1.0e30
return face_signed(work, (usize)face, p)
## The signed distance from `p` to face `face`: over its triangles, the
## side of the nearest point's feature's pseudo-normal.
func face_signed(work: const RasterWork*, face: usize, p: Vector3) -> f64:
# Face f's triangles follow those of the faces before it.
let offsets = work.mesh.offset_span()
var best = 1.0e300
var inside = false
for triangle in (usize)offsets[face] - face * 2..<(usize)offsets[face + 1] - (face + 1) * 2:
let found = closest_feature(p, work.shape.vertex(triangle, 0), work.shape.vertex(triangle, 1), work.shape.vertex(triangle, 2))
let d = found.point.subtract(p).length()
if d < best:
best = d
inside = p.subtract(found.point).dot(feature_normal(work.shape, triangle, found.feature)) < 0.0
return -best if inside else best
## The nearest point of triangle abc to `p` and the feature it lies on:
## 0..2 the vertices a, b, c; 3..5 the edges ab, bc, ca; 6 the face
## (Ericson, Real-Time Collision Detection 5.1.5). A triangle without area
## (meshes keep such faces) is its three edges.
pub struct Feature:
pub let point: Vector3
pub let feature: u8
pub func closest_feature(p: Vector3, a: Vector3, b: Vector3, c: Vector3) -> Feature:
let ab = b.subtract(a)
let ac = c.subtract(a)
let normal = ab.cross(ac)
if not (normal.dot(normal) > 0.0): return closest_edge(p, a, b, c)
let ap = p.subtract(a)
let d1 = ab.dot(ap)
let d2 = ac.dot(ap)
if d1 <= 0.0 and d2 <= 0.0: return Feature(a, 0)
let bp = p.subtract(b)
let d3 = ab.dot(bp)
let d4 = ac.dot(bp)
if d3 >= 0.0 and d4 <= d3: return Feature(b, 1)
let vc = d1 * d4 - d3 * d2
if vc <= 0.0 and d1 >= 0.0 and d3 <= 0.0: return Feature(a.add(ab.multiply_scalar(d1 / (d1 - d3))), 3)
let cp = p.subtract(c)
let d5 = ab.dot(cp)
let d6 = ac.dot(cp)
if d6 >= 0.0 and d5 <= d6: return Feature(c, 2)
let vb = d5 * d2 - d1 * d6
if vb <= 0.0 and d2 >= 0.0 and d6 <= 0.0: return Feature(a.add(ac.multiply_scalar(d2 / (d2 - d6))), 5)
let va = d3 * d6 - d5 * d4
if va <= 0.0 and (d4 - d3) >= 0.0 and (d5 - d6) >= 0.0:
return Feature(b.add(c.subtract(b).multiply_scalar((d4 - d3) / ((d4 - d3) + (d5 - d6)))), 4)
let denominator = 1.0 / (va + vb + vc)
return Feature(a.add(ab.multiply_scalar(vb * denominator)).add(ac.multiply_scalar(vc * denominator)), 6)
## The nearest point to `p` on the edges of a triangle without area.
func closest_edge(p: Vector3, a: Vector3, b: Vector3, c: Vector3) -> Feature:
var best = Feature(on_segment(p, a, b), 3)
let bc = Feature(on_segment(p, b, c), 4)
if bc.point.subtract(p).length() < best.point.subtract(p).length(): best = bc
let ca = Feature(on_segment(p, c, a), 5)
if ca.point.subtract(p).length() < best.point.subtract(p).length(): best = ca
return best
## The nearest point to `p` on segment ab (a when it has no length).
func on_segment(p: Vector3, a: Vector3, b: Vector3) -> Vector3:
let ab = b.subtract(a)
let length2 = ab.dot(ab)
if not (length2 > 0.0): return a
let t = math.max(0.0, math.min(1.0, p.subtract(a).dot(ab) / length2))
return a.add(ab.multiply_scalar(t))
## The pseudo-normal of `feature` of `triangle`.
func feature_normal(shape: const MeshShape*, triangle: usize, feature: u8) -> Vector3:
if feature < 3: return shape.point_normals.items[(usize)shape.corners.items[triangle * 3 + (usize)feature]]
if feature < 6: return shape.edge_normals.items[triangle * 3 + (usize)(feature - 3)]
return shape.face_normals.items[(usize)shape.faces.items[triangle]]