dymokomi/luce-cad
Source of release 0.9.2
0.9.2 / src / sketch / residuals.lucb
## The solver's rows: each constraint that is not a substitution becomes
## one or two residuals, zero when it holds, and each arc two internal rows
## keeping its ends on its circle. A row reads up to eight parameters and
## computes its residual and every partial in one pass (`evaluate`), no
## expression trees. Lengths are millimeters; rows that are ratios or
## angles are multiplied by the sketch's size `scale`, so every row is a
## length and the rows weigh alike in the solve.
pub enum RowKind as u8:
## Point (0, 1) on the line through (2, 3) and (4, 5), off it by
## `side` times parameter 6 (or the constant `value` when 6 is -1).
on_line = 0
## Point (0, 1) at parameter 4 (or `value`) from (2, 3), plus `side`
## times parameter 5 when there is one (two circles touching).
on_circle = 1
## The directions (2, 3) - (0, 1) and (6, 7) - (4, 5): parallel,
## perpendicular, at angle `value`, or of equal lengths.
parallel = 2
perpendicular = 3
angle = 4
equal_length = 5
## `value` plus the sum of `weights` times parameters 0..3.
linear = 6
## Points (0, 1) and (2, 3) mirrored about the line through (4, 5) and
## (6, 7): halfway along it, then on either side alike.
mirror_along = 7
mirror_across = 8
## Point (0, 1) on the ellipse about (2, 3) whose major axis ends at
## (4, 5), of minor radius parameter 6: its distance from the ellipse,
## near enough on it (the major radius times how far its scaled radius
## is from 1).
on_ellipse = 9
## A control point spline's end P0 (0, 1), P1 (2, 3), P2 (4, 5) as
## curved as radius parameter 6: `value` (the end's curvature factor)
## times cross(P1 - P0, P2 - P1) r / |P1 - P0|³ equals `side`.
curvature = 10
## The same end's curvature not changing along it (as a line's, circle's
## or arc's does not): P0..P3 (0..7), `weights` its first knots after the
## end (u_{p+1}, u_{p+2}, u_{p+3}) and its degree.
curvature_rate = 11
pub struct Row:
pub var kind: RowKind
## The user constraint it comes from, or -1 for an arc's own rows.
pub var constraint: i64
pub var at: i64[8]
pub var value: f64
pub var side: f64
pub var weights: f64[4]
## One row's residual and its partials by `at`'s parameters.
pub struct Evaluated:
pub var residual: f64
pub var partial: f64[8]
## The rows of `sketch`: its constraints that are not substitutions, and
## each arc's own.
pub func rows_of(sketch: const Sketch*, out: Growing[Row]*) -> !:
out.clear()
for index in 0..<sketch.entities.count:
let e = sketch.entities.items[index]
if e.kind != EntityKind.arc: continue
for end in [e.third, e.fourth]:
try out.push(row(RowKind.on_circle, -1, [px(sketch, end), px(sketch, end) + 1, px(sketch, e.first), px(sketch, e.first) + 1, e.second, -1, -1, -1]))
for index in 0..<sketch.constraints.count:
let k = sketch.constraints.items[index]
if k.driven: continue
try rows_for(sketch, k, (i64)index, out)
func rows_for(sketch: const Sketch*, k: SketchConstraint, index: i64, out: Growing[Row]*) -> !:
if (k.kind == ConstraintKind.tangent or k.kind == ConstraintKind.curvature or k.kind == ConstraintKind.smooth) and (sketch.entity(k.first).kind == EntityKind.spline or sketch.entity(k.first).kind == EntityKind.conic):
try end_rows(sketch, k, index, out)
return
if k.kind == ConstraintKind.on_curve:
let curve = sketch.entity(k.second)
if curve.kind == EntityKind.ellipse:
try out.push(row(RowKind.on_ellipse, index, [px(sketch, k.first), px(sketch, k.first) + 1, px(sketch, curve.first), px(sketch, curve.first) + 1, px(sketch, curve.third), px(sketch, curve.third) + 1, curve.second, -1]))
elif curve.kind == EntityKind.line:
try out.push(row(RowKind.on_line, index, point_line(sketch, k.first, k.second, -1)))
else:
try out.push(row(RowKind.on_circle, index, [px(sketch, k.first), px(sketch, k.first) + 1, px(sketch, curve.first), px(sketch, curve.first) + 1, curve.second, -1, -1, -1]))
elif k.kind == ConstraintKind.parallel or k.kind == ConstraintKind.perpendicular or k.kind == ConstraintKind.angle or (k.kind == ConstraintKind.equal and sketch.entity(k.first).kind == EntityKind.line):
let kind = RowKind.parallel if k.kind == ConstraintKind.parallel else (RowKind.perpendicular if k.kind == ConstraintKind.perpendicular else (RowKind.angle if k.kind == ConstraintKind.angle else RowKind.equal_length))
var r = row(kind, index, two_lines(sketch, k.first, k.second))
r.value = k.value
try out.push(r)
elif k.kind == ConstraintKind.tangent:
try out.push(tangent_row(sketch, k, index))
elif k.kind == ConstraintKind.midpoint:
let line = sketch.entity(k.second)
for axis in 0..<(i64)2:
var r = row(RowKind.linear, index, [px(sketch, k.first) + axis, px(sketch, line.first) + axis, px(sketch, line.second) + axis, -1, -1, -1, -1, -1])
r.weights = [1.0, -0.5, -0.5, 0.0]
try out.push(r)
elif k.kind == ConstraintKind.collinear:
let other = sketch.entity(k.second)
try out.push(row(RowKind.on_line, index, point_line(sketch, other.first, k.first, -1)))
try out.push(row(RowKind.on_line, index, point_line(sketch, other.second, k.first, -1)))
elif k.kind == ConstraintKind.symmetric:
let axis = sketch.entity(k.third)
let at = [px(sketch, k.first), px(sketch, k.first) + 1, px(sketch, k.second), px(sketch, k.second) + 1, px(sketch, axis.first), px(sketch, axis.first) + 1, px(sketch, axis.second), px(sketch, axis.second) + 1]
try out.push(row(RowKind.mirror_along, index, at))
try out.push(row(RowKind.mirror_across, index, at))
elif k.kind == ConstraintKind.distance:
var r = row(RowKind.on_circle, index, [px(sketch, k.second), px(sketch, k.second) + 1, px(sketch, k.first), px(sketch, k.first) + 1, -1, -1, -1, -1])
r.value = k.value
try out.push(r)
elif k.kind == ConstraintKind.line_distance:
var r = row(RowKind.on_line, index, point_line(sketch, k.first, k.second, -1))
r.value = k.value
r.side = k.side
try out.push(r)
elif k.kind == ConstraintKind.horizontal_distance or k.kind == ConstraintKind.vertical_distance:
let axis = 0 if k.kind == ConstraintKind.horizontal_distance else 1
var r = row(RowKind.linear, index, [px(sketch, k.second) + axis, px(sketch, k.first) + axis, -1, -1, -1, -1, -1, -1])
r.weights = [1.0, -1.0, 0.0, 0.0]
r.value = -k.side * k.value
try out.push(r)
## Tangent or curvature at a control point spline's or a conic's end
## (`side` +1 its start, -1 its end) against a line, circle or arc: its end
## direction P1 - P0 along the line, or square to the circle's radius at P0;
## for curvature also P2 - P1 along the line, or the curvature row.
func end_rows(sketch: const Sketch*, k: SketchConstraint, index: i64, out: Growing[Row]*) -> !:
let curve = sketch.entity(k.first)
let other = sketch.entity(k.second)
let (p0, p1, p2) = curve_end_points(sketch, curve, k.side)
let p3 = fourth_end_point(sketch, curve, k.side)
let curved = k.kind == ConstraintKind.curvature or k.kind == ConstraintKind.smooth
if other.kind == EntityKind.line:
# Along the line: P0 P1, and for G2 P1 P2, for G3 P2 P3 too.
let chain = [p0, p1, p2 if curved else -1, p3 if k.kind == ConstraintKind.smooth else -1]
for at in 0..<(usize)3:
let a = chain[at]
let b = chain[at + 1]
if a < 0 or b < 0: break
try out.push(row(RowKind.parallel, index, [px(sketch, a), px(sketch, a) + 1, px(sketch, b), px(sketch, b) + 1, px(sketch, other.first), px(sketch, other.first) + 1, px(sketch, other.second), px(sketch, other.second) + 1]))
return
let c = other.first
try out.push(row(RowKind.perpendicular, index, [px(sketch, p0), px(sketch, p0) + 1, px(sketch, p1), px(sketch, p1) + 1, px(sketch, c), px(sketch, c) + 1, px(sketch, p0), px(sketch, p0) + 1]))
if curved and p2 >= 0:
var r = row(RowKind.curvature, index, [px(sketch, p0), px(sketch, p0) + 1, px(sketch, p1), px(sketch, p1) + 1, px(sketch, p2), px(sketch, p2) + 1, other.second, -1])
r.value = end_curvature_factor(curve.second, spline_degree(curve))
r.side = k.value
try out.push(r)
if k.kind == ConstraintKind.smooth and p3 >= 0:
var r = row(RowKind.curvature_rate, index, [px(sketch, p0), px(sketch, p0) + 1, px(sketch, p1), px(sketch, p1) + 1, px(sketch, p2), px(sketch, p2) + 1, px(sketch, p3), px(sketch, p3) + 1])
let extra = (f64)(curve.second - spline_degree(curve))
r.weights = [math.min(1.0, extra), math.min(2.0, extra), math.min(3.0, extra), (f64)spline_degree(curve)]
try out.push(r)
## The first three points from a spline's or conic's end (`side` +1 its
## start, -1 its end): the end, the next (a conic's apex) and the one after
## (-1 for a conic or a two-point spline).
pub func curve_end_points(sketch: const Sketch*, curve: SketchEntity, side: f64) -> (i64, i64, i64):
if curve.kind == EntityKind.conic:
return (curve.first if side >= 0.0 else curve.third, curve.fourth, -1)
let n = curve.second
if side >= 0.0:
return (sketch.spline_point(curve, 0), sketch.spline_point(curve, 1), sketch.spline_point(curve, 2) if n > 2 else -1)
return (sketch.spline_point(curve, n - 1), sketch.spline_point(curve, n - 2), sketch.spline_point(curve, n - 3) if n > 2 else -1)
## A spline's fourth point from its end (`side` +1 its start, -1 its end),
## or -1 when it has fewer.
func fourth_end_point(sketch: const Sketch*, curve: SketchEntity, side: f64) -> i64:
if curve.kind != EntityKind.spline or curve.second < 4: return -1
return sketch.spline_point(curve, 3) if side >= 0.0 else sketch.spline_point(curve, curve.second - 4)
## A line, arc or circle touching a circle or arc: a line's distance from
## the center is the radius; two circles' centers are the sum (`side` +1)
## or difference (-1) of their radii apart.
func tangent_row(sketch: const Sketch*, k: SketchConstraint, index: i64) -> Row:
var a = sketch.entity(k.first)
var b = sketch.entity(k.second)
if b.kind == EntityKind.line:
let swap = a
a = b
b = swap
if a.kind == EntityKind.line:
var r = row(RowKind.on_line, index, [px(sketch, b.first), px(sketch, b.first) + 1, px(sketch, a.first), px(sketch, a.first) + 1, px(sketch, a.second), px(sketch, a.second) + 1, b.second, -1])
r.side = k.side
return r
var r = row(RowKind.on_circle, index, [px(sketch, b.first), px(sketch, b.first) + 1, px(sketch, a.first), px(sketch, a.first) + 1, a.second, b.second, -1, -1])
r.side = k.side
return r
## Point `point` against line `line`, offset by parameter `offset`.
func point_line(sketch: const Sketch*, point: i64, line: i64, offset: i64) -> i64[8]:
let l = sketch.entity(line)
return [px(sketch, point), px(sketch, point) + 1, px(sketch, l.first), px(sketch, l.first) + 1, px(sketch, l.second), px(sketch, l.second) + 1, offset, -1]
func two_lines(sketch: const Sketch*, first: i64, second: i64) -> i64[8]:
let a = sketch.entity(first)
let b = sketch.entity(second)
return [px(sketch, a.first), px(sketch, a.first) + 1, px(sketch, a.second), px(sketch, a.second) + 1, px(sketch, b.first), px(sketch, b.first) + 1, px(sketch, b.second), px(sketch, b.second) + 1]
func row(kind: RowKind, constraint: i64, at: i64[8]) -> Row:
return Row(kind, constraint, at, 0.0, 1.0, [0.0, 0.0, 0.0, 0.0])
## Point `point`'s x parameter.
func px(sketch: const Sketch*, point: i64) -> i64:
return sketch.entity(point).first
## Row `r`'s residual and partials, its parameters read by `read` from
## `context` (the solver's current values), scaled by `scale`.
pub func evaluate(r: const Row*, read: func(i64, void*) -> f64, context: void*, scale: f64) -> Evaluated:
var v: f64[8] = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
for i in 0..<(usize)8:
if r.at[i] >= 0: v[i] = read(r.at[i], context)
var e = Evaluated(0.0, [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0])
if r.kind == RowKind.on_line:
# Signed distance cross(b - a, p - a) / |b - a| minus the offset.
let dx = v[4] - v[2]
let dy = v[5] - v[3]
let wx = v[0] - v[2]
let wy = v[1] - v[3]
let length = math.max(math.hypot(dx, dy), 1e-12 * scale)
let cross = dx * wy - dy * wx
let d = cross / length
let offset = v[6] if r.at[6] >= 0 else r.value
e.residual = d - r.side * offset
# d/dp = (-dy, dx)/len; d/db = (wy, -wx)/len - d (b - a)/len^2;
# d/da = -(d/dp + d/db).
let l2 = length * length
e.partial[0] = -dy / length
e.partial[1] = dx / length
e.partial[4] = wy / length - d * dx / l2
e.partial[5] = -wx / length - d * dy / l2
e.partial[2] = -(e.partial[0] + e.partial[4])
e.partial[3] = -(e.partial[1] + e.partial[5])
if r.at[6] >= 0: e.partial[6] = -r.side
elif r.kind == RowKind.on_circle:
# |p - c| minus the radius (and `side` times a second radius).
let dx = v[0] - v[2]
let dy = v[1] - v[3]
let length = math.max(math.hypot(dx, dy), 1e-12 * scale)
let radius = v[4] if r.at[4] >= 0 else r.value
let other = v[5] if r.at[5] >= 0 else 0.0
e.residual = length - (radius + r.side * other)
e.partial[0] = dx / length
e.partial[1] = dy / length
e.partial[2] = -dx / length
e.partial[3] = -dy / length
if r.at[4] >= 0: e.partial[4] = -1.0
if r.at[5] >= 0: e.partial[5] = -r.side
elif r.kind == RowKind.linear:
e.residual = r.value
for i in 0..<(usize)4:
if r.at[i] < 0: continue
e.residual += r.weights[i] * v[i]
e.partial[i] = r.weights[i]
elif r.kind == RowKind.mirror_along or r.kind == RowKind.mirror_across:
mirror_row(v[..<8], r.kind == RowKind.mirror_along, scale, &e)
elif r.kind == RowKind.on_ellipse or r.kind == RowKind.curvature or r.kind == RowKind.curvature_rate:
# Central differences: these rows are smooth and cheap, and their
# closed-form partials long.
e.residual = curved_residual(v[..<8], r, scale)
let h = 1e-7 * scale
for i in 0..<(usize)8:
if r.at[i] < 0: continue
var up = v
var down = v
up[i] += h
down[i] -= h
e.partial[i] = (curved_residual(up[..<8], r, scale) - curved_residual(down[..<8], r, scale)) / (2.0 * h)
else:
two_directions(v[..<8], r, scale, &e)
return e
## Parallel, perpendicular, angle and equal-length rows: from the two
## directions d1 = (2, 3) - (0, 1) and d2 = (6, 7) - (4, 5).
func two_directions(v: const f64[], r: const Row*, scale: f64, e: Evaluated*):
let ax = v[2] - v[0]
let ay = v[3] - v[1]
let bx = v[6] - v[4]
let by = v[7] - v[5]
let la = math.max(math.hypot(ax, ay), 1e-12 * scale)
let lb = math.max(math.hypot(bx, by), 1e-12 * scale)
# Partials by d1 (ga) and d2 (gb); the points' are ± these.
var gax = 0.0
var gay = 0.0
var gbx = 0.0
var gby = 0.0
if r.kind == RowKind.equal_length:
e.residual = la - lb
gax = ax / la
gay = ay / la
gbx = -bx / lb
gby = -by / lb
else:
let cross = ax * by - ay * bx
let dot = ax * bx + ay * by
let n = la * lb
if r.kind == RowKind.parallel:
# sin of the angle between them: cross / (|a||b|).
e.residual = scale * cross / n
gax = scale * (by / n - cross * ax / (la * la * n))
gay = scale * (-bx / n - cross * ay / (la * la * n))
gbx = scale * (-ay / n - cross * bx / (lb * lb * n))
gby = scale * (ax / n - cross * by / (lb * lb * n))
elif r.kind == RowKind.perpendicular:
e.residual = scale * dot / n
gax = scale * (bx / n - dot * ax / (la * la * n))
gay = scale * (by / n - dot * ay / (la * la * n))
gbx = scale * (ax / n - dot * bx / (lb * lb * n))
gby = scale * (ay / n - dot * by / (lb * lb * n))
else:
# The angle from d1 to d2, atan2(cross, dot), minus the target,
# wrapped to (-pi, pi]: d(angle)/d(d1) = (ay, -ax)/|a|^2 and
# d(angle)/d(d2) = (-by, bx)/|b|^2.
var delta = math.atan2(cross, dot) - r.value
while delta > math.pi: delta -= 2.0 * math.pi
while delta <= -math.pi: delta += 2.0 * math.pi
e.residual = scale * delta
gax = scale * ay / (la * la)
gay = -scale * ax / (la * la)
gbx = -scale * by / (lb * lb)
gby = scale * bx / (lb * lb)
e.partial[0] = -gax
e.partial[1] = -gay
e.partial[2] = gax
e.partial[3] = gay
e.partial[4] = -gbx
e.partial[5] = -gby
e.partial[6] = gbx
e.partial[7] = gby
## Points p (0, 1) and q (2, 3) mirrored about the line through a (4, 5)
## and b (6, 7), with d = b - a. Along: q - p is square to the line,
## dot(q - p, d) / |d|. Across: they are equally far either side, the sum
## of their signed distances (cross(d, p - a) + cross(d, q - a)) / |d|.
func mirror_row(v: const f64[], along: bool, scale: f64, e: Evaluated*):
let dx = v[6] - v[4]
let dy = v[7] - v[5]
let length = math.max(math.hypot(dx, dy), 1e-12 * scale)
let l2 = length * length
if along:
let wx = v[2] - v[0]
let wy = v[3] - v[1]
let dot = wx * dx + wy * dy
e.residual = dot / length
e.partial[0] = -dx / length
e.partial[1] = -dy / length
e.partial[2] = dx / length
e.partial[3] = dy / length
# By d: w / |d| - dot d / |d|^3; b gets it, a its negative.
let gx = wx / length - dot * dx / (l2 * length)
let gy = wy / length - dot * dy / (l2 * length)
e.partial[6] = gx
e.partial[7] = gy
e.partial[4] = -gx
e.partial[5] = -gy
return
let pxa = v[0] - v[4]
let pya = v[1] - v[5]
let qxa = v[2] - v[4]
let qya = v[3] - v[5]
let cross = dx * (pya + qya) - dy * (pxa + qxa)
e.residual = cross / length
e.partial[0] = -dy / length
e.partial[1] = dx / length
e.partial[2] = -dy / length
e.partial[3] = dx / length
# By d = b - a: (sy, -sx)/|d| - cross d/|d|^3 with s = (p - a) + (q - a);
# a also moves both p - a and q - a.
let gx = (pya + qya) / length - cross * dx / (l2 * length)
let gy = -(pxa + qxa) / length - cross * dy / (l2 * length)
e.partial[6] = gx
e.partial[7] = gy
e.partial[4] = -gx + 2.0 * dy / length
e.partial[5] = -gy - 2.0 * dx / length
## The on_ellipse, curvature and curvature_rate rows' residuals (see
## RowKind).
func curved_residual(v: const f64[], r: const Row*, scale: f64) -> f64:
if r.kind == RowKind.curvature_rate:
# The end's first three derivatives C1, C2, C3 from its derivative
# control points (The NURBS Book, eq. 3.8), its knots clamped at the
# end: u_1 = u_2 = u_3 = 0, then u_{p+1}, u_{p+2}, u_{p+3}.
let p = r.weights[3]
let k1 = r.weights[0]
let k2 = r.weights[1]
let k3 = r.weights[2]
let q10x = p * (v[2] - v[0]) / k1
let q10y = p * (v[3] - v[1]) / k1
let q11x = p * (v[4] - v[2]) / k2
let q11y = p * (v[5] - v[3]) / k2
let q12x = p * (v[6] - v[4]) / k3
let q12y = p * (v[7] - v[5]) / k3
let q20x = (p - 1.0) * (q11x - q10x) / k1
let q20y = (p - 1.0) * (q11y - q10y) / k1
let q21x = (p - 1.0) * (q12x - q11x) / k2
let q21y = (p - 1.0) * (q12y - q11y) / k2
let q3x = (p - 2.0) * (q21x - q20x) / k1
let q3y = (p - 2.0) * (q21y - q20y) / k1
let speed = math.max(math.hypot(q10x, q10y), 1e-12)
let c2 = q10x * q20y - q10y * q20x
let c3 = q10x * q3y - q10y * q3x
let dot = q10x * q20x + q10y * q20y
# The curvature's rate along the curve: (C1 x C3 |C1|^2 - 3 (C1 x C2)(C1 . C2)) / |C1|^6.
let rate = (c3 * speed * speed - 3.0 * c2 * dot) / math.pow(speed, 6.0)
# A length: the rate times the end's first leg cubed (the sketch's
# size cubed would make the row too steep to settle).
let leg = math.max(math.hypot(v[2] - v[0], v[3] - v[1]), 1e-12)
return leg * leg * leg * rate
if r.kind == RowKind.on_ellipse:
let dx = v[4] - v[2]
let dy = v[5] - v[3]
let a = math.max(math.hypot(dx, dy), 1e-12 * scale)
let b = math.max(v[6], 1e-12 * scale)
let wx = v[0] - v[2]
let wy = v[1] - v[3]
let along = (wx * dx + wy * dy) / a
let across = (dx * wy - dy * wx) / a
return a * (math.hypot(along / a, across / b) - 1.0)
let d1x = v[2] - v[0]
let d1y = v[3] - v[1]
let d2x = v[4] - v[2]
let d2y = v[5] - v[3]
let length = math.max(math.hypot(d1x, d1y), 1e-12 * scale)
let cross = d1x * d2y - d1y * d2x
return scale * (r.value * cross * v[6] / (length * length * length) - r.side)