Luce packages
LuceLuce BaseLuciaOS

dymokomi/luce-cad

Source of release 0.9.4

0.9.4 / src / modeling / march.lucb

## Where two surfaces meet when no closed form does (cylinders at an
## angle, cones at an angle, tori, B-spline surfaces): marching, after
## OpenCASCADE's IntWalk and SolveSpace's surfinter. Each surface is a
## field f = 0: an analytic surface's implicit function, a B-spline's
## signed distance to its nearest point (along its normal there, found by
## Newton from the last one); a curve on both runs along the cross product
## of their gradients. Start points are where one face's boundary or a grid
## over it crosses the other surface; each is traced both ways (a step
## along the curve, then Newton back onto both surfaces) until it leaves
## the faces' box or closes. Each traced curve becomes a cubic B-spline
## through its points (all on both surfaces), a carrier like any other.

## A surface as marching reads it, and for a B-spline the parameters of the
## nearest point found last (the next search starts there), once found.
struct Field:
    var s: const Surface*
    var seed: Vector3
    var seeded: bool

func field_of(s: const Surface*) -> Field:
    return Field(s, Vector3(0.5, 0.5, 0.0), false)

## The field's value at `p` (zero on the surface) and its gradient.
func implicit(field: Field*, p: Vector3) -> (f64, Vector3)!:
    let s = field.s
    if s.kind == surface_spline:
        # The signed distance to the nearest point, from the last one (a
        # lattice of starts the first time).
        let uv = try nearest_parameters(*s, p, field.seed, not field.seeded)
        field.seed = uv
        field.seeded = true
        let q = try evaluate(*s, uv)
        let n = try (try nurbs_normal(*s, uv)).normalized()
        return (p.subtract(q).dot(n), n)
    let d = p.subtract(s.origin)
    if s.kind == surface_plane: return (d.dot(s.z), s.z)
    if s.kind == surface_sphere: return (d.dot(d) - s.radius * s.radius, d.multiply_scalar(2.0))
    let h = d.dot(s.z)
    let q = d.subtract(s.z.multiply_scalar(h))
    if s.kind == surface_cylinder: return (q.dot(q) - s.radius * s.radius, q.multiply_scalar(2.0))
    let length = math.max(q.length(), nonzero)
    if s.kind == surface_cone:
        let slope = math.tan(s.angle)
        return (length - (s.radius + h * slope), q.multiply_scalar(1.0 / length).subtract(s.z.multiply_scalar(slope)))
    let ring = s.origin.add(q.multiply_scalar(s.radius / length))
    let offset = p.subtract(ring)
    return (offset.dot(offset) - s.angle * s.angle, offset.multiply_scalar(2.0))

## A B-spline field's search from a lattice again (a seed far from where
## the last one was found).
func unseed(field: Field*):
    field.seeded = false

## `p` moved onto both surfaces (Gauss-Newton, the least move), or none.
func onto_both(s1: Field*, s2: Field*, start: Vector3, tol: f64) -> Vector3?!:
    var p = start
    for _ in 0..<50:
        let (f1, g1) = try implicit(s1, p)
        let (f2, g2) = try implicit(s2, p)
        # Solve J J^T y = f for the 2x2, move by -J^T y.
        let a11 = g1.dot(g1)
        let a12 = g1.dot(g2)
        let a22 = g2.dot(g2)
        let det = a11 * a22 - a12 * a12
        if math.abs(det) < nonzero: return none
        let y1 = (f1 * a22 - f2 * a12) / det
        let y2 = (a11 * f2 - a12 * f1) / det
        let delta = g1.multiply_scalar(y1).add(g2.multiply_scalar(y2))
        p = p.subtract(delta)
        if delta.length() <= tol * 1e-3: return p
    let (f1, g1) = try implicit(s1, p)
    let (f2, g2) = try implicit(s2, p)
    if math.abs(f1) / math.max(g1.length(), nonzero) <= tol and math.abs(f2) / math.max(g2.length(), nonzero) <= tol: return p
    return none

## The curve's direction at `p`: across both gradients.
func curve_direction(s1: Field*, s2: Field*, p: Vector3) -> Vector3?!:
    let (_, g1) = try implicit(s1, p)
    let (_, g2) = try implicit(s2, p)
    let t = g1.cross(g2)
    # Gradients (nearly) parallel: the surfaces touch here, a point where
    # tracing ends.
    if t.length() <= g1.length() * g2.length() * 1e-10: return none
    return try t.normalized()

func in_box(p: Vector3, low: Vector3, high: Vector3) -> bool:
    return p.x >= low.x and p.y >= low.y and p.z >= low.z and p.x <= high.x and p.y <= high.y and p.z <= high.z

## Trace from `seed` along `heading` (or against it) onto `out`: false when
## it came back to its start (a closed curve).
func trace(s1: Field*, s2: Field*, seed: Vector3, heading: Vector3, step: f64, low: Vector3, high: Vector3, tol: f64, out: Growing[Vector3]*) -> bool!:
    var p = seed
    var t = heading
    for count in 0..<20000:
        let guess = p.add(t.multiply_scalar(step))
        let next = (try onto_both(s1, s2, guess, tol)) else return true
        let direction = (try curve_direction(s1, s2, next)) else return true
        try out.push(next)
        if count > 3 and next.subtract(seed).length() < step * 0.75: return false
        if not in_box(next, low, high): return true
        t = direction if direction.dot(t) > 0.0 else direction.multiply_scalar(-1.0)
        p = next
    return true

## The curves where faces fa and fb of `w` meet, traced, as spline
## carriers onto `out`; `low`..`high` bounds where both faces are.
pub func marched(w: Shape*, fa: usize, fb: usize, low: Vector3, high: Vector3, size: f64, tol: f64, out: Growing[Carrier]*) -> !:
    var f1 = field_of(&w.surfaces.items[(usize)w.faces.items[fa].surface])
    var f2 = field_of(&w.surfaces.items[(usize)w.faces.items[fb].surface])
    let s1 = &f1
    let s2 = &f2
    let step = size * 0.004
    # Start points: sign changes of each surface's function along the
    # other face's boundary and across a grid over it.
    var seeds = Growing[Vector3]()
    defer seeds.close()
    try boundary_seeds(w, fa, s2, s1, tol, &seeds)
    try boundary_seeds(w, fb, s1, s2, tol, &seeds)
    try grid_seeds(w, fa, s2, s1, tol, &seeds)
    try grid_seeds(w, fb, s1, s2, tol, &seeds)
    # Every point traced so far, to skip seeds on curves already traced.
    var traced = try new_point_cells(step * 2.0)
    defer traced.close()
    var back = Growing[Vector3]()
    defer back.close()
    var line = Growing[Vector3]()
    defer line.close()
    var net = Growing[Vector3]()
    defer net.close()
    var knots = Growing[f64]()
    defer knots.close()
    var weights = Growing[f64]()
    defer weights.close()
    # The traced curves' points, each curve's from its start, to tell a seed
    # on one of them from a seed on another branch close by.
    var polylines = Growing[Vector3]()
    defer polylines.close()
    var polyline_starts = Growing[usize]()
    defer polyline_starts.close()
    try polyline_starts.push(0)
    for seed in seeds.view():
        # A seed on a curve already traced adds nothing: near its points and
        # on its polyline (a chord strays from the curve by step² / 8r at
        # most; another branch a little way off is not skipped).
        if traced.near(seed) and on_polylines(seed, polylines.view(), polyline_starts.view(), math.max(tol * 10.0, step * 0.05)): continue
        # Each trace searches B-splines afresh from its seed.
        unseed(s1)
        unseed(s2)
        let heading = (try curve_direction(s1, s2, seed)) else continue
        # Traced finer until the cubic through the points stays on both
        # surfaces between them (a B-spline's curvature asks for more).
        var spacing = step
        for attempt in 0..<5:
            try traced_line(s1, s2, seed, heading, spacing, low, high, tol, &line, &back)
            if line.count < 4: break
            try interpolate_cubic(line.view(), &net, &knots)
            weights.clear()
            for _ in net.view(): try weights.push(1.0)
            if attempt == 4 or (try stays_on(s1, s2, net.view(), weights.view(), knots.view(), line.count * 4, tol)): break
            spacing *= 0.5
        if line.count < 4: continue
        for p in line.view():
            try traced.add(p)
            try polylines.push(p)
        try polyline_starts.push(polylines.count)
        let spline = try w.store_spline(net.view(), weights.view(), knots.view(), 3, Matrix4())
        try out.push(Carrier(2, Vector3(), Vector3(), Vector3(), 0.0, 0.0, spline))

## Whether `p` lies within `reach` of a segment of the polylines in `points`
## (each from `starts[k]` to `starts[k + 1]`).
func on_polylines(p: Vector3, points: const Vector3[], starts: const usize[], reach: f64) -> bool:
    for k in 0..<starts.length - 1:
        for at in starts[k] + 1..<starts[k + 1]:
            if segment_distance(points[at - 1], points[at], p, p) <= reach: return true
    return false

## The curve through `seed` on both fields, traced both ways (or round
## once, closed) `step` apart, onto `line` (`back` is scratch).
func traced_line(s1: Field*, s2: Field*, seed: Vector3, heading: Vector3, step: f64, low: Vector3, high: Vector3, tol: f64, line: Growing[Vector3]*, back: Growing[Vector3]*) -> !:
    line.clear()
    back.clear()
    unseed(s1)
    unseed(s2)
    try line.push(seed)
    let open = try trace(s1, s2, seed, heading, step, low, high, tol, line)
    if not open:
        # Closed: end on the start exactly.
        try line.push(seed)
        return
    unseed(s1)
    unseed(s2)
    _ = try trace(s1, s2, seed, heading.multiply_scalar(-1.0), step, low, high, tol, back)
    # The backward run reversed, then the forward one.
    var joined = Growing[Vector3]()
    defer joined.close()
    var at = back.count
    while at > 0:
        at -= 1
        try joined.push(back.items[at])
    for p in line.view(): try joined.push(p)
    try line.assign(joined.view())

## Whether the cubic `net` stays within half the tolerance of both fields
## at `samples` points along it.
func stays_on(s1: Field*, s2: Field*, net: const Vector3[], weights: const f64[], knots: const f64[], samples: usize, tol: f64) -> bool!:
    let low = knots[3]
    let high = knots[net.length]
    unseed(s1)
    unseed(s2)
    for k in 0..<samples + 1:
        let p = try NurbsCurve.at(net, weights, 3, knots, low + (high - low) * (f64)k / (f64)samples)
        for field in [s1, s2]:
            let (value, gradient) = try implicit(field, p)
            if math.abs(value) / math.max(gradient.length(), nonzero) > tol * 0.5: return false
    return true

## Points binned in cubes of side `reach`, the cubes hashed into a fixed
## number of buckets (chained through `next`): whether a point lies within
## `reach` of any looks only at the 27 cubes around it.
struct PointCells:
    var reach: f64
    var heads: Growing[i64]
    var next: Growing[i64]
    var points: Growing[Vector3]

    func close():
        self.heads.close()
        self.next.close()
        self.points.close()

    ## The bucket of cube (i, j, k): Teschner et al.'s spatial hash.
    func bucket(i: i64, j: i64, k: i64) -> usize:
        let count = (i64)self.heads.count
        var h = ((i *% 73856093) ^ (j *% 19349663) ^ (k *% 83492791)) % count
        if h < 0: h += count
        return (usize)h

    func cube(value: f64) -> i64:
        return (i64)math.floor(value / self.reach)

    func add(p: Vector3) -> !:
        let b = self.bucket(self.cube(p.x), self.cube(p.y), self.cube(p.z))
        try self.points.push(p)
        try self.next.push(self.heads.items[b])
        self.heads.items[b] = (i64)self.points.count - 1

    ## Whether a point added lies within `reach` of `p`.
    func near(p: Vector3) -> bool:
        let i = self.cube(p.x)
        let j = self.cube(p.y)
        let k = self.cube(p.z)
        for di in -1..<(i64)2:
            for dj in -1..<(i64)2:
                for dk in -1..<(i64)2:
                    var at = self.heads.items[self.bucket(i + di, j + dj, k + dk)]
                    while at >= 0:
                        if self.points.items[(usize)at].subtract(p).length() < self.reach: return true
                        at = self.next.items[(usize)at]
        return false

func new_point_cells(reach: f64) -> PointCells!:
    var made = PointCells(reach, Growing[i64](), Growing[i64](), Growing[Vector3]())
    errdefer made.close()
    try made.heads.fill(4096, -1)
    return made

## Where `other`'s function changes sign along face `f`'s boundary (which
## lies on `own`), refined onto both, onto `seeds`.
func boundary_seeds(w: const Shape*, f: usize, other: Field*, own: Field*, tol: f64, seeds: Growing[Vector3]*) -> !:
    for use in w.face_uses(f):
        let e = edge_index(use)
        let count = 128
        var previous = (try implicit(other, try w.edge_at(e, 0.0))).0
        for k in 1..<count + 1:
            let fraction = (f64)k / (f64)count
            let value = (try implicit(other, try w.edge_at(e, fraction))).0
            if (previous <= 0.0) != (value <= 0.0):
                var a = fraction - 1.0 / (f64)count
                var b = fraction
                for _ in 0..<60:
                    let m = (a + b) * 0.5
                    if ((try implicit(other, try w.edge_at(e, m))).0 <= 0.0) == (previous <= 0.0): a = m
                    else: b = m
                if let p = try onto_both(own, other, try w.edge_at(e, (a + b) * 0.5), tol): try seeds.push(p)
            previous = value

## Where `other`'s function changes sign between neighbors of a grid over
## face `f`'s chart (closed curves inside it), onto `seeds`.
func grid_seeds(w: const Shape*, f: usize, other: Field*, own: Field*, tol: f64, seeds: Growing[Vector3]*) -> !:
    var chart = Growing[Vector3]()
    defer chart.close()
    var starts = Growing[usize]()
    defer starts.close()
    try face_chart(w, f, &chart, &starts)
    var low = Vector3(1.0e300, 1.0e300, 0.0)
    var high = Vector3(-1.0e300, -1.0e300, 0.0)
    for q in chart.view():
        low = Vector3(math.min(low.x, q.x), math.min(low.y, q.y), 0.0)
        high = Vector3(math.max(high.x, q.x), math.max(high.y, q.y), 0.0)
    let n = 32
    for i in 0..<n + 1:
        # Each column searches a B-spline afresh.
        unseed(other)
        var previous = 0.0
        for j in 0..<n + 1:
            let u = low.x + (high.x - low.x) * (f64)i / (f64)n
            let v = low.y + (high.y - low.y) * (f64)j / (f64)n
            let p = try surface_point(*own.s, Vector3(u, v, 0.0))
            let value = (try implicit(other, p)).0
            if j > 0 and (previous <= 0.0) != (value <= 0.0):
                if let q = try onto_both(own, other, p, tol): try seeds.push(q)
            previous = value