use crate::b2_collision::*;
use crate::b2_growable_stack::B2growableStack;
use crate::b2_math::*;
use crate::b2_common::*;
use crate::private::collision::b2_dynamic_tree as private;
pub const B2_NULL_NODE: i32 = -1;
#[derive(Default, Clone, Debug)]
pub struct B2treeNode<UserDataType> {
pub(crate) aabb: B2AABB,
pub(crate) user_data: Option<UserDataType>,
pub(crate) parent: i32,
pub(crate) child1: i32,
pub(crate) child2: i32,
pub(crate) height: i32,
pub(crate) moved: bool,
}
impl<UserDataType> B2treeNode<UserDataType> {
pub fn is_leaf(&self) -> bool {
return self.child1 == B2_NULL_NODE;
}
}
#[derive(Default, Clone, Debug)]
pub struct B2dynamicTree<UserDataType> {
pub(crate) m_root: i32,
pub(crate) m_nodes: Vec<B2treeNode<UserDataType>>,
pub(crate) m_node_count: i32,
pub(crate) m_node_capacity: i32,
pub(crate) m_free_list: i32,
pub(crate) m_insertion_count: i32,
}
impl<UserDataType: Clone + Default> B2dynamicTree<UserDataType> {
pub fn new() -> Self {
return private::b2_dynamic_tree();
}
pub fn create_proxy(&mut self, aabb: B2AABB, user_data: &UserDataType) -> i32 {
return private::create_proxy(self, aabb, user_data);
}
pub fn destroy_proxy(&mut self, proxy_id: i32) {
private::destroy_proxy(self, proxy_id);
}
pub fn move_proxy(&mut self, proxy_id: i32, aabb1: B2AABB, displacement: B2vec2) -> bool {
return private::move_proxy(self, proxy_id, aabb1, displacement);
}
pub fn get_user_data(&self, proxy_id: i32) -> Option<UserDataType> {
return inline::get_user_data(self, proxy_id);
}
pub fn was_moved(&self, proxy_id: i32) -> bool {
return inline::was_moved(self, proxy_id);
}
pub fn clear_moved(&mut self, proxy_id: i32) {
inline::clear_moved(self, proxy_id);
}
pub fn get_fat_aabb(&self, proxy_id: i32) -> B2AABB {
return inline::get_fat_aabb(self, proxy_id);
}
pub fn query<F: QueryCallback>(&self, callback: F, aabb: B2AABB) {
inline::query(self, callback, aabb);
}
pub fn ray_cast<T: RayCastCallback>(&self, callback: T, input: &B2rayCastInput) {
inline::ray_cast(self, callback, input);
}
pub fn validate(&self) {
private::validate(self);
}
pub fn get_height(&self) -> i32 {
return private::get_height(self);
}
pub fn get_max_balance(&self) -> i32 {
return private::get_max_balance(self);
}
pub fn get_area_ration(&self) -> f32 {
return private::get_area_ratio(self);
}
pub fn rebuild_bottom_up(&mut self) {
private::rebuild_bottom_up(self);
}
pub fn shift_origin(&mut self, new_origin: B2vec2) {
private::shift_origin(self, new_origin);
}
pub(crate) fn allocate_node(&mut self) -> i32 {
return private::allocate_node(self);
}
pub(crate) fn free_node(&mut self, node: i32) {
private::free_node(self, node);
}
pub(crate) fn insert_leaf(&mut self, node: i32) {
private::insert_leaf(self, node);
}
pub(crate) fn remove_leaf(&mut self, node: i32) {
private::remove_leaf(self, node);
}
pub(crate) fn balance(&mut self, index: i32) -> i32 {
return private::balance(self, index);
}
pub(crate) fn compute_height(&self) -> i32 {
return private::compute_height(self);
}
pub(crate) fn compute_height_by_node(&self, node_id: i32) -> i32 {
return private::compute_height_by_node(self, node_id);
}
pub(crate) fn validate_structure(&self, index: i32) {
private::validate_structure(self, index);
}
pub(crate) fn validate_metrics(&self, index: i32) {
private::validate_metrics(self, index);
}
}
pub trait QueryCallback: FnMut(i32) -> bool {}
impl <F> QueryCallback for F where F: FnMut(i32) -> bool {}
pub trait RayCastCallback: FnMut(&B2rayCastInput, i32) -> f32 {}
impl <F> RayCastCallback for F where F: FnMut(&B2rayCastInput, i32) -> f32 {}
mod inline {
use super::*;
pub fn get_user_data<UserDataType: Clone + Default>(
self_: &B2dynamicTree<UserDataType>,
proxy_id: i32,
) -> Option<UserDataType> {
return self_.m_nodes[proxy_id as usize].user_data.clone();
}
pub fn was_moved<UserDataType: Clone + Default>(
self_: &B2dynamicTree<UserDataType>,
proxy_id: i32,
) -> bool {
return self_.m_nodes[proxy_id as usize].moved;
}
pub fn clear_moved<UserDataType: Clone + Default>(
self_: &mut B2dynamicTree<UserDataType>,
proxy_id: i32,
) {
self_.m_nodes[proxy_id as usize].moved = false;
}
pub fn get_fat_aabb<UserDataType: Clone + Default>(
self_: &B2dynamicTree<UserDataType>,
proxy_id: i32,
) -> B2AABB {
return self_.m_nodes[proxy_id as usize].aabb;
}
pub fn query<UserDataType, F: QueryCallback>(
self_: &B2dynamicTree<UserDataType>,
mut callback: F,
aabb: B2AABB,
) {
let mut stack = B2growableStack::<i32, 256>::new();
stack.push(&self_.m_root);
while stack.get_count() > 0 {
let node_id: i32 = stack.pop();
if node_id == B2_NULL_NODE {
continue;
}
let node = &self_.m_nodes[node_id as usize];
if b2_test_overlap(node.aabb, aabb) {
if node.is_leaf() {
let proceed: bool = callback(node_id);
if proceed == false {
return;
}
} else {
stack.push(&node.child1);
stack.push(&node.child2);
}
}
}
}
pub fn ray_cast<T: RayCastCallback, UserDataType>(
self_: &B2dynamicTree<UserDataType>,
mut callback: T,
input: &B2rayCastInput,
) {
let p1: B2vec2 = input.p1;
let p2: B2vec2 = input.p2;
let mut r: B2vec2 = p2 - p1;
b2_assert(r.length_squared() > 0.0);
r.normalize();
let v: B2vec2 = b2_cross_scalar_by_vec(1.0, r);
let abs_v: B2vec2 = b2_abs_vec2(v);
let mut max_fraction: f32 = input.max_fraction;
let mut segment_aabb = B2AABB::default();
{
let t: B2vec2 = p1 + max_fraction * (p2 - p1);
segment_aabb.lower_bound = b2_min_vec2(p1, t);
segment_aabb.upper_bound = b2_max_vec2(p1, t);
}
let mut stack = B2growableStack::<i32, 256>::new();
stack.push(&self_.m_root);
while stack.get_count() > 0 {
let node_id: i32 = stack.pop();
if node_id == B2_NULL_NODE {
continue;
}
let node = &self_.m_nodes[node_id as usize];
if b2_test_overlap(node.aabb, segment_aabb) == false {
continue;
}
let c: B2vec2 = node.aabb.get_center();
let h: B2vec2 = node.aabb.get_extents();
let separation: f32 = b2_abs(b2_dot(v, p1 - c)) - b2_dot(abs_v, h);
if separation > 0.0 {
continue;
}
if node.is_leaf() {
let sub_input = B2rayCastInput {
p1: input.p1,
p2: input.p2,
max_fraction: max_fraction,
};
let value: f32 = callback(&sub_input, node_id);
if value == 0.0 {
return;
}
if value > 0.0 {
max_fraction = value;
let t: B2vec2 = p1 + max_fraction * (p2 - p1);
segment_aabb.lower_bound = b2_min_vec2(p1, t);
segment_aabb.upper_bound = b2_max_vec2(p1, t);
}
} else {
stack.push(&node.child1);
stack.push(&node.child2);
}
}
}
}