use crate::coords::{tan_project, CoordCartesian, CoordCelestial, CoordUnitSphere};
use crate::settings::{QuadCentreMode, SolverConfig};
use crate::wcs::WcsTan;
use nalgebra::{
Matrix2, Matrix3, Matrix3x4, Matrix4x2, Matrix4x3, RowVector2, SMatrix, Unit, Vector2, Vector3,
SVD, U2,
};
use std::ops::SubAssign;
pub fn calculate_wcs_from_quad_match(
orig: &Matrix4x3<f64>,
img: &Matrix4x2<f64>,
config: &SolverConfig,
) -> Option<WcsTan> {
let crval = calc_crval_xyz(&orig, &config.quad_centre_mode);
project_world_to_proj_plane(&orig, &crval).map(|orig_proj| {
let r = kabsch(&orig_proj, &img);
let crpix = calc_crpix(&img, &config.quad_centre_mode);
WcsTan::new(crpix, CoordCelestial::from(&crval), r.transpose())
})
}
pub fn calc_crpix(img: &Matrix4x2<f64>, quad_centre_mode: &QuadCentreMode) -> Vector2<f64> {
match quad_centre_mode {
QuadCentreMode::CentreAb => img.fixed_rows::<2>(0).row_mean().transpose(),
QuadCentreMode::CentreAbcd => img.row_mean().transpose(),
QuadCentreMode::CentreA => Vector2::new(img.m11, img.m12),
}
}
pub fn calc_crval_xyz(orig: &Matrix4x3<f64>, quad_centre_mode: &QuadCentreMode) -> Vector3<f64> {
match quad_centre_mode {
QuadCentreMode::CentreAb => orig.fixed_rows::<2>(0).row_mean().transpose(),
QuadCentreMode::CentreAbcd => orig.row_mean().transpose(),
QuadCentreMode::CentreA => Vector3::new(orig.m11, orig.m12, orig.m13).normalize(),
}
}
pub fn project_world_to_proj_plane(
orig: &Matrix4x3<f64>,
crval: &Vector3<f64>,
) -> Option<Matrix4x2<f64>> {
let w2i: Matrix3<f64> = WcsTan::create_world_to_iwc_matrix(CoordCelestial::from(crval));
let iwc: Matrix3x4<f64> = w2i * orig.transpose();
let proj_a = WcsTan::iwc_2_proj_plane(&Vector3::new(
iwc.column(0).x,
iwc.column(0).y,
iwc.column(0).z,
));
let proj_b = WcsTan::iwc_2_proj_plane(&Vector3::new(
iwc.column(1).x,
iwc.column(1).y,
iwc.column(1).z,
));
let proj_c = WcsTan::iwc_2_proj_plane(&Vector3::new(
iwc.column(2).x,
iwc.column(2).y,
iwc.column(2).z,
));
let proj_d = WcsTan::iwc_2_proj_plane(&Vector3::new(
iwc.column(3).x,
iwc.column(3).y,
iwc.column(3).z,
));
if proj_a.is_none() || proj_b.is_none() || proj_c.is_none() || proj_d.is_none() {
return None;
}
let proj_a = proj_a.unwrap();
let proj_b = proj_b.unwrap();
let proj_c = proj_c.unwrap();
let proj_d = proj_d.unwrap();
let new_projected = Matrix4x2::new(
proj_a.x, proj_a.y, proj_b.x, proj_b.y, proj_c.x, proj_c.y, proj_d.x, proj_d.y,
);
Some(new_projected)
}
pub fn kabsch(proj: &Matrix4x2<f64>, img: &Matrix4x2<f64>) -> Matrix2<f64> {
let img_0 = mtx_subtract_centroid(&img);
let proj_0 = mtx_subtract_centroid(&proj);
let xcov = mtx_cross_covariance_4x2(img_0, proj_0);
let svd: SVD<f64, U2, U2> = xcov.svd(true, true);
let r: Matrix2<f64> = svd.v_t.unwrap().transpose() * svd.u.unwrap().transpose();
let scale: f64 = (proj_0.variance() / img_0.variance()).sqrt();
scale * r
}
pub fn mtx_from_cartesian_quad(quad: [CoordCartesian; 4]) -> Matrix4x2<f64> {
let res: Matrix4x2<f64> = Matrix4x2::new(
quad[0].x, quad[0].y, quad[1].x, quad[1].y, quad[2].x, quad[2].y, quad[3].x, quad[3].y,
);
res
}
pub fn mtx_from_unit_sphere_quad(quad: [CoordUnitSphere; 4]) -> Matrix4x3<f64> {
Matrix4x3::new(
quad[0].x, quad[0].y, quad[0].z, quad[1].x, quad[1].y, quad[1].z, quad[2].x, quad[2].y,
quad[2].z, quad[3].x, quad[3].y, quad[3].z,
)
}
pub fn mtx_subtract_centroid<const M: usize, const N: usize>(
mtx: &SMatrix<f64, M, N>,
) -> SMatrix<f64, M, N> {
let centroid = mtx.row_mean();
let mut result = mtx.clone_owned();
for mut row in result.row_iter_mut() {
row.sub_assign(¢roid);
}
result
}
pub fn mtx_cross_covariance_4x2(x: Matrix4x2<f64>, y: Matrix4x2<f64>) -> Matrix2<f64> {
y.tr_mul(&x)
}
pub fn mtx_tan_project_2(s: &Matrix4x3<f64>, r: &Vector3<f64>) -> Option<Matrix4x2<f64>> {
#[allow(invalid_value)]
let mut projected: [RowVector2<f64>; 4] =
unsafe { std::mem::MaybeUninit::uninit().assume_init() };
for (idx, row) in s.row_iter().enumerate() {
let projected_row = tan_project(&row.transpose(), &r);
projected_row?;
projected[idx] = projected_row.unwrap().transpose();
}
Some(Matrix4x2::from_rows(&projected))
}
#[deprecated]
pub fn mtx_tan_project_3(s: &Matrix4x3<f64>, r: &Unit<Vector3<f64>>) -> Option<Matrix4x2<f64>> {
let r_tup = CoordUnitSphere::new(r.x, r.y, r.z);
let r1 = tan_project(&s.row(0).transpose(), &r_tup);
let r2 = tan_project(&s.row(1).transpose(), &r_tup);
let r3 = tan_project(&s.row(2).transpose(), &r_tup);
let r4 = tan_project(&s.row(3).transpose(), &r_tup);
if let Some(r1) = r1 {
if let Some(r2) = r2 {
if let Some(r3) = r3 {
if let Some(r4) = r4 {
return Some(Matrix4x2::from_rows(&[
r1.transpose(),
r2.transpose(),
r3.transpose(),
r4.transpose(),
]));
}
}
}
}
None
}
#[deprecated]
pub fn mtx_tan_project(s: &Matrix4x3<f64>, r: &Unit<Vector3<f64>>) -> Option<Matrix4x2<f64>> {
let r_tup = CoordUnitSphere::new(r.x, r.y, r.z);
let r1 = tan_project(&CoordUnitSphere::new(s.m11, s.m12, s.m13), &r_tup);
let r2 = tan_project(&CoordUnitSphere::new(s.m21, s.m22, s.m23), &r_tup);
let r3 = tan_project(&CoordUnitSphere::new(s.m31, s.m32, s.m33), &r_tup);
let r4 = tan_project(&CoordUnitSphere::new(s.m41, s.m42, s.m43), &r_tup);
if let Some(r1) = r1 {
if let Some(r2) = r2 {
if let Some(r3) = r3 {
if let Some(r4) = r4 {
return Some(Matrix4x2::new(
r1.x, r1.y, r2.x, r2.y, r3.x, r3.y, r4.x, r4.y,
));
}
}
}
}
None
}