use crate::{DiagnosticSeverity, KernelDiagnostics, KernelStage, KnotVector, NurbsCurve, Vec3, Vec4};
pub fn solve_dense(mut matrix: Vec<Vec<f64>>, mut rhs: Vec<f64>) -> Result<Vec<f64>, String> {
let count = rhs.len();
if count == 0 || matrix.len() != count || matrix.iter().any(|row| row.len() != count) {
return Err("solve_dense: matrix must be square and match RHS".into());
}
let scale = matrix
.iter()
.flatten()
.fold(0.0_f64, |maximum, value| maximum.max(value.abs()));
if scale == 0.0 {
return Err("solve_dense: singular matrix".into());
}
for column in 0..count {
let pivot = (column..count)
.max_by(|&a, &b| matrix[a][column].abs().total_cmp(&matrix[b][column].abs()))
.unwrap();
if matrix[pivot][column].abs() <= 1e-13 * scale {
return Err("solve_dense: singular matrix".into());
}
matrix.swap(column, pivot);
rhs.swap(column, pivot);
let diagonal = matrix[column][column];
for row in column + 1..count {
let factor = matrix[row][column] / diagonal;
matrix[row][column] = 0.0;
for entry in column + 1..count {
matrix[row][entry] -= factor * matrix[column][entry];
}
rhs[row] -= factor * rhs[column];
}
}
let mut result = vec![0.0; count];
for row in (0..count).rev() {
let remainder: f64 = (row + 1..count)
.map(|column| matrix[row][column] * result[column])
.sum();
result[row] = (rhs[row] - remainder) / matrix[row][row];
}
Ok(result)
}
pub(crate) fn solve_dense_shared<const K: usize>(
mut matrix: Vec<Vec<f64>>,
mut rhs: [Vec<f64>; K],
) -> Result<[Vec<f64>; K], String> {
let count = rhs.first().map_or(0, Vec::len);
if count == 0
|| matrix.len() != count
|| matrix.iter().any(|row| row.len() != count)
|| rhs.iter().any(|side| side.len() != count)
{
return Err("solve_dense: matrix must be square and match RHS".into());
}
let scale = matrix
.iter()
.flatten()
.fold(0.0_f64, |maximum, value| maximum.max(value.abs()));
if scale == 0.0 {
return Err("solve_dense: singular matrix".into());
}
let mut pivot_row: Vec<(usize, f64)> = Vec::with_capacity(count);
for column in 0..count {
let pivot = (column..count)
.max_by(|&a, &b| matrix[a][column].abs().total_cmp(&matrix[b][column].abs()))
.unwrap();
if matrix[pivot][column].abs() <= 1e-13 * scale {
return Err("solve_dense: singular matrix".into());
}
matrix.swap(column, pivot);
for side in rhs.iter_mut() {
side.swap(column, pivot);
}
let diagonal = matrix[column][column];
pivot_row.clear();
pivot_row.extend(
(column + 1..count)
.map(|entry| (entry, matrix[column][entry]))
.filter(|&(_, value)| value != 0.0),
);
for row in column + 1..count {
let factor = matrix[row][column] / diagonal;
matrix[row][column] = 0.0;
if factor != 0.0 {
let target = &mut matrix[row];
for &(entry, value) in &pivot_row {
target[entry] -= factor * value;
}
}
for side in rhs.iter_mut() {
let subtrahend = factor * side[column];
side[row] -= subtrahend;
}
}
}
let mut results: [Vec<f64>; K] = std::array::from_fn(|_| vec![0.0; count]);
for (side, result) in rhs.iter().zip(results.iter_mut()) {
for row in (0..count).rev() {
let remainder: f64 = (row + 1..count)
.map(|column| matrix[row][column] * result[column])
.sum();
result[row] = (side[row] - remainder) / matrix[row][row];
}
}
Ok(results)
}
pub fn solve_small<const N: usize>(
mut matrix: [[f64; N]; N],
mut rhs: [f64; N],
count: usize,
) -> Result<[f64; N], String> {
if count == 0 || count > N {
return Err("solve_small: invalid system size".into());
}
let scale = matrix
.iter()
.take(count)
.flat_map(|row| row.iter().take(count))
.fold(0.0_f64, |maximum, value| maximum.max(value.abs()));
if scale == 0.0 {
return Err("solve_small: singular matrix".into());
}
for column in 0..count {
let pivot = (column..count)
.max_by(|&a, &b| matrix[a][column].abs().total_cmp(&matrix[b][column].abs()))
.unwrap();
if matrix[pivot][column].abs() <= 1e-13 * scale {
return Err("solve_small: singular matrix".into());
}
matrix.swap(column, pivot);
rhs.swap(column, pivot);
let diagonal = matrix[column][column];
for row in column + 1..count {
let factor = matrix[row][column] / diagonal;
matrix[row][column] = 0.0;
for entry in column + 1..count {
matrix[row][entry] -= factor * matrix[column][entry];
}
rhs[row] -= factor * rhs[column];
}
}
let mut result = [0.0; N];
for row in (0..count).rev() {
let remainder: f64 = (row + 1..count)
.map(|column| matrix[row][column] * result[column])
.sum();
result[row] = (rhs[row] - remainder) / matrix[row][row];
}
Ok(result)
}
pub fn solve_collocation(
matrix: &[Vec<f64>],
rhs: &[f64],
degree: usize,
) -> Result<Vec<f64>, String> {
let count = rhs.len();
let bandwidth = degree.max(1).min(count.saturating_sub(1).max(1));
if let Ok(solution) = solve_banded(matrix, rhs, bandwidth) {
let scale = rhs
.iter()
.fold(0.0_f64, |maximum, value| maximum.max(value.abs()));
let tolerance = 1e-9 * scale.max(1.0);
let mut residual = 0.0_f64;
for row in 0..count {
let lo = row.saturating_sub(bandwidth);
let hi = (row + bandwidth).min(count - 1);
let mut accumulated = 0.0;
for col in lo..=hi {
accumulated += matrix[row][col] * solution[col];
}
residual = residual.max((accumulated - rhs[row]).abs());
}
if residual <= tolerance {
return Ok(solution);
}
}
solve_dense(matrix.to_vec(), rhs.to_vec())
}
pub(crate) struct CollocationBand {
count: usize,
bandwidth: usize,
data: Vec<f64>,
}
impl CollocationBand {
pub(crate) fn new(count: usize, degree: usize) -> Self {
let bandwidth = degree.max(1).min(count.saturating_sub(1).max(1));
CollocationBand { count, bandwidth, data: vec![0.0; count * (2 * bandwidth + 1)] }
}
fn slot(&self, row: usize, column: usize) -> Option<usize> {
let offset = column as isize - row as isize + self.bandwidth as isize;
(offset >= 0 && offset <= 2 * self.bandwidth as isize)
.then(|| row * (2 * self.bandwidth + 1) + offset as usize)
}
pub(crate) fn set(&mut self, row: usize, column: usize, value: f64) -> bool {
match self.slot(row, column) {
Some(index) => {
self.data[index] = value;
true
}
None => value == 0.0,
}
}
fn get(&self, row: usize, column: usize) -> f64 {
self.slot(row, column).map_or(0.0, |index| self.data[index])
}
fn to_dense(&self) -> Vec<Vec<f64>> {
(0..self.count)
.map(|row| (0..self.count).map(|column| self.get(row, column)).collect())
.collect()
}
}
pub(crate) fn solve_collocation_banded(band: &CollocationBand, rhs: &[f64]) -> Result<Vec<f64>, String> {
let count = rhs.len();
if count == 0 || band.count != count {
return Err("solve_banded: matrix must be square and match RHS".into());
}
let bandwidth = band.bandwidth;
if let Ok(solution) = solve_band_storage(band, rhs) {
let scale = rhs
.iter()
.fold(0.0_f64, |maximum, value| maximum.max(value.abs()));
let tolerance = 1e-9 * scale.max(1.0);
let mut residual = 0.0_f64;
for row in 0..count {
let lo = row.saturating_sub(bandwidth);
let hi = (row + bandwidth).min(count - 1);
let mut accumulated = 0.0;
for col in lo..=hi {
accumulated += band.get(row, col) * solution[col];
}
residual = residual.max((accumulated - rhs[row]).abs());
}
if residual <= tolerance {
return Ok(solution);
}
}
solve_dense(band.to_dense(), rhs.to_vec())
}
fn solve_band_storage(band: &CollocationBand, rhs: &[f64]) -> Result<Vec<f64>, String> {
let count = rhs.len();
let bandwidth = band.bandwidth;
let width = 2 * bandwidth + 1;
let mut matrix = band.data.clone();
let mut rhs = rhs.to_vec();
let at = |row: usize, column: usize| row * width + column + bandwidth - row;
for column in 0..count {
let pivot = matrix[at(column, column)];
if pivot.abs() <= 1e-300 {
return Err("solve_banded: singular matrix".into());
}
let maximum_row = (column + bandwidth).min(count - 1);
for row in column + 1..=maximum_row {
let factor = matrix[at(row, column)] / pivot;
if factor == 0.0 {
continue;
}
let maximum_column = (column + bandwidth).min(count - 1);
for entry in column..=maximum_column {
matrix[at(row, entry)] -= factor * matrix[at(column, entry)];
}
rhs[row] -= factor * rhs[column];
}
}
let mut result = vec![0.0; count];
for row in (0..count).rev() {
let mut value = rhs[row];
let maximum_column = (row + bandwidth).min(count - 1);
for column in row + 1..=maximum_column {
value -= matrix[at(row, column)] * result[column];
}
if matrix[at(row, row)].abs() <= 1e-300 {
return Err("solve_banded: singular matrix".into());
}
result[row] = value / matrix[at(row, row)];
}
Ok(result)
}
fn solve_collocation_banded_axes(
band: &CollocationBand,
mut rhs: [Vec<f64>; 3],
) -> Result<[Vec<f64>; 3], String> {
let original = rhs.clone();
let count = band.count;
let bandwidth = band.bandwidth;
let width = 2 * bandwidth + 1;
let at = |row: usize, column: usize| row * width + column + bandwidth - row;
let fallback = || Ok([
solve_collocation_banded(band, &original[0])?,
solve_collocation_banded(band, &original[1])?,
solve_collocation_banded(band, &original[2])?,
]);
if count == 0 || rhs.iter().any(|axis| axis.len() != count) {
return fallback();
}
let mut matrix = band.data.clone();
for column in 0..count {
let pivot = matrix[at(column, column)];
if pivot.abs() <= 1e-300 { return fallback(); }
let maximum_row = (column + bandwidth).min(count - 1);
for row in column + 1..=maximum_row {
let factor = matrix[at(row, column)] / pivot;
if factor == 0.0 { continue; }
let maximum_column = (column + bandwidth).min(count - 1);
for entry in column..=maximum_column {
matrix[at(row, entry)] -= factor * matrix[at(column, entry)];
}
for axis in &mut rhs {
axis[row] -= factor * axis[column];
}
}
}
let mut solutions = std::array::from_fn(|_| vec![0.0; count]);
for axis in 0..3 {
for row in (0..count).rev() {
let mut value = rhs[axis][row];
let maximum_column = (row + bandwidth).min(count - 1);
for column in row + 1..=maximum_column {
value -= matrix[at(row, column)] * solutions[axis][column];
}
if matrix[at(row, row)].abs() <= 1e-300 { return fallback(); }
solutions[axis][row] = value / matrix[at(row, row)];
}
let scale = original[axis].iter().fold(0.0_f64, |m, v| m.max(v.abs()));
let tolerance = 1e-9 * scale.max(1.0);
let mut residual = 0.0_f64;
for row in 0..count {
let lo = row.saturating_sub(bandwidth);
let hi = (row + bandwidth).min(count - 1);
let mut accumulated = 0.0;
for col in lo..=hi {
accumulated += band.get(row, col) * solutions[axis][col];
}
residual = residual.max((accumulated - original[axis][row]).abs());
}
if residual > tolerance {
solutions[axis] = solve_dense(band.to_dense(), original[axis].clone())?;
}
}
Ok(solutions)
}
fn assert_interpolant_bits(before: &NurbsCurve, after: &NurbsCurve) {
let bits = |c: &NurbsCurve| (c.degree,
c.knots.iter().map(|v| v.to_bits()).collect::<Vec<_>>(),
c.control_points.iter().map(|p| (p.x.to_bits(), p.y.to_bits(), p.z.to_bits(), p.w.to_bits())).collect::<Vec<_>>());
assert_eq!(bits(before), bits(after), "shared interpolant elimination moved a coefficient");
}
pub fn solve_banded(
matrix: &[Vec<f64>],
rhs: &[f64],
bandwidth: usize,
) -> Result<Vec<f64>, String> {
let count = rhs.len();
if count == 0 || matrix.len() != count || matrix.iter().any(|row| row.len() != count) {
return Err("solve_banded: matrix must be square and match RHS".into());
}
let mut matrix = matrix.to_vec();
let mut rhs = rhs.to_vec();
for column in 0..count {
let pivot = matrix[column][column];
if pivot.abs() <= 1e-300 {
return Err("solve_banded: singular matrix".into());
}
let maximum_row = (column + bandwidth).min(count - 1);
for row in column + 1..=maximum_row {
let factor = matrix[row][column] / pivot;
if factor == 0.0 {
continue;
}
let maximum_column = (column + bandwidth).min(count - 1);
for entry in column..=maximum_column {
matrix[row][entry] -= factor * matrix[column][entry];
}
rhs[row] -= factor * rhs[column];
}
}
let mut result = vec![0.0; count];
for row in (0..count).rev() {
let mut value = rhs[row];
let maximum_column = (row + bandwidth).min(count - 1);
for column in row + 1..=maximum_column {
value -= matrix[row][column] * result[column];
}
if matrix[row][row].abs() <= 1e-300 {
return Err("solve_banded: singular matrix".into());
}
result[row] = value / matrix[row][row];
}
Ok(result)
}
pub fn interpolate_homogeneous(
points: &[Vec4],
degree: usize,
parameters: &[f64],
) -> Result<NurbsCurve, String> {
if points.len() < 2 || parameters.len() != points.len() {
return Err(
"interpolate_homogeneous: points and parameters must have matching length >= 2".into(),
);
}
if parameters.windows(2).any(|pair| pair[1] <= pair[0]) {
return Err("interpolate_homogeneous: parameters must be strictly increasing".into());
}
let n = points.len() - 1;
let degree = degree.min(n);
let m = n + degree + 1;
let mut knots = vec![0.0; m + 1];
for knot in &mut knots[m - degree..=m] {
*knot = 1.0;
}
for j in 1..=n.saturating_sub(degree) {
knots[j + degree] = parameters[j..j + degree].iter().sum::<f64>() / degree as f64;
}
let knot_vector = KnotVector::new(knots.clone(), degree)?;
let mut matrix = vec![vec![0.0; n + 1]; n + 1];
for (row, ¶meter) in parameters.iter().enumerate() {
let span = knot_vector.find_span(parameter);
let basis = knot_vector.basis_functions(span, parameter);
for (offset, value) in basis.into_iter().enumerate() {
matrix[row][span - degree + offset] = value;
}
}
let solve_axis = |axis: fn(&Vec4) -> f64| {
solve_collocation(
&matrix,
&points.iter().map(axis).collect::<Vec<_>>(),
degree,
)
};
let xs = solve_axis(|point| point.x)?;
let ys = solve_axis(|point| point.y)?;
let zs = solve_axis(|point| point.z)?;
let ws = solve_axis(|point| point.w)?;
NurbsCurve::new(
degree,
knots,
(0..=n)
.map(|index| Vec4 {
x: xs[index],
y: ys[index],
z: zs[index],
w: ws[index],
})
.collect(),
)
}
pub fn interpolate_curve(
points: &[Vec3],
degree: usize,
parameters: &[f64],
) -> Result<NurbsCurve, String> {
let option = std::env::var("BREP_INTERPOLANT_AXIS_REUSE").unwrap_or_default();
let enabled = matches!(option.as_str(), "on" | "1" | "check");
if option == "check" {
let before = interpolate_curve_with_axis_reuse(points, degree, parameters, false);
let after = interpolate_curve_with_axis_reuse(points, degree, parameters, true);
match (&before, &after) {
(Ok(a), Ok(b)) => assert_interpolant_bits(a, b),
(Err(a), Err(b)) => assert_eq!(a, b),
_ => panic!("shared interpolant elimination changed the result"),
}
eprintln!("BREP_INTERPOLANT_AXIS_CENSUS stations={} matched=true", points.len());
return before;
}
interpolate_curve_with_axis_reuse(points, degree, parameters, enabled)
}
fn interpolate_curve_with_axis_reuse(
points: &[Vec3],
degree: usize,
parameters: &[f64],
reuse_axes: bool,
) -> Result<NurbsCurve, String> {
if points.len() < 2 || parameters.len() != points.len() {
return Err(
"interpolate_curve: points and parameters must have matching length >= 2".into(),
);
}
if parameters.windows(2).any(|pair| pair[1] <= pair[0]) {
return Err("interpolate_curve: parameters must be strictly increasing".into());
}
let n = points.len() - 1;
let degree = degree.min(n);
let m = n + degree + 1;
let mut knots = vec![parameters[0]; m + 1];
for knot in &mut knots[m - degree..=m] {
*knot = parameters[n];
}
for j in 1..=n.saturating_sub(degree) {
knots[j + degree] = parameters[j..j + degree].iter().sum::<f64>() / degree as f64;
}
let knot_vector = KnotVector::new(knots.clone(), degree)?;
let mut band = CollocationBand::new(n + 1, degree);
let mut dense: Option<Vec<Vec<f64>>> = None;
for (row, ¶meter) in parameters.iter().enumerate() {
let span = knot_vector.find_span(parameter);
let basis = knot_vector.basis_functions(span, parameter);
for (offset, value) in basis.into_iter().enumerate() {
let column = span - degree + offset;
if !band.set(row, column, value) {
dense.get_or_insert_with(|| band.to_dense())[row][column] = value;
} else if let Some(dense) = dense.as_mut() {
dense[row][column] = value;
}
}
}
let solve_axis = |axis: fn(Vec3) -> f64| {
let rhs = points.iter().copied().map(axis).collect::<Vec<_>>();
match &dense {
Some(matrix) => solve_collocation(matrix, &rhs, degree),
None => solve_collocation_banded(&band, &rhs),
}
};
let [xs, ys, zs] = if reuse_axes && dense.is_none() {
solve_collocation_banded_axes(&band, [
points.iter().map(|p| p.x).collect(),
points.iter().map(|p| p.y).collect(),
points.iter().map(|p| p.z).collect(),
])?
} else {
[solve_axis(|point| point.x)?, solve_axis(|point| point.y)?, solve_axis(|point| point.z)?]
};
NurbsCurve::new(
degree,
knots,
(0..=n)
.map(|index| Vec4::from_point(Vec3::new(xs[index], ys[index], zs[index]), 1.0))
.collect(),
)
}
pub fn interpolate_curve_with_end_tangents(
points: &[Vec3],
parameters: &[f64],
start_tangent: Vec3,
end_tangent: Vec3,
) -> Result<NurbsCurve, String> {
if points.len() < 2 || parameters.len() != points.len() {
return Err(
"interpolate_curve_with_end_tangents: points and parameters must match, >= 2".into(),
);
}
if parameters.windows(2).any(|pair| pair[1] <= pair[0]) {
return Err("interpolate_curve_with_end_tangents: parameters must increase".into());
}
let degree = 3usize;
let n = points.len() - 1;
let control_count = n + 3;
let t0 = parameters[0];
let t1 = parameters[n];
const PT_KNOTS_DEFAULT: bool = true;
static PT_KNOTS: std::sync::OnceLock<bool> = std::sync::OnceLock::new();
let pt_knots = *PT_KNOTS.get_or_init(|| {
match std::env::var("BREP_FIT_PT_KNOTS").as_deref() {
Ok("0") => false,
Ok("1") => true,
_ => PT_KNOTS_DEFAULT,
}
});
let mut knots = vec![t0; degree + 1];
for j in 0..n.saturating_sub(1) {
let window = if pt_knots {
¶meters[j..j + degree]
} else {
¶meters[j + 1..(j + degree).min(n) + 1]
};
knots.push(window.iter().sum::<f64>() / window.len() as f64);
}
knots.extend(std::iter::repeat(t1).take(degree + 1));
if knots.len() != control_count + degree + 1 {
return Err(format!(
"interpolate_curve_with_end_tangents: internal knot count {} for {} controls",
knots.len(),
control_count
));
}
let knot_vector = KnotVector::new(knots.clone(), degree)?;
let mut matrix = vec![vec![0.0; control_count]; control_count];
let mut rhs_points = vec![Vec3::default(); control_count];
matrix[0][0] = 1.0;
rhs_points[0] = points[0];
let start_span = knots[degree + 1] - t0;
matrix[1][0] = -(degree as f64) / start_span;
matrix[1][1] = (degree as f64) / start_span;
rhs_points[1] = start_tangent;
for (index, ¶meter) in parameters.iter().enumerate().take(n).skip(1) {
let row = index + 1;
let span = knot_vector.find_span(parameter);
let basis = knot_vector.basis_functions(span, parameter);
for (offset, value) in basis.into_iter().enumerate() {
matrix[row][span - degree + offset] = value;
}
rhs_points[row] = points[index];
}
let end_span = t1 - knots[control_count - 1];
matrix[control_count - 2][control_count - 2] = -(degree as f64) / end_span;
matrix[control_count - 2][control_count - 1] = (degree as f64) / end_span;
rhs_points[control_count - 2] = end_tangent;
matrix[control_count - 1][control_count - 1] = 1.0;
rhs_points[control_count - 1] = points[n];
let solve_axis = |axis: fn(Vec3) -> f64| {
solve_dense(
matrix.clone(),
rhs_points.iter().copied().map(axis).collect::<Vec<_>>(),
)
};
let xs = solve_axis(|point| point.x)?;
let ys = solve_axis(|point| point.y)?;
let zs = solve_axis(|point| point.z)?;
NurbsCurve::new(
degree,
knots,
(0..control_count)
.map(|index| Vec4::from_point(Vec3::new(xs[index], ys[index], zs[index]), 1.0))
.collect(),
)
}
pub fn interpolate_curve_thinned(
points: &[Vec3],
degree: usize,
maximum_points: usize,
) -> Result<NurbsCurve, String> {
if maximum_points < 2 {
return Err("interpolate_curve_thinned: maximum point count must be at least 2".into());
}
if points.len() <= maximum_points {
let parameters = chord_parameters(points);
return interpolate_curve(points, degree, ¶meters);
}
let step = (points.len() - 1) as f64 / (maximum_points - 1) as f64;
let thinned = (0..maximum_points)
.map(|index| points[(index as f64 * step).round() as usize])
.collect::<Vec<_>>();
let parameters = chord_parameters(&thinned);
interpolate_curve(&thinned, degree, ¶meters)
}
fn chord_parameters(points: &[Vec3]) -> Vec<f64> {
if points.len() < 2 {
return vec![0.0; points.len()];
}
let mut parameters = vec![0.0; points.len()];
for index in 1..points.len() {
parameters[index] = parameters[index - 1] + points[index].sub(points[index - 1]).length();
}
let length = parameters[points.len() - 1];
if length <= 1e-15 {
for (index, parameter) in parameters.iter_mut().enumerate() {
*parameter = index as f64 / (points.len() - 1) as f64;
}
} else {
for parameter in &mut parameters {
*parameter /= length;
}
}
parameters
}
pub fn simplify_polyline_indices(points: &[Vec3], tolerance: f64) -> Vec<usize> {
if points.len() <= 2 {
return (0..points.len()).collect();
}
let mut keep = vec![false; points.len()];
keep[0] = true;
keep[points.len() - 1] = true;
let mut stack = vec![(0usize, points.len() - 1)];
while let Some((start, end)) = stack.pop() {
if end - start < 2 {
continue;
}
let a = points[start];
let direction = points[end].sub(a);
let length_squared = direction.length_squared().max(1e-300);
let mut worst = None;
let mut worst_distance = tolerance;
for (index, point) in points.iter().enumerate().take(end).skip(start + 1) {
let fraction = point.sub(a).dot(direction) / length_squared;
let fraction = fraction.clamp(0.0, 1.0);
let distance = point.sub(a.add(direction.scale(fraction))).length();
if distance > worst_distance {
worst_distance = distance;
worst = Some(index);
}
}
if let Some(index) = worst {
keep[index] = true;
stack.push((start, index));
stack.push((index, end));
}
}
keep.into_iter()
.enumerate()
.filter_map(|(index, keep)| keep.then_some(index))
.collect()
}
pub fn simplify_polyline(points: &[Vec3], tolerance: f64) -> Vec<Vec3> {
simplify_polyline_indices(points, tolerance)
.into_iter()
.map(|index| points[index])
.collect()
}
pub fn interpolate_curve_local(
points: &[Vec3],
parameters: &[f64],
tension: f64,
) -> Result<NurbsCurve, String> {
if points.len() != parameters.len() || points.len() < 2 {
return Err(
"interpolate_curve_local: points and parameters must have matching length >= 2".into(),
);
}
if points.len() == 2 {
return interpolate_curve(points, 1, parameters);
}
let count = points.len();
let mut tangents = vec![Vec3::default(); count];
tangents[0] = points[1]
.sub(points[0])
.scale(1.0 / (parameters[1] - parameters[0]));
tangents[count - 1] = points[count - 1]
.sub(points[count - 2])
.scale(1.0 / (parameters[count - 1] - parameters[count - 2]));
for index in 1..count - 1 {
let previous_interval = parameters[index] - parameters[index - 1];
let next_interval = parameters[index + 1] - parameters[index];
let total = previous_interval + next_interval;
let previous_secant = points[index]
.sub(points[index - 1])
.scale(1.0 / previous_interval);
let next_secant = points[index + 1]
.sub(points[index])
.scale(1.0 / next_interval);
let mut tangent =
points[index - 1]
.scale(-next_interval / (previous_interval * total))
.add(points[index].scale(
(next_interval - previous_interval) / (previous_interval * next_interval),
))
.add(points[index + 1].scale(previous_interval / (next_interval * total)));
if previous_secant.dot(next_secant) <= 0.0
|| tangent.dot(previous_secant) <= 0.0
|| tangent.dot(next_secant) <= 0.0
{
tangent = Vec3::default();
} else {
let maximum = 3.0 * previous_secant.length().min(next_secant.length());
if tangent.length() > maximum {
tangent = tangent.normalized()?.scale(maximum);
}
}
tangents[index] = tangent;
}
if tension != 1.0 {
for tangent in &mut tangents {
*tangent = tangent.scale(tension);
}
}
let mut control_points = vec![Vec4::from_point(points[0], 1.0)];
for index in 0..count - 1 {
let interval = parameters[index + 1] - parameters[index];
control_points.extend([
Vec4::from_point(
points[index].add(tangents[index].scale(interval / 3.0)),
1.0,
),
Vec4::from_point(
points[index + 1].sub(tangents[index + 1].scale(interval / 3.0)),
1.0,
),
Vec4::from_point(points[index + 1], 1.0),
]);
}
let mut knots = vec![parameters[0]; 4];
for parameter in ¶meters[1..count - 1] {
knots.extend([*parameter; 3]);
}
knots.extend([parameters[count - 1]; 4]);
NurbsCurve::new(3, knots, control_points)
}
const LADDER_BUDGET_FACTOR: usize = 2;
pub const MAX_FIT_STATIONS: usize = 1024;
const MEASURE_NEWTON_STEPS: usize = 3;
const MEASURE_SPAN_FRACTIONS: [f64; 3] = [0.25, 0.5, 0.75];
const STALL_FACTOR: f64 = 0.9;
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct PolylineFitReport {
pub deviation: f64,
pub rms: f64,
pub between_stations: f64,
pub tolerance: f64,
pub floor: f64,
pub stations: usize,
pub controls: usize,
pub input_points: usize,
pub pool_points: usize,
pub dropped_band: f64,
pub rungs: usize,
pub local_lane: bool,
pub lane_switched: bool,
pub exit: PolylineFitExit,
}
impl PolylineFitReport {
pub fn met_tolerance(&self) -> bool {
self.deviation <= self.floor
}
pub fn met_requested_tolerance(&self) -> bool {
self.deviation <= self.tolerance
}
pub fn saturated(&self) -> bool {
self.stations >= self.pool_points
}
}
#[derive(Clone, Copy, Debug, PartialEq, Eq, Hash)]
pub enum PolylineFitExit {
Converged,
StationCeiling,
InputExhausted,
Stalled,
Unrefined,
}
#[derive(Clone, Debug)]
pub struct PolylineFit {
pub curve: NurbsCurve,
pub parameters: Vec<f64>,
pub kept: Vec<Vec3>,
pub report: PolylineFitReport,
}
pub(crate) fn polyline_station_pool(points: &[Vec3], tolerance: f64) -> Vec<usize> {
station_pool(points, tolerance)
}
fn station_pool(points: &[Vec3], tolerance: f64) -> Vec<usize> {
let simplified = simplify_polyline_indices(points, tolerance);
if simplified.len() > 2 {
let total: f64 = simplified
.windows(2)
.map(|pair| points[pair[1]].sub(points[pair[0]]).length())
.sum();
let floor = (tolerance * 0.01).max(total * 1e-4);
let first = points[simplified[0]];
let last = points[simplified[simplified.len() - 1]];
let mut conditioned = vec![simplified[0]];
for index in &simplified[1..simplified.len() - 1] {
let point = points[*index];
if point.sub(first).length() > floor
&& point.sub(last).length() > floor
&& point.sub(points[*conditioned.last().unwrap()]).length() > floor
{
conditioned.push(*index);
}
}
conditioned.push(simplified[simplified.len() - 1]);
conditioned
} else {
let mut distinct: Vec<usize> = Vec::new();
for index in simplified {
if distinct.last().is_none_or(|previous: &usize| {
points[index].sub(points[*previous]).length() > tolerance * 0.01
}) {
distinct.push(index);
}
}
distinct
}
}
fn decimate_stations(pool: &[usize], maximum_points: usize) -> Vec<usize> {
if pool.len() <= maximum_points {
return pool.to_vec();
}
let step = (pool.len() - 1) as f64 / (maximum_points - 1) as f64;
(0..maximum_points)
.map(|index| pool[(index as f64 * step).round() as usize])
.collect()
}
fn station_parameters(points: &[Vec3], stations: &[usize]) -> Option<Vec<f64>> {
let total: f64 = stations
.windows(2)
.map(|pair| points[pair[1]].sub(points[pair[0]]).length())
.sum();
if total <= 0.0 {
return None;
}
let mut parameters = vec![0.0; stations.len()];
let mut accumulated = 0.0;
for index in 1..stations.len() {
accumulated += points[stations[index]]
.sub(points[stations[index - 1]])
.length();
parameters[index] = accumulated / total;
}
*parameters.last_mut().unwrap() = 1.0;
Some(parameters)
}
fn distance_to_segment(point: Vec3, from: Vec3, to: Vec3) -> f64 {
point.distance_to_segment(from, to, 0.0)
}
fn distance_to_curve_near(
curve: &NurbsCurve,
point: Vec3,
seed: f64,
lower: f64,
upper: f64,
) -> Result<f64, String> {
let mut parameter = seed.clamp(lower, upper);
let mut best = f64::INFINITY;
for _ in 0..MEASURE_NEWTON_STEPS {
let derivatives = curve.derivatives_small(parameter, 2)?;
let residual = derivatives[0].sub(point);
best = best.min(residual.length_squared());
let slope = derivatives[1].dot(residual);
let curvature = derivatives[2].dot(residual) + derivatives[1].length_squared();
if curvature.abs() <= f64::MIN_POSITIVE {
break;
}
let next = (parameter - slope / curvature).clamp(lower, upper);
if next == parameter {
break;
}
parameter = next;
}
let residual = curve.evaluate(parameter)?.sub(point).length_squared();
Ok(best.min(residual).sqrt())
}
fn measure_polyline_fit(
curve: &NurbsCurve,
points: &[Vec3],
stations: &[usize],
parameters: &[f64],
arc: &[f64],
) -> Result<(f64, f64, f64), String> {
let mut worst = 0.0_f64;
let mut sum_squares = 0.0_f64;
let mut samples = 0usize;
let mut between = 0.0_f64;
let last = stations.len() - 1;
for span in 0..last {
let (from, to) = (stations[span], stations[span + 1]);
let (t0, t1) = (parameters[span], parameters[span + 1]);
let lower = parameters[span.saturating_sub(1)];
let upper = parameters[(span + 2).min(last)];
let length = arc[to] - arc[from];
for index in from..to {
let seed = if length > 0.0 {
t0 + (t1 - t0) * (arc[index] - arc[from]) / length
} else {
t0
};
let distance = distance_to_curve_near(curve, points[index], seed, lower, upper)?;
worst = worst.max(distance);
sum_squares += distance * distance;
samples += 1;
}
let window_start = from.saturating_sub(1);
let window_end = (to + 1).min(points.len() - 1);
for fraction in MEASURE_SPAN_FRACTIONS {
let sample = curve.evaluate(t0 + (t1 - t0) * fraction)?;
let mut nearest = f64::INFINITY;
for index in window_start..window_end {
nearest = nearest.min(distance_to_segment(
sample,
points[index],
points[index + 1],
));
}
if nearest.is_finite() {
between = between.max(nearest);
}
}
}
let distance = distance_to_curve_near(
curve,
points[stations[last]],
parameters[last],
parameters[last.saturating_sub(1)],
parameters[last],
)?;
worst = worst.max(distance);
sum_squares += distance * distance;
samples += 1;
let rms = (sum_squares / samples as f64).sqrt();
Ok((worst, rms, between))
}
fn dropped_band(points: &[Vec3], pool: &[usize]) -> f64 {
let mut worst = 0.0_f64;
for span in 0..pool.len() - 1 {
let (from, to) = (pool[span], pool[span + 1]);
for index in from + 1..to {
worst = worst.max(distance_to_segment(
points[index],
points[from],
points[to],
));
}
}
worst
}
fn cumulative_chord(points: &[Vec3]) -> Vec<f64> {
let mut arc = vec![0.0; points.len()];
for index in 1..points.len() {
arc[index] = arc[index - 1] + points[index].sub(points[index - 1]).length();
}
arc
}
fn fit_rung(
points: &[Vec3],
pool: &[usize],
arc: &[f64],
maximum_points: usize,
local_interpolation: bool,
degree: usize,
) -> Result<(NurbsCurve, Vec<usize>, Vec<f64>, f64, f64, f64), String> {
let stations = decimate_stations(pool, maximum_points);
fit_station_set(points, stations, arc, local_interpolation, degree)
}
fn fit_station_set(
points: &[Vec3],
stations: Vec<usize>,
arc: &[f64],
local_interpolation: bool,
degree: usize,
) -> Result<(NurbsCurve, Vec<usize>, Vec<f64>, f64, f64, f64), String> {
let split_started = crate::profile::profile_enabled().then(web_time::Instant::now);
let parameters = station_parameters(points, &stations)
.ok_or_else(|| "fit_polyline: degenerate polyline".to_string())?;
let kept = stations.iter().map(|index| points[*index]).collect::<Vec<_>>();
let closed = kept.len() > degree + 1
&& kept[0].sub(kept[kept.len() - 1]).length() <= 1e-12 * (1.0 + kept[0].length());
let params_done = split_started.map(|_| web_time::Instant::now());
let curve = if local_interpolation {
interpolate_curve_local(&kept, ¶meters, 1.0)?
} else if closed
&& degree > 3
&& degree % 2 == 1
&& std::env::var("BREP_SECTION_FIT_PERIODIC").as_deref() != Ok("0")
{
interpolate_curve_closed_of_degree(&kept[..kept.len() - 1], ¶meters, degree)?
} else {
interpolate_curve(&kept, degree.min(kept.len() - 1), ¶meters)?
};
let interp_done = split_started.map(|_| web_time::Instant::now());
let (deviation, rms, between) =
measure_polyline_fit(&curve, points, &stations, ¶meters, arc)?;
if let (Some(a), Some(b), Some(c)) = (split_started, params_done, interp_done) {
STATION_SET_SPLIT.with(|cell| {
let mut cell = cell.borrow_mut();
cell.0 += 1;
cell.1 += stations.len() as u64;
cell.2 += points.len() as u64;
cell.3 += (b - a).as_secs_f64() * 1e3;
cell.4 += (c - b).as_secs_f64() * 1e3;
cell.5 += c.elapsed().as_secs_f64() * 1e3;
});
}
Ok((curve, stations, parameters, deviation, rms, between))
}
thread_local! {
static STATION_SET_SPLIT: std::cell::RefCell<(u64, u64, u64, f64, f64, f64)> = const { std::cell::RefCell::new((0, 0, 0, 0.0, 0.0, 0.0)) };
}
pub(crate) fn station_set_split_take() -> (u64, u64, u64, f64, f64, f64) {
STATION_SET_SPLIT.with(|cell| std::mem::take(&mut *cell.borrow_mut()))
}
pub fn fit_polyline_at(
points: &[Vec3],
tolerance: f64,
maximum_points: usize,
local_interpolation: bool,
) -> Result<PolylineFit, String> {
let fit = fit_polyline_inner(points, tolerance, maximum_points, local_interpolation, false, 3)?;
record_polyline_fit(&fit.report);
Ok(fit)
}
pub fn fit_polyline(
points: &[Vec3],
tolerance: f64,
maximum_points: usize,
local_interpolation: bool,
) -> Result<PolylineFit, String> {
fit_polyline_of_degree(points, tolerance, maximum_points, local_interpolation, 3)
}
pub fn fit_polyline_of_degree(
points: &[Vec3],
tolerance: f64,
maximum_points: usize,
local_interpolation: bool,
degree: usize,
) -> Result<PolylineFit, String> {
fit_polyline_of_degree_with_recovery(points, tolerance, maximum_points, local_interpolation, degree)
.map(|(fit, _)| fit)
}
pub(crate) fn fit_polyline_of_degree_with_recovery(
points: &[Vec3],
tolerance: f64,
maximum_points: usize,
local_interpolation: bool,
degree: usize,
) -> Result<(PolylineFit, bool), String> {
let refine = std::env::var("BREP_POLYLINE_FIT_REFINE").as_deref() != Ok("0");
let asked = fit_polyline_inner(points, tolerance, maximum_points, local_interpolation, refine, degree)?;
if asked.report.met_tolerance() || !refine {
record_polyline_fit(&asked.report);
return Ok((asked, false));
}
let mut other =
fit_polyline_inner(points, tolerance, maximum_points, !local_interpolation, refine, degree)?;
let spent = asked.report.rungs + other.report.rungs;
let mut best = if other.report.deviation < asked.report.deviation {
other.report.lane_switched = true;
other
} else {
asked
};
let mut spent = spent;
let mut recovered = false;
if !best.report.met_tolerance() {
let (adaptive, rounds) = fit_polyline_adaptive(points, tolerance, maximum_points, degree)?;
spent += rounds;
if let Some(mut adaptive) = adaptive {
adaptive.report.lane_switched = local_interpolation;
best = adaptive;
recovered = true;
}
}
best.report.rungs = spent;
record_polyline_fit(&best.report);
Ok((best, recovered))
}
fn fit_polyline_adaptive(
points: &[Vec3],
tolerance: f64,
maximum_points: usize,
degree: usize,
) -> Result<(Option<PolylineFit>, usize), String> {
let pool = station_pool(points, tolerance);
let arc = cumulative_chord(points);
let band = dropped_band(points, &pool);
let floor = tolerance.max(band);
let budget = maximum_points.max(2);
let ceiling = pool.len().min(MAX_FIT_STATIONS)
.min(budget.saturating_mul(LADDER_BUDGET_FACTOR)).max(budget);
let mut stations = decimate_stations(&pool, budget);
for round in 0..=8 {
let Ok((curve, current, parameters, deviation, rms, between)) =
fit_station_set(points, stations, &arc, false, degree)
else { return Ok((None, round + 1)); };
stations = current;
if deviation <= floor {
let report = PolylineFitReport {
deviation, rms, between_stations: between, tolerance, floor,
stations: stations.len(), controls: curve.control_points.len(),
input_points: points.len(), pool_points: pool.len(), dropped_band: band,
rungs: round + 1, local_lane: false, lane_switched: false,
exit: PolylineFitExit::Converged,
};
return Ok((Some(PolylineFit {
curve, parameters, kept: stations.iter().map(|&i| points[i]).collect(), report,
}), round + 1));
}
if stations.len() >= ceiling || round == 8 { return Ok((None, round + 1)); }
let mut candidates = Vec::new();
for span in 0..stations.len() - 1 {
let (from, to) = (stations[span], stations[span + 1]);
let (t0, t1) = (parameters[span], parameters[span + 1]);
let lower = parameters[span.saturating_sub(1)];
let upper = parameters[(span + 2).min(stations.len() - 1)];
let mut candidate: Option<(usize, f64)> = None;
for &index in pool.iter().filter(|&&i| i > from && i < to) {
let length = arc[to] - arc[from];
let seed = if length > 0.0 {
t0 + (t1 - t0) * (arc[index] - arc[from]) / length
} else { t0 };
let error = distance_to_curve_near(&curve, points[index], seed, lower, upper)?;
if candidate.is_none_or(|(_, previous)| error > previous) {
candidate = Some((index, error));
}
}
if let Some((index, error)) = candidate {
if error > floor { candidates.push((index, error)); }
}
}
candidates.sort_by(|a, b| b.1.total_cmp(&a.1).then_with(|| a.0.cmp(&b.0)));
let added = candidates.len().min(16).min(ceiling - stations.len());
if added == 0 { return Ok((None, round + 1)); }
stations.extend(candidates.iter().take(added).map(|&(index, _)| index));
stations.sort_unstable();
}
unreachable!("the bounded refinement returns on its last round")
}
#[derive(Clone, Copy, Debug, PartialEq, Eq)]
pub(crate) enum SeededPath {
Accepted { rounds: usize },
Fallback(SeededFallback),
}
#[derive(Clone, Copy, Debug, PartialEq, Eq)]
pub(crate) enum SeededFallback {
NonFiniteInput,
Degenerate,
RefineDisabled,
PoolTooSmall,
NonFiniteBandOrArc,
NonFiniteFloor,
SeedsOverCeiling,
SeedsOutsideStationPool,
StationSetFailed,
NonFiniteFitReading,
SpanUnreadable,
CeilingReached,
RoundsSpent,
NothingToAdd,
EndpointUnreadable,
TightStationOverTolerance,
}
#[derive(Clone, Copy, Debug, Default, PartialEq)]
pub(crate) struct RepresentativeCost {
pub(crate) representatives: usize,
pub(crate) witnesses: usize,
pub(crate) candidate_fits: usize,
pub(crate) full_refit: bool,
pub(crate) start_offset: f64,
pub(crate) end_offset: f64,
}
pub(crate) fn polyline_station_pool_floor(points: &[Vec3], tolerance: f64) -> f64 {
let simplified = simplify_polyline_indices(points, tolerance);
if simplified.len() > 2 {
let total: f64 = simplified.windows(2).map(|pair| points[pair[1]].sub(points[pair[0]]).length()).sum();
(tolerance * 0.01).max(total * 1e-4)
} else {
tolerance * 0.01
}
}
pub(crate) fn fit_polyline_seeded_with_path(
points: &[Vec3],
seeds: &[usize],
tolerance: f64,
full_budget: usize,
local_interpolation: bool,
degree: usize,
) -> Result<(PolylineFit, SeededPath), String> {
let full_refit = |reason: SeededFallback| {
fit_polyline_of_degree(points, tolerance, full_budget, local_interpolation, degree)
.map(|fit| (fit, SeededPath::Fallback(reason)))
};
if !tolerance.is_finite() || points.iter().any(|p| !p.x.is_finite() || !p.y.is_finite() || !p.z.is_finite()) {
return full_refit(SeededFallback::NonFiniteInput);
}
if points.len() < 3 {
return full_refit(SeededFallback::Degenerate);
}
if std::env::var("BREP_POLYLINE_FIT_REFINE").as_deref() == Ok("0") {
return full_refit(SeededFallback::RefineDisabled);
}
let pool = station_pool(points, tolerance);
if pool.len() < 2 {
return full_refit(SeededFallback::PoolTooSmall);
}
let arc = cumulative_chord(points);
let band = dropped_band(points, &pool);
if !band.is_finite() || arc.iter().any(|value| !value.is_finite()) {
return full_refit(SeededFallback::NonFiniteBandOrArc);
}
let floor = tolerance.max(band);
if !floor.is_finite() {
return full_refit(SeededFallback::NonFiniteFloor);
}
let budget = full_budget.max(2);
let ceiling = pool
.len()
.min(MAX_FIT_STATIONS)
.min(budget.saturating_mul(LADDER_BUDGET_FACTOR))
.max(budget);
let mut stations = seeds
.iter()
.copied()
.filter(|&index| index < points.len())
.chain([0, points.len() - 1])
.collect::<Vec<_>>();
stations.sort_unstable();
stations.dedup();
if stations.len() > ceiling {
return full_refit(SeededFallback::SeedsOverCeiling);
}
if stations.iter().any(|index| pool.binary_search(index).is_err()) {
return full_refit(SeededFallback::SeedsOutsideStationPool);
}
for round in 0..=8 {
let Ok((curve, current, parameters, deviation, rms, between)) =
fit_station_set(points, stations, &arc, local_interpolation, degree)
else {
return full_refit(SeededFallback::StationSetFailed);
};
stations = current;
if !deviation.is_finite() || !rms.is_finite() || !between.is_finite() {
return full_refit(SeededFallback::NonFiniteFitReading);
}
if deviation <= floor {
let report = PolylineFitReport {
deviation,
rms,
between_stations: between,
tolerance,
floor,
stations: stations.len(),
controls: curve.control_points.len(),
input_points: points.len(),
pool_points: pool.len(),
dropped_band: band,
rungs: round + 1,
local_lane: local_interpolation,
lane_switched: false,
exit: PolylineFitExit::Converged,
};
record_polyline_fit(&report);
let kept = stations.iter().map(|&index| points[index]).collect();
return Ok((PolylineFit { curve, parameters, kept, report }, SeededPath::Accepted { rounds: round + 1 }));
}
if stations.len() >= ceiling {
return full_refit(SeededFallback::CeilingReached);
}
if round == 8 {
return full_refit(SeededFallback::RoundsSpent);
}
let mut candidates = Vec::new();
for span in 0..stations.len() - 1 {
let (from, to) = (stations[span], stations[span + 1]);
let (t0, t1) = (parameters[span], parameters[span + 1]);
let lower = parameters[span.saturating_sub(1)];
let upper = parameters[(span + 2).min(stations.len() - 1)];
let mut candidate: Option<(usize, f64)> = None;
for &index in pool.iter().filter(|&&i| i > from && i < to) {
let length = arc[to] - arc[from];
let seed = if length > 0.0 { t0 + (t1 - t0) * (arc[index] - arc[from]) / length } else { t0 };
let Ok(error) = distance_to_curve_near(&curve, points[index], seed, lower, upper) else {
return full_refit(SeededFallback::SpanUnreadable);
};
if !error.is_finite() {
return full_refit(SeededFallback::SpanUnreadable);
}
if candidate.is_none_or(|(_, previous)| error > previous) {
candidate = Some((index, error));
}
}
if let Some((index, error)) = candidate {
if error > floor {
candidates.push((index, error));
}
}
}
candidates.sort_by(|a, b| b.1.total_cmp(&a.1).then_with(|| a.0.cmp(&b.0)));
let added = candidates.len().min(16).min(ceiling - stations.len());
if added == 0 {
return full_refit(SeededFallback::NothingToAdd);
}
stations.extend(candidates.iter().take(added).map(|&(index, _)| index));
stations.sort_unstable();
}
full_refit(SeededFallback::RoundsSpent)
}
pub(crate) fn fit_polyline_representatives_with_path(
points: &[Vec3],
seeds: &[usize],
tolerance: f64,
full_budget: usize,
local_interpolation: bool,
degree: usize,
) -> Result<(PolylineFit, SeededPath, RepresentativeCost), String> {
fit_polyline_representatives_with_witnesses(points, seeds, &[], tolerance, tolerance, full_budget, local_interpolation, degree)
}
#[allow(clippy::too_many_arguments)]
pub(crate) fn fit_polyline_representatives_with_witnesses(
points: &[Vec3],
seeds: &[usize],
tight: &[usize],
tight_tolerance: f64,
tolerance: f64,
full_budget: usize,
local_interpolation: bool,
degree: usize,
) -> Result<(PolylineFit, SeededPath, RepresentativeCost), String> {
let mut cost = RepresentativeCost { witnesses: points.len(), start_offset: f64::NAN, end_offset: f64::NAN, ..RepresentativeCost::default() };
let full_refit = |reason: SeededFallback, mut cost: RepresentativeCost| {
cost.full_refit = true;
fit_polyline_of_degree(points, tolerance, full_budget, local_interpolation, degree)
.map(|fit| (fit, SeededPath::Fallback(reason), cost))
};
if !tolerance.is_finite()
|| !tight_tolerance.is_finite()
|| points.iter().any(|p| !p.x.is_finite() || !p.y.is_finite() || !p.z.is_finite())
{
return full_refit(SeededFallback::NonFiniteInput, cost);
}
if points.len() < 3 {
return full_refit(SeededFallback::Degenerate, cost);
}
if std::env::var("BREP_POLYLINE_FIT_REFINE").as_deref() == Ok("0") {
return full_refit(SeededFallback::RefineDisabled, cost);
}
let mut tight: Vec<usize> = tight.iter().copied().filter(|&index| index < points.len()).collect();
tight.sort_unstable();
tight.dedup();
let pool = station_pool(points, tolerance);
if pool.len() < 2 || pool[0] != 0 || pool[pool.len() - 1] != points.len() - 1 {
return full_refit(SeededFallback::PoolTooSmall, cost);
}
let arc = cumulative_chord(points);
let band = dropped_band(points, &pool);
if !band.is_finite() || arc.iter().any(|value| !value.is_finite()) {
return full_refit(SeededFallback::NonFiniteBandOrArc, cost);
}
let budget = full_budget.max(2);
let ceiling = pool
.len()
.min(MAX_FIT_STATIONS)
.min(budget.saturating_mul(LADDER_BUDGET_FACTOR))
.max(budget);
let mut stations = seeds
.iter()
.copied()
.filter(|index| pool.binary_search(index).is_ok())
.chain([0, points.len() - 1])
.collect::<Vec<_>>();
stations.sort_unstable();
stations.dedup();
cost.representatives = stations.len();
if stations.len() > ceiling {
return full_refit(SeededFallback::SeedsOverCeiling, cost);
}
for round in 0..=8 {
cost.candidate_fits += 1;
let Ok((curve, current, parameters, deviation, rms, between)) =
fit_station_set(points, stations, &arc, local_interpolation, degree)
else {
return full_refit(SeededFallback::StationSetFailed, cost);
};
stations = current;
if !deviation.is_finite() || !rms.is_finite() || !between.is_finite() {
return full_refit(SeededFallback::NonFiniteFitReading, cost);
}
let mut tight_over: Vec<usize> = Vec::new();
for &index in &tight {
let span = match stations.binary_search(&index) {
Ok(position) => {
let distance = curve.evaluate(parameters[position]).map(|p| p.sub(points[index]).length());
match distance {
Ok(distance) if distance.is_finite() => {
if distance > tight_tolerance {
return full_refit(SeededFallback::TightStationOverTolerance, cost);
}
}
_ => return full_refit(SeededFallback::SpanUnreadable, cost),
}
continue;
}
Err(at) if at == 0 || at >= stations.len() => return full_refit(SeededFallback::SpanUnreadable, cost),
Err(at) => at - 1,
};
let (from, to) = (stations[span], stations[span + 1]);
let (t0, t1) = (parameters[span], parameters[span + 1]);
let length = arc[to] - arc[from];
let seed = if length > 0.0 { t0 + (t1 - t0) * (arc[index] - arc[from]) / length } else { t0 };
let lower = parameters[span.saturating_sub(1)];
let upper = parameters[(span + 2).min(stations.len() - 1)];
let Ok(error) = distance_to_curve_near(&curve, points[index], seed, lower, upper) else {
return full_refit(SeededFallback::SpanUnreadable, cost);
};
if !error.is_finite() {
return full_refit(SeededFallback::SpanUnreadable, cost);
}
if error > tight_tolerance {
tight_over.push(index);
}
}
if deviation <= tolerance && tight_over.is_empty() {
let ends = curve
.domain()
.and_then(|[t0, t1]| Ok((curve.evaluate(t0)?, curve.evaluate(t1)?)));
let Ok((start, end)) = ends else {
return full_refit(SeededFallback::EndpointUnreadable, cost);
};
cost.start_offset = start.sub(points[0]).length();
cost.end_offset = end.sub(points[points.len() - 1]).length();
if !(cost.start_offset.is_finite() && cost.end_offset.is_finite()) {
cost.start_offset = f64::NAN;
cost.end_offset = f64::NAN;
return full_refit(SeededFallback::EndpointUnreadable, cost);
}
let report = PolylineFitReport {
deviation,
rms,
between_stations: between,
tolerance,
floor: tolerance,
stations: stations.len(),
controls: curve.control_points.len(),
input_points: points.len(),
pool_points: pool.len(),
dropped_band: band,
rungs: round + 1,
local_lane: local_interpolation,
lane_switched: false,
exit: PolylineFitExit::Converged,
};
record_polyline_fit(&report);
let kept = stations.iter().map(|&index| points[index]).collect();
return Ok((PolylineFit { curve, parameters, kept, report }, SeededPath::Accepted { rounds: round + 1 }, cost));
}
if stations.len() >= ceiling {
return full_refit(SeededFallback::CeilingReached, cost);
}
if round == 8 {
return full_refit(SeededFallback::RoundsSpent, cost);
}
let mut candidates = Vec::new();
for span in 0..stations.len() - 1 {
let (from, to) = (stations[span], stations[span + 1]);
let (t0, t1) = (parameters[span], parameters[span + 1]);
let lower = parameters[span.saturating_sub(1)];
let upper = parameters[(span + 2).min(stations.len() - 1)];
let mut candidate: Option<(usize, f64)> = None;
for &index in pool.iter().filter(|&&i| i > from && i < to) {
let length = arc[to] - arc[from];
let seed = if length > 0.0 { t0 + (t1 - t0) * (arc[index] - arc[from]) / length } else { t0 };
let Ok(error) = distance_to_curve_near(&curve, points[index], seed, lower, upper) else {
return full_refit(SeededFallback::SpanUnreadable, cost);
};
if !error.is_finite() {
return full_refit(SeededFallback::SpanUnreadable, cost);
}
if candidate.is_none_or(|(_, previous)| error > previous) {
candidate = Some((index, error));
}
}
if let Some((index, error)) = candidate {
if error > tolerance {
candidates.push((index, error));
}
}
}
for &index in &tight_over {
let at = stations.binary_search(&index).unwrap_err();
let (from, to) = (stations[at - 1], stations[at]);
let nearest = pool
.iter()
.copied()
.filter(|&member| member > from && member < to)
.min_by(|&a, &b| (arc[a] - arc[index]).abs().total_cmp(&(arc[b] - arc[index]).abs()).then_with(|| a.cmp(&b)));
if let Some(member) = nearest {
match candidates.iter_mut().find(|(chosen, _)| *chosen == member) {
Some(entry) => entry.1 = f64::INFINITY,
None => candidates.push((member, f64::INFINITY)),
}
}
}
candidates.sort_by(|a, b| b.1.total_cmp(&a.1).then_with(|| a.0.cmp(&b.0)));
let added = candidates.len().min(16).min(ceiling - stations.len());
if added == 0 {
return full_refit(SeededFallback::NothingToAdd, cost);
}
stations.extend(candidates.iter().take(added).map(|&(index, _)| index));
stations.sort_unstable();
}
full_refit(SeededFallback::RoundsSpent, cost)
}
fn fit_polyline_inner(
points: &[Vec3],
tolerance: f64,
maximum_points: usize,
local_interpolation: bool,
refine: bool,
degree: usize,
) -> Result<PolylineFit, String> {
let pool = station_pool(points, tolerance);
if pool.len() < 2 {
return Err("fit_polyline: degenerate polyline".into());
}
let arc = cumulative_chord(points);
let band = dropped_band(points, &pool);
let floor = tolerance.max(band);
let mut budget = maximum_points.max(2);
let ceiling = pool
.len()
.min(MAX_FIT_STATIONS)
.min(budget.saturating_mul(LADDER_BUDGET_FACTOR))
.max(budget);
let (mut curve, mut stations, mut parameters, mut deviation, mut rms, mut between) =
fit_rung(points, &pool, &arc, budget, local_interpolation, degree)?;
let mut rungs = 1usize;
let mut exit = if deviation <= floor {
PolylineFitExit::Converged
} else {
PolylineFitExit::Unrefined
};
while refine && deviation > floor {
if budget >= ceiling {
exit = if ceiling >= pool.len() {
PolylineFitExit::InputExhausted
} else {
PolylineFitExit::StationCeiling
};
break;
}
budget = (budget * 2).min(ceiling);
let rung = fit_rung(points, &pool, &arc, budget, local_interpolation, degree)?;
rungs += 1;
if rung.3 >= deviation * STALL_FACTOR {
if rung.3 < deviation {
(curve, stations, parameters, deviation, rms, between) = rung;
}
exit = if deviation <= floor {
PolylineFitExit::Converged
} else {
PolylineFitExit::Stalled
};
break;
}
(curve, stations, parameters, deviation, rms, between) = rung;
if deviation <= floor {
exit = PolylineFitExit::Converged;
}
}
let report = PolylineFitReport {
deviation,
rms,
between_stations: between,
tolerance,
floor,
stations: stations.len(),
controls: curve.control_points.len(),
input_points: points.len(),
pool_points: pool.len(),
dropped_band: band,
rungs,
local_lane: local_interpolation,
lane_switched: false,
exit,
};
Ok(PolylineFit {
curve,
parameters,
kept: stations.iter().map(|index| points[*index]).collect(),
report,
})
}
#[derive(Clone, Copy, Debug, Default, PartialEq)]
pub struct PolylineFitLedger {
pub fits: u64,
pub unmet: u64,
pub station_ceiling: u64,
pub input_exhausted: u64,
pub stalled: u64,
pub unrefined: u64,
pub refined: u64,
pub met_at_pool: u64,
pub lane_switched: u64,
pub extra_rungs: u64,
pub worst_unmet_deviation: f64,
pub worst_dropped_band: f64,
}
impl PolylineFitLedger {
fn record(&mut self, report: &PolylineFitReport) {
self.fits += 1;
self.extra_rungs += (report.rungs - 1) as u64;
if report.rungs > 1 {
self.refined += 1;
}
self.worst_dropped_band = self.worst_dropped_band.max(report.dropped_band);
if report.lane_switched {
self.lane_switched += 1;
}
if report.met_tolerance() {
if report.saturated() {
self.met_at_pool += 1;
}
return;
}
self.unmet += 1;
match report.exit {
PolylineFitExit::Converged => {}
PolylineFitExit::StationCeiling => self.station_ceiling += 1,
PolylineFitExit::InputExhausted => self.input_exhausted += 1,
PolylineFitExit::Stalled => self.stalled += 1,
PolylineFitExit::Unrefined => self.unrefined += 1,
}
self.worst_unmet_deviation = self.worst_unmet_deviation.max(report.deviation);
}
fn fold(&mut self, child: &PolylineFitLedger) {
self.fits += child.fits;
self.unmet += child.unmet;
self.station_ceiling += child.station_ceiling;
self.input_exhausted += child.input_exhausted;
self.stalled += child.stalled;
self.unrefined += child.unrefined;
self.refined += child.refined;
self.met_at_pool += child.met_at_pool;
self.lane_switched += child.lane_switched;
self.extra_rungs += child.extra_rungs;
self.worst_unmet_deviation = self.worst_unmet_deviation.max(child.worst_unmet_deviation);
self.worst_dropped_band = self.worst_dropped_band.max(child.worst_dropped_band);
}
pub fn report_into(&self, diagnostics: &mut KernelDiagnostics) {
diagnostics.count_n("fit.polyline.fits", self.fits);
diagnostics.count_n("fit.polyline.unmet_tolerance", self.unmet);
diagnostics.count_n("fit.polyline.exit.station_ceiling", self.station_ceiling);
diagnostics.count_n("fit.polyline.exit.input_exhausted", self.input_exhausted);
diagnostics.count_n("fit.polyline.exit.stalled", self.stalled);
diagnostics.count_n("fit.polyline.exit.unrefined", self.unrefined);
diagnostics.count_n("fit.polyline.refined", self.refined);
diagnostics.count_n("fit.polyline.met_at_pool", self.met_at_pool);
diagnostics.count_n("fit.polyline.lane_switched", self.lane_switched);
diagnostics.count_n("fit.polyline.extra_rungs", self.extra_rungs);
diagnostics.measure_max("fit.polyline.worst_unmet_deviation", self.worst_unmet_deviation);
diagnostics.measure_max("fit.polyline.worst_dropped_band", self.worst_dropped_band);
if self.unmet > 0 {
diagnostics.event(
DiagnosticSeverity::Degraded,
KernelStage::Intersect,
"fit.polyline.unmet_tolerance",
format!(
"{} of {} polyline fits did not reach the tolerance they were given \
(station ceiling {}, input exhausted {}, stalled {}, unrefined {}); \
worst deviation {:.3e}; the simplification's own band reaches {:.3e}",
self.unmet,
self.fits,
self.station_ceiling,
self.input_exhausted,
self.stalled,
self.unrefined,
self.worst_unmet_deviation,
self.worst_dropped_band,
),
);
}
}
}
thread_local! {
static POLYLINE_FIT_SCOPES: std::cell::RefCell<Vec<PolylineFitLedger>> =
const { std::cell::RefCell::new(Vec::new()) };
}
pub struct PolylineFitScope {
_thread_bound: std::marker::PhantomData<*const ()>,
}
impl PolylineFitScope {
pub fn open() -> Self {
POLYLINE_FIT_SCOPES.with(|scopes| scopes.borrow_mut().push(PolylineFitLedger::default()));
Self {
_thread_bound: std::marker::PhantomData,
}
}
pub fn close(self) -> PolylineFitLedger {
let ledger = POLYLINE_FIT_SCOPES.with(|scopes| {
let mut scopes = scopes.borrow_mut();
let ledger = scopes.pop().unwrap_or_default();
if let Some(parent) = scopes.last_mut() {
parent.fold(&ledger);
}
ledger
});
std::mem::forget(self);
ledger
}
}
impl Drop for PolylineFitScope {
fn drop(&mut self) {
POLYLINE_FIT_SCOPES.with(|scopes| {
scopes.borrow_mut().pop();
});
}
}
fn record_polyline_fit(report: &PolylineFitReport) {
if std::env::var_os("BREP_DEBUG_POLYLINE_FIT").is_some() {
eprintln!("POLYLINE-FIT {report:?}");
}
POLYLINE_FIT_SCOPES.with(|scopes| {
if let Some(open) = scopes.borrow_mut().last_mut() {
open.record(report);
}
});
}
fn raw_basis(knots: &[f64], degree: usize, span: usize, parameter: f64) -> Vec<f64> {
crate::curve::basis_functions_raw(knots, degree, span, parameter, 0.0)
}
fn raw_insert_knot(knots: &mut Vec<f64>, controls: &mut Vec<Vec3>, degree: usize, parameter: f64) {
let span = knots
.iter()
.rposition(|&knot| knot <= parameter + 1e-14)
.unwrap()
.min(controls.len() - 1);
let (fresh, fresh_knots) = crate::curve::boehm_insert_raw(controls, knots, degree, parameter, span,
|previous, current, numerator, denominator| {
let alpha = if denominator.abs() > 0.0 { numerator / denominator } else { 0.0 };
previous.scale(1.0 - alpha).add(current.scale(alpha))
});
*controls = fresh;
*knots = fresh_knots;
}
fn closed_collocation_matrix(raw_knots: &[f64], parameters: &[f64], degree: usize, station_count: usize) -> Vec<Vec<f64>> {
let mut matrix = vec![vec![0.0; station_count]; station_count];
for i in 0..station_count {
let span = i + degree;
let basis = raw_basis(raw_knots, degree, span, parameters[i]);
for (offset, value) in basis.iter().enumerate() {
let index = (i as i64 + offset as i64 - degree as i64).rem_euclid(station_count as i64) as usize;
matrix[i][index] += value;
}
}
matrix
}
pub fn interpolate_curve_closed(points: &[Vec3], parameters: &[f64]) -> Result<NurbsCurve, String> {
interpolate_curve_closed_of_degree(points, parameters, 3)
}
pub fn interpolate_curve_closed_of_degree(
points: &[Vec3],
parameters: &[f64],
degree: usize,
) -> Result<NurbsCurve, String> {
if degree % 2 == 0 || degree < 3 {
return Err("interpolate_curve_closed: the degree must be odd and at least 3".into());
}
let station_count = points.len();
if station_count < degree + 1 {
return Err("interpolate_curve_closed: need more stations than the degree".into());
}
if parameters.len() != station_count + 1 {
return Err(
"interpolate_curve_closed: parameters must have one more entry than points".into(),
);
}
if parameters.windows(2).any(|pair| pair[1] <= pair[0]) {
return Err("interpolate_curve_closed: parameters must increase".into());
}
let period = parameters[station_count] - parameters[0];
let cyclic = |j: i64| -> f64 {
let s = station_count as i64;
let wrap = j.div_euclid(s);
parameters[j.rem_euclid(s) as usize] + wrap as f64 * period
};
let raw_knots: Vec<f64> = (-(degree as i64)..=(station_count as i64 + degree as i64)).map(cyclic).collect();
let matrix = closed_collocation_matrix(&raw_knots, parameters, degree, station_count);
let [xs, ys, zs] = solve_dense_shared(
matrix,
[
points.iter().map(|point| point.x).collect::<Vec<_>>(),
points.iter().map(|point| point.y).collect::<Vec<_>>(),
points.iter().map(|point| point.z).collect::<Vec<_>>(),
],
)?;
let cyclic_controls: Vec<Vec3> = (0..station_count)
.map(|index| Vec3::new(xs[index], ys[index], zs[index]))
.collect();
let mut window_controls: Vec<Vec3> = (-(degree as i64)..(station_count as i64))
.map(|j| cyclic_controls[j.rem_euclid(station_count as i64) as usize])
.collect();
let mut window_knots = raw_knots.clone();
for _ in 0..degree {
raw_insert_knot(
&mut window_knots,
&mut window_controls,
degree,
parameters[0],
);
}
for _ in 0..degree {
raw_insert_knot(
&mut window_knots,
&mut window_controls,
degree,
parameters[station_count],
);
}
let first = window_knots
.iter()
.position(|&knot| (knot - parameters[0]).abs() < 1e-12)
.ok_or("interpolate_curve_closed: clamp lost the start knot")?;
let last = window_knots
.iter()
.rposition(|&knot| (knot - parameters[station_count]).abs() < 1e-12)
.ok_or("interpolate_curve_closed: clamp lost the end knot")?;
let clamped_knots: Vec<f64> = window_knots[first..=last].to_vec();
let control_count = clamped_knots.len() - degree - 1;
let clamped_controls: Vec<Vec4> = window_controls[first..first + control_count]
.iter()
.map(|point| Vec4::from_point(*point, 1.0))
.collect();
NurbsCurve::new(degree, clamped_knots, clamped_controls)
}