Use KDTree for testing. Also: changed Callback to use List instead of ArrayList