ras_commander_fast_map/
scipy_interp.rs

1//! Exact port of SciPy's 2-D `LinearNDInterpolator` evaluation over a caller-supplied `Delaunay`.
2//!
3//! Ported from `scipy/spatial/_qhull.pyx` (`_find_simplex`, `_find_simplex_directed`,
4//! `_find_simplex_bruteforce`, `_barycentric_inside`, `_barycentric_coordinate_single`,
5//! `_barycentric_coordinates`, `_lift_point`, `_distplane`, `_is_point_fully_outside`) and
6//! `scipy/interpolate/_interpnd.pyx` (`LinearNDInterpolator._do_evaluate`): the same
7//! operation order, `eps = 100*DBL_EPSILON`, `eps_broad = sqrt(DBL_EPSILON)`, the walk start
8//! carried across query points in row-major order, and the value accumulation
9//! `out = 0; out += c[k]*z[simplex[k]]` for k = 0..2. No fused multiply-add.
10//!
11//! The barycentric transforms are SciPy's own (`Delaunay.transform`, supplied): SciPy computes
12//! them with LAPACK LU and no scalar 2x2 variant tested reproduced its bits. The transform's
13//! origin is the simplex's LAST vertex, as in SciPy (r1 QA M1: the r1 kernel used the first).
14//!
15//! Two selection modes give identical values:
16//! - [`ScipyDelaunay::evaluate_serial`]: the complete serial walk, bit-for-bit SciPy semantics.
17//! - [`ScipyDelaunay::evaluate_guarded`]: order-independent per-pixel evaluation. It is exact
18//!   only where the SciPy result cannot depend on the walk: the point lies inside some candidate
19//!   simplex with every barycentric coordinate >= 0, and every simplex accepted by SciPy's
20//!   `eps` test yields bitwise the same value. Any other pixel is reported as ambiguous and the
21//!   caller must use the serial walk. Degenerate (NaN-transform) simplices disable guarded
22//!   mode entirely, because SciPy's `eps_broad` brute-force branch is path dependent.
23use crate::MapError;
24use rstar::{AABB, RTree, RTreeObject};
25
26pub const EPS: f64 = 100.0 * f64::EPSILON;
27
28pub fn eps_broad() -> f64 {
29    f64::EPSILON.sqrt()
30}
31
32/// One SciPy `Delaunay` object, as supplied by the Python adapter.
33pub struct ScipyDelaunay {
34    pub xy: Vec<[f64; 2]>,
35    pub z: Vec<f64>,
36    pub simplices: Vec<[u32; 3]>,
37    /// `transform[s]` = rows 0-1: inverse matrix (row-major), row 2: origin (last vertex).
38    pub transform: Vec<[f64; 6]>,
39    pub neighbors: Vec<[i32; 3]>,
40    pub equations: Vec<[f64; 4]>,
41    pub paraboloid_scale: f64,
42    pub paraboloid_shift: f64,
43    pub min_bound: [f64; 2],
44    pub max_bound: [f64; 2],
45    pub degenerate: usize,
46    tree: Option<RTree<Env>>,
47}
48
49#[derive(Clone)]
50struct Env {
51    id: u32,
52    bounds: AABB<[f64; 2]>,
53}
54
55impl RTreeObject for Env {
56    type Envelope = AABB<[f64; 2]>;
57    fn envelope(&self) -> Self::Envelope {
58        self.bounds
59    }
60}
61
62/// Result of locating and interpolating one query point.
63#[derive(Clone, Copy, Debug, PartialEq)]
64pub enum Located {
65    Outside,
66    Value {
67        simplex: u32,
68        value: f64,
69    },
70    /// Guarded mode only: the SciPy result may depend on the walk order.
71    Ambiguous,
72}
73
74#[derive(Default, Debug, Clone, Copy, serde::Serialize)]
75pub struct WalkStats {
76    pub points: u64,
77    pub outside_bbox: u64,
78    pub bruteforce_calls: u64,
79    pub directed_steps: u64,
80}
81
82pub struct DelaunayParts {
83    pub samples: Vec<[f64; 3]>,
84    pub simplices: Vec<[u32; 3]>,
85    pub transform: Vec<[f64; 6]>,
86    pub neighbors: Vec<[i32; 3]>,
87    pub equations: Vec<[f64; 4]>,
88    pub paraboloid_scale: f64,
89    pub paraboloid_shift: f64,
90    pub min_bound: [f64; 2],
91    pub max_bound: [f64; 2],
92}
93
94impl ScipyDelaunay {
95    pub fn new(p: DelaunayParts) -> Result<Self, MapError> {
96        let n = p.samples.len();
97        let m = p.simplices.len();
98        if m == 0 {
99            return Err(MapError::Invalid("Delaunay has no simplices".into()));
100        }
101        if p.transform.len() != m || p.neighbors.len() != m || p.equations.len() != m {
102            return Err(MapError::Invalid(
103                "transform/neighbors/equations length differs from simplices".into(),
104            ));
105        }
106        if m > i32::MAX as usize {
107            return Err(MapError::Budget("too many simplices".into()));
108        }
109        if let Some(i) = p
110            .samples
111            .iter()
112            .position(|s| !s.iter().all(|v| v.is_finite()))
113        {
114            return Err(MapError::Invalid(format!("sample {i} is not finite")));
115        }
116        for (s, t) in p.simplices.iter().enumerate() {
117            if t.iter().any(|v| *v as usize >= n) {
118                return Err(MapError::Invalid(format!(
119                    "simplex {s} vertex out of range"
120                )));
121            }
122            if p.neighbors[s].iter().any(|v| *v < -1 || *v >= m as i32) {
123                return Err(MapError::Invalid(format!(
124                    "simplex {s} neighbor out of range"
125                )));
126            }
127            if !p.equations[s].iter().all(|v| v.is_finite()) {
128                return Err(MapError::Invalid(format!(
129                    "simplex {s} equation not finite"
130                )));
131            }
132        }
133        let degenerate = p.transform.iter().filter(|t| t[0].is_nan()).count();
134        for (s, t) in p.transform.iter().enumerate() {
135            let nan = t.iter().filter(|v| v.is_nan()).count();
136            if !(nan == 0 || nan == 6) || t.iter().any(|v| v.is_infinite()) {
137                return Err(MapError::Invalid(format!(
138                    "simplex {s} transform is partially NaN or infinite"
139                )));
140            }
141        }
142        if !(p.paraboloid_scale.is_finite() && p.paraboloid_shift.is_finite())
143            || !p
144                .min_bound
145                .iter()
146                .chain(p.max_bound.iter())
147                .all(|v| v.is_finite())
148        {
149            return Err(MapError::Invalid("Delaunay scalars not finite".into()));
150        }
151        let xy = p.samples.iter().map(|s| [s[0], s[1]]).collect();
152        let z = p.samples.iter().map(|s| s[2]).collect();
153        Ok(Self {
154            xy,
155            z,
156            simplices: p.simplices,
157            transform: p.transform,
158            neighbors: p.neighbors,
159            equations: p.equations,
160            paraboloid_scale: p.paraboloid_scale,
161            paraboloid_shift: p.paraboloid_shift,
162            min_bound: p.min_bound,
163            max_bound: p.max_bound,
164            degenerate,
165            tree: None,
166        })
167    }
168
169    pub fn nsimplex(&self) -> usize {
170        self.simplices.len()
171    }
172
173    /// Builds the candidate index used by guarded mode (simplex envelopes with a margin).
174    pub fn build_index(&mut self) {
175        let envs = self
176            .simplices
177            .iter()
178            .enumerate()
179            .map(|(id, t)| {
180                let p = t.map(|v| self.xy[v as usize]);
181                let lo = [
182                    p[0][0].min(p[1][0]).min(p[2][0]),
183                    p[0][1].min(p[1][1]).min(p[2][1]),
184                ];
185                let hi = [
186                    p[0][0].max(p[1][0]).max(p[2][0]),
187                    p[0][1].max(p[1][1]).max(p[2][1]),
188                ];
189                // Generous margin: SciPy's eps test admits points marginally outside a simplex.
190                let span = (hi[0] - lo[0]).max(hi[1] - lo[1]);
191                let scale = lo[0]
192                    .abs()
193                    .max(lo[1].abs())
194                    .max(hi[0].abs())
195                    .max(hi[1].abs());
196                let margin = span * 1e-9 + scale * 1e-12 + 1e-9;
197                Env {
198                    id: id as u32,
199                    bounds: AABB::from_corners(
200                        [lo[0] - margin, lo[1] - margin],
201                        [hi[0] + margin, hi[1] + margin],
202                    ),
203                }
204            })
205            .collect();
206        self.tree = Some(RTree::bulk_load(envs));
207    }
208
209    pub fn approx_bytes(&self) -> u64 {
210        let m = self.simplices.len() as u64;
211        let n = self.xy.len() as u64;
212        n * 24 + m * (12 + 48 + 12 + 32) + if self.tree.is_some() { m * 72 } else { 0 }
213    }
214
215    #[inline]
216    fn fully_outside(&self, x: [f64; 2]) -> bool {
217        (0..2).any(|i| x[i] < self.min_bound[i] - EPS || x[i] > self.max_bound[i] + EPS)
218    }
219
220    /// `_barycentric_coordinate_single` for i < ndim.
221    #[inline]
222    fn coord(t: &[f64; 6], x: [f64; 2], i: usize) -> f64 {
223        let mut c = 0.0;
224        c += t[2 * i] * (x[0] - t[4]);
225        c += t[2 * i + 1] * (x[1] - t[5]);
226        c
227    }
228
229    /// `_barycentric_coordinates`.
230    #[inline]
231    fn coords(t: &[f64; 6], x: [f64; 2]) -> [f64; 3] {
232        let c0 = Self::coord(t, x, 0);
233        let c1 = Self::coord(t, x, 1);
234        let mut c2 = 1.0;
235        c2 -= c0;
236        c2 -= c1;
237        [c0, c1, c2]
238    }
239
240    #[inline]
241    fn in_range(v: f64, lo: f64) -> bool {
242        lo <= v && v <= 1.0 + EPS
243    }
244
245    /// `_barycentric_inside`.
246    #[inline]
247    fn inside(t: &[f64; 6], x: [f64; 2], c: &mut [f64; 3]) -> bool {
248        c[2] = 1.0;
249        for i in 0..2 {
250            c[i] = Self::coord(t, x, i);
251            c[2] -= c[i];
252            if !Self::in_range(c[i], -EPS) {
253                return false;
254            }
255        }
256        Self::in_range(c[2], -EPS)
257    }
258
259    #[inline]
260    fn value(&self, s: usize, c: &[f64; 3]) -> f64 {
261        let v = self.simplices[s];
262        let mut out = 0.0;
263        for k in 0..3 {
264            out += c[k] * self.z[v[k] as usize];
265        }
266        out
267    }
268
269    fn bruteforce(&self, c: &mut [f64; 3], x: [f64; 2], st: &mut WalkStats) -> i32 {
270        st.bruteforce_calls += 1;
271        if self.fully_outside(x) {
272            return -1;
273        }
274        let eb = eps_broad();
275        for s in 0..self.nsimplex() {
276            let t = &self.transform[s];
277            if !t[0].is_nan() {
278                if Self::inside(t, x, c) {
279                    return s as i32;
280                }
281            } else {
282                for k in 0..3 {
283                    let nb = self.neighbors[s][k];
284                    if nb == -1 {
285                        continue;
286                    }
287                    let tn = &self.transform[nb as usize];
288                    if tn[0].is_nan() {
289                        continue;
290                    }
291                    *c = Self::coords(tn, x);
292                    let mut ok = true;
293                    for (cm, nm) in c.iter().zip(self.neighbors[nb as usize]) {
294                        let lo = if nm == s as i32 { -eb } else { -EPS };
295                        if !Self::in_range(*cm, lo) {
296                            ok = false;
297                            break;
298                        }
299                    }
300                    if ok {
301                        return nb;
302                    }
303                }
304            }
305        }
306        -1
307    }
308
309    fn directed(&self, c: &mut [f64; 3], x: [f64; 2], start: &mut i32, st: &mut WalkStats) -> i32 {
310        let m = self.nsimplex() as i32;
311        let mut s = *start;
312        if s < 0 || s >= m {
313            s = 0;
314        }
315        let cycles = 1 + self.nsimplex() / 4;
316        let mut converged = false;
317        for _ in 0..cycles {
318            if s == -1 {
319                converged = true;
320                break;
321            }
322            st.directed_steps += 1;
323            let t = &self.transform[s as usize];
324            let mut inside = 1i32;
325            for k in 0..3 {
326                if k < 2 {
327                    c[k] = Self::coord(t, x, k);
328                } else {
329                    c[2] = 1.0;
330                    c[2] -= c[0];
331                    c[2] -= c[1];
332                }
333                if c[k] < -EPS {
334                    let nb = self.neighbors[s as usize][k];
335                    if nb == -1 {
336                        *start = s;
337                        return -1;
338                    }
339                    s = nb;
340                    inside = -1;
341                    break;
342                } else if c[k] <= 1.0 + EPS {
343                } else {
344                    inside = 0;
345                }
346            }
347            if inside == -1 {
348                continue;
349            } else if inside == 1 {
350                converged = true;
351                break;
352            } else {
353                s = self.bruteforce(c, x, st);
354                converged = true;
355                break;
356            }
357        }
358        if !converged {
359            s = self.bruteforce(c, x, st);
360        }
361        *start = s;
362        s
363    }
364
365    #[inline]
366    fn distplane(&self, s: usize, z: &[f64; 3]) -> f64 {
367        let e = &self.equations[s];
368        let mut d = e[3];
369        for k in 0..3 {
370            d += e[k] * z[k];
371        }
372        d
373    }
374
375    /// `_find_simplex`, mutating the carried `start` exactly as SciPy does.
376    pub fn find_simplex(
377        &self,
378        c: &mut [f64; 3],
379        x: [f64; 2],
380        start: &mut i32,
381        st: &mut WalkStats,
382    ) -> i32 {
383        st.points += 1;
384        if self.fully_outside(x) {
385            st.outside_bbox += 1;
386            return -1;
387        }
388        let m = self.nsimplex() as i32;
389        let mut s = *start;
390        if s < 0 || s >= m {
391            s = 0;
392        }
393        let mut z = [x[0], x[1], 0.0];
394        z[2] += x[0] * x[0];
395        z[2] += x[1] * x[1];
396        z[2] *= self.paraboloid_scale;
397        z[2] += self.paraboloid_shift;
398        let mut best = self.distplane(s as usize, &z);
399        let mut changed = true;
400        while changed {
401            if best > 0.0 {
402                break;
403            }
404            changed = false;
405            for k in 0..3 {
406                let nb = self.neighbors[s as usize][k];
407                if nb == -1 {
408                    continue;
409                }
410                let d = self.distplane(nb as usize, &z);
411                if d > best + EPS * (1.0 + best.abs()) {
412                    s = nb;
413                    best = d;
414                    changed = true;
415                }
416            }
417        }
418        *start = s;
419        self.directed(c, x, start, st)
420    }
421
422    /// SciPy-exact serial evaluation over the full grid, in row-major order (start carried).
423    /// `keep(row, col)` false sets the output to NaN but the walk still visits the point,
424    /// because skipping points would change the carried start.
425    pub fn evaluate_serial(
426        &self,
427        xs: &[f64],
428        ys: &[f64],
429        mut sink: impl FnMut(usize, usize, Option<(u32, f64)>),
430    ) -> WalkStats {
431        let mut st = WalkStats::default();
432        let mut start = 0i32;
433        let mut c = [0.0; 3];
434        for (r, &y) in ys.iter().enumerate() {
435            for (col, &x) in xs.iter().enumerate() {
436                let s = self.find_simplex(&mut c, [x, y], &mut start, &mut st);
437                if s < 0 {
438                    sink(r, col, None);
439                } else {
440                    sink(r, col, Some((s as u32, self.value(s as usize, &c))));
441                }
442            }
443        }
444        st
445    }
446
447    /// Order-independent evaluation of one point (see module docs). Needs [`Self::build_index`].
448    pub fn locate_guarded(&self, x: [f64; 2]) -> Located {
449        if self.fully_outside(x) {
450            return Located::Outside;
451        }
452        let tree = self
453            .tree
454            .as_ref()
455            .expect("build_index before guarded evaluation");
456        let mut first: Option<(u32, f64)> = None;
457        let mut any = false;
458        let mut strictly_inside = false;
459        let mut differ = false;
460        let mut c = [0.0; 3];
461        for e in tree.locate_in_envelope_intersecting(AABB::from_point(x)) {
462            let s = e.id as usize;
463            let t = &self.transform[s];
464            if Self::inside(t, x, &mut c) {
465                any = true;
466                if c.iter().all(|v| *v >= 0.0) {
467                    strictly_inside = true;
468                }
469                let v = self.value(s, &c);
470                match first {
471                    None => first = Some((e.id, v)),
472                    Some((id, fv)) => {
473                        if fv.to_bits() != v.to_bits() {
474                            differ = true;
475                        }
476                        if e.id < id {
477                            first = Some((e.id, fv));
478                        }
479                    }
480                }
481            }
482        }
483        match (any, strictly_inside, differ) {
484            (false, _, _) => Located::Outside,
485            (true, true, false) => {
486                let (s, v) = first.expect("candidate");
487                Located::Value {
488                    simplex: s,
489                    value: v,
490                }
491            }
492            _ => Located::Ambiguous,
493        }
494    }
495
496    pub fn guarded_allowed(&self) -> bool {
497        self.degenerate == 0
498    }
499}
500
501#[cfg(test)]
502mod tests {
503    use super::*;
504
505    /// Unit square split on the diagonal; transforms as SciPy would give (origin = last vertex).
506    fn square() -> ScipyDelaunay {
507        let samples = vec![[0., 0., 0.], [1., 0., 1.], [0., 1., 1.], [1., 1., 2.]];
508        // simplex 0: (1,3,2) last=2 (0,1): T = [[p1-p2],[p3-p2]]^T columns
509        // Build inverse transforms numerically for the test.
510        let simplices = vec![[1u32, 3, 2], [0u32, 1, 2]];
511        let mut transform = Vec::new();
512        for s in &simplices {
513            let p = s.map(|v| samples[v as usize]);
514            let r = [p[2][0], p[2][1]];
515            let t = [
516                [p[0][0] - r[0], p[1][0] - r[0]],
517                [p[0][1] - r[1], p[1][1] - r[1]],
518            ];
519            let det = t[0][0] * t[1][1] - t[0][1] * t[1][0];
520            transform.push([
521                t[1][1] / det,
522                -t[0][1] / det,
523                -t[1][0] / det,
524                t[0][0] / det,
525                r[0],
526                r[1],
527            ]);
528        }
529        let mut d = ScipyDelaunay::new(DelaunayParts {
530            samples,
531            simplices,
532            transform,
533            neighbors: vec![[-1, 1, -1], [0, -1, -1]],
534            equations: vec![[0., 0., -1., 1.], [0., 0., -1., 1.]],
535            paraboloid_scale: 1.0,
536            paraboloid_shift: 0.0,
537            min_bound: [0., 0.],
538            max_bound: [1., 1.],
539        })
540        .unwrap();
541        d.build_index();
542        d
543    }
544
545    #[test]
546    fn plane_values_and_outside() {
547        let d = square();
548        let mut c = [0.0; 3];
549        let mut start = 0;
550        let mut st = WalkStats::default();
551        let s = d.find_simplex(&mut c, [0.25, 0.25], &mut start, &mut st);
552        assert_eq!(s, 1);
553        assert_eq!(d.value(1, &c), 0.5);
554        assert_eq!(d.find_simplex(&mut c, [2.0, 2.0], &mut start, &mut st), -1);
555        match d.locate_guarded([0.75, 0.75]) {
556            Located::Value { simplex, value } => {
557                assert_eq!(simplex, 0);
558                assert!((value - 1.5).abs() < 1e-15);
559            }
560            other => panic!("{other:?}"),
561        }
562        assert_eq!(d.locate_guarded([3.0, 0.5]), Located::Outside);
563    }
564
565    #[test]
566    fn serial_and_guarded_agree_on_grid() {
567        let d = square();
568        let xs: Vec<f64> = (0..11).map(|i| i as f64 / 10.0).collect();
569        let mut serial = vec![f64::NAN; 121];
570        d.evaluate_serial(&xs, &xs, |r, c, v| {
571            serial[r * 11 + c] = v.map_or(f64::NAN, |(_, z)| z)
572        });
573        for r in 0..11 {
574            for c in 0..11 {
575                if let Located::Value { value, .. } = d.locate_guarded([xs[c], xs[r]]) {
576                    assert_eq!(value.to_bits(), serial[r * 11 + c].to_bits());
577                }
578            }
579        }
580    }
581
582    #[test]
583    fn rejects_bad_inputs() {
584        let base = || DelaunayParts {
585            samples: vec![[0., 0., 0.], [1., 0., 1.], [0., 1., 1.]],
586            simplices: vec![[0, 1, 2]],
587            transform: vec![[1., 0., 0., 1., 0., 1.]],
588            neighbors: vec![[-1, -1, -1]],
589            equations: vec![[0., 0., -1., 1.]],
590            paraboloid_scale: 1.0,
591            paraboloid_shift: 0.0,
592            min_bound: [0., 0.],
593            max_bound: [1., 1.],
594        };
595        let mut p = base();
596        p.simplices = vec![[0, 1, 3]];
597        assert!(ScipyDelaunay::new(p).is_err());
598        let mut p = base();
599        p.neighbors = vec![[5, -1, -1]];
600        assert!(ScipyDelaunay::new(p).is_err());
601        let mut p = base();
602        p.transform = vec![[f64::NAN, 0., 0., 1., 0., 1.]];
603        assert!(ScipyDelaunay::new(p).is_err());
604        let mut p = base();
605        p.transform = vec![[f64::NAN; 6]];
606        assert_eq!(ScipyDelaunay::new(p).unwrap().degenerate, 1);
607    }
608}
ras_commander_fast_map/scipy_interp.rs

ras_commander_fast_map/
scipy_interp.rs
