use super::analytic::in_period;
const MAX_DEGREE: usize = 25;
pub(crate) use crate::curves::BSplineConstructionError;
pub(crate) fn flat_knots(knots: &[f64], mults: &[u32]) -> Vec<f64> {
let mut out = Vec::with_capacity(mults.iter().map(|&m| m as usize).sum());
for (&k, &m) in knots.iter().zip(mults.iter()) {
for _ in 0..m {
out.push(k);
}
}
out
}
pub(crate) fn param_range(
flat: &[f64],
degree: usize,
periodic: bool,
sum_mults: usize,
) -> (f64, f64) {
debug_assert_eq!(flat.len(), sum_mults);
if periodic {
(flat[0], flat[flat.len() - 1])
} else {
(flat[degree], flat[flat.len() - 1 - degree])
}
}
pub(crate) fn locate_span(flat: &[f64], degree: usize, periodic: bool, u: f64) -> (usize, f64) {
let lo = degree;
let hi = flat.len() - 1 - degree;
let first = flat[lo];
let last = flat[hi];
let u = if periodic {
in_period(u, first, last)
} else {
u.clamp(first, last)
};
let mut span = lo;
let (mut left, mut right) = (lo, hi);
while left < right {
let mid = left + (right - left) / 2;
if flat[mid] <= u {
span = mid;
left = mid + 1;
} else {
right = mid;
}
}
while span > lo && flat[span] == flat[span - 1] {
span -= 1;
}
while span < hi - 1 && flat[span] >= flat[span + 1] {
span += 1;
}
(span, u)
}
fn deboor(
u: f64,
degree: usize,
flat: &[f64],
poles: &[f64],
dim: usize,
span: usize,
out: &mut [f64],
) {
let mut d = vec![0.0f64; (degree + 1) * dim];
for i in 0..=degree {
let idx = span - degree + i;
d[i * dim..(i + 1) * dim].copy_from_slice(&poles[idx * dim..(idx + 1) * dim]);
}
for r in 1..=degree {
for i in (r..=degree).rev() {
let j = span - degree + i;
let alpha = (u - flat[j]) / (flat[j + degree - r + 1] - flat[j]);
for c in 0..dim {
let lo = d[(i - 1) * dim + c];
let hi = d[i * dim + c];
d[i * dim + c] = (1.0 - alpha) * lo + alpha * hi;
}
}
}
out[..dim].copy_from_slice(&d[degree * dim..degree * dim + dim]);
}
fn derivative_curve(
degree: usize,
flat: &[f64],
poles: &[f64],
dim: usize,
) -> (Vec<f64>, Vec<f64>) {
let n = poles.len() / dim;
let mut q = vec![0.0f64; (n - 1) * dim];
for i in 0..n - 1 {
let denom = flat[i + degree + 1] - flat[i + 1];
if denom != 0.0 {
let scale = degree as f64 / denom;
for c in 0..dim {
q[i * dim + c] = scale * (poles[(i + 1) * dim + c] - poles[i * dim + c]);
}
}
}
let new_flat = flat[1..flat.len() - 1].to_vec();
(q, new_flat)
}
#[allow(dead_code)]
pub(crate) fn eval_d0(
u: f64,
degree: usize,
periodic: bool,
flat: &[f64],
poles: &[f64],
dim: usize,
out: &mut [f64],
) {
eval_dn(u, degree, periodic, flat, poles, dim, 0, out);
}
#[allow(clippy::too_many_arguments)] pub(crate) fn eval_dn(
u: f64,
degree: usize,
periodic: bool,
flat: &[f64],
poles: &[f64],
dim: usize,
n: usize,
out: &mut [f64],
) {
if periodic {
eval_dn_periodic(u, degree, flat, poles, dim, n, out);
} else {
eval_dn_nonperiodic(u, degree, flat, poles, dim, n, out);
}
}
fn eval_dn_nonperiodic(
u: f64,
degree: usize,
flat: &[f64],
poles: &[f64],
dim: usize,
n: usize,
out: &mut [f64],
) {
let (span, uu) = locate_span(flat, degree, false, u);
let mut cur_poles = poles.to_vec();
let mut cur_flat = flat.to_vec();
let mut cur_degree = degree;
for k in 0..=n {
let slot = &mut out[k * dim..(k + 1) * dim];
if k > degree {
slot.iter_mut().for_each(|c| *c = 0.0);
continue;
}
let sp = span - k;
deboor(uu, cur_degree, &cur_flat, &cur_poles, dim, sp, slot);
if cur_degree >= 1 && k < n {
let (q, nf) = derivative_curve(cur_degree, &cur_flat, &cur_poles, dim);
cur_poles = q;
cur_flat = nf;
cur_degree -= 1;
}
}
}
fn eval_dn_periodic(
u: f64,
degree: usize,
flat: &[f64],
poles: &[f64],
dim: usize,
n: usize,
out: &mut [f64],
) {
let n_poles = poles.len() / dim;
let first = flat[0];
let last = flat[flat.len() - 1];
let period = last - first;
let uw = in_period(u, first, last);
let mut block_len = flat.len();
while block_len > 0 && flat[block_len - 1] == last {
block_len -= 1;
}
let block = &flat[..block_len];
let mut ext_flat = Vec::with_capacity(block_len * 4);
for shift in [-period, 0.0, period, 2.0 * period] {
ext_flat.extend(block.iter().map(|&k| k + shift));
}
let mid_off = block_len;
let mut span = mid_off + block_len - 1;
for s in mid_off..mid_off + block_len {
if ext_flat[s] <= uw && uw < ext_flat[s + 1] {
span = s;
break;
}
}
let n_ext = ext_flat.len() - degree - 1;
let mut ext_poles = vec![0.0f64; n_ext * dim];
for k in 0..n_ext {
let logical = k as isize - mid_off as isize + degree as isize;
let pidx = logical.rem_euclid(n_poles as isize) as usize;
ext_poles[k * dim..(k + 1) * dim].copy_from_slice(&poles[pidx * dim..(pidx + 1) * dim]);
}
let mut cur_poles = ext_poles;
let mut cur_flat = ext_flat;
let mut cur_degree = degree;
for k in 0..=n {
let slot = &mut out[k * dim..(k + 1) * dim];
if k > degree {
slot.iter_mut().for_each(|c| *c = 0.0);
continue;
}
let sp = span - k;
deboor(uw, cur_degree, &cur_flat, &cur_poles, dim, sp, slot);
if cur_degree >= 1 && k < n {
let (q, nf) = derivative_curve(cur_degree, &cur_flat, &cur_poles, dim);
cur_poles = q;
cur_flat = nf;
cur_degree -= 1;
}
}
}
pub(crate) fn rational_derivatives(homog: &[f64], n: usize, out: &mut [f64]) {
debug_assert!(
n <= 2,
"rational derivatives only implemented up to order 2"
);
let w0 = homog[3];
for c in 0..3 {
out[c] = homog[c] / w0;
}
if n >= 1 {
let w1 = homog[4 + 3];
for c in 0..3 {
out[3 + c] = (homog[4 + c] - out[c] * w1) / w0;
}
}
if n >= 2 {
let w1 = homog[4 + 3];
let w2 = homog[8 + 3];
for c in 0..3 {
out[6 + c] = (homog[8 + c] - 2.0 * out[3 + c] * w1 - out[c] * w2) / w0;
}
}
}
pub(crate) fn validate_direction(
degree: usize,
n_poles: usize,
knots: &[f64],
mults: &[u32],
periodic: bool,
) -> Result<(), BSplineConstructionError> {
if !(1..=MAX_DEGREE).contains(°ree) {
return Err(BSplineConstructionError::InvalidDegree);
}
if knots.len() < 2 || mults.len() < 2 || knots.len() != mults.len() {
return Err(BSplineConstructionError::TooFewKnots);
}
for pair in knots.windows(2) {
if pair[1] <= pair[0] {
return Err(BSplineConstructionError::KnotsNotIncreasing);
}
}
let last = mults.len() - 1;
let deg = degree as u32;
if periodic {
if mults[0] != mults[last] {
return Err(BSplineConstructionError::PeriodicEndMultiplicityMismatch);
}
if mults.iter().any(|&m| m > deg) {
return Err(BSplineConstructionError::MultiplicityTooLarge);
}
} else {
if mults[0] > deg + 1 || mults[last] > deg + 1 {
return Err(BSplineConstructionError::MultiplicityTooLarge);
}
if mults[1..last].iter().any(|&m| m > deg) {
return Err(BSplineConstructionError::MultiplicityTooLarge);
}
}
let sum_mults: u32 = mults.iter().sum();
let expected = if periodic {
sum_mults as i64 - mults[last] as i64
} else {
sum_mults as i64 - degree as i64 - 1
};
if expected < 0 || n_poles as i64 != expected {
return Err(BSplineConstructionError::PoleCountMismatch);
}
Ok(())
}
#[cfg(test)]
mod tests {
use super::*;
#[test]
fn test_flat_knots_repeats() {
assert_eq!(
flat_knots(&[0.0, 1.0, 2.0], &[3, 1, 3]),
vec![0.0, 0.0, 0.0, 1.0, 2.0, 2.0, 2.0]
);
}
#[test]
fn test_flat_knots_all_single() {
assert_eq!(
flat_knots(&[0.0, 1.0, 2.0], &[1, 1, 1]),
vec![0.0, 1.0, 2.0]
);
}
#[test]
fn test_degree1_line_midpoint_and_derivative() {
let poles = [0.0, 0.0, 0.0, 2.0, 0.0, 0.0];
let flat = flat_knots(&[0.0, 1.0], &[2, 2]);
let mut out = [0.0f64; 6];
eval_dn(0.5, 1, false, &flat, &poles, 3, 1, &mut out);
assert_close(&out[0..3], &[1.0, 0.0, 0.0]);
assert_close(&out[3..6], &[2.0, 0.0, 0.0]);
}
#[test]
fn test_degree2_bezier_by_hand() {
let poles = [0.0, 0.0, 0.0, 1.0, 2.0, 0.0, 2.0, 0.0, 0.0];
let flat = flat_knots(&[0.0, 1.0], &[3, 3]);
let mut out = [0.0f64; 9];
eval_dn(0.5, 2, false, &flat, &poles, 3, 2, &mut out);
assert_close(&out[0..3], &[1.0, 1.0, 0.0]);
assert_close(&out[3..6], &[2.0, 0.0, 0.0]);
}
#[test]
fn test_rational_quarter_circle_on_unit_circle() {
let w = std::f64::consts::FRAC_1_SQRT_2; let homog_poles = [
1.0,
0.0,
0.0,
1.0, 1.0 * w,
1.0 * w,
0.0,
w, 0.0,
1.0,
0.0,
1.0, ];
let flat = flat_knots(&[0.0, 1.0], &[3, 3]);
for t in [0.0, 0.25, 0.5, 1.0] {
let mut h = [0.0f64; 4];
eval_d0(t, 2, false, &flat, &homog_poles, 4, &mut h);
let mut euc = [0.0f64; 3];
rational_derivatives(&h, 0, &mut euc);
let r = (euc[0] * euc[0] + euc[1] * euc[1]).sqrt();
assert!((r - 1.0).abs() < 1e-12, "t={t} radius={r}");
}
let mut h = [0.0f64; 4];
eval_d0(1.0, 2, false, &flat, &homog_poles, 4, &mut h);
let mut euc = [0.0f64; 3];
rational_derivatives(&h, 0, &mut euc);
assert_close(&euc, &[0.0, 1.0, 0.0]);
}
#[test]
fn test_periodic_value_repeats_over_period() {
let poles = periodic_ring_poles();
let flat = flat_knots(&[0.0, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0], &[1, 1, 1, 1, 1, 1, 1]);
for u in [0.3, 1.7, 4.9] {
let mut a = [0.0f64; 9];
let mut b = [0.0f64; 9];
eval_dn(u, 3, true, &flat, &poles, 3, 2, &mut a);
eval_dn(u + 6.0, 3, true, &flat, &poles, 3, 2, &mut b);
assert_close(&a, &b);
}
}
fn periodic_ring_poles() -> Vec<f64> {
vec![
2.0,
0.0,
0.3, 1.0000000000000002,
1.7320508075688772,
-0.3, -0.9999999999999996,
1.7320508075688774,
0.3, -2.0,
2.4492935982947064e-16,
-0.3, -1.0000000000000009,
-1.7320508075688767,
0.3, 1.0000000000000002,
-1.7320508075688772,
-0.3, ]
}
#[test]
fn test_validation_ok_nonperiodic() {
assert_eq!(
validate_direction(3, 6, &[0.0, 1.0, 2.0, 3.0], &[4, 1, 1, 4], false),
Ok(())
);
}
#[test]
fn test_validation_ok_periodic() {
assert_eq!(
validate_direction(3, 6, &[0.0, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0], &[1; 7], true),
Ok(())
);
}
#[test]
fn test_validation_invalid_degree_low() {
assert_eq!(
validate_direction(0, 1, &[0.0, 1.0], &[1, 1], false),
Err(BSplineConstructionError::InvalidDegree)
);
}
#[test]
fn test_validation_invalid_degree_high() {
assert_eq!(
validate_direction(26, 1, &[0.0, 1.0], &[1, 1], false),
Err(BSplineConstructionError::InvalidDegree)
);
}
#[test]
fn test_validation_too_few_knots() {
assert_eq!(
validate_direction(1, 1, &[0.0], &[2], false),
Err(BSplineConstructionError::TooFewKnots)
);
}
#[test]
fn test_validation_knots_not_increasing() {
assert_eq!(
validate_direction(1, 2, &[1.0, 1.0], &[2, 2], false),
Err(BSplineConstructionError::KnotsNotIncreasing)
);
}
#[test]
fn test_validation_multiplicity_too_large_interior() {
assert_eq!(
validate_direction(2, 3, &[0.0, 1.0, 2.0], &[3, 3, 3], false),
Err(BSplineConstructionError::MultiplicityTooLarge)
);
}
#[test]
fn test_validation_multiplicity_too_large_end() {
assert_eq!(
validate_direction(3, 3, &[0.0, 1.0], &[5, 3], false),
Err(BSplineConstructionError::MultiplicityTooLarge)
);
}
#[test]
fn test_validation_periodic_mult_too_large() {
assert_eq!(
validate_direction(2, 4, &[0.0, 1.0, 2.0, 3.0], &[3, 1, 1, 3], true),
Err(BSplineConstructionError::MultiplicityTooLarge)
);
}
#[test]
fn test_validation_periodic_end_mismatch() {
assert_eq!(
validate_direction(3, 6, &[0.0, 1.0, 2.0, 3.0], &[2, 1, 1, 3], true),
Err(BSplineConstructionError::PeriodicEndMultiplicityMismatch)
);
}
#[test]
fn test_validation_pole_count_mismatch_nonperiodic() {
assert_eq!(
validate_direction(3, 5, &[0.0, 1.0, 2.0, 3.0], &[4, 1, 1, 4], false),
Err(BSplineConstructionError::PoleCountMismatch)
);
}
#[test]
fn test_validation_pole_count_mismatch_periodic() {
assert_eq!(
validate_direction(3, 7, &[0.0, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0], &[1; 7], true),
Err(BSplineConstructionError::PoleCountMismatch)
);
}
#[test]
fn test_weight_error_variants_display() {
assert!(
!BSplineConstructionError::WeightCountMismatch
.to_string()
.is_empty()
);
assert!(
!BSplineConstructionError::NonPositiveWeight
.to_string()
.is_empty()
);
}
#[test]
fn test_fixture_replay_all_cases() {
replay_fixture();
}
fn assert_close(actual: &[f64], expected: &[f64]) {
assert_eq!(actual.len(), expected.len());
for (i, (&a, &e)) in actual.iter().zip(expected.iter()).enumerate() {
let tol = 1e-9 * f64::max(1.0, e.abs());
assert!((a - e).abs() <= tol, "index {i}: {a} vs {e}");
}
}
use serde_json::Value;
fn replay_fixture() {
let path = concat!(
env!("CARGO_MANIFEST_DIR"),
"/tests/fixtures/curves_bspline.json"
);
let text = std::fs::read_to_string(path).expect("read fixture");
let json: Value = serde_json::from_str(&text).expect("parse fixture");
let cases = json["cases"].as_array().expect("cases array");
for case in cases {
replay_case(case);
}
}
fn arr_f64(v: &Value) -> Vec<f64> {
v.as_array()
.unwrap()
.iter()
.map(|x| x.as_f64().unwrap())
.collect()
}
fn replay_case(case: &Value) {
let name = case["name"].as_str().unwrap();
let degree = case["degree"].as_u64().unwrap() as usize;
let periodic = case["periodic"].as_bool().unwrap();
let knots = arr_f64(&case["knots"]);
let mults: Vec<u32> = case["mults"]
.as_array()
.unwrap()
.iter()
.map(|x| x.as_u64().unwrap() as u32)
.collect();
let pole_rows: Vec<Vec<f64>> = case["poles"]
.as_array()
.unwrap()
.iter()
.map(arr_f64)
.collect();
let weights: Option<Vec<f64>> = case
.get("weights")
.and_then(|w| if w.is_null() { None } else { Some(arr_f64(w)) });
let flat = flat_knots(&knots, &mults);
let rational = weights.is_some();
let dim = if rational { 4 } else { 3 };
let mut poles = Vec::with_capacity(pole_rows.len() * dim);
for (i, p) in pole_rows.iter().enumerate() {
if let Some(ws) = &weights {
let w = ws[i];
poles.extend_from_slice(&[p[0] * w, p[1] * w, p[2] * w, w]);
} else {
poles.extend_from_slice(&p[0..3]);
}
}
for sample in case["samples"].as_array().unwrap() {
let u = sample["u"].as_f64().unwrap();
let mut raw = vec![0.0f64; 3 * dim];
eval_dn(u, degree, periodic, &flat, &poles, dim, 2, &mut raw);
let euc = if rational {
let mut e = [0.0f64; 9];
rational_derivatives(&raw, 2, &mut e);
e.to_vec()
} else {
raw
};
for (key, idx) in [("point", 0usize), ("d1", 1), ("d2", 2)] {
let expected = arr_f64(&sample[key]);
let got = &euc[idx * 3..idx * 3 + 3];
for (c, (&g, &e)) in got.iter().zip(expected.iter()).enumerate() {
let tol = 1e-7 * f64::max(1.0, e.abs());
assert!(
(g - e).abs() <= tol,
"{name} u={u} {key}[{c}]: got {g}, expected {e} (tol {tol})"
);
}
}
}
}
}