use super::matcher::QuadMatch;
use super::solution::Solution;
use crate::dynamic_index::star_index_container::{ReadableStarIndexContainer, QIDX, SIDX};
use crate::kabsch::{calculate_wcs_from_quad_match, mtx_from_unit_sphere_quad};
use crate::optimize_wcs_skew::optimize_wcs_skew;
use crate::settings::Settings;
use crate::structs_f64::quad::Quad;
use crate::structs_f64::query::{Query, QueryPoint};
use nalgebra::{Matrix4x2, Matrix4x3};
pub fn solver<'a>(
close_matches: &'a mut (dyn Iterator<Item = QuadMatch> + 'a),
query: &'a Query,
index: &'a ReadableStarIndexContainer,
config: &'a Settings,
) -> impl Iterator<Item = Solution> + 'a {
close_matches.map(move |quad_match| solve_item(quad_match, query, index, config))
}
pub fn solve_item(
quad_match: QuadMatch,
query: &Query,
index: &ReadableStarIndexContainer,
config: &Settings,
) -> Solution {
let starindex_quad: &Quad = &index.get_quad(quad_match.starindex_quad_idx as QIDX);
let query_mtx = build_quad_mtx(&quad_match.query_quad, &query.points);
let star_mtx = build_stars_mtx(starindex_quad, &index);
let wcs = calculate_wcs_from_quad_match(&star_mtx, &query_mtx, &config.solver).unwrap();
let skew_optimized_wcs = optimize_wcs_skew(&wcs, &star_mtx, &query_mtx);
Solution::new(
quad_match.query_quad,
quad_match.starindex_quad_idx,
quad_match.code_dist,
skew_optimized_wcs,
index,
)
}
fn build_quad_mtx(query_quad: &Quad, points: &[QueryPoint]) -> Matrix4x2<f64> {
let query_point_a = &points[query_quad.a];
let query_point_b = &points[query_quad.b];
let query_point_c = &points[query_quad.c];
let query_point_d = &points[query_quad.d];
Matrix4x2::new(
query_point_a.x,
query_point_a.y,
query_point_b.x,
query_point_b.y,
query_point_c.x,
query_point_c.y,
query_point_d.x,
query_point_d.y,
)
}
fn build_stars_mtx(starindex_quad: &Quad, index: &ReadableStarIndexContainer) -> Matrix4x3<f64> {
let star_a = index.get_star(starindex_quad.a as SIDX);
let star_b = index.get_star(starindex_quad.b as SIDX);
let star_c = index.get_star(starindex_quad.c as SIDX);
let star_d = index.get_star(starindex_quad.d as SIDX);
mtx_from_unit_sphere_quad([
star_a.spherical(),
star_b.spherical(),
star_c.spherical(),
star_d.spherical(),
])
}