use crate::calib::{
CalibDistortion, CalibTca, CalibVignetting, DistortionModel, TcaModel, VignettingModel,
};
use crate::lens::Lens;
use crate::mod_coord::{
dist_poly3, dist_poly5, dist_ptlens, undist_poly3, undist_poly5, undist_ptlens,
};
use crate::mod_pc::{
self, Direction as PerspectiveDirection, PerspectiveState, build_perspective_state,
};
use crate::mod_subpix::{tca_linear, tca_poly3_forward, tca_poly3_reverse};
#[derive(Debug, Clone, Copy)]
struct DistortionPass {
model: DistortionModel,
reverse: bool,
}
#[derive(Debug, Clone, Copy)]
struct TcaPass {
model: TcaModel,
reverse: bool,
}
#[derive(Debug, Clone, Copy)]
struct VignettingPass {
model: VignettingModel,
reverse: bool,
}
#[derive(Debug, Clone)]
pub struct Modifier {
focal: f32,
reverse: bool,
real_focal: f64,
norm_scale: f64,
norm_unscale: f64,
center_x: f64,
center_y: f64,
distortion: Option<DistortionPass>,
tca: Option<TcaPass>,
vignetting: Option<VignettingPass>,
perspective: Option<PerspectiveState>,
}
impl Modifier {
pub fn new(
lens: &Lens,
focal: f32,
crop: f32,
image_width: u32,
image_height: u32,
reverse: bool,
) -> Self {
let width = if image_width >= 2 {
(image_width - 1) as f64
} else {
1.0
};
let height = if image_height >= 2 {
(image_height - 1) as f64
} else {
1.0
};
let real_focal = match lens.interpolate_distortion(focal) {
Some(lcd) => lcd.real_focal.map(|f| f as f64).unwrap_or(focal as f64),
None => focal as f64,
};
let norm_scale =
(36.0_f64.hypot(24.0)) / crop as f64 / ((width + 1.0).hypot(height + 1.0)) / real_focal;
let norm_unscale = 1.0 / norm_scale;
let size = width.min(height);
let center_x = (width / 2.0 + size / 2.0 * lens.center_x as f64) * norm_scale;
let center_y = (height / 2.0 + size / 2.0 * lens.center_y as f64) * norm_scale;
let _ = (width, height, crop);
Self {
focal,
reverse,
real_focal,
norm_scale,
norm_unscale,
center_x,
center_y,
distortion: None,
tca: None,
vignetting: None,
perspective: None,
}
}
pub fn enable_distortion_correction(&mut self, lens: &Lens) -> bool {
let Some(lcd) = lens.interpolate_distortion(self.focal) else {
return false;
};
let model = rescale_distortion(&lcd, lens.aspect_ratio, lens.crop_factor, self.real_focal);
if matches!(model, DistortionModel::None) {
return false;
}
if let DistortionModel::Poly3 { k1 } = model {
if k1 == 0.0 {
return false;
}
}
self.distortion = Some(DistortionPass {
model,
reverse: self.reverse,
});
true
}
pub fn enable_tca_correction(&mut self, lens: &Lens) -> bool {
let Some(lctca) = lens.interpolate_tca(self.focal) else {
return false;
};
let model = rescale_tca(
&lctca,
lens.aspect_ratio,
lens.crop_factor,
self.real_focal,
self.reverse,
);
if matches!(model, TcaModel::None) {
return false;
}
self.tca = Some(TcaPass {
model,
reverse: self.reverse,
});
true
}
pub fn enable_vignetting_correction(
&mut self,
lens: &Lens,
aperture: f32,
distance: f32,
) -> bool {
let Some(lcv) = lens.interpolate_vignetting(self.focal, aperture, distance) else {
return false;
};
let model = rescale_vignetting(&lcv, lens.crop_factor, self.real_focal);
if matches!(model, VignettingModel::None) {
return false;
}
self.vignetting = Some(VignettingPass {
model,
reverse: self.reverse,
});
true
}
pub fn enable_perspective_correction(&mut self, x: &[f32], y: &[f32], d: f32) -> bool {
let count = x.len();
if count != y.len() || !(4..=8).contains(&count) {
return false;
}
let mut xn = Vec::with_capacity(count);
let mut yn = Vec::with_capacity(count);
for i in 0..count {
xn.push(x[i] as f64 * self.norm_scale - self.center_x);
yn.push(y[i] as f64 * self.norm_scale - self.center_y);
}
match build_perspective_state(&xn, &yn, d, self.reverse) {
Some(state) => {
self.perspective = Some(state);
true
}
None => false,
}
}
pub fn apply_geometry_distortion(
&self,
x_start: f32,
y_start: f32,
width: usize,
rows: usize,
coords: &mut [f32],
) -> bool {
if self.distortion.is_none() && self.perspective.is_none() {
return false;
}
if rows == 0 || width == 0 {
return false;
}
debug_assert_eq!(coords.len(), 2 * width * rows);
let xu = x_start as f64 * self.norm_scale - self.center_x;
let yu = y_start as f64 * self.norm_scale - self.center_y;
let ns = self.norm_scale;
for row in 0..rows {
let y = (yu + ns * row as f64) as f32;
let row_off = row * width * 2;
for i in 0..width {
let x = (xu + ns * i as f64) as f32;
coords[row_off + 2 * i] = x;
coords[row_off + 2 * i + 1] = y;
}
let row_slice = &mut coords[row_off..row_off + 2 * width];
if !self.reverse {
if let Some(pass) = self.distortion {
apply_distortion_kernel(&pass, row_slice);
}
if let Some(p) = self.perspective {
debug_assert_eq!(p.direction, PerspectiveDirection::Correction);
mod_pc::apply_correction_kernel(&p, row_slice);
}
} else {
if let Some(p) = self.perspective {
debug_assert_eq!(p.direction, PerspectiveDirection::Distortion);
mod_pc::apply_distortion_kernel(&p, row_slice);
}
if let Some(pass) = self.distortion {
apply_distortion_kernel(&pass, row_slice);
}
}
for i in 0..width {
let cx = coords[row_off + 2 * i] as f64;
let cy = coords[row_off + 2 * i + 1] as f64;
coords[row_off + 2 * i] = ((cx + self.center_x) * self.norm_unscale) as f32;
coords[row_off + 2 * i + 1] = ((cy + self.center_y) * self.norm_unscale) as f32;
}
}
true
}
pub fn apply_subpixel_distortion(
&self,
x_start: f32,
y_start: f32,
width: usize,
rows: usize,
coords: &mut [f32],
) -> bool {
let Some(pass) = self.tca else {
return false;
};
if rows == 0 || width == 0 {
return false;
}
debug_assert_eq!(coords.len(), 6 * width * rows);
let xu = x_start as f64 * self.norm_scale - self.center_x;
let yu = y_start as f64 * self.norm_scale - self.center_y;
let ns = self.norm_scale;
for row in 0..rows {
let y = (yu + ns * row as f64) as f32;
let row_off = row * width * 6;
for i in 0..width {
let x = (xu + ns * i as f64) as f32;
let off = row_off + 6 * i;
coords[off] = x;
coords[off + 1] = y;
coords[off + 2] = x;
coords[off + 3] = y;
coords[off + 4] = x;
coords[off + 5] = y;
}
apply_tca_kernel(&pass, &mut coords[row_off..row_off + 6 * width]);
for i in 0..3 * width {
let off = row_off + 2 * i;
let cx = coords[off] as f64;
let cy = coords[off + 1] as f64;
coords[off] = ((cx + self.center_x) * self.norm_unscale) as f32;
coords[off + 1] = ((cy + self.center_y) * self.norm_unscale) as f32;
}
}
true
}
pub fn apply_color_modification_f32(
&self,
pixels: &mut [f32],
x_start: f32,
y_start: f32,
width: usize,
rows: usize,
channels: usize,
) -> bool {
self.apply_color_modification(
pixels,
x_start,
y_start,
width,
rows,
channels,
|chunk, c| {
for p in chunk {
*p *= c;
}
},
)
}
pub fn apply_color_modification_u16(
&self,
pixels: &mut [u16],
x_start: f32,
y_start: f32,
width: usize,
rows: usize,
channels: usize,
) -> bool {
self.apply_color_modification(
pixels,
x_start,
y_start,
width,
rows,
channels,
|chunk, c| {
let mut c10 = (c as f64 * 1024.0) as i32;
if c10 > (31 << 10) {
c10 = 31 << 10;
}
for p in chunk {
let r = (i32::from(*p) * c10 + 512) >> 10;
*p = clamp_bits_16(r);
}
},
)
}
pub fn apply_color_modification_u8(
&self,
pixels: &mut [u8],
x_start: f32,
y_start: f32,
width: usize,
rows: usize,
channels: usize,
) -> bool {
self.apply_color_modification(
pixels,
x_start,
y_start,
width,
rows,
channels,
|chunk, c| {
let mut c12 = (c as f64 * 4096.0) as i32;
if c12 > (2047 << 12) {
c12 = 2047 << 12;
}
for p in chunk {
let r = (i32::from(*p) * c12 + 2048) >> 12;
*p = clamp_bits_8(r);
}
},
)
}
fn apply_color_modification<T>(
&self,
pixels: &mut [T],
x_start: f32,
y_start: f32,
width: usize,
rows: usize,
channels: usize,
mut op: impl FnMut(&mut [T], f32),
) -> bool {
let Some(pass) = self.vignetting else {
return false;
};
if rows == 0 || width == 0 || channels == 0 {
return false;
}
debug_assert_eq!(pixels.len(), width * rows * channels);
let VignettingModel::Pa { k1, k2, k3 } = pass.model else {
return false;
};
let ns = self.norm_scale as f32;
let d1 = 2.0_f32 * ns;
let d2 = ns * ns;
let xu = (x_start as f64 * self.norm_scale - self.center_x) as f32;
let yu = (y_start as f64 * self.norm_scale - self.center_y) as f32;
for row in 0..rows {
let y = yu + ns * row as f32;
let mut x = xu;
let mut r2 = x * x + y * y;
let row_off = row * width * channels;
for col in 0..width {
let r4 = r2 * r2;
let r6 = r4 * r2;
let gain = 1.0 + k1 * r2 + k2 * r4 + k3 * r6;
let mult = if pass.reverse { gain } else { 1.0 / gain };
let pix_off = row_off + col * channels;
op(&mut pixels[pix_off..pix_off + channels], mult);
r2 += d1 * x + d2;
x += ns;
}
}
true
}
}
fn rescale_distortion(
lcd: &CalibDistortion,
aspect_ratio: f32,
crop: f32,
real_focal: f64,
) -> DistortionModel {
let hugin_scale_in_mm =
(36.0_f64).hypot(24.0) / crop as f64 / (aspect_ratio as f64).hypot(1.0) / 2.0;
let hugin_scaling = (real_focal / hugin_scale_in_mm) as f32;
let hs = hugin_scaling as f64;
match lcd.model {
DistortionModel::None => DistortionModel::None,
DistortionModel::Poly3 { k1 } => {
let d = 1.0_f64 - k1 as f64;
DistortionModel::Poly3 {
k1: (k1 as f64 * hs.powi(2) / d.powi(3)) as f32,
}
}
DistortionModel::Poly5 { k1, k2 } => DistortionModel::Poly5 {
k1: (k1 as f64 * hs.powi(2)) as f32,
k2: (k2 as f64 * hs.powi(4)) as f32,
},
DistortionModel::Ptlens { a, b, c } => {
let d = 1.0_f64 - a as f64 - b as f64 - c as f64;
DistortionModel::Ptlens {
a: (a as f64 * hs.powi(3) / d.powi(4)) as f32,
b: (b as f64 * hs.powi(2) / d.powi(3)) as f32,
c: (c as f64 * hs / d.powi(2)) as f32,
}
}
}
}
fn rescale_tca(
lctca: &CalibTca,
aspect_ratio: f32,
crop: f32,
real_focal: f64,
reverse: bool,
) -> TcaModel {
let hugin_scale_in_mm =
(36.0_f64).hypot(24.0) / crop as f64 / (aspect_ratio as f64).hypot(1.0) / 2.0;
let hugin_scaling = (real_focal / hugin_scale_in_mm) as f32;
match lctca.model {
TcaModel::None => TcaModel::None,
TcaModel::Linear { kr, kb } => {
if reverse {
TcaModel::Linear {
kr: 1.0 / kr,
kb: 1.0 / kb,
}
} else {
TcaModel::Linear { kr, kb }
}
}
TcaModel::Poly3 { red, blue } => {
let hs = hugin_scaling as f64;
TcaModel::Poly3 {
red: [
red[0],
(red[1] as f64 * hs) as f32,
(red[2] as f64 * hs.powi(2)) as f32,
],
blue: [
blue[0],
(blue[1] as f64 * hs) as f32,
(blue[2] as f64 * hs.powi(2)) as f32,
],
}
}
}
}
fn rescale_vignetting(lcv: &CalibVignetting, crop: f32, real_focal: f64) -> VignettingModel {
let hugin_scale_in_mm = (36.0_f64).hypot(24.0) / crop as f64 / 2.0;
let hugin_scaling = (real_focal / hugin_scale_in_mm) as f32;
let hs = hugin_scaling as f64;
match lcv.model {
VignettingModel::None => VignettingModel::None,
VignettingModel::Pa { k1, k2, k3 } => VignettingModel::Pa {
k1: (k1 as f64 * hs.powi(2)) as f32,
k2: (k2 as f64 * hs.powi(4)) as f32,
k3: (k3 as f64 * hs.powi(6)) as f32,
},
}
}
fn apply_distortion_kernel(pass: &DistortionPass, row: &mut [f32]) {
let n = row.len() / 2;
match (pass.model, pass.reverse) {
(DistortionModel::Poly3 { k1 }, false) => {
for i in 0..n {
let (xo, yo) = dist_poly3(row[2 * i], row[2 * i + 1], k1);
row[2 * i] = xo;
row[2 * i + 1] = yo;
}
}
(DistortionModel::Poly3 { k1 }, true) => {
for i in 0..n {
let (xo, yo) = undist_poly3(row[2 * i], row[2 * i + 1], k1);
row[2 * i] = xo;
row[2 * i + 1] = yo;
}
}
(DistortionModel::Poly5 { k1, k2 }, false) => {
for i in 0..n {
let (xo, yo) = dist_poly5(row[2 * i], row[2 * i + 1], k1, k2);
row[2 * i] = xo;
row[2 * i + 1] = yo;
}
}
(DistortionModel::Poly5 { k1, k2 }, true) => {
for i in 0..n {
let (xo, yo) = undist_poly5(row[2 * i], row[2 * i + 1], k1, k2);
row[2 * i] = xo;
row[2 * i + 1] = yo;
}
}
(DistortionModel::Ptlens { a, b, c }, false) => {
for i in 0..n {
let (xo, yo) = dist_ptlens(row[2 * i], row[2 * i + 1], a, b, c);
row[2 * i] = xo;
row[2 * i + 1] = yo;
}
}
(DistortionModel::Ptlens { a, b, c }, true) => {
for i in 0..n {
let (xo, yo) = undist_ptlens(row[2 * i], row[2 * i + 1], a, b, c);
row[2 * i] = xo;
row[2 * i + 1] = yo;
}
}
(DistortionModel::None, _) => {}
}
}
fn apply_tca_kernel(pass: &TcaPass, row: &mut [f32]) {
let n = row.len() / 6;
match pass.model {
TcaModel::None => {}
TcaModel::Linear { kr, kb } => {
for i in 0..n {
let off = 6 * i;
let x = row[off];
let y = row[off + 1];
let (xr, yr, xb, yb) = tca_linear(x, y, kr, kb);
row[off] = xr;
row[off + 1] = yr;
row[off + 4] = xb;
row[off + 5] = yb;
}
}
TcaModel::Poly3 { red, blue } => {
for i in 0..n {
let off = 6 * i;
let x = row[off];
let y = row[off + 1];
let (xr, yr, xb, yb) = if pass.reverse {
tca_poly3_reverse(x, y, red, blue)
} else {
tca_poly3_forward(x, y, red, blue)
};
row[off] = xr;
row[off + 1] = yr;
row[off + 4] = xb;
row[off + 5] = yb;
}
}
}
}
fn clamp_bits_8(x: i32) -> u8 {
let shifted = (x as u32) >> 8;
if shifted != 0 {
((!shifted) >> 24) as u8
} else {
x as u8
}
}
fn clamp_bits_16(x: i32) -> u16 {
let shifted = (x as u32) >> 16;
if shifted != 0 {
((!shifted) >> 16) as u16
} else {
x as u16
}
}