Adding kdTree traversal code