fixed traversal and optimize for kd tree