ras_commander_fast_map/
interp.rs

1//! Prepared, reusable interpolator over caller-supplied global simplices.
2//!
3//! The triangle evaluation, inclusion tolerance and tie rule are copied from the baseline
4//! prototype kernel (`prototype/kernel/src/lib.rs`). Differences: degenerate simplices are
5//! skipped (counted) instead of aborting, and the structure is built once and shared read-only by
6//! all tiles and AOIs of a profile.
7use crate::MapError;
8use rstar::{AABB, RTree, RTreeObject};
9
10pub const MAX_SAMPLES: usize = 50_000_000;
11pub const MAX_SIMPLICES: usize = 100_000_000;
12/// Reciprocal-condition threshold below which a supplied simplex is treated as singular.
13pub const RCOND_LIMIT: f64 = 1000.0 * f64::EPSILON;
14
15#[derive(Clone)]
16struct Triangle {
17    id: u32,
18    bounds: AABB<[f64; 2]>,
19    a: [f64; 2],
20    u: [f64; 2],
21    v: [f64; 2],
22    determinant: f64,
23    z: [f64; 3],
24}
25
26impl RTreeObject for Triangle {
27    type Envelope = AABB<[f64; 2]>;
28    fn envelope(&self) -> Self::Envelope {
29        self.bounds
30    }
31}
32
33impl Triangle {
34    /// Baseline kernel formula, unchanged.
35    #[inline]
36    fn at(&self, p: [f64; 2]) -> Option<f64> {
37        let x = p[0] - self.a[0];
38        let y = p[1] - self.a[1];
39        let b = (x * self.v[1] - y * self.v[0]) / self.determinant;
40        let c = (self.u[0] * y - self.u[1] * x) / self.determinant;
41        let a = 1.0 - b - c;
42        let eps = 100.0 * f64::EPSILON;
43        if a >= -eps && b >= -eps && c >= -eps {
44            Some(a * self.z[0] + b * self.z[1] + c * self.z[2])
45        } else {
46            None
47        }
48    }
49}
50
51/// Exact 1-norm reciprocal condition number of the 2x2 transform [u v].
52fn rcond(u: [f64; 2], v: [f64; 2], det: f64) -> f64 {
53    let norm = (u[0].abs() + u[1].abs()).max(v[0].abs() + v[1].abs());
54    let inv = (v[1].abs() + u[1].abs()).max(v[0].abs() + u[0].abs()) / det.abs();
55    1.0 / (norm * inv)
56}
57
58pub struct PreparedInterpolator {
59    tree: RTree<Triangle>,
60    pub samples: usize,
61    pub simplices_supplied: usize,
62    pub triangles_valid: usize,
63    pub degenerate_skipped: usize,
64}
65
66impl PreparedInterpolator {
67    pub fn new(samples: &[[f64; 3]], simplices: &[[usize; 3]]) -> Result<Self, MapError> {
68        if samples.len() > MAX_SAMPLES || simplices.len() > MAX_SIMPLICES {
69            return Err(MapError::Budget(format!(
70                "samples {} / simplices {} exceed limits {MAX_SAMPLES} / {MAX_SIMPLICES}",
71                samples.len(),
72                simplices.len()
73            )));
74        }
75        if let Some(i) = samples
76            .iter()
77            .position(|s| !s.iter().all(|x| x.is_finite()))
78        {
79            return Err(MapError::Invalid(format!("sample {i} is not finite")));
80        }
81        let mut triangles = Vec::with_capacity(simplices.len());
82        let mut degenerate = 0usize;
83        for (id, t) in simplices.iter().enumerate() {
84            if let Some(bad) = t.iter().find(|i| **i >= samples.len()) {
85                return Err(MapError::Invalid(format!(
86                    "simplex {id} index {bad} out of bounds for {} samples",
87                    samples.len()
88                )));
89            }
90            let (a, b, c) = (samples[t[0]], samples[t[1]], samples[t[2]]);
91            let u = [b[0] - a[0], b[1] - a[1]];
92            let v = [c[0] - a[0], c[1] - a[1]];
93            if !u.iter().chain(v.iter()).all(|x| x.is_finite()) {
94                return Err(MapError::Invalid(format!(
95                    "simplex {id} edge vector overflows"
96                )));
97            }
98            let determinant = u[0] * v[1] - u[1] * v[0];
99            if !determinant.is_finite() {
100                return Err(MapError::Invalid(format!(
101                    "simplex {id} determinant overflows"
102                )));
103            }
104            if determinant == 0.0 || rcond(u, v, determinant) < RCOND_LIMIT {
105                degenerate += 1;
106                continue;
107            }
108            triangles.push(Triangle {
109                id: id as u32,
110                a: [a[0], a[1]],
111                u,
112                v,
113                determinant,
114                z: [a[2], b[2], c[2]],
115                bounds: AABB::from_corners(
116                    [a[0].min(b[0]).min(c[0]), a[1].min(b[1]).min(c[1])],
117                    [a[0].max(b[0]).max(c[0]), a[1].max(b[1]).max(c[1])],
118                ),
119            });
120        }
121        let valid = triangles.len();
122        Ok(Self {
123            tree: RTree::bulk_load(triangles),
124            samples: samples.len(),
125            simplices_supplied: simplices.len(),
126            triangles_valid: valid,
127            degenerate_skipped: degenerate,
128        })
129    }
130
131    /// Lowest containing simplex id and interpolated value (baseline tie rule).
132    #[inline]
133    pub fn locate(&self, p: [f64; 2]) -> Option<(u32, f64)> {
134        let mut best: Option<(u32, f64)> = None;
135        for t in self
136            .tree
137            .locate_in_envelope_intersecting(AABB::from_point(p))
138        {
139            if let Some(z) = t.at(p)
140                && best.is_none_or(|(id, _)| t.id < id)
141            {
142                best = Some((t.id, z));
143            }
144        }
145        best
146    }
147
148    /// Self-accounted bytes of the prepared structure (triangles plus R-tree node overhead).
149    pub fn approx_bytes(&self) -> u64 {
150        (self.triangles_valid as u64) * prepared_bytes_per_simplex()
151    }
152}
153
154/// In-memory size of one prepared triangle (the `Vec` element handed to `RTree::bulk_load`).
155pub fn triangle_bytes() -> u64 {
156    std::mem::size_of::<Triangle>() as u64
157}
158
159/// Self-accounted resident bytes per valid simplex (triangle plus estimated R-tree overhead).
160pub fn prepared_bytes_per_simplex() -> u64 {
161    triangle_bytes() + 48
162}
163
164#[cfg(test)]
165mod tests {
166    use super::*;
167
168    #[test]
169    fn plane_and_lowest_id_tie() {
170        let s = [[0., 0., 0.], [1., 0., 1.], [0., 1., 1.], [1., 1., 2.]];
171        // Two triangles sharing the diagonal (1,0)-(0,1); the plane z = x + y.
172        let p = PreparedInterpolator::new(&s, &[[1, 2, 3], [0, 1, 2]]).unwrap();
173        let (id, z) = p.locate([0.5, 0.5]).unwrap();
174        assert_eq!(id, 0);
175        assert!((z - 1.0).abs() < 1e-15);
176        assert!(p.locate([2.0, 2.0]).is_none());
177    }
178
179    #[test]
180    fn degenerate_skipped_and_counted() {
181        let s = [[0., 0., 0.], [1., 0., 1.], [2., 0., 2.], [0., 1., 1.]];
182        let p = PreparedInterpolator::new(&s, &[[0, 1, 2], [0, 1, 3], [0, 0, 3]]).unwrap();
183        assert_eq!(p.degenerate_skipped, 2);
184        assert_eq!(p.triangles_valid, 1);
185        // Remaining valid simplex keeps its original id.
186        assert_eq!(p.locate([0.25, 0.25]).unwrap().0, 1);
187    }
188
189    #[test]
190    fn near_degenerate_sliver_skipped() {
191        let s = [[0., 0., 0.], [1., 0., 1.], [0.5, 1e-14, 1.]];
192        let p = PreparedInterpolator::new(&s, &[[0, 1, 2]]).unwrap();
193        assert_eq!(p.degenerate_skipped, 1);
194    }
195
196    #[test]
197    fn invalid_inputs_fail() {
198        let s = [[0., 0., 0.], [1., 0., 1.], [0., 1., f64::NAN]];
199        assert!(PreparedInterpolator::new(&s, &[[0, 1, 2]]).is_err());
200        let s = [[0., 0., 0.], [1., 0., 1.], [0., 1., 1.]];
201        assert!(PreparedInterpolator::new(&s, &[[0, 1, 3]]).is_err());
202        let s = [[-1e308, 0., 0.], [1e308, 0., 1.], [0., 1e308, 1.]];
203        assert!(PreparedInterpolator::new(&s, &[[0, 1, 2]]).is_err());
204    }
205}
ras_commander_fast_map/interp.rs

ras_commander_fast_map/
interp.rs
