box2d-rust 1.2.0

// Port of box2d-cpp-reference/test/test_dynamic_tree.c (all subtests).
// SPDX-FileCopyrightText: 2025 Erin Catto
// SPDX-License-Identifier: MIT

use crate::collision::RayCastInput;
use crate::dynamic_tree::DynamicTree;
use crate::math_functions::{add, sub, Aabb, Vec2};

fn v(x: f32, y: f32) -> Vec2 {
    Vec2 { x, y }
}

fn aabb(lx: f32, ly: f32, ux: f32, uy: f32) -> Aabb {
    Aabb {
        lower_bound: v(lx, ly),
        upper_bound: v(ux, uy),
    }
}

#[test]
fn tree_create_destroy() {
    let a = aabb(-1.0, -1.0, 2.0, 2.0);

    let mut tree = DynamicTree::new(16);
    tree.create_proxy(a, 1, 0);

    assert!(tree.node_count() > 0);
    assert!(tree.proxy_count() == 1);

    tree.destroy();

    assert!(tree.node_count() == 0);
    assert!(tree.proxy_count() == 0);
}

/// The C RayCastCallbackFcn records the proxy id and returns 0 (terminate).
fn ray_cast_hit(tree: &DynamicTree, p1: Vec2, p2: Vec2) -> i32 {
    let input = RayCastInput {
        origin: p1,
        translation: sub(p2, p1),
        max_fraction: 1.0,
    };

    let mut proxy_hit = -1;
    tree.ray_cast(&input, 1, |_input, proxy_id, _user_data| {
        proxy_hit = proxy_id;
        0.0
    });
    proxy_hit
}

#[test]
fn tree_ray_cast() {
    // Test AABB centered at origin with bounds [-1, -1] to [1, 1]
    let a = aabb(-1.0, -1.0, 1.0, 1.0);
    let mut tree = DynamicTree::new(16);
    let proxy_id = tree.create_proxy(a, 1, 0);

    // Test 1: Ray hits AABB from left side
    assert_eq!(ray_cast_hit(&tree, v(-3.0, 0.0), v(3.0, 0.0)), proxy_id);

    // Test 2: Ray hits AABB from right side
    assert_eq!(ray_cast_hit(&tree, v(3.0, 0.0), v(-3.0, 0.0)), proxy_id);

    // Test 3: Ray hits AABB from bottom
    assert_eq!(ray_cast_hit(&tree, v(0.0, -3.0), v(0.0, 3.0)), proxy_id);

    // Test 4: Ray hits AABB from top
    assert_eq!(ray_cast_hit(&tree, v(0.0, 3.0), v(0.0, -3.0)), proxy_id);

    // Test 5: Ray misses AABB completely (parallel to x-axis)
    assert_eq!(ray_cast_hit(&tree, v(-3.0, 2.0), v(3.0, 2.0)), -1);

    // Test 6: Ray misses AABB completely (parallel to y-axis)
    assert_eq!(ray_cast_hit(&tree, v(2.0, -3.0), v(2.0, 3.0)), -1);

    // Test 7: Ray starts inside AABB
    assert_eq!(ray_cast_hit(&tree, v(0.0, 0.0), v(2.0, 0.0)), proxy_id);

    // Test 8: Ray hits corner of AABB (diagonal ray)
    assert_eq!(ray_cast_hit(&tree, v(-2.0, -2.0), v(2.0, 2.0)), proxy_id);

    // Test 9: Ray parallel to AABB edge but outside
    assert_eq!(ray_cast_hit(&tree, v(-2.0, 1.5), v(2.0, 1.5)), -1);

    // Test 10: Ray parallel to AABB edge and exactly on boundary
    assert_eq!(ray_cast_hit(&tree, v(-2.0, 1.0), v(2.0, 1.0)), proxy_id);

    // Test 11: Very short ray that doesn't reach AABB
    assert_eq!(ray_cast_hit(&tree, v(-3.0, 0.0), v(-2.5, 0.0)), -1);

    // Test 12: Zero-length ray (degenerate case)
    assert_eq!(ray_cast_hit(&tree, v(0.0, 0.0), v(0.0, 0.0)), proxy_id);

    // Test 13: Ray hits AABB at exact boundary condition (t = 1.0)
    assert_eq!(ray_cast_hit(&tree, v(-2.0, 0.0), v(-1.0, 0.0)), proxy_id);

    tree.destroy();
}

#[test]
fn tree_multiple_proxies() {
    let mut tree = DynamicTree::new(16);

    let a1 = aabb(-5.0, -1.0, -3.0, 1.0);
    let a2 = aabb(-1.0, -1.0, 1.0, 1.0);
    let a3 = aabb(3.0, -1.0, 5.0, 1.0);

    let id1 = tree.create_proxy(a1, 0x1, 42);
    let id2 = tree.create_proxy(a2, 0x2, 43);
    let id3 = tree.create_proxy(a3, 0x4, 44);

    assert!(tree.proxy_count() == 3);

    assert!(tree.user_data(id1) == 42);
    assert!(tree.user_data(id2) == 43);
    assert!(tree.user_data(id3) == 44);

    assert!(tree.category_bits(id1) == 0x1);
    assert!(tree.category_bits(id2) == 0x2);
    assert!(tree.category_bits(id3) == 0x4);

    tree.destroy();
}

#[test]
fn tree_query() {
    let mut tree = DynamicTree::new(16);

    let a1 = aabb(-5.0, -1.0, -3.0, 1.0);
    let a2 = aabb(-1.0, -1.0, 1.0, 1.0);
    let a3 = aabb(3.0, -1.0, 5.0, 1.0);

    let _id1 = tree.create_proxy(a1, 0xFF, 0);
    let id2 = tree.create_proxy(a2, 0xFF, 0);
    let _id3 = tree.create_proxy(a3, 0xFF, 0);

    let query_a = aabb(-2.0, -2.0, 2.0, 2.0);

    let mut found_flags = [0i32; 32];
    let stats = tree.query(query_a, 0xFFFFFFFF, |proxy_id, _user_data| {
        found_flags[proxy_id as usize] = 1;
        true // continue the query
    });

    // We expect at least the middle proxy to be visited.
    assert!(found_flags[id2 as usize] == 1);
    assert!(stats.leaf_visits >= 1);

    // Test QueryAll using list collector
    let mut list: Vec<i32> = Vec::new();
    let all_stats = tree.query_all(query_a, |proxy_id, _user_data| {
        list.push(proxy_id);
        true
    });
    assert!(!list.is_empty()); // at least one proxy should be collected
    assert!(all_stats.leaf_visits >= 1);

    tree.destroy();
}

#[test]
fn tree_move_and_enlarge() {
    let mut tree = DynamicTree::new(16);

    let a = aabb(0.0, 0.0, 1.0, 1.0);
    let id = tree.create_proxy(a, 0x1, 100);

    // Move proxy to a new place
    let moved = aabb(10.0, 10.0, 11.0, 11.0);
    tree.move_proxy(id, moved);

    let got = tree.aabb(id);
    assert!(got.lower_bound.x == moved.lower_bound.x);
    assert!(got.lower_bound.y == moved.lower_bound.y);
    assert!(got.upper_bound.x == moved.upper_bound.x);
    assert!(got.upper_bound.y == moved.upper_bound.y);

    // Now enlarge the proxy
    let enlarge = aabb(9.5, 9.5, 11.5, 11.5);
    tree.enlarge_proxy(id, enlarge);

    let got2 = tree.aabb(id);
    assert!(got2.lower_bound.x <= enlarge.lower_bound.x + 1e-6);
    assert!(got2.upper_bound.x >= enlarge.upper_bound.x - 1e-6);

    tree.destroy();
}

#[test]
fn tree_rebuild_and_validate() {
    let mut tree = DynamicTree::new(16);

    // Create a number of proxies to make rebuild meaningful
    for i in 0..12 {
        let x = i as f32 * 2.0;
        let a = aabb(x - 0.5, -0.5, x + 0.5, 0.5);
        tree.create_proxy(a, 0xFF, i as u64);
    }

    let sorted = tree.rebuild(true);

    assert!(sorted >= 0);
    assert!(tree.byte_count() > 0);
    assert!(tree.height() > 0);

    tree.destroy();
}

#[test]
fn tree_row_height() {
    let mut tree = DynamicTree::new(16);

    let column_count = 200;
    for i in 0..column_count {
        let x = 1.0 * i as f32;
        let a = aabb(x, 0.0, x + 1.0, 1.0);
        tree.create_proxy(a, 1, i as u64);
    }

    let min_height = (column_count as f32).log2();

    assert!((tree.height() as f32) < 2.0 * min_height);

    tree.destroy();
}

#[test]
fn tree_grid_height() {
    let mut tree = DynamicTree::new(16);

    let column_count = 20;
    let row_count = 20;
    for i in 0..column_count {
        let x = 1.0 * i as f32;
        for j in 0..row_count {
            let y = 1.0 * j as f32;
            let a = aabb(x, y, x + 1.0, y + 1.0);
            tree.create_proxy(a, 1, i as u64);
        }
    }

    let min_height = ((row_count * column_count) as f32).log2();

    assert!((tree.height() as f32) < 2.0 * min_height);

    tree.destroy();
}

const GRID_COUNT: usize = 20;

#[test]
fn tree_grid_movement() {
    let mut tree = DynamicTree::new(16);

    let mut proxy_ids = [0i32; GRID_COUNT * GRID_COUNT];
    let mut index = 0usize;
    for i in 0..GRID_COUNT {
        let x = 1.0 * i as f32;
        for j in 0..GRID_COUNT {
            let y = 1.0 * j as f32;
            let a = aabb(x, y, x + 1.0, y + 1.0);
            proxy_ids[index] = tree.create_proxy(a, 1, i as u64);
            index += 1;
        }
    }

    assert!(index == GRID_COUNT * GRID_COUNT);

    let min_height = ((GRID_COUNT * GRID_COUNT) as f32).log2();

    let height1 = tree.height();
    assert!((height1 as f32) < 2.0 * min_height);

    let offset = v(10.0, 20.0);
    index = 0;
    for _i in 0..GRID_COUNT {
        for _j in 0..GRID_COUNT {
            let mut a = tree.aabb(proxy_ids[index]);
            a.lower_bound = add(a.lower_bound, offset);
            a.upper_bound = add(a.upper_bound, offset);
            tree.move_proxy(proxy_ids[index], a);
            index += 1;
        }
    }

    let height2 = tree.height();
    assert!((height2 as f32) < 3.0 * min_height);

    tree.rebuild(true);

    let height3 = tree.height();
    assert!((height3 as f32) < 2.0 * min_height);

    tree.destroy();
}