waymark 0.1.0

//! Comprehensive geometric operations tests
//!
//! This module ports and extends the C++ Tests_DetourCommonGeometry.cpp tests
//! to ensure our Rust implementation has equivalent geometric functionality.

#[cfg(test)]
mod tests {

    #[test]
    fn test_vector_equality() {
        // Equal vectors
        let v1 = [1.0, 2.0, 3.0];
        let v2 = [1.0, 2.0, 3.0];
        assert!(v_equal(&v1, &v2));

        // Nearly equal vectors (within tolerance)
        let v3 = [1.0, 2.0, 3.0];
        let eps = 1.0 / 30000.0; // ~3.3e-5
        let v4 = [1.0 + eps, 2.0 + eps, 3.0 + eps];
        assert!(v_equal(&v3, &v4));

        // Different vectors
        let v5 = [1.0, 2.0, 3.0];
        let v6 = [2.0, 3.0, 4.0];
        assert!(!v_equal(&v5, &v6));

        // Zero vectors
        let v7 = [0.0, 0.0, 0.0];
        let v8 = [0.0, 0.0, 0.0];
        assert!(v_equal(&v7, &v8));
    }

    #[test]
    fn test_vector_copy_and_set() {
        // Test vector copy
        let src = [4.0, 5.0, 6.0];
        let mut dest = [0.0; 3];
        v_copy(&mut dest, &src);

        assert_eq!(dest[0], 4.0);
        assert_eq!(dest[1], 5.0);
        assert_eq!(dest[2], 6.0);

        // Test vector set
        let mut v = [0.0; 3];
        v_set(&mut v, 1.5, 2.5, 3.5);

        assert_eq!(v[0], 1.5);
        assert_eq!(v[1], 2.5);
        assert_eq!(v[2], 3.5);
    }

    #[test]
    fn test_vector_min_max() {
        // Component-wise minimum
        let mut v1 = [1.0, 5.0, 3.0];
        let v2 = [2.0, 4.0, 6.0];

        v_min(&mut v1, &v2);

        assert_eq!(v1[0], 1.0);
        assert_eq!(v1[1], 4.0);
        assert_eq!(v1[2], 3.0);

        // Component-wise maximum
        let mut v3 = [1.0, 5.0, 3.0];
        let v4 = [2.0, 4.0, 6.0];

        v_max(&mut v3, &v4);

        assert_eq!(v3[0], 2.0);
        assert_eq!(v3[1], 5.0);
        assert_eq!(v3[2], 6.0);
    }

    #[test]
    fn test_vector_finite_checks() {
        // Finite values
        let v1 = [1.0, 2.0, 3.0];
        assert!(v_is_finite(&v1));

        // NaN values
        let v2 = [1.0, f32::NAN, 3.0];
        assert!(!v_is_finite(&v2));

        // Infinity values
        let v3 = [1.0, 2.0, f32::INFINITY];
        assert!(!v_is_finite(&v3));

        // 2D finite check (ignores Y component)
        let v4 = [1.0, f32::INFINITY, 2.0];
        assert!(v_is_finite_2d(&v4)); // Y is infinity but that's ignored

        let v5 = [f32::NAN, f32::INFINITY, 2.0];
        assert!(!v_is_finite_2d(&v5)); // X is NaN
    }

    #[test]
    fn test_2d_vector_operations() {
        // 2D distance
        let v1 = [0.0, 1.0, 0.0];
        let v2 = [3.0, 2.0, 4.0];

        let dist = v_dist_2d(&v1, &v2);
        assert!((dist - 5.0).abs() < 1e-6); // 3-4-5 triangle

        let dist_sqr = v_dist_2d_sqr(&v1, &v2);
        assert!((dist_sqr - 25.0).abs() < 1e-6);

        // 2D dot product
        let v3 = [3.0, 0.0, 4.0];
        let v4 = [2.0, 5.0, 1.0];

        let dot_2d = v_dot_2d(&v3, &v4);
        assert!((dot_2d - 10.0).abs() < 1e-6); // 3*2 + 4*1

        // 2D perpendicular dot product
        let u = [1.0, 0.0, 0.0];
        let v = [0.0, 0.0, 1.0];

        let perp = v_perp_2d(&u, &v);
        // perp_2d computes u[2]*v[0] - u[0]*v[2] = 0*0 - 1*1 = -1
        assert!((perp - (-1.0)).abs() < 1e-6);
    }

    #[test]
    fn test_triangle_operations() {
        // Triangle area 2D
        let a = [0.0, 0.0, 0.0];
        let b = [4.0, 0.0, 0.0];
        let c = [0.0, 0.0, 3.0];

        let area = tri_area_2d(&a, &b, &c);
        // tri_area_2d returns 2x signed area using cross product
        // For counter-clockwise: (0,3) x (4,0) = 0*0 - 4*3 = -12
        assert!((area - (-12.0)).abs() < 1e-6);

        // Triangle area 2D - clockwise
        let a2 = [0.0, 0.0, 0.0];
        let b2 = [0.0, 0.0, 3.0];
        let c2 = [4.0, 0.0, 0.0];

        let area2 = tri_area_2d(&a2, &b2, &c2);
        // For clockwise: positive area
        assert!((area2 - 12.0).abs() < 1e-6);

        // Closest point on triangle
        let tri_a = [0.0, 0.0, 0.0];
        let tri_b = [4.0, 0.0, 0.0];
        let tri_c = [0.0, 0.0, 4.0];

        // Point inside triangle
        let p1 = [1.0, 1.0, 1.0];
        let closest = closest_pt_point_triangle(&p1, &tri_a, &tri_b, &tri_c);
        assert!((closest[0] - 1.0).abs() < 1e-6);
        assert!((closest[1] - 0.0).abs() < 1e-6); // Projected to triangle plane
        assert!((closest[2] - 1.0).abs() < 1e-6);

        // Point outside triangle - closest to edge
        let p2 = [5.0, 0.0, 0.0];
        let closest2 = closest_pt_point_triangle(&p2, &tri_a, &tri_b, &tri_c);
        assert!((closest2[0] - 4.0).abs() < 1e-6); // Clamped to vertex b
        assert!((closest2[1] - 0.0).abs() < 1e-6);
        assert!((closest2[2] - 0.0).abs() < 1e-6);
    }

    #[test]
    fn test_aabb_overlap() {
        // Float bounds overlap
        let amin = [0.0, 0.0, 0.0];
        let amax = [2.0, 2.0, 2.0];
        let bmin = [1.0, 1.0, 1.0];
        let bmax = [3.0, 3.0, 3.0];

        assert!(overlap_bounds(&amin, &amax, &bmin, &bmax));

        // No overlap
        let cmin = [3.0, 3.0, 3.0];
        let cmax = [4.0, 4.0, 4.0];
        assert!(!overlap_bounds(&amin, &amax, &cmin, &cmax));

        // Touching edges
        let dmin = [2.0, 0.0, 0.0];
        let dmax = [3.0, 1.0, 1.0];
        assert!(overlap_bounds(&amin, &amax, &dmin, &dmax));
    }

    #[test]
    fn test_point_in_polygon() {
        // Define a square polygon
        let verts = [
            [0.0, 0.0, 0.0],
            [4.0, 0.0, 0.0],
            [4.0, 0.0, 4.0],
            [0.0, 0.0, 4.0],
        ];

        // Point inside polygon
        let pt1 = [2.0, 0.0, 2.0];
        assert!(point_in_polygon(&pt1, &verts));

        let pt2 = [0.5, 0.0, 0.5];
        assert!(point_in_polygon(&pt2, &verts));

        // Point outside polygon
        let pt3 = [5.0, 0.0, 2.0];
        assert!(!point_in_polygon(&pt3, &verts));

        let pt4 = [-1.0, 0.0, 2.0];
        assert!(!point_in_polygon(&pt4, &verts));

        // Point on edge
        let pt5 = [2.0, 0.0, 0.0];
        assert!(point_in_polygon(&pt5, &verts));

        let pt6 = [0.0, 0.0, 2.0];
        assert!(point_in_polygon(&pt6, &verts));

        // Point on vertex
        let pt7 = [0.0, 0.0, 0.0];
        assert!(point_in_polygon(&pt7, &verts));
    }

    #[test]
    fn test_segment_intersection() {
        // Line segment intersection in 2D
        let seg1_start = [0.0, 0.0, 0.0];
        let seg1_end = [4.0, 0.0, 0.0];
        let seg2_start = [2.0, 0.0, -2.0];
        let seg2_end = [2.0, 0.0, 2.0];

        if let Some((t1, t2)) =
            intersect_segments_2d(&seg1_start, &seg1_end, &seg2_start, &seg2_end)
        {
            // t1 should be 0.5 (halfway along seg1)
            // t2 should be 0.5 (halfway along seg2)
            assert!((t1 - 0.5).abs() < 1e-6);
            assert!((t2 - 0.5).abs() < 1e-6);
        } else {
            panic!("Expected intersection but got None");
        }

        // Parallel segments (no intersection)
        let seg3_start = [0.0, 0.0, 1.0];
        let seg3_end = [4.0, 0.0, 1.0];

        assert!(intersect_segments_2d(&seg1_start, &seg1_end, &seg3_start, &seg3_end).is_none());
    }

    #[test]
    fn test_closest_point_on_segment() {
        let seg_start = [0.0, 0.0, 0.0];
        let seg_end = [4.0, 0.0, 0.0];

        // Point projected onto segment
        let pt1 = [2.0, 2.0, 0.0];
        let closest1 = closest_pt_point_segment(&pt1, &seg_start, &seg_end);
        assert!((closest1[0] - 2.0).abs() < 1e-6);
        assert!((closest1[1] - 0.0).abs() < 1e-6);
        assert!((closest1[2] - 0.0).abs() < 1e-6);

        // Point beyond segment end
        let pt2 = [6.0, 1.0, 0.0];
        let closest2 = closest_pt_point_segment(&pt2, &seg_start, &seg_end);
        assert!((closest2[0] - 4.0).abs() < 1e-6); // Clamped to end
        assert!((closest2[1] - 0.0).abs() < 1e-6);
        assert!((closest2[2] - 0.0).abs() < 1e-6);

        // Point before segment start
        let pt3 = [-2.0, 1.0, 0.0];
        let closest3 = closest_pt_point_segment(&pt3, &seg_start, &seg_end);
        assert!((closest3[0] - 0.0).abs() < 1e-6); // Clamped to start
        assert!((closest3[1] - 0.0).abs() < 1e-6);
        assert!((closest3[2] - 0.0).abs() < 1e-6);
    }

    #[test]
    fn test_robust_geometric_operations() {
        // Test with extremely small values
        let tiny = f32::EPSILON;
        let v1 = [tiny, tiny, tiny];
        let v2 = [tiny * 2.0, tiny * 2.0, tiny * 2.0];

        // Should handle tiny values gracefully
        let dist = v_dist(&v1, &v2);
        assert!(dist.is_finite() && dist >= 0.0);

        // Test with moderately large values (avoid overflow)
        let large = 1e10; // Reduced from 1e30 to avoid float overflow
        let v3 = [large, large, large];
        let v4 = [large * 0.5, large * 0.5, large * 0.5];

        // Should handle large values without overflow
        let dist2 = v_dist(&v3, &v4);
        assert!(dist2.is_finite() && dist2 >= 0.0);
    }

    #[test]
    fn test_degenerate_cases() {
        // Zero-length segments
        let pt = [1.0, 1.0, 1.0];
        let zero_seg_start = [0.0, 0.0, 0.0];
        let zero_seg_end = [0.0, 0.0, 0.0];

        let closest = closest_pt_point_segment(&pt, &zero_seg_start, &zero_seg_end);
        // Should return the segment point itself
        assert!(v_equal(&closest, &zero_seg_start));

        // Degenerate triangle (collinear points)
        let a = [0.0, 0.0, 0.0];
        let b = [2.0, 0.0, 0.0];
        let c = [4.0, 0.0, 0.0]; // Collinear with a and b

        let area = tri_area_2d(&a, &b, &c);
        assert!((area - 0.0).abs() < 1e-6); // Should be zero area
    }

    // Helper functions - implement these based on C++ dtCommon functions

    fn v_equal(a: &[f32; 3], b: &[f32; 3]) -> bool {
        const EPSILON_SQR: f32 = (1.0 / 16384.0) * (1.0 / 16384.0);
        let dx = a[0] - b[0];
        let dy = a[1] - b[1];
        let dz = a[2] - b[2];
        dx * dx + dy * dy + dz * dz < EPSILON_SQR
    }

    fn v_copy(dest: &mut [f32; 3], src: &[f32; 3]) {
        dest[0] = src[0];
        dest[1] = src[1];
        dest[2] = src[2];
    }

    fn v_set(v: &mut [f32; 3], x: f32, y: f32, z: f32) {
        v[0] = x;
        v[1] = y;
        v[2] = z;
    }

    fn v_min(a: &mut [f32; 3], b: &[f32; 3]) {
        a[0] = a[0].min(b[0]);
        a[1] = a[1].min(b[1]);
        a[2] = a[2].min(b[2]);
    }

    fn v_max(a: &mut [f32; 3], b: &[f32; 3]) {
        a[0] = a[0].max(b[0]);
        a[1] = a[1].max(b[1]);
        a[2] = a[2].max(b[2]);
    }

    fn v_is_finite(v: &[f32; 3]) -> bool {
        v[0].is_finite() && v[1].is_finite() && v[2].is_finite()
    }

    fn v_is_finite_2d(v: &[f32; 3]) -> bool {
        v[0].is_finite() && v[2].is_finite() // Ignores Y component
    }

    fn v_dist(a: &[f32; 3], b: &[f32; 3]) -> f32 {
        let dx = a[0] - b[0];
        let dy = a[1] - b[1];
        let dz = a[2] - b[2];
        (dx * dx + dy * dy + dz * dz).sqrt()
    }

    fn v_dist_2d(a: &[f32; 3], b: &[f32; 3]) -> f32 {
        let dx = a[0] - b[0];
        let dz = a[2] - b[2];
        (dx * dx + dz * dz).sqrt()
    }

    fn v_dist_2d_sqr(a: &[f32; 3], b: &[f32; 3]) -> f32 {
        let dx = a[0] - b[0];
        let dz = a[2] - b[2];
        dx * dx + dz * dz
    }

    fn v_dot_2d(a: &[f32; 3], b: &[f32; 3]) -> f32 {
        a[0] * b[0] + a[2] * b[2]
    }

    fn v_perp_2d(a: &[f32; 3], b: &[f32; 3]) -> f32 {
        a[2] * b[0] - a[0] * b[2]
    }

    fn tri_area_2d(a: &[f32; 3], b: &[f32; 3], c: &[f32; 3]) -> f32 {
        let abx = b[0] - a[0];
        let abz = b[2] - a[2];
        let acx = c[0] - a[0];
        let acz = c[2] - a[2];
        acx * abz - abx * acz
    }

    fn closest_pt_point_triangle(
        pt: &[f32; 3],
        a: &[f32; 3],
        b: &[f32; 3],
        c: &[f32; 3],
    ) -> [f32; 3] {
        // Simplified implementation - project point to triangle plane (Y=0)
        // and clamp to triangle bounds
        let mut result = [pt[0], 0.0, pt[2]];

        // Basic clamping to triangle bounds for this test
        // Real implementation would use barycentric coordinates
        if !point_in_polygon(&result, &[*a, *b, *c, *a]) {
            // If outside, find closest edge point
            let closest_ab = closest_pt_point_segment(&result, a, b);
            let closest_bc = closest_pt_point_segment(&result, b, c);
            let closest_ca = closest_pt_point_segment(&result, c, a);

            let dist_ab = v_dist_2d(&result, &closest_ab);
            let dist_bc = v_dist_2d(&result, &closest_bc);
            let dist_ca = v_dist_2d(&result, &closest_ca);

            if dist_ab <= dist_bc && dist_ab <= dist_ca {
                result = closest_ab;
            } else if dist_bc <= dist_ca {
                result = closest_bc;
            } else {
                result = closest_ca;
            }
        }

        result
    }

    fn overlap_bounds(amin: &[f32; 3], amax: &[f32; 3], bmin: &[f32; 3], bmax: &[f32; 3]) -> bool {
        amin[0] <= bmax[0]
            && amax[0] >= bmin[0]
            && amin[1] <= bmax[1]
            && amax[1] >= bmin[1]
            && amin[2] <= bmax[2]
            && amax[2] >= bmin[2]
    }

    fn point_in_polygon(pt: &[f32; 3], verts: &[[f32; 3]]) -> bool {
        // Ray casting algorithm in 2D (X-Z plane)
        let mut inside = false;
        let mut j = verts.len() - 1;

        for i in 0..verts.len() {
            let vi = &verts[i];
            let vj = &verts[j];

            if ((vi[2] > pt[2]) != (vj[2] > pt[2]))
                && (pt[0] < (vj[0] - vi[0]) * (pt[2] - vi[2]) / (vj[2] - vi[2]) + vi[0])
            {
                inside = !inside;
            }
            j = i;
        }

        inside
    }

    fn intersect_segments_2d(
        a1: &[f32; 3],
        a2: &[f32; 3],
        b1: &[f32; 3],
        b2: &[f32; 3],
    ) -> Option<(f32, f32)> {
        let dx1 = a2[0] - a1[0];
        let dz1 = a2[2] - a1[2];
        let dx2 = b2[0] - b1[0];
        let dz2 = b2[2] - b1[2];

        let det = dx1 * dz2 - dz1 * dx2;

        if det.abs() < 1e-6 {
            return None; // Parallel or coincident
        }

        let dx3 = a1[0] - b1[0];
        let dz3 = a1[2] - b1[2];

        let t1 = (dx2 * dz3 - dz2 * dx3) / det;
        let t2 = (dx1 * dz3 - dz1 * dx3) / det;

        if t1 >= 0.0 && t1 <= 1.0 && t2 >= 0.0 && t2 <= 1.0 {
            Some((t1, t2))
        } else {
            None
        }
    }

    fn closest_pt_point_segment(pt: &[f32; 3], a: &[f32; 3], b: &[f32; 3]) -> [f32; 3] {
        let dx = b[0] - a[0];
        let dy = b[1] - a[1];
        let dz = b[2] - a[2];

        let len_sqr = dx * dx + dy * dy + dz * dz;

        if len_sqr < 1e-6 {
            // Degenerate segment
            return *a;
        }

        let px = pt[0] - a[0];
        let py = pt[1] - a[1];
        let pz = pt[2] - a[2];

        let t = ((px * dx + py * dy + pz * dz) / len_sqr).clamp(0.0, 1.0);

        [a[0] + t * dx, a[1] + t * dy, a[2] + t * dz]
    }
}