614 lines
17 KiB
Rust
614 lines
17 KiB
Rust
//! Path fitting algorithms for converting raw points to smooth curves
|
|
//!
|
|
//! Provides two main algorithms:
|
|
//! - Ramer-Douglas-Peucker (RDP) simplification for corner detection
|
|
//! - Schneider curve fitting for smooth Bezier curves
|
|
//!
|
|
//! Based on:
|
|
//! - simplify.js by Vladimir Agafonkin
|
|
//! - fit-curve by Philip J. Schneider (Graphics Gems, 1990)
|
|
|
|
use kurbo::{BezPath, Point, Vec2};
|
|
|
|
/// Configuration for RDP simplification
|
|
#[derive(Debug, Clone, Copy)]
|
|
pub struct RdpConfig {
|
|
/// Tolerance for simplification (default: 10.0)
|
|
/// Higher values = more simplification (fewer points)
|
|
pub tolerance: f64,
|
|
/// Whether to use highest quality (skip radial distance filter)
|
|
pub highest_quality: bool,
|
|
}
|
|
|
|
impl Default for RdpConfig {
|
|
fn default() -> Self {
|
|
Self {
|
|
tolerance: 10.0,
|
|
highest_quality: false,
|
|
}
|
|
}
|
|
}
|
|
|
|
/// Configuration for Schneider curve fitting
|
|
#[derive(Debug, Clone, Copy)]
|
|
pub struct SchneiderConfig {
|
|
/// Maximum error tolerance (default: 30.0)
|
|
/// Lower values = more accurate curves (more segments)
|
|
pub max_error: f64,
|
|
}
|
|
|
|
impl Default for SchneiderConfig {
|
|
fn default() -> Self {
|
|
Self { max_error: 30.0 }
|
|
}
|
|
}
|
|
|
|
/// Simplify a polyline using Ramer-Douglas-Peucker algorithm
|
|
///
|
|
/// This is a two-stage process:
|
|
/// 1. Radial distance filter (unless highest_quality is true)
|
|
/// 2. Douglas-Peucker recursive simplification
|
|
pub fn simplify_rdp(points: &[Point], config: RdpConfig) -> Vec<Point> {
|
|
if points.len() <= 2 {
|
|
return points.to_vec();
|
|
}
|
|
|
|
let sq_tolerance = config.tolerance * config.tolerance;
|
|
|
|
let mut simplified = if config.highest_quality {
|
|
points.to_vec()
|
|
} else {
|
|
simplify_radial_dist(points, sq_tolerance)
|
|
};
|
|
|
|
simplified = simplify_douglas_peucker(&simplified, sq_tolerance);
|
|
|
|
simplified
|
|
}
|
|
|
|
/// First stage: Remove points that are too close to the previous point
|
|
fn simplify_radial_dist(points: &[Point], sq_tolerance: f64) -> Vec<Point> {
|
|
if points.is_empty() {
|
|
return Vec::new();
|
|
}
|
|
|
|
let mut result = vec![points[0]];
|
|
let mut prev_point = points[0];
|
|
|
|
for &point in &points[1..] {
|
|
if sq_dist(point, prev_point) > sq_tolerance {
|
|
result.push(point);
|
|
prev_point = point;
|
|
}
|
|
}
|
|
|
|
// Always include the last point if it's different from the previous one
|
|
if let Some(&last) = points.last() {
|
|
if last != prev_point {
|
|
result.push(last);
|
|
}
|
|
}
|
|
|
|
result
|
|
}
|
|
|
|
/// Second stage: Douglas-Peucker recursive simplification
|
|
fn simplify_douglas_peucker(points: &[Point], sq_tolerance: f64) -> Vec<Point> {
|
|
if points.len() < 2 {
|
|
return points.to_vec();
|
|
}
|
|
|
|
let last = points.len() - 1;
|
|
let mut simplified = vec![points[0]];
|
|
simplify_dp_step(points, 0, last, sq_tolerance, &mut simplified);
|
|
simplified.push(points[last]);
|
|
|
|
simplified
|
|
}
|
|
|
|
/// Recursive Douglas-Peucker step
|
|
fn simplify_dp_step(
|
|
points: &[Point],
|
|
first: usize,
|
|
last: usize,
|
|
sq_tolerance: f64,
|
|
simplified: &mut Vec<Point>,
|
|
) {
|
|
let mut max_sq_dist = sq_tolerance;
|
|
let mut index = 0;
|
|
|
|
for i in first + 1..last {
|
|
let sq_dist = sq_seg_dist(points[i], points[first], points[last]);
|
|
|
|
if sq_dist > max_sq_dist {
|
|
index = i;
|
|
max_sq_dist = sq_dist;
|
|
}
|
|
}
|
|
|
|
if max_sq_dist > sq_tolerance {
|
|
if index - first > 1 {
|
|
simplify_dp_step(points, first, index, sq_tolerance, simplified);
|
|
}
|
|
simplified.push(points[index]);
|
|
if last - index > 1 {
|
|
simplify_dp_step(points, index, last, sq_tolerance, simplified);
|
|
}
|
|
}
|
|
}
|
|
|
|
/// Square distance between two points
|
|
#[inline]
|
|
fn sq_dist(p1: Point, p2: Point) -> f64 {
|
|
let dx = p1.x - p2.x;
|
|
let dy = p1.y - p2.y;
|
|
dx * dx + dy * dy
|
|
}
|
|
|
|
/// Square distance from a point to a line segment
|
|
fn sq_seg_dist(p: Point, p1: Point, p2: Point) -> f64 {
|
|
let mut x = p1.x;
|
|
let mut y = p1.y;
|
|
let dx = p2.x - x;
|
|
let dy = p2.y - y;
|
|
|
|
if dx != 0.0 || dy != 0.0 {
|
|
let t = ((p.x - x) * dx + (p.y - y) * dy) / (dx * dx + dy * dy);
|
|
|
|
if t > 1.0 {
|
|
x = p2.x;
|
|
y = p2.y;
|
|
} else if t > 0.0 {
|
|
x += dx * t;
|
|
y += dy * t;
|
|
}
|
|
}
|
|
|
|
let dx = p.x - x;
|
|
let dy = p.y - y;
|
|
dx * dx + dy * dy
|
|
}
|
|
|
|
/// Fit Bezier curves to a set of points using Schneider's algorithm
|
|
///
|
|
/// Returns a BezPath containing the fitted cubic Bezier curves
|
|
pub fn fit_bezier_curves(points: &[Point], config: SchneiderConfig) -> BezPath {
|
|
if points.len() < 2 {
|
|
return BezPath::new();
|
|
}
|
|
|
|
// Remove duplicate points
|
|
let mut unique_points = Vec::new();
|
|
unique_points.push(points[0]);
|
|
for i in 1..points.len() {
|
|
if points[i] != points[i - 1] {
|
|
unique_points.push(points[i]);
|
|
}
|
|
}
|
|
|
|
if unique_points.len() < 2 {
|
|
return BezPath::new();
|
|
}
|
|
|
|
let len = unique_points.len();
|
|
let left_tangent = create_tangent(unique_points[1], unique_points[0]);
|
|
let right_tangent = create_tangent(unique_points[len - 2], unique_points[len - 1]);
|
|
|
|
let curves = fit_cubic(&unique_points, left_tangent, right_tangent, config.max_error);
|
|
|
|
// Convert curves to BezPath
|
|
let mut path = BezPath::new();
|
|
if curves.is_empty() {
|
|
return path;
|
|
}
|
|
|
|
// Start at the first point
|
|
path.move_to(curves[0][0]);
|
|
|
|
// Add all the curves
|
|
for curve in curves {
|
|
path.curve_to(curve[1], curve[2], curve[3]);
|
|
}
|
|
|
|
path
|
|
}
|
|
|
|
/// Fit a cubic Bezier curve to a set of points
|
|
///
|
|
/// Returns an array of Bezier curves, where each curve is [p0, p1, p2, p3]
|
|
fn fit_cubic(
|
|
points: &[Point],
|
|
left_tangent: Vec2,
|
|
right_tangent: Vec2,
|
|
error: f64,
|
|
) -> Vec<[Point; 4]> {
|
|
const MAX_ITERATIONS: usize = 20;
|
|
|
|
// Use heuristic if region only has two points
|
|
if points.len() == 2 {
|
|
let dist = (points[1] - points[0]).hypot() / 3.0;
|
|
let bez_curve = [
|
|
points[0],
|
|
points[0] + left_tangent * dist,
|
|
points[1] + right_tangent * dist,
|
|
points[1],
|
|
];
|
|
return vec![bez_curve];
|
|
}
|
|
|
|
// Parameterize points and attempt to fit curve
|
|
let u = chord_length_parameterize(points);
|
|
let (mut bez_curve, mut max_error, mut split_point) =
|
|
generate_and_report(points, &u, &u, left_tangent, right_tangent);
|
|
|
|
if max_error < error {
|
|
return vec![bez_curve];
|
|
}
|
|
|
|
// If error not too large, try reparameterization and iteration
|
|
if max_error < error * error {
|
|
let mut u_prime = u.clone();
|
|
let mut prev_err = max_error;
|
|
let mut prev_split = split_point;
|
|
|
|
for _ in 0..MAX_ITERATIONS {
|
|
u_prime = reparameterize(&bez_curve, points, &u_prime);
|
|
|
|
let result = generate_and_report(points, &u, &u_prime, left_tangent, right_tangent);
|
|
bez_curve = result.0;
|
|
max_error = result.1;
|
|
split_point = result.2;
|
|
|
|
if max_error < error {
|
|
return vec![bez_curve];
|
|
}
|
|
|
|
// If development grinds to a halt, abort
|
|
if split_point == prev_split {
|
|
let err_change = max_error / prev_err;
|
|
if err_change > 0.9999 && err_change < 1.0001 {
|
|
break;
|
|
}
|
|
}
|
|
|
|
prev_err = max_error;
|
|
prev_split = split_point;
|
|
}
|
|
}
|
|
|
|
// Fitting failed -- split at max error point and fit recursively
|
|
let mut beziers = Vec::new();
|
|
|
|
// Calculate tangent at split point
|
|
let mut center_vector = points[split_point - 1] - points[split_point + 1];
|
|
|
|
// Handle case where points are the same
|
|
if center_vector.hypot() == 0.0 {
|
|
center_vector = points[split_point - 1] - points[split_point];
|
|
center_vector = Vec2::new(-center_vector.y, center_vector.x);
|
|
}
|
|
|
|
let to_center_tangent = normalize(center_vector);
|
|
let from_center_tangent = -to_center_tangent;
|
|
|
|
// Recursively fit curves
|
|
beziers.extend(fit_cubic(
|
|
&points[0..=split_point],
|
|
left_tangent,
|
|
to_center_tangent,
|
|
error,
|
|
));
|
|
beziers.extend(fit_cubic(
|
|
&points[split_point..],
|
|
from_center_tangent,
|
|
right_tangent,
|
|
error,
|
|
));
|
|
|
|
beziers
|
|
}
|
|
|
|
/// Generate a Bezier curve and compute its error
|
|
fn generate_and_report(
|
|
points: &[Point],
|
|
params_orig: &[f64],
|
|
params_prime: &[f64],
|
|
left_tangent: Vec2,
|
|
right_tangent: Vec2,
|
|
) -> ([Point; 4], f64, usize) {
|
|
let bez_curve = generate_bezier(points, params_prime, left_tangent, right_tangent);
|
|
let (max_error, split_point) = compute_max_error(points, &bez_curve, params_orig);
|
|
|
|
(bez_curve, max_error, split_point)
|
|
}
|
|
|
|
/// Use least-squares method to find Bezier control points
|
|
fn generate_bezier(
|
|
points: &[Point],
|
|
parameters: &[f64],
|
|
left_tangent: Vec2,
|
|
right_tangent: Vec2,
|
|
) -> [Point; 4] {
|
|
let first_point = points[0];
|
|
let last_point = points[points.len() - 1];
|
|
|
|
// Compute the A matrix
|
|
let mut a = Vec::new();
|
|
for &u in parameters {
|
|
let ux = 1.0 - u;
|
|
let a0 = left_tangent * (3.0 * u * ux * ux);
|
|
let a1 = right_tangent * (3.0 * ux * u * u);
|
|
a.push([a0, a1]);
|
|
}
|
|
|
|
// Create C and X matrices
|
|
let mut c = [[0.0, 0.0], [0.0, 0.0]];
|
|
let mut x = [0.0, 0.0];
|
|
|
|
for i in 0..points.len() {
|
|
let u = parameters[i];
|
|
let ai = a[i];
|
|
|
|
c[0][0] += dot(ai[0], ai[0]);
|
|
c[0][1] += dot(ai[0], ai[1]);
|
|
c[1][0] += dot(ai[0], ai[1]);
|
|
c[1][1] += dot(ai[1], ai[1]);
|
|
|
|
let tmp = points[i] - bezier_q(&[first_point, first_point, last_point, last_point], u);
|
|
|
|
x[0] += dot(ai[0], tmp);
|
|
x[1] += dot(ai[1], tmp);
|
|
}
|
|
|
|
// Compute determinants
|
|
let det_c0_c1 = c[0][0] * c[1][1] - c[1][0] * c[0][1];
|
|
let det_c0_x = c[0][0] * x[1] - c[1][0] * x[0];
|
|
let det_x_c1 = x[0] * c[1][1] - x[1] * c[0][1];
|
|
|
|
// Derive alpha values
|
|
let alpha_l = if det_c0_c1 == 0.0 {
|
|
0.0
|
|
} else {
|
|
det_x_c1 / det_c0_c1
|
|
};
|
|
let alpha_r = if det_c0_c1 == 0.0 {
|
|
0.0
|
|
} else {
|
|
det_c0_x / det_c0_c1
|
|
};
|
|
|
|
// If alpha is negative or too small, use heuristic
|
|
let seg_length = (last_point - first_point).hypot();
|
|
let epsilon = 1.0e-6 * seg_length;
|
|
|
|
let (p1, p2) = if alpha_l < epsilon || alpha_r < epsilon {
|
|
// Fall back on standard formula
|
|
(
|
|
first_point + left_tangent * (seg_length / 3.0),
|
|
last_point + right_tangent * (seg_length / 3.0),
|
|
)
|
|
} else {
|
|
(
|
|
first_point + left_tangent * alpha_l,
|
|
last_point + right_tangent * alpha_r,
|
|
)
|
|
};
|
|
|
|
[first_point, p1, p2, last_point]
|
|
}
|
|
|
|
/// Reparameterize points using Newton-Raphson
|
|
fn reparameterize(bezier: &[Point; 4], points: &[Point], parameters: &[f64]) -> Vec<f64> {
|
|
parameters
|
|
.iter()
|
|
.zip(points.iter())
|
|
.map(|(&p, &point)| newton_raphson_root_find(bezier, point, p))
|
|
.collect()
|
|
}
|
|
|
|
/// Use Newton-Raphson iteration to find better root
|
|
fn newton_raphson_root_find(bez: &[Point; 4], point: Point, u: f64) -> f64 {
|
|
let d = bezier_q(bez, u) - point;
|
|
let qprime = bezier_qprime(bez, u);
|
|
let numerator = dot(d, qprime);
|
|
let qprimeprime = bezier_qprimeprime(bez, u);
|
|
let denominator = dot(qprime, qprime) + 2.0 * dot(d, qprimeprime);
|
|
|
|
if denominator == 0.0 {
|
|
u
|
|
} else {
|
|
u - numerator / denominator
|
|
}
|
|
}
|
|
|
|
/// Assign parameter values using chord length
|
|
fn chord_length_parameterize(points: &[Point]) -> Vec<f64> {
|
|
let mut u = Vec::new();
|
|
let mut curr_u = 0.0;
|
|
|
|
u.push(0.0);
|
|
|
|
for i in 1..points.len() {
|
|
curr_u += (points[i] - points[i - 1]).hypot();
|
|
u.push(curr_u);
|
|
}
|
|
|
|
let total_length = u[u.len() - 1];
|
|
u.iter().map(|&x| x / total_length).collect()
|
|
}
|
|
|
|
/// Find maximum squared distance of points to fitted curve
|
|
fn compute_max_error(points: &[Point], bez: &[Point; 4], parameters: &[f64]) -> (f64, usize) {
|
|
let mut max_dist = 0.0;
|
|
let mut split_point = points.len() / 2;
|
|
|
|
let t_dist_map = map_t_to_relative_distances(bez, 10);
|
|
|
|
for i in 0..points.len() {
|
|
let point = points[i];
|
|
let t = find_t(bez, parameters[i], &t_dist_map, 10);
|
|
|
|
let v = bezier_q(bez, t) - point;
|
|
let dist = v.x * v.x + v.y * v.y;
|
|
|
|
if dist > max_dist {
|
|
max_dist = dist;
|
|
split_point = i;
|
|
}
|
|
}
|
|
|
|
(max_dist, split_point)
|
|
}
|
|
|
|
/// Sample t values and map to relative distances along curve
|
|
fn map_t_to_relative_distances(bez: &[Point; 4], b_parts: usize) -> Vec<f64> {
|
|
let mut b_t_dist = vec![0.0];
|
|
let mut b_t_prev = bez[0];
|
|
let mut sum_len = 0.0;
|
|
|
|
for i in 1..=b_parts {
|
|
let b_t_curr = bezier_q(bez, i as f64 / b_parts as f64);
|
|
sum_len += (b_t_curr - b_t_prev).hypot();
|
|
b_t_dist.push(sum_len);
|
|
b_t_prev = b_t_curr;
|
|
}
|
|
|
|
// Normalize to 0..1
|
|
b_t_dist.iter().map(|&x| x / sum_len).collect()
|
|
}
|
|
|
|
/// Find t value for a given parameter distance
|
|
fn find_t(_bez: &[Point; 4], param: f64, t_dist_map: &[f64], b_parts: usize) -> f64 {
|
|
if param < 0.0 {
|
|
return 0.0;
|
|
}
|
|
if param > 1.0 {
|
|
return 1.0;
|
|
}
|
|
|
|
for i in 1..=b_parts {
|
|
if param <= t_dist_map[i] {
|
|
let t_min = (i - 1) as f64 / b_parts as f64;
|
|
let t_max = i as f64 / b_parts as f64;
|
|
let len_min = t_dist_map[i - 1];
|
|
let len_max = t_dist_map[i];
|
|
|
|
let t = (param - len_min) / (len_max - len_min) * (t_max - t_min) + t_min;
|
|
return t;
|
|
}
|
|
}
|
|
|
|
1.0
|
|
}
|
|
|
|
/// Evaluate cubic Bezier at parameter t
|
|
fn bezier_q(ctrl_poly: &[Point; 4], t: f64) -> Point {
|
|
let tx = 1.0 - t;
|
|
let p_a = ctrl_poly[0].to_vec2() * (tx * tx * tx);
|
|
let p_b = ctrl_poly[1].to_vec2() * (3.0 * tx * tx * t);
|
|
let p_c = ctrl_poly[2].to_vec2() * (3.0 * tx * t * t);
|
|
let p_d = ctrl_poly[3].to_vec2() * (t * t * t);
|
|
|
|
(p_a + p_b + p_c + p_d).to_point()
|
|
}
|
|
|
|
/// Evaluate first derivative of cubic Bezier at parameter t
|
|
fn bezier_qprime(ctrl_poly: &[Point; 4], t: f64) -> Vec2 {
|
|
let tx = 1.0 - t;
|
|
let p_a = (ctrl_poly[1] - ctrl_poly[0]) * (3.0 * tx * tx);
|
|
let p_b = (ctrl_poly[2] - ctrl_poly[1]) * (6.0 * tx * t);
|
|
let p_c = (ctrl_poly[3] - ctrl_poly[2]) * (3.0 * t * t);
|
|
|
|
p_a + p_b + p_c
|
|
}
|
|
|
|
/// Evaluate second derivative of cubic Bezier at parameter t
|
|
fn bezier_qprimeprime(ctrl_poly: &[Point; 4], t: f64) -> Vec2 {
|
|
let v0 = ctrl_poly[2].to_vec2() - ctrl_poly[1].to_vec2() * 2.0 + ctrl_poly[0].to_vec2();
|
|
let v1 = ctrl_poly[3].to_vec2() - ctrl_poly[2].to_vec2() * 2.0 + ctrl_poly[1].to_vec2();
|
|
v0 * (6.0 * (1.0 - t)) + v1 * (6.0 * t)
|
|
}
|
|
|
|
/// Create a unit tangent vector from A to B
|
|
fn create_tangent(point_a: Point, point_b: Point) -> Vec2 {
|
|
normalize(point_a - point_b)
|
|
}
|
|
|
|
/// Normalize a vector to unit length
|
|
fn normalize(v: Vec2) -> Vec2 {
|
|
let len = v.hypot();
|
|
if len == 0.0 {
|
|
Vec2::ZERO
|
|
} else {
|
|
v / len
|
|
}
|
|
}
|
|
|
|
/// Dot product of two vectors
|
|
fn dot(v1: Vec2, v2: Vec2) -> f64 {
|
|
v1.x * v2.x + v1.y * v2.y
|
|
}
|
|
|
|
#[cfg(test)]
|
|
mod tests {
|
|
use super::*;
|
|
|
|
#[test]
|
|
fn test_rdp_simplification() {
|
|
let points = vec![
|
|
Point::new(0.0, 0.0),
|
|
Point::new(1.0, 0.1),
|
|
Point::new(2.0, 0.0),
|
|
Point::new(3.0, 0.0),
|
|
Point::new(4.0, 0.0),
|
|
Point::new(5.0, 0.0),
|
|
];
|
|
|
|
let config = RdpConfig {
|
|
tolerance: 0.5,
|
|
highest_quality: false,
|
|
};
|
|
|
|
let simplified = simplify_rdp(&points, config);
|
|
|
|
// Should simplify the nearly-straight line
|
|
assert!(simplified.len() < points.len());
|
|
assert_eq!(simplified[0], points[0]);
|
|
assert_eq!(simplified[simplified.len() - 1], points[points.len() - 1]);
|
|
}
|
|
|
|
#[test]
|
|
fn test_schneider_curve_fitting() {
|
|
let points = vec![
|
|
Point::new(0.0, 0.0),
|
|
Point::new(50.0, 100.0),
|
|
Point::new(100.0, 50.0),
|
|
Point::new(150.0, 100.0),
|
|
];
|
|
|
|
let config = SchneiderConfig { max_error: 30.0 };
|
|
|
|
let path = fit_bezier_curves(&points, config);
|
|
|
|
// Should create a valid BezPath
|
|
assert!(!path.is_empty());
|
|
}
|
|
|
|
#[test]
|
|
fn test_chord_length_parameterization() {
|
|
let points = vec![
|
|
Point::new(0.0, 0.0),
|
|
Point::new(1.0, 0.0),
|
|
Point::new(2.0, 0.0),
|
|
];
|
|
|
|
let params = chord_length_parameterize(&points);
|
|
|
|
// Should start at 0 and end at 1
|
|
assert_eq!(params[0], 0.0);
|
|
assert_eq!(params[params.len() - 1], 1.0);
|
|
// Should be evenly spaced for uniform spacing
|
|
assert!((params[1] - 0.5).abs() < 0.01);
|
|
}
|
|
}
|