solvastro 0.1.1

A Rust blind plate solver
Documentation
use std::f64::consts::FRAC_1_SQRT_2;

use generator::{done, Generator, Gn};
use itertools::Itertools;

use crate::structs_f64::quad::Quad;
use crate::structs_f64::query::{Query, QueryPoint};

const RESET_USE_COUNT_AT_SHRINK: bool = true;
const INITIAL_ITEM_SAMPLE_SIZE: usize = 20;
const MAX_SAMPLE_SIZE: usize = 60;
const MAX_AB_USE: usize = 10;
const MAX_TOTAL_REUSE: usize = 40;
const MAX_AB_SHRINK_FACTOR: f64 = FRAC_1_SQRT_2;
const MAX_AB2_MIN_AB2_RATIO: f64 = FRAC_1_SQRT_2;
const INITIAL_AB_IMAGE_RATIO: f64 = 1f64;
const TERMINAL_AB_IMAGE_RATIO: f64 = 0.1f64;

// const MIN_AB2_INITIAL: f64 = 4000000f64;
// const MAX_AB2_INITIAL: f64 = 5000000f64;

pub struct QuadProducer<'a> {
    query: &'a Query,
}

impl<'a> QuadProducer<'a> {
    pub fn new(query: &Query) -> QuadProducer<'_> {
        QuadProducer { query }
    }

    pub fn generate_query_quads(&self) -> Generator<'_, (), Quad> {
        // we need to do almost exactly the same here as StarIndex::generate_quad_list.
        // the difference is, we are already in a 2d projected space, rather than
        // the unit circle coords that generate_quad_list works with.
        let points: Vec<QueryPoint> = self.query.points.clone();
        // let mut quad_count: usize = 1;

        let small_dimension = self.query.img_w.min(self.query.img_h);
        let large_dimension = self.query.img_w.max(self.query.img_h);

        Gn::new_scoped(move |mut s| {
            let mut max_ab: f64 = large_dimension as f64 * INITIAL_AB_IMAGE_RATIO;
            let mut min_ab: f64 = max_ab * MAX_AB2_MIN_AB2_RATIO;

            let mut points_times_used = vec![0; points.len()];

            let sorted_vmag = &self.query.point_indices_by_magnitude();
            //println!("query point indices by mag descending: {:?}", &sorted_vmag);

            // gradually expand the sample of the quads after the first batch of items
            let mut sample_size = INITIAL_ITEM_SAMPLE_SIZE;
            let mut first_sample = true;
            while (sample_size < sorted_vmag.len() || first_sample)
                && sample_size <= MAX_SAMPLE_SIZE
            {
                //println!("sample size: {:?}", sample_size);

                let sample_range = 0..sample_size.min(sorted_vmag.len());
                // println!(
                //     "sample size: {:?}, sorted_vmag len: {:?}, sample_range: {:?}",
                //     sample_size,
                //     sorted_vmag.len(),
                //     &sample_range,
                // );
                let sorted_vmag_sample = &sorted_vmag[sample_range];

                while min_ab > (small_dimension as f64 * TERMINAL_AB_IMAGE_RATIO) {
                    // println!("max_ab = {:}, min_ab = {:}", max_ab, min_ab);
                    let max_ab2 = max_ab * max_ab;
                    let min_ab2 = min_ab * min_ab;

                    'a_loop: for (a_by_mag_pos, &a_pos) in sorted_vmag_sample.iter().enumerate() {
                        let a = &points[a_pos];
                        if points_times_used[a_pos] >= MAX_TOTAL_REUSE {
                            //println!("skip A (A overuse)");
                            continue;
                        }

                        'b_loop: for (_b_by_mag_pos, &b_pos) in
                            sorted_vmag_sample.iter().enumerate().skip(a_by_mag_pos + 1)
                        {
                            let b = &points[b_pos];
                            if points_times_used[b_pos] >= MAX_TOTAL_REUSE {
                                //println!("skip B (B overuse)");
                                continue;
                            }

                            if !check_scale(a, b, min_ab2, max_ab2) {
                                continue;
                            }

                            let mut ab_used: usize = 0;

                            let interior_point_indices =
                                get_interior_point_indices(&points, a_pos, a, b_pos, b);

                            'c_loop: for (c_pos, &c_idx) in
                                interior_point_indices.iter().enumerate()
                            {
                                for &d_idx in interior_point_indices.iter().skip(c_pos + 1) {
                                    if ab_used >= MAX_AB_USE {
                                        //println!("skip B (AB overuse)");
                                        continue 'b_loop;
                                    }

                                    let c_times_used: usize = points_times_used[c_idx];
                                    if points_times_used[c_idx] >= MAX_TOTAL_REUSE {
                                        //println!("skip C (C overuse)");
                                        continue 'c_loop;
                                    }

                                    let d_times_used: usize = points_times_used[d_idx];
                                    if d_times_used >= MAX_TOTAL_REUSE {
                                        //println!("skip D (D overuse)");
                                        continue;
                                    }

                                    let a_times_used: usize = points_times_used[a_pos];
                                    if a_times_used == MAX_TOTAL_REUSE {
                                        //println!("skip A (A overuse)");
                                        continue 'a_loop;
                                    }

                                    let b_times_used: usize = points_times_used[b_pos];
                                    if b_times_used == MAX_TOTAL_REUSE {
                                        //println!("skip B (B overuse)");
                                        continue 'b_loop;
                                    }

                                    ab_used += 1;
                                    points_times_used[a_pos] = a_times_used + 1;
                                    points_times_used[b_pos] = b_times_used + 1;
                                    points_times_used[c_idx] = c_times_used + 1;
                                    points_times_used[d_idx] = d_times_used + 1;

                                    let quad = Quad {
                                        a: a_pos,
                                        b: b_pos,
                                        c: c_idx,
                                        d: d_idx,
                                    };

                                    // let ab_dist = (((a.x - b.x) * (a.x - b.x))
                                    //     + ((a.y - b.y) * (a.y - b.y)))
                                    //     .sqrt();

                                    //println!("#{:} {:?} (ab={:}, ab use = {:}, total use  = ({:}, {:}, {:}, {:}))", quad_count, &quad, ab_dist, ab_used, a_times_used, b_times_used, c_times_used, d_times_used);
                                    // info!("query quad #{}: {:?}", quad_count, &quad);
                                    // if quad.a == 24 && quad.b == 39 && quad.c == 8 && quad.d == 16 {
                                    //     info!("generated the right quad");
                                    // }

                                    s.yield_(quad);
                                    // quad_count += 1;
                                }
                            }
                        }
                    }

                    max_ab *= MAX_AB_SHRINK_FACTOR;
                    min_ab = max_ab * MAX_AB2_MIN_AB2_RATIO;

                    if RESET_USE_COUNT_AT_SHRINK {
                        points_times_used = vec![0; points.len()];
                    }
                }

                // increase the sample size, mark points as unused, and reset AB size to original
                first_sample = false;
                sample_size = (sample_size + INITIAL_ITEM_SAMPLE_SIZE).min(sorted_vmag.len());
                points_times_used = vec![0; points.len()];
                max_ab = large_dimension as f64 * INITIAL_AB_IMAGE_RATIO;
                min_ab = max_ab * MAX_AB2_MIN_AB2_RATIO;
            }

            done!();
        })
    }
}

fn get_interior_point_indices(
    points: &[QueryPoint],
    a_pos: usize,
    a: &QueryPoint,
    b_pos: usize,
    b: &QueryPoint,
) -> Vec<usize> {
    points
        .iter()
        .enumerate()
        .filter(|&(d_pos, d)| a_pos != d_pos && b_pos != d_pos && can_contain(a, b, d))
        .map(|(c_pos, _c)| c_pos)
        .sorted_by(|&aa, &bb| {
            points[bb]
                .amplitude
                .partial_cmp(&points[aa].amplitude)
                .unwrap()
        })
        .collect()
}

fn check_scale(a: &QueryPoint, b: &QueryPoint, min_ab_sq: f64, max_ab_sq: f64) -> bool {
    let dist: f64 = ((a.x - b.x) * (a.x - b.x)) + ((a.y - b.y) * (a.y - b.y));
    dist >= min_ab_sq && dist <= max_ab_sq
}

fn can_contain(a: &QueryPoint, b: &QueryPoint, d: &QueryPoint) -> bool {
    let ab = (b.x - a.x, b.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 ad_x: f64 = d.x - a.x;
    let ad_y: f64 = d.y - a.y;

    let x: f64 = ad_x * cos_theta + ad_y * sin_theta;
    let y: f64 = -ad_x * sin_theta + ad_y * cos_theta;

    // make sure it's in the circle centered at (0.5, 0.5)...
    // (x-1/2)^2 + (y-1/2)^2   <=   r^2
    // x^2-x+1/4 + y^2-y+1/4   <=   (1/sqrt(2))^2
    // x^2-x + y^2-y + 1/2     <=   1/2
    // x^2-x + y^2-y           <=   0
    let r: f64 = (x * x - x) + (y * y - y);

    r.is_sign_negative()
}