scan_core/cv/

geometry.rs

use crate::Point;

pub fn perimeter(poly: &[Point]) -> f32 {
    let mut p = 0.0;
    for i in 0..poly.len() {
        let j = (i + 1) % poly.len();
        let dx = poly[i].x - poly[j].x;
        let dy = poly[i].y - poly[j].y;
        p += (dx * dx + dy * dy).sqrt();
    }
    p
}

pub fn polygon_area(poly: &[Point]) -> f32 {
    let mut area = 0.0;
    let len = poly.len();
    for i in 0..len {
        let j = (i + 1) % len;
        area += poly[i].x * poly[j].y;
        area -= poly[j].x * poly[i].y;
    }
    area.abs() / 2.0
}

pub fn min_edge_length(poly: &[Point]) -> f32 {
    let mut min = f32::MAX;
    for i in 0..poly.len() {
        let j = (i + 1) % poly.len();
        let dx = poly[i].x - poly[j].x;
        let dy = poly[i].y - poly[j].y;
        let d = dx * dx + dy * dy;
        if d < min {
            min = d;
        }
    }
    min.sqrt()
}

pub fn is_contour_convex(poly: &[Point]) -> bool {
    let len = poly.len();
    if len < 3 { return false; }

    let mut prev_pt = poly[len - 1];
    let mut cur_pt = poly[0];

    let mut dx0 = cur_pt.x - prev_pt.x;
    let mut dy0 = cur_pt.y - prev_pt.y;

    let mut orientation = 0;

    for i in 0..len {
        let next_idx = (i + 1) % len;
        prev_pt = cur_pt;
        cur_pt = poly[next_idx];

        let dx = cur_pt.x - prev_pt.x;
        let dy = cur_pt.y - prev_pt.y;

        let dxdy0 = dx * dy0;
        let dydx0 = dy * dx0;

        if dydx0 > dxdy0 {
            orientation |= 1;
        } else if dydx0 < dxdy0 {
            orientation |= 2;
        }

        if orientation == 3 {
            return false;
        }

        dx0 = dx;
        dy0 = dy;
    }

    true
}

/// Approximate polygon — exact port of CV.approxPolyDP from js-aruco2.
/// This version handles CLOSED contours correctly using the circular indexing
/// approach from the JS implementation.
pub fn approx_poly_dp(contour: &[Point], epsilon: f32) -> Vec<Point> {
    let len = contour.len();
    if len <= 2 {
        return contour.to_vec();
    }

    let epsilon_sq = epsilon * epsilon;

    // Phase 1: Find the farthest point pair (like JS does 3 iterations)
    let mut right_start = 0usize;
    let mut k = 0usize;
    let mut max_dist_phase1: f32 = 0.0;

    for _ in 0..3 {
        let mut local_max: f32 = 0.0;
        k = (k + right_start) % len;
        let start_pt = contour[k];
        k = (k + 1) % len;

        for _j in 1..len {
            let pt = contour[k];
            k = (k + 1) % len;

            let dx = pt.x - start_pt.x;
            let dy = pt.y - start_pt.y;
            let dist = dx * dx + dy * dy;

            if dist > local_max {
                local_max = dist;
                right_start = _j;
            }
        }
        max_dist_phase1 = local_max;
    }

    if max_dist_phase1 <= epsilon_sq {
        let start_pt = contour[k % len];
        return vec![Point { x: start_pt.x, y: start_pt.y }];
    }

    // Phase 2: Recursive subdivision using stack
    let slice_start = k;
    let slice_end = right_start + slice_start;

    let right_slice_start = if right_start + slice_start >= len {
        right_start + slice_start - len
    } else {
        right_start + slice_start
    };
    let right_slice_end = if slice_start < right_slice_start {
        slice_start + len
    } else {
        slice_start
    };

    let mut poly = Vec::new();
    let mut stack: Vec<(usize, usize)> = Vec::new();

    stack.push((right_slice_start, right_slice_end));
    stack.push((slice_start, slice_end));

    while let Some((s_start, s_end)) = stack.pop() {
        let end_pt = contour[s_end % len];
        let mut kk = s_start % len;
        let start_pt = contour[kk];
        kk = (kk + 1) % len;

        if s_end <= s_start + 1 {
            poly.push(Point { x: start_pt.x, y: start_pt.y });
        } else {
            let mut max_dist: f32 = 0.0;
            let mut max_idx = 0;

            let dx = end_pt.x - start_pt.x;
            let dy = end_pt.y - start_pt.y;

            for i in (s_start + 1)..s_end {
                let pt = contour[kk];
                kk = (kk + 1) % len;

                let dist = ((pt.y - start_pt.y) * dx - (pt.x - start_pt.x) * dy).abs();

                if dist > max_dist {
                    max_dist = dist;
                    max_idx = i;
                }
            }

            let le_eps = max_dist * max_dist <= epsilon_sq * (dx * dx + dy * dy);

            if le_eps {
                poly.push(Point { x: start_pt.x, y: start_pt.y });
            } else {
                // Split
                stack.push((max_idx, s_end));
                stack.push((s_start, max_idx));
            }
        }
    }

    poly
}

/// Ensure candidate corners are in clockwise order.
/// Port of AR.Detector.prototype.clockwiseCorners from JS.
pub fn clockwise_corners(candidates: &mut Vec<Vec<Point>>) {
    for c in candidates.iter_mut() {
        if c.len() != 4 { continue; }

        let dx1 = c[1].x - c[0].x;
        let dy1 = c[1].y - c[0].y;
        let dx2 = c[2].x - c[0].x;
        let dy2 = c[2].y - c[0].y;

        if (dx1 * dy2 - dy1 * dx2) < 0.0 {
            c.swap(1, 3);
        }
    }
}

/// Filter out candidates that are too close to each other.
/// Port of AR.Detector.prototype.notTooNear from JS.
pub fn not_too_near(candidates: &[Vec<Point>], min_dist: f32) -> Vec<Vec<Point>> {
    let len = candidates.len();
    let mut too_near = vec![false; len];

    for i in 0..len {
        if too_near[i] { continue; }
        for j in (i + 1)..len {
            if too_near[j] { continue; }

            let mut dist: f32 = 0.0;
            for k in 0..4 {
                let dx = candidates[i][k].x - candidates[j][k].x;
                let dy = candidates[i][k].y - candidates[j][k].y;
                dist += dx * dx + dy * dy;
            }

            if (dist / 4.0) < (min_dist * min_dist) {
                if perimeter(&candidates[i]) < perimeter(&candidates[j]) {
                    too_near[i] = true;
                } else {
                    too_near[j] = true;
                }
            }
        }
    }

    candidates.iter().enumerate()
        .filter(|(i, _)| !too_near[*i])
        .map(|(_, c)| c.clone())
        .collect()
}

/// Count non-zero pixels in a rectangular region of a grayscale image.
/// Port of CV.countNonZero from JS.
pub fn count_non_zero(data: &[u8], img_width: usize, x: usize, y: usize, w: usize, h: usize) -> usize {
    let mut nz = 0;
    for row in y..(y + h) {
        for col in x..(x + w) {
            if data[row * img_width + col] != 0 {
                nz += 1;
            }
        }
    }
    nz
}

/// Refine a corner position using image gradients (sub-pixel precision).
/// Port of ArUcoScanner.prototype._refineCorner from original JS engine.
pub fn refine_corner(data: &[u8], width: usize, height: usize, p: Point, window: i32) -> Point {
    let x = p.x.floor() as i32;
    let y = p.y.floor() as i32;

    // 1. Check local contrast to avoid refining in flat areas
    let mut min_i = 255u8;
    let mut max_i = 0u8;

    for dy in -window..=window {
        for dx in -window..=window {
            let px = x + dx;
            let py = y + dy;
            if px >= 0 && px < width as i32 && py >= 0 && py < height as i32 {
                let v = data[py as usize * width + px as usize];
                if v < min_i { min_i = v; }
                if v > max_i { max_i = v; }
            }
        }
    }

    // If contrast is too low, don't refine
    if (max_i as i32 - min_i as i32) < 30 {
        return p;
    }

    // 2. Compute sums for the linear system (gradient based refinement)
    let mut s_ix2 = 0.0;
    let mut s_iy2 = 0.0;
    let mut s_ixy = 0.0;
    let mut s_ix2x = 0.0;
    let mut s_iy2y = 0.0;

    for dy in -window..=window {
        for dx in -window..=window {
            let px = x + dx;
            let py = y + dy;

            // Gradient needs neighbors
            if px > 0 && px < (width as i32 - 1) && py > 0 && py < (height as i32 - 1) {
                let px_idx = py as usize * width + px as usize;
                
                // Central difference
                let ix = (data[px_idx + 1] as f32 - data[px_idx - 1] as f32) / 2.0;
                let iy = (data[px_idx + width] as f32 - data[px_idx - width] as f32) / 2.0;

                s_ix2 += ix * ix;
                s_iy2 += iy * iy;
                s_ixy += ix * iy;
                s_ix2x += ix * ix * px as f32 + ix * iy * py as f32;
                s_iy2y += iy * iy * py as f32 + ix * iy * px as f32;
            }
        }
    }

    let det = s_ix2 * s_iy2 - s_ixy * s_ixy;
    if det.abs() < 0.0001 {
        return p;
    }

    let nx = (s_ix2x * s_iy2 - s_ixy * s_iy2y) / det;
    let ny = (s_ix2 * s_iy2y - s_ix2x * s_ixy) / det;

    // Distance threshold: if it's too far (more than 3 pixels), the result might be wrong
    let dist_sq = (nx - p.x) * (nx - p.x) + (ny - p.y) * (ny - p.y);
    if dist_sq > 9.0 {
        return p;
    }

    Point { x: nx, y: ny }
}

/// Select the corner of a polygon that is most "extreme" in the given directions.
/// Port of ArUcoScanner.prototype._getExtreme from JS.
/// mode_x: true for max, false for min
/// mode_y: true for max, false for min
pub fn get_extreme_corner(corners: &[Point], mode_x_max: bool, mode_y_max: bool) -> Point {
    let mut best_score = f32::NEG_INFINITY;
    let mut best_idx = 0;

    for (i, p) in corners.iter().enumerate() {
        let x_val = if mode_x_max { p.x } else { -p.x };
        let y_val = if mode_y_max { p.y } else { -p.y };
        let score = x_val + y_val;

        if score > best_score {
            best_score = score;
            best_idx = i;
        }
    }
    corners[best_idx]
}