dymokomi/luce-cad
Source of release 0.9.2
0.9.2 / src / modeling / patch_fit.lucb
## The surface where three G2 or G3 fillets meet at a corner of three flat
## faces (setback.lucb). The fillets stop short of the corner (the
## setback). Between their end sections (A curves, each on a fillet swept
## straight) and curves on the faces (B curves) is a six-sided hole. Six
## quads fill it, one at each of its corners: degree 5 B-spline nets (one
## span for G2; two for G3, whose curves have three). Their outer sides
## are halves of its curves; their inner sides (seams) run from the
## curves' middles to a center.
## The outer sides are exact. Next to an A curve, a quad's first rows are
## the curve's control points moved along the sweep; next to a B curve
## they lie in its plane. So the patch meets the fillets and faces with
## their own continuity: two rows beyond the side for G2, three for G3.
## Across the seams the quads meet in G1 exactly, with the fixed
## coefficients of a vertex where six meet: one quad's cross derivative is
## the next one's turned back, plus t times the seam's. They meet in G2 by
## least squares. Their remaining freedom goes to the smoothest surface
## (least thin-plate energy). It is one linear solve, with no iteration.
## The literature fits one trimmed surface into the hole instead. Its
## sides are then only near the curves (1e-5 of the corner at best),
## which the booleans cannot use.
## A side of the hole: how the surface beyond its curve goes on. Either
## swept straight along `direction` (`swept`: a fillet's end section), or
## flat, in the plane of normal `direction` (a face).
struct HoleSide:
var swept: bool
var direction: Vector3
## A control point of the quads: `base`, plus unknowns `first..<first + count`
## times `axes`.
struct Slot:
var base: Vector3
var first: usize
var count: usize
var axes: Vector3[3]
## The normal equations of the fit, built a row at a time. A row is a
## combination of control points dotted with one direction.
struct Rows:
var size: usize
var matrix: Growing[f64]
var rhs: Growing[f64]
var row: Growing[f64]
var marked: Growing[bool]
var touched: Growing[usize]
var constant: f64
func close():
self.matrix.close()
self.rhs.close()
self.row.close()
self.marked.close()
self.touched.close()
## Control point `slot`, times `weight`, dotted with `axis`, in this row.
func add(slot: const Slot*, weight: f64, axis: Vector3) -> !:
if weight == 0.0: return
self.constant += weight * slot.base.dot(axis)
for j in 0..<slot.count:
let at = slot.first + j
if not self.marked.items[at]:
self.marked.items[at] = true
try self.touched.push(at)
self.row.items[at] += weight * slot.axes[j].dot(axis)
## The row, which should equal `target`, weighted `weight`.
func commit(target: f64, weight: f64):
let n = self.size
let goal = target - self.constant
for i in self.touched.view():
let wi = weight * self.row.items[i]
for j in self.touched.view(): self.matrix.items[i * n + j] += wi * self.row.items[j]
self.rhs.items[i] += wi * goal
for i in self.touched.view():
self.row.items[i] = 0.0
self.marked.items[i] = false
self.touched.clear()
self.constant = 0.0
## The Bernstein basis of `degree` at `t`, and its derivatives to the
## third (order k at out[k * 9 + i]).
func bernstein(degree: i64, t: f64, out: f64[]):
let n = (usize)degree
# The basis of each degree from n - 3 up to n (level d - n + 3).
var levels: f64[36]
var b: f64[9]
for i in 0..<(usize)9: b[i] = 0.0
for i in 0..<(usize)36: levels[i] = 0.0
b[0] = 1.0
for d in 0..<n + 1:
if d > 0:
var i = d
while i > 0:
b[i] = (1.0 - t) * b[i] + t * b[i - 1]
i -= 1
b[0] = (1.0 - t) * b[0]
if d + 3 >= n:
for i in 0..<d + 1: levels[(d + 3 - n) * 9 + i] = b[i]
let binomial: f64[4] = [1.0, 3.0, 3.0, 1.0]
for k in 0..<(usize)4:
# Beyond the degree, derivatives vanish.
if k > n:
for i in 0..<n + 1: out[k * 9 + i] = 0.0
continue
var factor = 1.0
for f in 0..<k: factor *= (f64)(n - f)
for i in 0..<n + 1:
var sum = 0.0
for j in 0..<k + 1:
if j > i or i - j > n - k: continue
# (-1)^(k - j) C(k, j), C(k, j) from the row of k.
let c = binomial[j] if k == 3 else (1.0 if j == 0 or j == k else 2.0)
sum += (c if (k - j) % 2 == 0 else -c) * levels[(3 - k) * 9 + i - j]
out[k * 9 + i] = factor * sum
## Clamped degree 5 B-spline `points` (knots `knots`) split at the middle
## of its domain by knot insertion: the halves' points onto `first` and
## `second`, each from its start, and their knots (0 to 1) onto
## `first_knots` and `second_knots`.
func split_middle(points: const Vector3[], knots: const f64[], first: Growing[Vector3]*, second: Growing[Vector3]*, first_knots: Growing[f64]*, second_knots: Growing[f64]*) -> !:
let p: usize = 5
var rows = Growing[f64]()
defer rows.close()
for q in points:
try rows.push(q.x)
try rows.push(q.y)
try rows.push(q.z)
var u = Growing[f64]()
defer u.close()
try u.assign(knots)
var scratch = Growing[f64]()
defer scratch.close()
let low = knots[p]
let high = knots[knots.length - p - 1]
let middle = (low + high) * 0.5
var already: usize = 0
for k in knots:
if k == middle: already += 1
for _ in already..<p: try insert_knot(&rows, &u, &scratch, p, 3, middle)
# The middle's run of knots starts at s; control point s - 1 is the
# curve's point there.
var s: usize = 0
while u.items[s] != middle: s += 1
let count = rows.count // 3
for at in 0..<count:
let q = Vector3(rows.items[at * 3], rows.items[at * 3 + 1], rows.items[at * 3 + 2])
if at < s: try first.push(q)
if at >= s - 1: try second.push(q)
for at in 0..<s: try first_knots.push((u.items[at] - low) / (middle - low))
for _ in 0..<p + 1: try first_knots.push(1.0)
for _ in 0..<p + 1: try second_knots.push(0.0)
for at in s + p..<u.count: try second_knots.push((u.items[at] - middle) / (high - middle))
## The clamped degree 5 B-spline basis of `knots` at `t` and its first and
## second derivatives (order k at out[k * 9 + i], for every control point).
func spline_basis(knots: const f64[], t: f64, out: f64[]):
var ders: f64[27]
let count = knots.length - 6
let first = basis_derivatives(knots, (i64)count, 5, t, &ders)
for i in 0..<(usize)27: out[i] = 0.0
for k in 0..<(usize)3:
for j in 0..<(usize)6: out[k * 9 + first + j] = ders[k * 9 + j]
## The six quads filling the hole bounded by `curves`, onto `out`, their
## knots (the same both ways) onto `quad_knots`. `curves` are six degree 5
## B-splines of knots `knots` in a loop: curve k's control points are
## curves[k * n..] (n per curve), each curve starting where the one before
## ends. The surface beyond curve k goes on as `sides[k]` says, with
## `continuity` (2 or 3: that many rows beyond each side follow it).
## Quad k sits at the hole's corner where curve k starts. Its side v = 0
## is the first half of curve k; its side u = 0 is the second half of
## curve k - 1, from that corner back. Its corner (1, 1) is the center.
## Its m^2 control points are u-major (point (a, b) at a * m + b).
func corner_patch(curves: const Vector3[], knots: const f64[], sides: const HoleSide[], continuity: i64, out: Growing[Vector3]*, quad_knots: Growing[f64]*) -> !:
let n = knots.length - 6
let h = (usize)continuity + 1
if sides.length != 6 or curves.length != 6 * n or continuity < 2 or continuity > 3: error(invalid, "a corner patch needs six degree 5 curves, G2 or G3")
# Unit size about the curves' middle, so the weights mean the same at
# any scale.
var origin = Vector3()
for p in curves: origin = origin.add(p)
origin = origin.multiply_scalar(1.0 / (f64)curves.length)
var scale = 0.0
for p in curves: scale = math.max(scale, p.subtract(origin).length())
if scale <= 0.0: error(invalid, "a corner patch's curves are a point")
var first = Growing[Vector3]()
defer first.close()
var second = Growing[Vector3]()
defer second.close()
var local = Growing[Vector3]()
defer local.close()
var second_knots = Growing[f64]()
defer second_knots.close()
for k in 0..<(usize)6:
local.clear()
for i in 0..<n: try local.push(curves[k * n + i].subtract(origin).multiply_scalar(1.0 / scale))
quad_knots.clear()
second_knots.clear()
try split_middle(local.view(), knots, &first, &second, quad_knots, &second_knots)
# Halves are m points; the second half run backwards has the first's
# knots (the curves are symmetric in their knots).
let m = first.count // 6
if m > 9 or second.count != 6 * m or m < h * 2 - 1: error(invalid, "a corner patch's curves are not split evenly")
let qk = quad_knots.view()
# What each control point is: kind 1 fixed (on a side), 2 a side's
# point moved along its sweep, 3 in a side's plane, 0 free. Keys are
# quad points (k * m + a) * m + b, a seam's points under the quad whose
# side u = 1 it is, the center last.
let center = 6 * m * m
var kinds = Growing[i64]()
defer kinds.close()
try kinds.fill(center + 1, 0)
var bases = Growing[Vector3]()
defer bases.close()
try bases.fill(center + 1, Vector3())
var ways = Growing[Vector3]()
defer ways.close()
try ways.fill(center + 1, Vector3())
for k in 0..<(usize)6:
let before = (k + 5) % 6
for a in 0..<m:
let key = patch_key(k, a, 0, m)
kinds.items[key] = 1
bases.items[key] = first.items[k * m + a]
for b in 0..<m:
let key = patch_key(k, 0, b, m)
kinds.items[key] = 1
bases.items[key] = second.items[before * m + m - 1 - b]
for k in 0..<(usize)6:
let before = (k + 5) % 6
for a in 1..<m:
for b in 1..<h: set_rule(&kinds, &bases, &ways, patch_key(k, a, b, m), first.items[k * m + a], &sides[k])
for a in 1..<h:
for b in 1..<m: set_rule(&kinds, &bases, &ways, patch_key(k, a, b, m), second.items[before * m + m - 1 - b], &sides[before])
# The unknowns behind each control point (keys no quad uses get none).
var used = Growing[bool]()
defer used.close()
try used.fill(center + 1, false)
for k in 0..<(usize)6:
for a in 0..<m:
for b in 0..<m: used.items[patch_key(k, a, b, m)] = true
var slots = Growing[Slot]()
defer slots.close()
var size: usize = 0
for key in 0..<center + 1:
let kind = kinds.items[key]
var slot = Slot(bases.items[key], size, 0, [Vector3(), Vector3(), Vector3()])
if not used.items[key]: slot.count = 0
elif kind == 2:
slot.count = 1
slot.axes[0] = ways.items[key]
elif kind == 3:
slot.count = 2
slot.axes[0] = try across(ways.items[key])
slot.axes[1] = ways.items[key].cross(slot.axes[0])
elif kind == 0:
slot.count = 3
slot.axes = [Vector3(1.0, 0.0, 0.0), Vector3(0.0, 1.0, 0.0), Vector3(0.0, 0.0, 1.0)]
size += slot.count
try slots.push(slot)
var rows = Rows(size, Growing[f64](), Growing[f64](), Growing[f64](), Growing[bool](), Growing[usize](), 0.0)
defer rows.close()
try rows.matrix.fill(size * size, 0.0)
try rows.rhs.fill(size, 0.0)
try rows.row.fill(size, 0.0)
try rows.marked.fill(size, false)
try fairness_rows(&rows, slots.view(), m, qk)
try seam_rows(&rows, slots.view(), m, qk)
try cholesky_solve(rows.matrix.items[..<size * size], rows.rhs.items[..<size], size)
for k in 0..<(usize)6:
for a in 0..<m:
for b in 0..<m:
let slot = slots.items[patch_key(k, a, b, m)]
var p = slot.base
for j in 0..<slot.count: p = p.add(slot.axes[j].multiply_scalar(rows.rhs.items[slot.first + j]))
try out.push(origin.add(p.multiply_scalar(scale)))
## Quad k's control point (a, b) as a key: a seam's points under the quad
## whose side u = 1 it is (quad k's side v = 1 is quad k - 1's side u = 1,
## run the same way), the center last.
func patch_key(k: usize, a: usize, b: usize, m: usize) -> usize:
if a == m - 1 and b == m - 1: return 6 * m * m
if b == m - 1: return (((k + 5) % 6) * m + m - 1) * m + a
return (k * m + a) * m + b
## The rule of a side for control point `key` (moved from `base` along
## its sweep, or in its plane), unless it is fixed. A sweep's rule wins:
## where a fillet's rows meet a face's, the fillet's lie in the face.
func set_rule(kinds: Growing[i64]*, bases: Growing[Vector3]*, ways: Growing[Vector3]*, key: usize, base: Vector3, side: const HoleSide*):
let kind = kinds.items[key]
if kind == 1 or kind == 2: return
if kind == 3 and not side.swept: return
kinds.items[key] = 2 if side.swept else 3
bases.items[key] = base
ways.items[key] = side.direction
## The thin-plate energy of every quad (S_uu^2 + 2 S_uv^2 + S_vv^2 over
## its square), weighted lightly: it only chooses among the surfaces the
## seams allow. Over a quad it is p^T M p (p its control points, each
## axis alike), M the same for every quad: made once by Gauss points in
## each span, then added for each quad's unknowns.
func fairness_rows(rows: Rows*, slots: const Slot[], m: usize, knots: const f64[]) -> !:
let nodes: f64[8] = [0.019855071751231856, 0.10166676129318664, 0.2372337950418355, 0.4082826787521751, 0.5917173212478249, 0.7627662049581645, 0.8983332387068134, 0.9801449282487681]
let weights: f64[8] = [0.05061426814518813, 0.11119051722668724, 0.15685332293894364, 0.18134189168918100, 0.18134189168918100, 0.15685332293894364, 0.11119051722668724, 0.05061426814518813]
let orders: usize[6] = [2, 0, 1, 1, 0, 2]
let factors: f64[3] = [1.0, 2.0, 1.0]
var points = Growing[f64]()
defer points.close()
var spans = Growing[f64]()
defer spans.close()
for at in 5..<knots.length - 6:
let a = knots[at]
let b = knots[at + 1]
if b <= a: continue
for q in 0..<(usize)8:
try points.push(a + (b - a) * nodes[q])
try spans.push((b - a) * weights[q])
# M over the m^2 control points (u-major).
let mm = m * m
var energy = Growing[f64]()
defer energy.close()
try energy.fill(mm * mm, 0.0)
var term = Growing[f64]()
defer term.close()
try term.fill(mm, 0.0)
var bu: f64[36]
var bv: f64[36]
for qu in 0..<points.count:
spline_basis(knots, points.items[qu], bu[..<36])
for qv in 0..<points.count:
spline_basis(knots, points.items[qv], bv[..<36])
let weight = 1e-5 * spans.items[qu] * spans.items[qv]
for t in 0..<(usize)3:
let du = orders[t * 2]
let dv = orders[t * 2 + 1]
for a in 0..<m:
for b in 0..<m: term.items[a * m + b] = bu[du * 9 + a] * bv[dv * 9 + b]
let w = weight * factors[t]
for i in 0..<mm:
let wi = w * term.items[i]
if wi == 0.0: continue
for j in 0..<mm: energy.items[i * mm + j] += wi * term.items[j]
# Each quad's share: p_i . p_j times M_ij, through its slots.
let n = rows.size
for k in 0..<(usize)6:
for i in 0..<mm:
let si = slots[patch_key(k, i // m, i % m, m)]
for j in 0..<mm:
let coefficient = energy.items[i * mm + j]
if coefficient == 0.0: continue
let sj = slots[patch_key(k, j // m, j % m, m)]
for x in 0..<si.count:
let axis = si.axes[x]
rows.rhs.items[si.first + x] -= coefficient * axis.dot(sj.base)
for y in 0..<sj.count: rows.matrix.items[(si.first + x) * n + sj.first + y] += coefficient * axis.dot(sj.axes[y])
## The seams' rows. Seam k is quad k's side u = 1 and quad k + 1's side
## v = 1, both from a curve's middle (t = 0) to the center. G1, weighted
## to hold: Q_k,u(1, t) + Q_k+1,v(t, 1) = t Q_k,v(1, t). That is C1 at the
## curve (where the halves meet smoothly) and, at the center, the six
## quads' corners an affine regular hexagon about it (2 cos 60 = 1).
## G2, by least squares: Q_k+1,vv = Q_k,uu - 2t Q_k,uv + t^2 Q_k,vv.
func seam_rows(rows: Rows*, slots: const Slot[], m: usize, knots: const f64[]) -> !:
let axes: Vector3[3] = [Vector3(1.0, 0.0, 0.0), Vector3(0.0, 1.0, 0.0), Vector3(0.0, 0.0, 1.0)]
var at_end: f64[36]
spline_basis(knots, 1.0, at_end[..<36])
var bt: f64[36]
let samples = 24
for s in 0..<samples:
let t = ((f64)s + 0.5) / (f64)samples
spline_basis(knots, t, bt[..<36])
for k in 0..<(usize)6:
let next = (k + 1) % 6
for axis in axes:
for a in 0..<m:
for b in 0..<m:
try rows.add(&slots[patch_key(k, a, b, m)], at_end[9 + a] * bt[b] - t * at_end[a] * bt[9 + b], axis)
try rows.add(&slots[patch_key(next, a, b, m)], bt[a] * at_end[9 + b], axis)
rows.commit(0.0, 1e3)
for axis in axes:
for a in 0..<m:
for b in 0..<m:
try rows.add(&slots[patch_key(next, a, b, m)], bt[a] * at_end[18 + b], axis)
let w = -at_end[18 + a] * bt[b] + 2.0 * t * at_end[9 + a] * bt[9 + b] - t * t * at_end[a] * bt[18 + b]
try rows.add(&slots[patch_key(k, a, b, m)], w, axis)
rows.commit(0.0, 1.0)
## Solves symmetric positive definite `matrix` (n x n) x = `rhs`, in
## place: the solution replaces `rhs`.
func cholesky_solve(matrix: f64[], rhs: 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 corner patch's fit 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
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]