rewrite KdTree(now it's generic and faster!)