use super::*;
pub(super) type SpanControls = [[f64; 2]; 4];
const FOOT_SIMPLE_ROOT_EPS: f64 = 1e-6;
const FOOT_STRAIGHT_EPS: f64 = 1e-9;
const FOOT_SCAN_SAMPLES: usize = 16;
const FOOT_NEWTON_MAX_ITER: usize = 40;
pub(super) const FOOT_EVAL_MARGIN: f64 = 1.0 / 64.0;
const FOOT_DAMP: f64 = 0.5;
const FOOT_DEGENERATE_EPS: f64 = 1e-12;
#[derive(Clone, Copy, Debug)]
pub(super) enum FootTarget {
Line { origin: [f64; 2], dir: [f64; 2] },
Circle { center: [f64; 2], rho: f64 },
}
#[derive(Clone, Copy, Debug)]
pub(super) struct FootHit {
pub span: usize,
pub t: f64,
pub simple: bool,
}
impl FootHit {
pub(super) fn global(&self) -> f64 {
self.span as f64 + self.t
}
}
pub(super) fn bernstein3(t: f64) -> [f64; 4] {
let u = 1.0 - t;
[u * u * u, 3.0 * u * u * t, 3.0 * u * t * t, t * t * t]
}
pub(super) fn eval_cubic(c: &SpanControls, t: f64) -> [f64; 2] {
let w = bernstein3(t);
let mut out = [0.0; 2];
for i in 0..4 {
out[0] += c[i][0] * w[i];
out[1] += c[i][1] * w[i];
}
out
}
pub(super) fn eval_cubic_d1(c: &SpanControls, t: f64) -> [f64; 2] {
let u = 1.0 - t;
let w = [3.0 * u * u, 6.0 * u * t, 3.0 * t * t];
let mut out = [0.0; 2];
for i in 0..3 {
out[0] += (c[i + 1][0] - c[i][0]) * w[i];
out[1] += (c[i + 1][1] - c[i][1]) * w[i];
}
out
}
pub(super) fn eval_cubic_d2(c: &SpanControls, t: f64) -> [f64; 2] {
let w = [6.0 * (1.0 - t), 6.0 * t];
let mut out = [0.0; 2];
for i in 0..2 {
for axis in 0..2 {
out[axis] += (c[i + 2][axis] - 2.0 * c[i + 1][axis] + c[i][axis]) * w[i];
}
}
out
}
fn cross(a: [f64; 2], b: [f64; 2]) -> f64 {
a[0] * b[1] - a[1] * b[0]
}
fn dot(a: [f64; 2], b: [f64; 2]) -> f64 {
a[0] * b[0] + a[1] * b[1]
}
fn sub(a: [f64; 2], b: [f64; 2]) -> [f64; 2] {
[a[0] - b[0], a[1] - b[1]]
}
fn norm(a: [f64; 2]) -> f64 {
a[0].hypot(a[1])
}
pub(super) fn span_extent(c: &SpanControls) -> f64 {
(1..4).fold(0.0f64, |acc, i| acc.max(norm(sub(c[i], c[0]))))
}
impl FootTarget {
fn g(&self, c: &SpanControls, t: f64) -> f64 {
let d1 = eval_cubic_d1(c, t);
match *self {
FootTarget::Line { dir, .. } => cross(dir, d1),
FootTarget::Circle { center, .. } => dot(sub(eval_cubic(c, t), center), d1),
}
}
fn g_prime(&self, c: &SpanControls, t: f64) -> (f64, f64) {
let d1 = eval_cubic_d1(c, t);
let d2 = eval_cubic_d2(c, t);
match *self {
FootTarget::Line { dir, .. } => (cross(dir, d2), norm(d2)),
FootTarget::Circle { center, .. } => {
let radial = sub(eval_cubic(c, t), center);
(
dot(d1, d1) + dot(radial, d2),
dot(d1, d1) + norm(radial) * norm(d2),
)
}
}
}
fn g_scale(&self, c: &SpanControls, t: f64) -> f64 {
let speed = norm(eval_cubic_d1(c, t));
match *self {
FootTarget::Line { .. } => speed,
FootTarget::Circle { center, .. } => speed * norm(sub(eval_cubic(c, t), center)),
}
}
fn root_is_simple(&self, c: &SpanControls, t: f64) -> bool {
let (derivative, scale) = self.g_prime(c, t);
scale > 0.0 && derivative.abs() > FOOT_SIMPLE_ROOT_EPS * scale
}
pub(super) fn residual(&self, c: &SpanControls, t: f64) -> f64 {
let p = eval_cubic(c, t);
match *self {
FootTarget::Line { origin, dir } => cross(dir, sub(p, origin)),
FootTarget::Circle { center, rho } => norm(sub(p, center)) - rho,
}
}
pub(super) fn length_scale(&self, c: &SpanControls) -> f64 {
match *self {
FootTarget::Line { .. } => span_extent(c),
FootTarget::Circle { rho, .. } => span_extent(c).max(rho),
}
}
}
fn line_hodograph_roots(c: &SpanControls, dir: [f64; 2], lo: f64, hi: f64) -> Vec<f64> {
let coefficients = [
cross(dir, sub(c[1], c[0])),
cross(dir, sub(c[2], c[1])),
cross(dir, sub(c[3], c[2])),
];
let scale = coefficients.iter().fold(0.0f64, |acc, &v| acc.max(v.abs()));
let extent = span_extent(c);
if !(scale > FOOT_STRAIGHT_EPS * extent.max(f64::MIN_POSITIVE)) {
return Vec::new(); }
let a = coefficients[0] - 2.0 * coefficients[1] + coefficients[2];
let b = 2.0 * (coefficients[1] - coefficients[0]);
let cc = coefficients[0];
let mut roots: Vec<f64> = Vec::new();
if a.abs() <= FOOT_STRAIGHT_EPS * scale {
if b.abs() > FOOT_STRAIGHT_EPS * scale {
roots.push(-cc / b);
}
} else {
let disc = b * b - 4.0 * a * cc;
if disc >= 0.0 {
roots.extend(crate::poly::solve_quadratic(cc, b, a));
}
}
roots.retain(|t| t.is_finite() && *t >= lo && *t <= hi);
roots.sort_by(|a, b| a.partial_cmp(b).unwrap_or(std::cmp::Ordering::Equal));
roots.dedup_by(|a, b| (*a - *b).abs() <= 1e-15);
roots
}
fn newton_root(target: &FootTarget, c: &SpanControls, seed: f64, lo: f64, hi: f64) -> Option<f64> {
let mut t = seed.clamp(lo, hi);
for _ in 0..FOOT_NEWTON_MAX_ITER {
let value = target.g(c, t);
let (derivative, scale) = target.g_prime(c, t);
if !(derivative.abs() > FOOT_SIMPLE_ROOT_EPS * scale.max(f64::MIN_POSITIVE)) {
return None; }
let step = value / derivative;
let next = t - step;
if !next.is_finite() || next < lo || next > hi {
return None;
}
let moved = (next - t).abs();
t = next;
if moved <= f64::EPSILON * (1.0 + t.abs()) {
break;
}
}
(target.g(c, t).abs() <= 1e-12 * target.g_scale(c, t).max(f64::MIN_POSITIVE)).then_some(t)
}
fn scan_roots(target: &FootTarget, c: &SpanControls, lo: f64, hi: f64) -> Vec<f64> {
let mut roots: Vec<f64> = Vec::new();
let step = (hi - lo) / FOOT_SCAN_SAMPLES as f64;
let mut previous = (lo, target.g(c, lo));
if previous.1 == 0.0 {
roots.push(lo);
}
for i in 1..=FOOT_SCAN_SAMPLES {
let t = if i == FOOT_SCAN_SAMPLES { hi } else { lo + step * i as f64 };
let value = target.g(c, t);
if value == 0.0 {
roots.push(t);
} else if previous.1 != 0.0 && (value > 0.0) != (previous.1 > 0.0) {
let mut a = previous;
let mut b = (t, value);
for _ in 0..60 {
let mid = 0.5 * (a.0 + b.0);
if mid <= a.0 || mid >= b.0 {
break;
}
let value_mid = target.g(c, mid);
if (value_mid > 0.0) == (a.1 > 0.0) {
a = (mid, value_mid);
} else {
b = (mid, value_mid);
}
}
let bracketed = 0.5 * (a.0 + b.0);
roots.push(newton_root(target, c, bracketed, lo, hi).unwrap_or(bracketed));
}
previous = (t, value);
}
roots.sort_by(|a, b| a.partial_cmp(b).unwrap_or(std::cmp::Ordering::Equal));
roots.dedup_by(|a, b| (*a - *b).abs() <= 1e-12);
roots
}
fn foot_roots_in_span(target: &FootTarget, c: &SpanControls, lo: f64, hi: f64) -> Vec<f64> {
match target {
FootTarget::Line { dir, .. } => line_hodograph_roots(c, *dir, lo, hi),
FootTarget::Circle { .. } => scan_roots(target, c, lo, hi),
}
}
pub(super) fn foot_in_span(
target: &FootTarget,
c: &SpanControls,
seed: f64,
lo: f64,
hi: f64,
) -> Option<f64> {
if let FootTarget::Circle { .. } = target {
if let Some(t) = newton_root(target, c, seed, lo, hi) {
return Some(t);
}
}
let roots = foot_roots_in_span(target, c, lo, hi);
roots
.into_iter()
.min_by(|a, b| {
(a - seed)
.abs()
.partial_cmp(&(b - seed).abs())
.unwrap_or(std::cmp::Ordering::Equal)
})
}
pub(super) fn solve_spline_foot(
target: &FootTarget,
spans: &[SpanControls],
seed: Option<f64>,
) -> Option<FootHit> {
let mut best: Option<(f64, FootHit)> = None;
for (span, controls) in spans.iter().enumerate() {
let roots = match seed {
Some(value) if (value.floor() as isize) == span as isize => {
match foot_in_span(target, controls, value - span as f64, 0.0, 1.0) {
Some(t) => vec![t],
None => foot_roots_in_span(target, controls, 0.0, 1.0),
}
}
_ => foot_roots_in_span(target, controls, 0.0, 1.0),
};
for t in roots {
let hit = FootHit {
span,
t,
simple: target.root_is_simple(controls, t),
};
let rank = match seed {
Some(value) => (hit.global() - value).abs(),
None => target.residual(controls, t).abs(),
};
if best.as_ref().map_or(true, |(current, _)| rank < *current) {
best = Some((rank, hit));
}
}
}
best.map(|(_, hit)| hit)
}
#[derive(Clone, Debug)]
pub(super) struct SplineBody {
pub seg_count: usize,
pub controls: Vec<usize>,
}
impl Engine {
pub(super) fn spline_body_info(&self, a: usize, b: usize) -> Option<SplineBody> {
let key_a = point_key(&self.points[a].id);
let key_b = point_key(&self.points[b].id);
for geometry in &self.geometries {
let Some(obj) = geometry.as_object() else {
continue;
};
let geometry_type = obj.get("type").and_then(Value::as_str).unwrap_or("");
if !is_spline_geometry_type(geometry_type) {
continue;
}
let Some(points) = obj.get("points").and_then(Value::as_array) else {
continue;
};
if points.len() < 4 {
continue;
}
let first = point_key(&points[0]);
let second = point_key(&points[1]);
if !((first == key_a && second == key_b) || (first == key_b && second == key_a)) {
continue;
}
let mut controls = Vec::with_capacity(points.len());
for id in points {
controls.push(self.point_index_of(id)?);
}
return Some(SplineBody {
seg_count: (points.len() - 1) / 3,
controls,
});
}
None
}
pub(super) fn span_point_indices(body: &SplineBody, span: usize) -> [usize; 4] {
let i0 = 3 * span;
[
body.controls[i0],
body.controls[i0 + 1],
body.controls[i0 + 2],
body.controls[i0 + 3],
]
}
fn span_controls_live(&self, body: &SplineBody) -> Vec<SpanControls> {
(0..body.seg_count)
.map(|span| {
Self::span_point_indices(body, span).map(|index| self.point_xy(index))
})
.collect()
}
pub(super) fn foot_signature_indices(&self, indices: &[Option<usize>]) -> Vec<Option<usize>> {
let mut out = indices.to_vec();
let pair = |first: usize, second: usize| {
match (
indices.get(first).copied().flatten(),
indices.get(second).copied().flatten(),
) {
(Some(a), Some(b)) => self.spline_body_info(a, b),
_ => None,
}
};
if let Some(body) = pair(0, 1).or_else(|| pair(2, 3)) {
out.extend(body.controls.iter().map(|index| Some(*index)));
}
out
}
pub(super) fn foot_roles(
&self,
indices: &[Option<usize>],
) -> Result<(SplineBody, usize, usize), String> {
let p0 = self.req(indices, 0)?;
let p1 = self.req(indices, 1)?;
let p2 = self.req(indices, 2)?;
let p3 = self.req(indices, 3)?;
match (self.spline_body_info(p0, p1), self.spline_body_info(p2, p3)) {
(Some(_), Some(_)) => Err(
"Span tangent: both pairs name a spline body; one of them must be the line or circle"
.to_string(),
),
(Some(body), None) => Ok((body, p2, p3)),
(None, Some(body)) => Ok((body, p0, p1)),
(None, None) => Err(
"Span tangent: no spline body in the constraint (its point list needs the spline's first two control points)"
.to_string(),
),
}
}
fn foot_target_live(&self, q0: usize, q1: usize) -> Result<FootTarget, String> {
let tolerance = self.tolerance();
let length = self.distance(q0, q1);
if self.is_line_pair(q0, q1) {
if length < tolerance {
return Err("Span tangent: the line is degenerate".to_string());
}
let origin = self.point_xy(q0);
let far = self.point_xy(q1);
Ok(FootTarget::Line {
origin,
dir: [(far[0] - origin[0]) / length, (far[1] - origin[1]) / length],
})
} else {
if length < tolerance {
return Err("Span tangent: the circle is degenerate".to_string());
}
Ok(FootTarget::Circle {
center: self.point_xy(q0),
rho: length,
})
}
}
pub(super) fn c_spline_foot_tangent(
&mut self,
constraint: &mut HotConstraint,
indices: &[Option<usize>],
) -> CResult {
let (body, q0, q1) = match self.foot_roles(indices) {
Ok(roles) => roles,
Err(message) => {
constraint.set_error(Value::String(message));
return Ok(Value::Null);
}
};
let target = match self.foot_target_live(q0, q1) {
Ok(target) => target,
Err(message) => {
constraint.set_error(Value::String(message));
return Ok(Value::Null);
}
};
let spans = self.span_controls_live(&body);
let Some(hit) = solve_spline_foot(&target, &spans, constraint.foot_t) else {
constraint.set_error(Value::String(match target {
FootTarget::Line { .. } => "Span tangent: the spline has no point where its tangent is parallel to the line".to_string(),
FootTarget::Circle { .. } => "Span tangent: the spline has no point where it faces the circle's centre squarely".to_string(),
}));
return Ok(Value::Null);
};
constraint.set_foot_t(hit.global());
if !hit.simple {
constraint.set_error(Value::String(format!(
"Span tangent: the touch point is an inflection of the spline (span {}, t {}) — the foot is a double root there and the constraint has no stable step",
hit.span,
fmt_number(hit.t)
)));
return Ok(Value::Null);
}
let controls = &spans[hit.span];
let residual = target.residual(controls, hit.t);
let scale = target.length_scale(controls);
if residual.abs() <= self.tolerance() * scale {
constraint.set_error(Value::Null);
return Ok(Value::Null);
}
let span = Self::span_point_indices(&body, hit.span);
let weights = self.free_span_weight(&span, hit.t);
let span_movable = weights > 0.0;
let target_movable = match target {
FootTarget::Line { .. } => !self.points[q0].fixed && !self.points[q1].fixed,
FootTarget::Circle { .. } => !self.points[q0].fixed || !self.points[q1].fixed,
};
if !span_movable && !target_movable {
constraint.set_error(Value::String(
"Span tangent: the spline span and the line or circle are both fully fixed".into(),
));
return Ok(Value::Null);
}
let share = if span_movable && target_movable { 0.5 } else { 1.0 };
let touch = eval_cubic(controls, hit.t);
match target {
FootTarget::Line { dir, .. } => {
constraint.set_error(Value::String(format!(
"Span tangent constraint not satisfied\n spline touch point {} from the line",
fmt_number(residual)
)));
let normal = [-dir[1], dir[0]];
if span_movable {
self.nudge_span_along(&span, hit.t, normal, -residual * FOOT_DAMP * share);
}
if target_movable {
let step = residual * FOOT_DAMP * share;
for index in [q0, q1] {
self.points[index].x += normal[0] * step;
self.points[index].y += normal[1] * step;
}
}
}
FootTarget::Circle { center, rho } => {
constraint.set_error(Value::String(format!(
"Span tangent constraint not satisfied\n spline touch point at radius {} against {}",
fmt_number(rho + residual),
fmt_number(rho)
)));
let radial = sub(touch, center);
let distance = norm(radial);
if distance <= FOOT_DEGENERATE_EPS {
constraint.set_error(Value::String(
"Span tangent: the touch point is the circle's centre".into(),
));
return Ok(Value::Null);
}
let unit = [radial[0] / distance, radial[1] / distance];
if span_movable {
self.nudge_span_along(&span, hit.t, unit, -residual * FOOT_DAMP * share);
}
if target_movable {
self.adjust_circle_radius(q0, q1, distance, FOOT_DAMP * share);
}
}
}
Ok(Value::Null)
}
fn free_span_weight(&self, span: &[usize; 4], t: f64) -> f64 {
self.span_nudge_weights(span, t)
.iter()
.map(|(_, weight)| weight * weight)
.sum()
}
fn span_nudge_weights(&self, span: &[usize; 4], t: f64) -> Vec<(usize, f64)> {
let weights = bernstein3(t);
let mut out: Vec<(usize, f64)> = Vec::with_capacity(4);
for slot in 0..4 {
let index = span[slot];
if self.points[index].fixed {
continue;
}
match out.iter_mut().find(|(existing, _)| *existing == index) {
Some((_, weight)) => *weight += weights[slot],
None => out.push((index, weights[slot])),
}
}
out
}
fn nudge_span_along(&mut self, span: &[usize; 4], t: f64, dir: [f64; 2], delta: f64) -> bool {
let weights = self.span_nudge_weights(span, t);
let square: f64 = weights.iter().map(|(_, weight)| weight * weight).sum();
if !(square > 0.0) || !delta.is_finite() {
return false;
}
for (index, weight) in weights {
let step = delta * weight / square;
self.points[index].x += dir[0] * step;
self.points[index].y += dir[1] * step;
}
true
}
pub(super) fn refresh_spline_foot_seeds(&mut self) {
for index in 0..self.constraints.len() {
if self.constraints[index].ctype != CType::SplineFootTangent {
continue;
}
let seed = self.constraints[index].foot_t;
let point_idx = self.constraints[index].point_idx.clone();
let resolved = self
.foot_roles(&point_idx)
.ok()
.and_then(|(body, q0, q1)| {
let target = self.foot_target_live(q0, q1).ok()?;
let spans = self.span_controls_live(&body);
solve_spline_foot(&target, &spans, seed)
});
if let Some(hit) = resolved {
self.constraints[index].set_foot_t(hit.global());
}
}
}
}