1use crate::accumulator::BinnedAccumulatorF64;
17
18fn find_interval(xs: &[f64], xq: f64) -> usize {
25 debug_assert!(xs.len() >= 2);
26 if xq <= xs[0] {
27 return 0;
28 }
29 if xq >= xs[xs.len() - 1] {
30 return xs.len() - 2;
31 }
32 let mut lo = 0usize;
34 let mut hi = xs.len() - 1;
35 while lo + 1 < hi {
36 let mid = lo + (hi - lo) / 2;
37 if xs[mid] <= xq {
38 lo = mid;
39 } else {
40 hi = mid;
41 }
42 }
43 lo
44}
45
46fn validate_sorted_data(x_data: &[f64], y_data: &[f64]) -> Result<(), String> {
49 if x_data.is_empty() {
50 return Err("interpolation requires at least one data point".to_string());
51 }
52 if x_data.len() != y_data.len() {
53 return Err(format!(
54 "x_data length ({}) must equal y_data length ({})",
55 x_data.len(),
56 y_data.len()
57 ));
58 }
59 if x_data.len() < 2 {
60 return Err("interpolation requires at least two data points".to_string());
61 }
62 for i in 1..x_data.len() {
63 if x_data[i] <= x_data[i - 1] {
64 return Err(format!(
65 "x_data must be strictly ascending; x_data[{}] = {} <= x_data[{}] = {}",
66 i,
67 x_data[i],
68 i - 1,
69 x_data[i - 1]
70 ));
71 }
72 }
73 Ok(())
74}
75
76pub fn interp1d_linear(
85 x_data: &[f64],
86 y_data: &[f64],
87 x_query: &[f64],
88) -> Result<Vec<f64>, String> {
89 validate_sorted_data(x_data, y_data)?;
90
91 let n = x_data.len();
92 let result: Vec<f64> = x_query
93 .iter()
94 .map(|&xq| {
95 if xq <= x_data[0] {
96 return y_data[0];
97 }
98 if xq >= x_data[n - 1] {
99 return y_data[n - 1];
100 }
101 let i = find_interval(x_data, xq);
102 let dx = x_data[i + 1] - x_data[i];
103 if dx.abs() < 1e-300 {
104 return y_data[i];
105 }
106 let t = (xq - x_data[i]) / dx;
107 y_data[i] + t * (y_data[i + 1] - y_data[i])
109 })
110 .collect();
111 Ok(result)
112}
113
114pub fn interp1d_nearest(
124 x_data: &[f64],
125 y_data: &[f64],
126 x_query: &[f64],
127) -> Result<Vec<f64>, String> {
128 validate_sorted_data(x_data, y_data)?;
129
130 let n = x_data.len();
131 let result: Vec<f64> = x_query
132 .iter()
133 .map(|&xq| {
134 if xq <= x_data[0] {
135 return y_data[0];
136 }
137 if xq >= x_data[n - 1] {
138 return y_data[n - 1];
139 }
140 let i = find_interval(x_data, xq);
141 let d_left = (xq - x_data[i]).abs();
142 let d_right = (x_data[i + 1] - xq).abs();
143 if d_left <= d_right {
145 y_data[i]
146 } else {
147 y_data[i + 1]
148 }
149 })
150 .collect();
151 Ok(result)
152}
153
154pub fn polyfit(x: &[f64], y: &[f64], degree: usize) -> Result<Vec<f64>, String> {
167 if x.len() != y.len() {
168 return Err(format!(
169 "x length ({}) must equal y length ({})",
170 x.len(),
171 y.len()
172 ));
173 }
174 let m = x.len(); let n = degree + 1; if m < n {
177 return Err(format!(
178 "need at least {} data points for degree {} fit, got {}",
179 n, degree, m
180 ));
181 }
182
183 let mut v_cols: Vec<Vec<f64>> = Vec::with_capacity(n);
186 for j in 0..n {
187 let col: Vec<f64> = x
188 .iter()
189 .map(|&xi| {
190 if j == 0 {
191 1.0
192 } else {
193 let mut val = 1.0;
195 for _ in 0..j {
196 val *= xi;
197 }
198 val
199 }
200 })
201 .collect();
202 v_cols.push(col);
203 }
204
205 let mut q_cols = v_cols; let mut r = vec![0.0f64; n * n]; for j in 0..n {
211 for i in 0..j {
213 let dot = {
214 let mut acc = BinnedAccumulatorF64::new();
215 for k in 0..m {
216 acc.add(q_cols[i][k] * q_cols[j][k]);
217 }
218 acc.finalize()
219 };
220 r[i * n + j] = dot;
221 for k in 0..m {
222 q_cols[j][k] -= dot * q_cols[i][k];
223 }
224 }
225 let norm = {
227 let mut acc = BinnedAccumulatorF64::new();
228 for k in 0..m {
229 acc.add(q_cols[j][k] * q_cols[j][k]);
230 }
231 acc.finalize()
232 }
233 .sqrt();
234
235 r[j * n + j] = norm;
236 if norm < 1e-15 {
237 return Err("polyfit: Vandermonde matrix is rank-deficient".to_string());
238 }
239 for k in 0..m {
240 q_cols[j][k] /= norm;
241 }
242 }
243
244 let mut qty = vec![0.0f64; n];
246 for j in 0..n {
247 let mut acc = BinnedAccumulatorF64::new();
248 for k in 0..m {
249 acc.add(q_cols[j][k] * y[k]);
250 }
251 qty[j] = acc.finalize();
252 }
253
254 let mut coeffs = vec![0.0f64; n];
256 for j in (0..n).rev() {
257 let mut acc = BinnedAccumulatorF64::new();
258 for k in (j + 1)..n {
259 acc.add(r[j * n + k] * coeffs[k]);
260 }
261 coeffs[j] = (qty[j] - acc.finalize()) / r[j * n + j];
262 }
263
264 Ok(coeffs)
265}
266
267pub fn polyval(coeffs: &[f64], x: &[f64]) -> Vec<f64> {
276 if coeffs.is_empty() {
277 return vec![0.0; x.len()];
278 }
279 x.iter()
280 .map(|&xi| {
281 let mut result = coeffs[coeffs.len() - 1];
283 for k in (0..coeffs.len() - 1).rev() {
284 result = result * xi + coeffs[k];
285 }
286 result
287 })
288 .collect()
289}
290
291pub struct CubicSpline {
303 pub x: Vec<f64>,
305 pub a: Vec<f64>,
307 pub b: Vec<f64>,
309 pub c: Vec<f64>,
311 pub d: Vec<f64>,
313}
314
315pub fn spline_cubic_natural(x: &[f64], y: &[f64]) -> Result<CubicSpline, String> {
320 validate_sorted_data(x, y)?;
321
322 let n = x.len(); let nm1 = n - 1; let h: Vec<f64> = (0..nm1).map(|i| x[i + 1] - x[i]).collect();
327
328 let mut c_all = vec![0.0f64; n]; if n > 2 {
340 let interior = n - 2; let mut a_tri = vec![0.0f64; interior]; let mut b_tri = vec![0.0f64; interior]; let mut c_tri = vec![0.0f64; interior]; let mut d_tri = vec![0.0f64; interior]; for i in 0..interior {
349 let ii = i + 1; a_tri[i] = h[ii - 1];
351 b_tri[i] = 2.0 * (h[ii - 1] + h[ii]);
352 c_tri[i] = h[ii];
353 d_tri[i] = 3.0 * ((y[ii + 1] - y[ii]) / h[ii] - (y[ii] - y[ii - 1]) / h[ii - 1]);
354 }
355
356 for i in 1..interior {
359 if b_tri[i - 1].abs() < 1e-300 {
360 return Err("spline_cubic_natural: zero pivot in Thomas algorithm".to_string());
361 }
362 let w = a_tri[i] / b_tri[i - 1];
363 b_tri[i] -= w * c_tri[i - 1];
364 d_tri[i] -= w * d_tri[i - 1];
365 }
366
367 if b_tri[interior - 1].abs() < 1e-300 {
369 return Err("spline_cubic_natural: zero pivot in Thomas algorithm".to_string());
370 }
371 c_all[interior] = d_tri[interior - 1] / b_tri[interior - 1]; for i in (0..interior - 1).rev() {
373 c_all[i + 1] = (d_tri[i] - c_tri[i] * c_all[i + 2]) / b_tri[i];
374 }
375 }
376 let mut a_coeff = Vec::with_capacity(nm1);
380 let mut b_coeff = Vec::with_capacity(nm1);
381 let mut c_coeff = Vec::with_capacity(nm1);
382 let mut d_coeff = Vec::with_capacity(nm1);
383
384 for i in 0..nm1 {
385 a_coeff.push(y[i]);
386 b_coeff.push(
387 (y[i + 1] - y[i]) / h[i] - h[i] * (2.0 * c_all[i] + c_all[i + 1]) / 3.0,
388 );
389 c_coeff.push(c_all[i]);
390 d_coeff.push((c_all[i + 1] - c_all[i]) / (3.0 * h[i]));
391 }
392
393 Ok(CubicSpline {
394 x: x.to_vec(),
395 a: a_coeff,
396 b: b_coeff,
397 c: c_coeff,
398 d: d_coeff,
399 })
400}
401
402pub fn spline_eval(spline: &CubicSpline, x_query: &[f64]) -> Result<Vec<f64>, String> {
407 if spline.x.len() < 2 {
408 return Err("spline must have at least two knots".to_string());
409 }
410
411 let result: Vec<f64> = x_query
412 .iter()
413 .map(|&xq| {
414 let i = find_interval(&spline.x, xq);
415 let t = xq - spline.x[i];
416 spline.a[i] + t * (spline.b[i] + t * (spline.c[i] + t * spline.d[i]))
418 })
419 .collect();
420 Ok(result)
421}
422
423#[cfg(test)]
428mod interpolate_tests {
429 use super::*;
430
431 const EPS: f64 = 1e-12;
432
433 #[test]
436 fn linear_interp_exact_at_data_points() {
437 let x_data = vec![0.0, 1.0, 2.0, 3.0, 4.0];
439 let y_data: Vec<f64> = x_data.iter().map(|&x| 2.0 * x + 1.0).collect();
440 let result = interp1d_linear(&x_data, &y_data, &x_data).unwrap();
441 for (i, &r) in result.iter().enumerate() {
442 assert!(
443 (r - y_data[i]).abs() < EPS,
444 "at x={}, expected {}, got {}",
445 x_data[i],
446 y_data[i],
447 r
448 );
449 }
450 }
451
452 #[test]
453 fn linear_interp_between_points() {
454 let x_data = vec![0.0, 1.0, 2.0, 3.0, 4.0];
455 let y_data: Vec<f64> = x_data.iter().map(|&x| 2.0 * x + 1.0).collect();
456 let x_query = vec![0.5, 1.5, 2.5, 3.5];
457 let result = interp1d_linear(&x_data, &y_data, &x_query).unwrap();
458 for (i, &xq) in x_query.iter().enumerate() {
459 let expected = 2.0 * xq + 1.0;
460 assert!(
461 (result[i] - expected).abs() < EPS,
462 "at x={}, expected {}, got {}",
463 xq,
464 expected,
465 result[i]
466 );
467 }
468 }
469
470 #[test]
471 fn linear_interp_extrapolation_clamps() {
472 let x_data = vec![1.0, 2.0, 3.0];
473 let y_data = vec![10.0, 20.0, 30.0];
474 let x_query = vec![-5.0, 0.0, 4.0, 100.0];
475 let result = interp1d_linear(&x_data, &y_data, &x_query).unwrap();
476 assert!((result[0] - 10.0).abs() < EPS); assert!((result[1] - 10.0).abs() < EPS); assert!((result[2] - 30.0).abs() < EPS); assert!((result[3] - 30.0).abs() < EPS); }
481
482 #[test]
483 fn linear_interp_validation_errors() {
484 assert!(interp1d_linear(&[], &[], &[1.0]).is_err());
485 assert!(interp1d_linear(&[1.0], &[1.0], &[1.0]).is_err());
486 assert!(interp1d_linear(&[1.0, 2.0], &[1.0], &[1.0]).is_err()); assert!(interp1d_linear(&[2.0, 1.0], &[1.0, 2.0], &[1.0]).is_err()); }
489
490 #[test]
493 fn nearest_interp_snaps_to_closest() {
494 let x_data = vec![0.0, 1.0, 3.0, 5.0];
495 let y_data = vec![10.0, 20.0, 30.0, 40.0];
496 let x_query = vec![0.3, 0.7, 2.5, 4.0];
501 let result = interp1d_nearest(&x_data, &y_data, &x_query).unwrap();
502 assert!((result[0] - 10.0).abs() < EPS);
503 assert!((result[1] - 20.0).abs() < EPS);
504 assert!((result[2] - 30.0).abs() < EPS);
505 assert!((result[3] - 30.0).abs() < EPS); }
507
508 #[test]
509 fn nearest_interp_at_data_points() {
510 let x_data = vec![0.0, 1.0, 2.0];
511 let y_data = vec![5.0, 15.0, 25.0];
512 let result = interp1d_nearest(&x_data, &y_data, &x_data).unwrap();
513 for (i, &r) in result.iter().enumerate() {
514 assert!((r - y_data[i]).abs() < EPS);
515 }
516 }
517
518 #[test]
519 fn nearest_interp_boundary_clamp() {
520 let x_data = vec![1.0, 2.0, 3.0];
521 let y_data = vec![10.0, 20.0, 30.0];
522 let result = interp1d_nearest(&x_data, &y_data, &[-1.0, 0.5, 3.5, 100.0]).unwrap();
523 assert!((result[0] - 10.0).abs() < EPS);
524 assert!((result[1] - 10.0).abs() < EPS);
525 assert!((result[2] - 30.0).abs() < EPS);
526 assert!((result[3] - 30.0).abs() < EPS);
527 }
528
529 #[test]
532 fn polyfit_degree1_linear_data() {
533 let x: Vec<f64> = (0..10).map(|i| i as f64).collect();
535 let y: Vec<f64> = x.iter().map(|&xi| 3.0 * xi + 2.0).collect();
536 let coeffs = polyfit(&x, &y, 1).unwrap();
537 assert_eq!(coeffs.len(), 2);
538 assert!(
539 (coeffs[0] - 2.0).abs() < 1e-10,
540 "intercept: expected 2.0, got {}",
541 coeffs[0]
542 );
543 assert!(
544 (coeffs[1] - 3.0).abs() < 1e-10,
545 "slope: expected 3.0, got {}",
546 coeffs[1]
547 );
548 }
549
550 #[test]
551 fn polyfit_degree2_quadratic_data() {
552 let x: Vec<f64> = (0..20).map(|i| i as f64 * 0.5).collect();
554 let y: Vec<f64> = x.iter().map(|&xi| 1.0 + 2.0 * xi + 3.0 * xi * xi).collect();
555 let coeffs = polyfit(&x, &y, 2).unwrap();
556 assert_eq!(coeffs.len(), 3);
557 assert!(
558 (coeffs[0] - 1.0).abs() < 1e-8,
559 "a0: expected 1.0, got {}",
560 coeffs[0]
561 );
562 assert!(
563 (coeffs[1] - 2.0).abs() < 1e-8,
564 "a1: expected 2.0, got {}",
565 coeffs[1]
566 );
567 assert!(
568 (coeffs[2] - 3.0).abs() < 1e-8,
569 "a2: expected 3.0, got {}",
570 coeffs[2]
571 );
572 }
573
574 #[test]
575 fn polyfit_validation_errors() {
576 assert!(polyfit(&[1.0], &[1.0, 2.0], 1).is_err()); assert!(polyfit(&[1.0], &[1.0], 1).is_err()); }
579
580 #[test]
583 fn polyval_roundtrip_with_polyfit() {
584 let x: Vec<f64> = (0..15).map(|i| i as f64 * 0.3).collect();
586 let y: Vec<f64> = x.iter().map(|&xi| 5.0 - xi + 0.5 * xi * xi).collect();
587 let coeffs = polyfit(&x, &y, 2).unwrap();
588 let y_eval = polyval(&coeffs, &x);
589 for (i, (&ye, &yo)) in y_eval.iter().zip(y.iter()).enumerate() {
590 assert!(
591 (ye - yo).abs() < 1e-8,
592 "at i={}, expected {}, got {}",
593 i,
594 yo,
595 ye
596 );
597 }
598 }
599
600 #[test]
601 fn polyval_empty_coeffs() {
602 let result = polyval(&[], &[1.0, 2.0, 3.0]);
603 assert_eq!(result.len(), 3);
604 for &r in &result {
605 assert!((r - 0.0).abs() < EPS);
606 }
607 }
608
609 #[test]
610 fn polyval_constant() {
611 let result = polyval(&[42.0], &[0.0, 1.0, 100.0]);
612 for &r in &result {
613 assert!((r - 42.0).abs() < EPS);
614 }
615 }
616
617 #[test]
620 fn spline_interpolates_data_points_exactly() {
621 let x = vec![0.0, 1.0, 2.0, 3.0, 4.0];
622 let y = vec![0.0, 1.0, 0.0, 1.0, 0.0];
623 let spline = spline_cubic_natural(&x, &y).unwrap();
624 let result = spline_eval(&spline, &x).unwrap();
625 for (i, (&r, &yi)) in result.iter().zip(y.iter()).enumerate() {
626 assert!(
627 (r - yi).abs() < EPS,
628 "at knot x={}, expected {}, got {}",
629 x[i],
630 yi,
631 r
632 );
633 }
634 }
635
636 #[test]
637 fn spline_linear_data_is_exact() {
638 let x = vec![0.0, 1.0, 2.0, 3.0];
641 let y: Vec<f64> = x.iter().map(|&xi| 2.0 * xi + 1.0).collect();
642 let spline = spline_cubic_natural(&x, &y).unwrap();
643 let x_query = vec![0.0, 0.5, 1.0, 1.5, 2.0, 2.5, 3.0];
644 let result = spline_eval(&spline, &x_query).unwrap();
645 for (i, &xq) in x_query.iter().enumerate() {
646 let expected = 2.0 * xq + 1.0;
647 assert!(
648 (result[i] - expected).abs() < 1e-10,
649 "at x={}, expected {}, got {}",
650 xq,
651 expected,
652 result[i]
653 );
654 }
655 }
656
657 #[test]
658 fn spline_derivative_continuity_at_knots() {
659 let x = vec![0.0, 1.0, 2.0, 3.0, 4.0];
661 let y = vec![0.0, 2.0, 1.0, 3.0, 0.5];
662 let spline = spline_cubic_natural(&x, &y).unwrap();
663
664 let delta = 1e-7;
665 for &knot in &[1.0, 2.0, 3.0] {
667 let left = spline_eval(&spline, &[knot - delta]).unwrap()[0];
668 let right = spline_eval(&spline, &[knot + delta]).unwrap()[0];
669 let at = spline_eval(&spline, &[knot]).unwrap()[0];
670 let deriv_left = (at - left) / delta;
671 let deriv_right = (right - at) / delta;
672 let diff = (deriv_left - deriv_right).abs();
673 assert!(
674 diff < 1e-4,
675 "derivative discontinuity at x={}: left={}, right={}, diff={}",
676 knot,
677 deriv_left,
678 deriv_right,
679 diff
680 );
681 }
682 }
683
684 #[test]
685 fn spline_second_derivative_zero_at_boundaries() {
686 let x = vec![0.0, 1.0, 2.0, 3.0, 4.0];
691 let y = vec![1.0, 3.0, 2.0, 5.0, 4.0];
692 let spline = spline_cubic_natural(&x, &y).unwrap();
693
694 assert!(
696 (2.0 * spline.c[0]).abs() < 1e-10,
697 "second derivative at left boundary = {}",
698 2.0 * spline.c[0]
699 );
700
701 let last = spline.a.len() - 1;
703 let h_last = x[last + 1] - x[last];
704 let second_deriv_right = 2.0 * spline.c[last] + 6.0 * spline.d[last] * h_last;
705 assert!(
706 second_deriv_right.abs() < 1e-10,
707 "second derivative at right boundary = {}",
708 second_deriv_right
709 );
710 }
711
712 #[test]
713 fn spline_validation_errors() {
714 assert!(spline_cubic_natural(&[], &[]).is_err());
715 assert!(spline_cubic_natural(&[1.0], &[1.0]).is_err());
716 assert!(spline_cubic_natural(&[2.0, 1.0], &[1.0, 2.0]).is_err()); }
718
719 #[test]
722 fn determinism_linear_interp() {
723 let x_data = vec![0.0, 1.0, 2.0, 3.0, 4.0, 5.0];
724 let y_data = vec![0.0, 0.8, 0.9, 0.1, -0.8, -1.0];
725 let x_query: Vec<f64> = (0..100).map(|i| i as f64 * 0.05).collect();
726 let r1 = interp1d_linear(&x_data, &y_data, &x_query).unwrap();
727 let r2 = interp1d_linear(&x_data, &y_data, &x_query).unwrap();
728 assert_eq!(r1.len(), r2.len());
729 for (a, b) in r1.iter().zip(r2.iter()) {
730 assert_eq!(a.to_bits(), b.to_bits(), "non-deterministic result detected");
731 }
732 }
733
734 #[test]
735 fn determinism_polyfit() {
736 let x: Vec<f64> = (0..50).map(|i| i as f64 * 0.1).collect();
737 let y: Vec<f64> = x.iter().map(|&xi| xi.sin()).collect();
738 let c1 = polyfit(&x, &y, 5).unwrap();
739 let c2 = polyfit(&x, &y, 5).unwrap();
740 assert_eq!(c1.len(), c2.len());
741 for (a, b) in c1.iter().zip(c2.iter()) {
742 assert_eq!(a.to_bits(), b.to_bits(), "non-deterministic polyfit");
743 }
744 }
745
746 #[test]
747 fn determinism_cubic_spline() {
748 let x = vec![0.0, 0.5, 1.0, 1.5, 2.0, 2.5, 3.0];
749 let y = vec![0.0, 0.48, 0.84, 1.0, 0.91, 0.60, 0.14];
750 let x_query: Vec<f64> = (0..60).map(|i| i as f64 * 0.05).collect();
751 let s1 = spline_cubic_natural(&x, &y).unwrap();
752 let s2 = spline_cubic_natural(&x, &y).unwrap();
753 let r1 = spline_eval(&s1, &x_query).unwrap();
754 let r2 = spline_eval(&s2, &x_query).unwrap();
755 for (a, b) in r1.iter().zip(r2.iter()) {
756 assert_eq!(a.to_bits(), b.to_bits(), "non-deterministic spline");
757 }
758 }
759}