dymokomi/luce-cad
Source of release 0.9.5
0.9.5 / 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).
## The central-difference step, as a share of the row source's range, the
## rows' turning takes.
let turning_difference_share: f64 = 1e-6
## The step into an end span, as a share of it, held samples take.
let hold_step_share: f64 = 0.01
## How far below the last knot, as a share of a span, a sample at the end
## is evaluated so it falls in the last span.
let below_end_share: f64 = 1e-12
## 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) * turning_difference_share
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]) * hold_step_share * (-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) * below_end_share), &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)