use super::LensDistortionError;
pub(super) const RADIAL_SEARCH_LIMIT: f32 = 8.0;
#[derive(Debug, Clone, Copy)]
pub(super) struct RadialBranch {
pub(super) k1: f32,
pub(super) k2: f32,
pub(super) k3: f32,
pub(super) r_u_max: f32,
pub(super) r_d_max: f32,
}
impl RadialBranch {
pub(super) fn new(k1: f32, k2: f32, k3: f32) -> Self {
let r_u_max = radial_stationary_point(k1, k2, k3).unwrap_or(RADIAL_SEARCH_LIMIT);
let mut branch = Self {
k1,
k2,
k3,
r_u_max,
r_d_max: 0.0,
};
branch.r_d_max = branch.forward(r_u_max);
branch
}
#[inline]
pub(super) fn forward(&self, r: f32) -> f32 {
let r2 = r * r;
r * (1.0 + self.k1 * r2 + self.k2 * r2 * r2 + self.k3 * r2 * r2 * r2)
}
#[inline]
pub(super) fn forward_derivative(&self, r: f32) -> f32 {
let r2 = r * r;
1.0 + 3.0 * self.k1 * r2 + 5.0 * self.k2 * r2 * r2 + 7.0 * self.k3 * r2 * r2 * r2
}
pub(super) fn invert(&self, r_d: f32) -> Result<f32, LensDistortionError> {
if !r_d.is_finite() {
return Err(LensDistortionError::OutsideDistortionDomain {
r_d,
r_d_max: self.r_d_max,
});
}
if r_d <= 0.0 {
return Ok(0.0);
}
if r_d > self.r_d_max {
return Err(LensDistortionError::OutsideDistortionDomain {
r_d,
r_d_max: self.r_d_max,
});
}
let mut lo = 0.0_f32;
let mut hi = self.r_u_max;
let mut r = r_d.clamp(lo, hi);
for _ in 0..RADIAL_INVERT_MAX_STEPS {
let f = self.forward(r) - r_d;
if f.abs() <= RADIAL_INVERT_TOL {
return Ok(r);
}
if f > 0.0 {
hi = r;
} else {
lo = r;
}
if hi - lo <= RADIAL_INVERT_TOL {
return Ok(0.5 * (lo + hi));
}
let d = self.forward_derivative(r);
let newton = if d.abs() > 1e-12 { r - f / d } else { f32::NAN };
r = if newton.is_finite() && newton > lo && newton < hi {
newton
} else {
0.5 * (lo + hi)
};
}
Ok(r)
}
}
const RADIAL_INVERT_MAX_STEPS: usize = 64;
const RADIAL_INVERT_TOL: f32 = 1e-7;
fn radial_stationary_point(k1: f32, k2: f32, k3: f32) -> Option<f32> {
let a = 7.0 * k3;
let b = 5.0 * k2;
let c = 3.0 * k1;
let s_limit = RADIAL_SEARCH_LIMIT * RADIAL_SEARCH_LIMIT;
let accept = |s: f32| -> Option<f32> {
if s > 0.0 && s <= s_limit {
Some(s.sqrt())
} else {
None
}
};
if a == 0.0 && b == 0.0 {
if c < 0.0 {
return accept(-1.0 / c);
}
return None;
}
if a == 0.0 {
let disc = c * c - 4.0 * b;
if disc < 0.0 {
return None;
}
let sq = disc.sqrt();
let s1 = (-c - sq) / (2.0 * b);
let s2 = (-c + sq) / (2.0 * b);
let mut best = f32::INFINITY;
for s in [s1, s2] {
if s > 0.0 && s < best {
best = s;
}
}
return if best.is_finite() { accept(best) } else { None };
}
let h = |s: f32| 1.0 + c * s + b * s * s + a * s * s * s;
const SCAN_STEPS: usize = 512;
let mut prev_s = 0.0_f32;
for i in 1..=SCAN_STEPS {
let s = s_limit * (i as f32) / (SCAN_STEPS as f32);
if h(s) <= 0.0 {
let mut lo = prev_s;
let mut hi = s;
for _ in 0..40 {
let mid = 0.5 * (lo + hi);
if h(mid) > 0.0 {
lo = mid;
} else {
hi = mid;
}
}
return accept(0.5 * (lo + hi));
}
prev_s = s;
}
None
}
#[cfg(test)]
mod tests {
use super::*;
#[test]
fn test_radial_branch_matches_analytic_fold() {
let b = RadialBranch::new(-0.1, 0.0, 0.0);
assert!(
(b.r_u_max - 1.825_741_9).abs() < 1e-4,
"r_u_max: {}",
b.r_u_max
);
assert!(
(b.r_d_max - 1.217_161_2).abs() < 1e-4,
"r_d_max: {}",
b.r_d_max
);
let mono = RadialBranch::new(-0.2, 0.05, 0.0);
assert!(
(mono.r_u_max - RADIAL_SEARCH_LIMIT).abs() < 1e-6,
"expected a monotonic branch, got r_u_max = {}",
mono.r_u_max
);
let pincushion = RadialBranch::new(0.2, 0.0, 0.0);
assert!((pincushion.r_u_max - RADIAL_SEARCH_LIMIT).abs() < 1e-6);
let near_fold = b.forward(b.r_u_max * 0.99);
let inverted = b.invert(near_fold).expect("inside the domain");
assert!(
(b.forward(inverted) - near_fold).abs() < 1e-5,
"forward(invert(r)) should reproduce r: {} vs {near_fold}",
b.forward(inverted)
);
let cubic = RadialBranch::new(-0.3, 0.0, 0.001);
assert!(
cubic.r_u_max > 1.0 && cubic.r_u_max < RADIAL_SEARCH_LIMIT,
"expected a fold from the cubic branch, got {}",
cubic.r_u_max
);
assert!(cubic.forward_derivative(cubic.r_u_max).abs() < 1e-3);
}
}