dymokomi/luce-cad
Source of release 0.9.4
0.9.4 / src / surfacing / fill_net.lucb
## Fill's net and its solve (surfacing Stage 7; fill.lucb has the plan):
## a polynomial B-spline net in the fill's normalized units (the loop's
## middle at the origin, its size one), some control points fixed (a Quad
## fill's boundary rows), the rest unknowns of the band system
## (fill_bands.lucb); the bending and membrane energy and the flatness
## pull as exact Gram matrices of the bases; the constraint rows of the
## stations and guides; and the G2 passes.
## A fill's net: the net (normalized units, weights 1), each control
## point's first unknown (its x; y and z follow) or -1 when fixed, the
## unknowns' count and the band's half-width, and the base the unknowns
## are measured from.
##
## The unknowns are corrections to a base surface (the chart's own plane
## N-sided, the corners' bilinear patch Quad), not the control points
## themselves: the band's rounding is relative to what it solves for, so a
## fill that is nearly its base (a planar loop's: exactly) comes out to the
## last digits rather than to the system's condition times them.
struct FillNet:
var net: Net
var unknown: Growing[i64]
var count: usize
var half: usize
var base: Growing[Weighted]
## The reference surface's control points (fill_reference.lucb; empty:
## none, the energy of S itself).
var reference: Growing[Weighted]
func close():
self.net.close()
self.unknown.close()
self.base.close()
self.reference.close()
## The base surface's point at (u, v).
func base_at(u: f64, v: f64) -> Vector3:
var bu: f64[12]
var bv: f64[12]
let fu = nurbs_basis(self.net.ku.view(), (i64)self.net.nu, self.net.du, math.clamp(u, self.net.u_low(), self.net.u_high()), &bu)
let fv = nurbs_basis(self.net.kv.view(), (i64)self.net.nv, self.net.dv, math.clamp(v, self.net.v_low(), self.net.v_high()), &bv)
var sum = Vector3()
for i in 0..<(usize)self.net.du + 1:
for j in 0..<(usize)self.net.dv + 1: sum = sum.add(self.base.items[(fu + i) * self.net.nv + fv + j].point().multiply_scalar(bu[i] * bv[j]))
return sum
## What the energy measures control point (i, j) from: its base less
## the reference's (the base alone without one).
func energy_offset(i: usize, j: usize) -> Vector3:
let q = self.base_point(i, j)
if self.reference.count == 0: return q
return q.subtract(self.reference.items[i * self.net.nv + j].point())
## Control point (i, j).
func point(i: usize, j: usize) -> Vector3:
return self.net.points.items[i * self.net.nv + j].point()
## Control point (i, j) of the base (a fixed one's own).
func base_point(i: usize, j: usize) -> Vector3:
return self.base.items[i * self.net.nv + j].point()
## A net of degrees `du`, `dv` on knots `ku`, `kv` with control points
## `fixed` (true: held where `start` puts them; empty: all free), the rest
## starting from `start` (their base), its unknowns numbered u-major.
func fill_net(du: i64, dv: i64, ku: const f64[], kv: const f64[], fixed: const bool[], start: const Weighted[]) -> FillNet!:
var net = empty_net()
errdefer net.close()
net.du = du
net.dv = dv
net.nu = ku.length - (usize)du - 1
net.nv = kv.length - (usize)dv - 1
try net.ku.assign(ku)
try net.kv.assign(kv)
if start.length == net.nu * net.nv: try net.points.assign(start)
else: try net.points.fill(net.nu * net.nv, Weighted(0.0, 0.0, 0.0, 1.0))
var unknown = Growing[i64]()
errdefer unknown.close()
var count: usize = 0
for k in 0..<net.nu * net.nv:
if fixed.length > 0 and fixed[k]:
try unknown.push(-1)
continue
try unknown.push((i64)count)
count += 3
# The half-width: the furthest pair of unknowns whose bases overlap.
var half: usize = 2
for i in 0..<net.nu:
for j in 0..<net.nv:
let a = unknown.items[i * net.nv + j]
if a < 0: continue
for k in i..<fewer(net.nu, i + (usize)du + 1):
for l in 0..<net.nv:
if (l + (usize)dv < j) or (l > j + (usize)dv): continue
let b = unknown.items[k * net.nv + l]
if b < 0: continue
let gap = (usize)math.abs((f64)(b - a)) + 2
if gap > half: half = gap
var base = Growing[Weighted]()
errdefer base.close()
try base.assign(net.points.view())
return FillNet(net, unknown, count, half, base, Growing[Weighted]())
## The Gram matrix of `knots`' bases' derivatives of order `r` (0 to 3):
## G[i k] = ∫ N_i^(r) N_k^(r), exact by eight Gauss points a span, onto
## `out` (count by count).
func gram(knots: const f64[], degree: i64, r: usize, out: Growing[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 p = (usize)degree
let count = knots.length - p - 1
out.clear()
try out.fill(count * count, 0.0)
var ders: f64[48]
for span in p..<count:
let a = knots[span]
let b = knots[span + 1]
if not (b > a): continue
for q in 0..<(usize)8:
let first = basis_jet(knots, (i64)count, degree, a + (b - a) * nodes[q], false, &ders)
let w = (b - a) * weights[q]
for x in 0..<p + 1:
for y in 0..<p + 1: out.items[(first + x) * count + first + y] += w * ders[r * 12 + x] * ders[r * 12 + y]
## The energy into `system`: w_b ∫(|S_uu|² + 2|S_uv|² + |S_vv|²) +
## w_t ∫(|S_uuu|² + 3|S_uuv|² + 3|S_uvv|² + |S_vvv|²) + w_s ∫(|S_u|² +
## |S_v|²) + w_f ∫(n0 . S)² (bending, its rate, membrane, and flatness: the
## pull toward the plane through the origin square to `plane`), each
## coordinate alike but the flatness; the base's share and fixed control
## points' on the right-hand side.
func add_energy(system: BandSystem*, fn: const FillNet*, bending: f64, third: f64, membrane: f64, flatness: f64, plane: Vector3) -> !:
let net = &fn.net
var gu: Growing[f64][4] = [Growing[f64](), Growing[f64](), Growing[f64](), Growing[f64]()]
var gv: Growing[f64][4] = [Growing[f64](), Growing[f64](), Growing[f64](), Growing[f64]()]
defer gu[0].close()
defer gu[1].close()
defer gu[2].close()
defer gu[3].close()
defer gv[0].close()
defer gv[1].close()
defer gv[2].close()
defer gv[3].close()
for r in 0..<(usize)4:
try gram(net.ku.view(), net.du, r, &gu[r])
try gram(net.kv.view(), net.dv, r, &gv[r])
let nu = net.nu
let nv = net.nv
let du = (usize)net.du
let dv = (usize)net.dv
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)]
let n0: f64[3] = [plane.x, plane.y, plane.z]
for i in 0..<nu:
for j in 0..<nv:
let a = fn.unknown.items[i * nv + j]
if a < 0: continue
for k in i - fewer(i, du)..<fewer(nu, i + du + 1):
for l in j - fewer(j, dv)..<fewer(nv, j + dv + 1):
let uu = gu[0].items[i * nu + k]
let u1 = gu[1].items[i * nu + k]
let u2 = gu[2].items[i * nu + k]
let u3 = gu[3].items[i * nu + k]
let vv = gv[0].items[j * nv + l]
let v1 = gv[1].items[j * nv + l]
let v2 = gv[2].items[j * nv + l]
let v3 = gv[3].items[j * nv + l]
# Bending and its rate measure S from the reference,
# the membrane (Tension: tightening) and the flatness S
# itself.
var shape = bending * (u2 * vv + 2.0 * u1 * v1 + uu * v2)
if third != 0.0: shape += third * (u3 * vv + 3.0 * u2 * v1 + 3.0 * u1 * v2 + uu * v3)
let tight = membrane * (u1 * vv + uu * v1)
let m = shape + tight
let f = flatness * uu * vv
# The base's share moves to the right (a fixed
# neighbor's whole value), the reference's taken off
# for the bending.
let q = fn.energy_offset(k, l)
let own = fn.base_point(k, l)
let along = own.dot(plane)
for c in 0..<(usize)3:
system.rhs.items[(usize)a + c] -= shape * q.dot(axes[c]) + tight * own.dot(axes[c]) + f * n0[c] * along
let b = fn.unknown.items[k * nv + l]
if b < 0: continue
for c in 0..<(usize)3:
let row = (usize)a + c
if (usize)b + c <= row: system.add(row, (usize)b + c, m)
if f != 0.0:
for e in 0..<(usize)3:
if (usize)b + e <= row: system.add(row, (usize)b + e, f * n0[c] * n0[e])
## The bases of `fn`'s net at chart point `uv` to the second derivative:
## (first u, first v) and the values (k-th derivative at k * 12 + i).
func bases_of(fn: const FillNet*, uv: Vector3, bu: f64[48]*, bv: f64[48]*) -> (usize, usize):
let net = &fn.net
let u = math.clamp(uv.x, net.u_low(), net.u_high())
let v = math.clamp(uv.y, net.v_low(), net.v_high())
let fu = basis_jet(net.ku.view(), (i64)net.nu, net.du, u, u >= net.u_high(), bu)
let fv = basis_jet(net.kv.view(), (i64)net.nv, net.dv, v, v >= net.v_high(), bv)
return (fu, fv)
## Control point (i, j) times `k` dotted with `axis` into `row`: its base
## into the constant, a free one's correction as unknowns.
func fill_put(fn: const FillNet*, row: FillRow*, i: usize, j: usize, axis: Vector3, k: f64) -> !:
if k == 0.0: return
row.constant += k * fn.base_point(i, j).dot(axis)
let at = fn.unknown.items[i * fn.net.nv + j]
if at < 0: return
try row.term((usize)at, k * axis.x)
try row.term((usize)at + 1, k * axis.y)
try row.term((usize)at + 2, k * axis.z)
## `row` as the derivative of S at `uv` along chart direction `d` of order
## `order` (0 the point, 1 S_d, 2 S_dd) dotted with `axis`.
func put_derivative(fn: const FillNet*, row: FillRow*, uv: Vector3, d: Vector3, order: usize, axis: Vector3) -> !:
var bu: f64[48]
var bv: f64[48]
let (fu, fv) = bases_of(fn, uv, &bu, &bv)
let du = (usize)fn.net.du
let dv = (usize)fn.net.dv
for a in 0..<du + 1:
for b in 0..<dv + 1:
var k = 0.0
if order == 0: k = bu[a] * bv[b]
elif order == 1: k = d.x * bu[12 + a] * bv[b] + d.y * bu[a] * bv[12 + b]
else: k = d.x * d.x * bu[24 + a] * bv[b] + 2.0 * d.x * d.y * bu[12 + a] * bv[12 + b] + d.y * d.y * bu[a] * bv[24 + b]
try fill_put(fn, row, fu + a, fv + b, axis, k)
## The derivatives of `fn`'s net at `uv`: S, S_u, S_v, S_uu, S_uv, S_vv.
func net_jet(fn: const FillNet*, uv: Vector3) -> Vector3[6]:
var bu: f64[48]
var bv: f64[48]
let (fu, fv) = bases_of(fn, uv, &bu, &bv)
var out: Vector3[6] = [Vector3(), Vector3(), Vector3(), Vector3(), Vector3(), Vector3()]
let pairs: usize[12] = [0, 0, 1, 0, 0, 1, 2, 0, 1, 1, 0, 2]
for a in 0..<(usize)fn.net.du + 1:
for b in 0..<(usize)fn.net.dv + 1:
let q = fn.point(fu + a, fv + b)
for k in 0..<(usize)6: out[k] = out[k].add(q.multiply_scalar(bu[pairs[2 * k] * 12 + a] * bv[pairs[2 * k + 1] * 12 + b]))
return out
## The constraint rows of a solve, kept after their matrix share went into
## the system: each row's unknowns and coefficients, its scale (weight over
## its length squared), constant and target, the shift the multipliers have
## added to its target, and whether the multipliers drive it to hold
## exactly.
##
## Penalties alone trade the constraints' exactness for the system's
## condition: a weight of ten million left a planar fill 2e-8 off its plane,
## one of a hundred thousand 2e-10. So the weight stays at a hundred
## thousand and the method of multipliers (Hestenes, Powell) takes the rest:
## every exact row's target is shifted (solve_held finds the shifts) and the
## same factored system solved again, so the rows hold while the matrix
## never stiffens. Eased rows (a conflicting corner's) stay
## plain penalties: a conflict has no answer to converge to.
struct HeldRows:
var index: Growing[usize]
var value: Growing[f64]
var offsets: Growing[usize]
var scale: Growing[f64]
var constant: Growing[f64]
var target: Growing[f64]
var shift: Growing[f64]
var enough: Growing[f64]
func close():
self.index.close()
self.value.close()
self.offsets.close()
self.scale.close()
self.constant.close()
self.target.close()
self.shift.close()
self.enough.close()
## `row` (to equal `target`, weighted `weight`, driven to hold within
## `enough`, or a penalty alone at zero) into `system`'s matrix and
## kept. Cleared.
func hold(system: BandSystem*, row: FillRow*, weight: f64, target: f64, enough: f64) -> !:
let length = row.norm()
if not (length > 0.0):
row.clear()
return
let w = weight / (length * length)
for p in 0..<row.index.count:
let ip = row.index.items[p]
for q in 0..<row.index.count:
let iq = row.index.items[q]
if iq <= ip: system.add(ip, iq, w * row.value.items[p] * row.value.items[q])
try self.index.push(ip)
try self.value.push(row.value.items[p])
try self.offsets.push(self.index.count)
try self.scale.push(w)
try self.constant.push(row.constant)
try self.target.push(target)
try self.shift.push(0.0)
try self.enough.push(enough)
row.clear()
## The right-hand side: `base` plus each row's share at its target and
## shift.
func right_side(base: const f64[], out: Growing[f64]*) -> !:
try out.assign(base)
var start: usize = 0
for r in 0..<self.offsets.count:
let goal = (self.target.items[r] + self.shift.items[r] - self.constant.items[r]) * self.scale.items[r]
for k in start..<self.offsets.items[r]: out.items[self.index.items[k]] += self.value.items[k] * goal
start = self.offsets.items[r]
## Each row's value at `x` less its target (`constants`: with its
## constant; else the row's linear part alone) onto `out`, zero for
## penalties.
func residuals(x: const f64[], constants: bool, out: Growing[f64]*) -> !:
out.clear()
var start: usize = 0
for r in 0..<self.offsets.count:
var sum = self.constant.items[r] - self.target.items[r] if constants else 0.0
for k in start..<self.offsets.items[r]: sum += self.value.items[k] * x[self.index.items[k]]
start = self.offsets.items[r]
try out.push(sum if self.enough.items[r] > 0.0 else 0.0)
## The right-hand side of the shifts `shifts` alone (no base, targets
## or constants) onto `out`, `size` long.
func shift_side(shifts: const f64[], size: usize, out: Growing[f64]*) -> !:
out.clear()
try out.fill(size, 0.0)
var start: usize = 0
for r in 0..<self.offsets.count:
let goal = shifts[r] * self.scale.items[r]
for k in start..<self.offsets.items[r]: out.items[self.index.items[k]] += self.value.items[k] * goal
start = self.offsets.items[r]
## The worst of the residuals `r` over their rows' `enough` (below 1:
## every driven row holds).
func worst_of(r: const f64[]) -> f64:
var worst = 0.0
for k in 0..<r.length:
if self.enough.items[k] > 0.0: worst = math.max(worst, math.abs(r[k]) / self.enough.items[k])
return worst
## The W-weighted dot product of two row vectors (W each row's scale).
func weighted_dot(a: const f64[], b: const f64[]) -> f64:
var sum = 0.0
for r in 0..<a.length: sum += self.scale.items[r] * a[r] * b[r]
return sum
## The answer of `system` (factored) for the base right side `base`, its
## exact rows in `held` driven to hold onto `x`. The multipliers' shifts λ
## move the residuals affinely, g(λ) = g(0) + C K⁻¹ Cᵀ W λ (C the rows, K
## the matrix, W the rows' scales), and that operator is symmetric and
## positive in the W-weighted product: conjugate gradients on λ settle
## them in a few dozen solves, where shifting each by its residual (the
## plain method of multipliers) crawls along the slow directions of a fine
## net and stalled hundredths of a degree short. They stop once every
## driven row holds within its `enough` (a tenth of its tolerance): the
## rows are stations of boundaries no net meets exactly (a circle's), and
## driving them on past that only bends the fill between them (a ball's cap
## sank a quarter of a percent of its radius). Returns the solves.
func solve_held(system: BandSystem*, held: HeldRows*, base: const f64[], x: Growing[f64]*) -> i64!:
let n = system.size
let rows = held.offsets.count
try held.right_side(base, &system.rhs)
try system.solve(x)
var solves: i64 = 1
var r = Growing[f64]()
defer r.close()
try held.residuals(x.view(), true, &r)
let worst = held.worst_of(r.view())
if worst < 1.0: return solves
var lambda = Growing[f64]()
defer lambda.close()
try lambda.fill(rows, 0.0)
# Solve A λ = -g(0): the residual of that equation is -g(λ).
for k in 0..<rows: r.items[k] = -r.items[k]
var p = Growing[f64]()
defer p.close()
try p.assign(r.view())
var ap = Growing[f64]()
defer ap.close()
var side = Growing[f64]()
defer side.close()
var y = Growing[f64]()
defer y.close()
var rr = held.weighted_dot(r.view(), r.view())
var best = worst
var best_lambda = Growing[f64]()
defer best_lambda.close()
try best_lambda.assign(lambda.view())
# Stalled: no tenth gained on the best for eight solves (rows that
# conflict have no exact answer, and driving them on only bends the
# fill between its stations).
var stalled = 0
for _ in 0..<200:
try held.shift_side(p.view(), n, &side)
try system.rhs.assign(side.view())
try system.solve(&y)
solves += 1
try held.residuals(y.view(), false, &ap)
let pap = held.weighted_dot(p.view(), ap.view())
if not (pap > 0.0): break
let alpha = rr / pap
for k in 0..<rows:
lambda.items[k] += alpha * p.items[k]
r.items[k] -= alpha * ap.items[k]
let now = held.worst_of(r.view())
if now < best * 0.9:
stalled = 0
else:
stalled += 1
if now < best:
best = now
try best_lambda.assign(lambda.view())
if now < 1.0 or stalled >= 8: break
let next = held.weighted_dot(r.view(), r.view())
let beta = next / rr
rr = next
for k in 0..<rows: p.items[k] = r.items[k] + beta * p.items[k]
for k in 0..<rows: held.shift.items[k] = best_lambda.items[k]
try held.right_side(base, &system.rhs)
try system.solve(x)
return solves + 1
func held_rows() -> HeldRows:
return HeldRows(Growing[usize](), Growing[f64](), Growing[usize](), Growing[f64](), Growing[f64](), Growing[f64](), Growing[f64](), Growing[f64]())
## The corrections `x` added to the base of `fn`'s free control points.
func take_answer(fn: FillNet*, x: const f64[]):
for k in 0..<fn.unknown.count:
let a = fn.unknown.items[k]
if a < 0: continue
let q = fn.base.items[k]
fn.net.points.items[k] = Weighted(q.x + x[(usize)a], q.y + x[(usize)a + 1], q.z + x[(usize)a + 2], 1.0)
## The chart point of `fn`'s net nearest `p`: the best of a 24 by 24
## sampling, then Newton (net_foot).
func net_nearest(fn: const FillNet*, p: Vector3) -> Vector3:
let net = &fn.net
var best = Vector3(net.u_low(), net.v_low(), 0.0)
var best_distance = 1.0e300
for i in 0..<(usize)25:
for j in 0..<(usize)25:
let uv = Vector3(net.u_low() + (net.u_high() - net.u_low()) * (f64)i / 24.0, net.v_low() + (net.v_high() - net.v_low()) * (f64)j / 24.0, 0.0)
let d = net.point_at(uv.x, uv.y).subtract(p).length()
if d < best_distance:
best_distance = d
best = uv
return net_foot(fn, p, best)
## The chart point of `fn`'s net nearest `p` from `seed`: Newton on the
## squared distance, clamped to the domain.
func net_foot(fn: const FillNet*, p: Vector3, seed: Vector3) -> Vector3:
let net = &fn.net
var uv = seed
for _ in 0..<30:
let j = net_jet(fn, uv)
let r = j[0].subtract(p)
let gu = r.dot(j[1])
let gv = r.dot(j[2])
let huu = j[1].dot(j[1]) + r.dot(j[3])
let huv = j[1].dot(j[2]) + r.dot(j[4])
let hvv = j[2].dot(j[2]) + r.dot(j[5])
let det = huu * hvv - huv * huv
if not (math.abs(det) > nonzero): break
let step = Vector3((hvv * gu - huv * gv) / det, (huu * gv - huv * gu) / det, 0.0)
uv = Vector3(math.clamp(uv.x - step.x, net.u_low(), net.u_high()), math.clamp(uv.y - step.y, net.v_low(), net.v_high()), 0.0)
if step.length() < 1e-14: break
return uv