use crate::dynamic_index::star_index_container::ReadableStarIndexContainer;
use crate::structs_f64::quad::{Quad, QuadCode};
use crate::structs_f64::query::{Query, QueryPoint};
const CODE_TOL: f64 = 0.02;
#[derive(Debug)]
pub struct QuadMatch {
pub query_quad: Quad,
pub starindex_quad_idx: usize,
pub code_dist: f64,
}
pub fn matcher<'a>(
candidates: &'a mut dyn Iterator<Item = Quad>,
query: &'a Query,
index: &'a ReadableStarIndexContainer,
) -> impl Iterator<Item = QuadMatch> + 'a {
let points: &Vec<QueryPoint> = &query.points;
candidates
.map(move |quad| (quad, calc_code(&quad, points)))
.flat_map(move |(quad, search_code)| {
index
.quad_codes_within_radius(&search_code, CODE_TOL)
.iter()
.map(move |nn| QuadMatch {
query_quad: quad,
starindex_quad_idx: nn.item as usize,
code_dist: nn.distance.into(),
})
.collect::<Vec<_>>()
})
}
pub fn calc_code(quad: &Quad, points: &[QueryPoint]) -> QuadCode {
let a: &QueryPoint = &points[quad.a];
let b: &QueryPoint = &points[quad.b];
let c: &QueryPoint = &points[quad.c];
let d: &QueryPoint = &points[quad.d];
let ab = (b.x - a.x, b.y - a.y);
let ac = (c.x - a.x, c.y - a.y);
let ad = (d.x - a.x, d.y - a.y);
let inv_scale: f64 = 1f64 / (ab.0 * ab.0 + ab.1 * ab.1);
let cos_theta: f64 = (ab.1 + ab.0) * inv_scale;
let sin_theta: f64 = (ab.1 - ab.0) * inv_scale;
let mut code = [
ac.0 * cos_theta + ac.1 * sin_theta,
-ac.0 * sin_theta + ac.1 * cos_theta,
ad.0 * cos_theta + ad.1 * sin_theta,
-ad.0 * sin_theta + ad.1 * cos_theta,
];
let mut rodeos = 0;
while rodeos <= 2 && (code[2] > code[0] || code[0] + code[2] > 1f64) {
rodeos += 1;
if code[2] > code[0] {
code = [code[2], code[3], code[0], code[1]];
}
if code[0] + code[2] > 1f64 {
code = [
1f64 - code[0],
1f64 - code[1],
1f64 - code[2],
1f64 - code[3],
];
}
}
assert!(code[2] <= code[0] && code[0] + code[2] <= 1f64);
code
}