Luce packages
LuceLuce BaseLuciaOS

dymokomi/luce-cad

Source of release 0.9.3

0.9.3 / src / modeling / row_fit.lucb

## Fitting a blend's rows of control points along its edge: each row a
## B-spline on the edge's parameter, sharing knots, least squares between
## exact ends and end tangents, spans halved where a row strays from where
## its source puts it by more than the tolerance (cap_rows.lucb's strips,
## curved_blend.lucb's).

## Where rows come from: every row's point at a parameter in [low, high]
## (`rows_at` with `context`), and how close a fit must stay to them.
struct RowSource:
    var context: void*
    var rows_at: func(void*, f64, Growing[Vector3]*) -> unit!
    var low: f64
    var high: f64
    var tolerance: f64

## `rows` rows from `source` fitted at degree `p` on `knots` (clamped on
## the source's interval; spans are added where needed) onto `net`
## (row-major): an error when no fit within the tolerance is found.
func fit_rows(source: const RowSource*, rows: usize, p: usize, knots: Growing[f64]*, net: Growing[Vector3]*) -> !:
    try fit_rows_led(source, rows, p, knots, net, Vector3(), 2, Vector3(), 2)

## fit_rows with `held` control points of each row held at the low end
## (2 to 4), the second on lying along unit `way` from the first, at the
## row's own speed and turning along it there: a fit that starts where a
## straight sweep along `way` ends meets it G1, G2 (3) or G3 (4) exactly.
## Likewise `held_end` at the high end along `end_way`.
func fit_rows_led(source: const RowSource*, rows: usize, p: usize, knots: Growing[f64]*, net: Growing[Vector3]*, way: Vector3, held: usize, end_way: Vector3, held_end: usize) -> !:
    if held < 2 or held > 4 or held > p or held_end < 2 or held_end > 4 or held_end > p: error(invalid, "a row fit holds two to four control points at each end")
    var marked = Growing[bool]()
    defer marked.close()
    for _ in 0..<12:
        let m = knots.count - p - 1
        marked.clear()
        try marked.fill(knots.count, m < held + held_end + 2)
        if m >= held + held_end + 2:
            try fit_once(source, rows, p, knots.view(), net, way, held, end_way, held_end)
            if not try mark_strays(source, rows, p, knots.view(), net.view(), &marked): break
        # Each marked span halved.
        var k = knots.count - 1
        while k > 0:
            k -= 1
            if marked.items[k] and knots.items[k + 1] > knots.items[k]: try knots.insert(k + 1, (knots.items[k] + knots.items[k + 1]) * 0.5)
        if knots.count > 4096: error(invalid, "a blend along this curved edge could not be fitted within the tolerance")
    if net.count != (knots.count - p - 1) * rows: error(invalid, "a blend along this curved edge could not be fitted within the tolerance")

## Every row's derivative by `t` at `t`, an end of the source's interval
## (a difference into the interval: rows may change shape along it, and a
## B-spline carrier ends there) onto `out`.
func rows_turning(source: const RowSource*, t: f64, out: Growing[Vector3]*) -> !:
    let low = source.low
    let high = source.high
    let h = (high - low) * 1e-6
    let a = t if t + h <= high else t - h
    var ahead = Growing[Vector3]()
    defer ahead.close()
    try source.rows_at(source.context, a + h, &ahead)
    try source.rows_at(source.context, a, out)
    for k in 0..<out.count: out.items[k] = ahead.items[k].subtract(out.items[k]).multiply_scalar(1.0 / h)

## Samples per knot span when fitting a row of degree `p`.
func samples_per_span(p: usize) -> usize:
    return 2 * p + 2

## One least-squares fit of each row (as fit_rows_led) on `knots`, onto
## `net` (row-major): the normal equations factored once, solved per row.
func fit_once(source: const RowSource*, rows: usize, p: usize, knots: const f64[], net: Growing[Vector3]*, way: Vector3, held: usize, end_way: Vector3, held_end: usize) -> !:
    let m = knots.length - p - 1
    let unknowns = m - held - held_end
    let tail = m - held_end
    let low = knots[p]
    let high = knots[m]
    net.clear()
    try net.fill(m * rows, Vector3())
    # The fixed ends: each row's end points and the next ones in, along
    # its end tangents.
    var at_low = Growing[Vector3]()
    defer at_low.close()
    var at_high = Growing[Vector3]()
    defer at_high.close()
    var turning_low = Growing[Vector3]()
    defer turning_low.close()
    var turning_high = Growing[Vector3]()
    defer turning_high.close()
    try source.rows_at(source.context, low, &at_low)
    try source.rows_at(source.context, high, &at_high)
    try rows_turning(source, low, &turning_low)
    try rows_turning(source, high, &turning_high)
    for r in 0..<rows:
        net.items[r * m] = at_low.items[r]
        net.items[r * m + 1] = at_low.items[r].add(turning_low.items[r].multiply_scalar((knots[p + 1] - low) / (f64)p))
        net.items[r * m + m - 1] = at_high.items[r]
        net.items[r * m + m - 2] = at_high.items[r].subtract(turning_high.items[r].multiply_scalar((high - knots[m - 1]) / (f64)p))
    if held > 2: try led_hold(source, rows, p, knots, way, held, false, net)
    if held_end > 2: try led_hold(source, rows, p, knots, end_way, held_end, true, net)
    var exact = Growing[Vector3]()
    defer exact.close()
    var matrix = Growing[f64]()
    defer matrix.close()
    try matrix.fill(unknowns * unknowns, 0.0)
    var rhs = Growing[f64]()
    defer rhs.close()
    try rhs.fill(unknowns * rows * 3, 0.0)
    var values: f64[12]
    let per = samples_per_span(p)
    for span in p..<m:
        let k0 = knots[span]
        let k1 = knots[span + 1]
        if k1 <= k0: continue
        for j in 0..<per:
            let t = k0 + (k1 - k0) * ((f64)j + 0.5) / (f64)per
            let first = nurbs_basis(knots, (i64)m, (i64)p, t, &values)
            for i in 0..<p + 1:
                let row = first + i
                if row < held or row >= tail: continue
                for k in 0..<p + 1:
                    let column = first + k
                    if column < held or column >= tail: continue
                    matrix.items[(row - held) * unknowns + column - held] += values[i] * values[k]
            try source.rows_at(source.context, t, &exact)
            for r in 0..<rows:
                var target = exact.items[r]
                for k in 0..<p + 1:
                    let column = first + k
                    if column < held or column >= tail: target = target.subtract(net.items[r * m + column].multiply_scalar(values[k]))
                for i in 0..<p + 1:
                    let row = first + i
                    if row < held or row >= tail: continue
                    let at = (r * unknowns + row - held) * 3
                    rhs.items[at] += values[i] * target.x
                    rhs.items[at + 1] += values[i] * target.y
                    rhs.items[at + 2] += values[i] * target.z
    try cholesky_factor(matrix.items[..<unknowns * unknowns], unknowns)
    var column = Growing[f64]()
    defer column.close()
    try column.fill(unknowns, 0.0)
    for r in 0..<rows:
        for axis in 0..<(usize)3:
            for i in 0..<unknowns: column.items[i] = rhs.items[(r * unknowns + i) * 3 + axis]
            cholesky_apply(matrix.items[..<unknowns * unknowns], column.items[..<unknowns], unknowns)
            for i in 0..<unknowns:
                let q = net.items[r * m + i + held]
                net.items[r * m + i + held] = Vector3(column.items[i] if axis == 0 else q.x, column.items[i] if axis == 1 else q.y, column.items[i] if axis == 2 else q.z)

## The third (and fourth) control points of each row at the low end (the
## high end when `at_end`) for fit_rows_led: the row's turning along `way`
## there (its offset along `way` from its end, a quintic through six
## samples), the control points from the clamped bases' derivatives, each
## on the line along `way` through the end.
func led_hold(source: const RowSource*, rows: usize, p: usize, knots: const f64[], way: Vector3, held: usize, at_end: bool, net: Growing[Vector3]*) -> !:
    let m = knots.length - p - 1
    let end = knots[m] if at_end else knots[p]
    # Samples step into the span: backward from the high end.
    let h = (knots[m] - knots[m - 1] if at_end else knots[p + 1] - knots[p]) * 0.01 * (-1.0 if at_end else 1.0)
    var samples = Growing[Vector3]()
    defer samples.close()
    for j in 0..<(usize)6:
        var out = Growing[Vector3]()
        defer out.close()
        try source.rows_at(source.context, end + h * (f64)j, &out)
        for q in out.view(): try samples.push(q)
    var ders: f64[48]
    _ = basis_jet(knots, (i64)m, (i64)p, end, at_end, &ders)
    for r in 0..<rows:
        # The offsets along `way` at the six samples, and their quintic's
        # derivatives at the end (Newton's forward differences, exact for
        # it; by t, h signed).
        var d: f64[6] = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
        for j in 0..<(usize)6: d[j] = samples.items[j * rows + r].subtract(samples.items[r]).dot(way)
        var differences: f64[6] = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
        for order in 1..<(usize)6:
            for j in 0..<(usize)6 - order: d[j] = d[j + 1] - d[j]
            differences[order] = d[0]
        let first = (differences[1] - differences[2] / 2.0 + differences[3] / 3.0 - differences[4] / 4.0 + differences[5] / 5.0) / h
        let second = (differences[2] - differences[3] + differences[4] * 11.0 / 12.0 - differences[5] * 5.0 / 6.0) / (h * h)
        let third = (differences[3] - differences[4] * 1.5 + differences[5] * 7.0 / 4.0) / (h * h * h)
        let turning: f64[4] = [0.0, first, second, third]
        # C^(k)(end) = sum over the bases of N_i^(k) P_i, each P_i the end
        # plus c_i way: the end's own term drops (the bases' derivatives
        # sum to zero), the rest solved from the end inward.
        let base = (m - 1) if at_end else 0
        let start = net.items[r * m + base]
        for k in 1..<held:
            var sum = turning[k]
            for i in 1..<k:
                let index = (base - i) if at_end else i
                let slot = (p - i) if at_end else i
                sum -= ders[k * 12 + slot] * net.items[r * m + index].subtract(start).dot(way)
            let coefficient = ders[k * 12 + ((p - k) if at_end else k)]
            if coefficient == 0.0: error(invalid, "a row fit's end has no room for its turning")
            net.items[r * m + ((base - k) if at_end else k)] = start.add(way.multiply_scalar(sum / coefficient))

## Marks onto `marked` (by knot index) the spans where a fitted row
## strays from its exact place by more than the tolerance; whether any.
func mark_strays(source: const RowSource*, rows: usize, p: usize, knots: const f64[], net: const Vector3[], marked: Growing[bool]*) -> bool!:
    let m = knots.length - p - 1
    let per = samples_per_span(p)
    var values: f64[12]
    var any = false
    var exact = Growing[Vector3]()
    defer exact.close()
    for span in p..<m:
        let k0 = knots[span]
        let k1 = knots[span + 1]
        if k1 <= k0: continue
        for j in 0..<per + 1:
            let t = k0 + (k1 - k0) * (f64)j / (f64)per
            let first = nurbs_basis(knots, (i64)m, (i64)p, math.min(t, knots[m] - (k1 - k0) * 1e-12), &values)
            try source.rows_at(source.context, t, &exact)
            for r in 0..<rows:
                var fitted = Vector3()
                for k in 0..<p + 1: fitted = fitted.add(net[r * m + first + k].multiply_scalar(values[k]))
                if fitted.subtract(exact.items[r]).length() > source.tolerance:
                    marked.items[span] = true
                    any = true
    return any

## Symmetric positive definite `matrix` (n x n) factored in place, L Láµ€
## (L in the lower triangle).
func cholesky_factor(matrix: f64[], n: usize) -> !:
    for j in 0..<n:
        var d = matrix[j * n + j]
        for k in 0..<j: d -= matrix[j * n + k] * matrix[j * n + k]
        if d <= 0.0: error(invalid, "a blend's fit along its edge is singular")
        let diagonal = math.sqrt(d)
        matrix[j * n + j] = diagonal
        for i in j + 1..<n:
            var s = matrix[i * n + j]
            for k in 0..<j: s -= matrix[i * n + k] * matrix[j * n + k]
            matrix[i * n + j] = s / diagonal

## `rhs` solved in place against a factored `matrix` (cholesky_factor).
func cholesky_apply(matrix: const f64[], rhs: f64[], n: usize):
    for i in 0..<n:
        var s = rhs[i]
        for k in 0..<i: s -= matrix[i * n + k] * rhs[k]
        rhs[i] = s / matrix[i * n + i]
    var i = n
    while i > 0:
        i -= 1
        var s = rhs[i]
        for k in i + 1..<n: s -= matrix[k * n + i] * rhs[k]
        rhs[i] = s / matrix[i * n + i]

## The law's points on edge `e` (between `low` and `high`) added to
## `knots` (of degree `p`, sorted) p - 1 times each: rows following a law
## meet there with their slopes alike (C1), so a fit must not smooth them.
func law_knots(law: const RadiusLaw*, w: const Shape*, e: usize, low: f64, high: f64, p: usize, knots: Growing[f64]*) -> !:
    var at = Growing[f64]()
    defer at.close()
    try law.points_on(w, e, low, high, &at)
    for t in at.view():
        var k: usize = 0
        while k < knots.count and knots.items[k] <= t: k += 1
        for _ in 0..<p - 1: try knots.insert(k, t)