Add AABB tree to utils