Solved a bug in closest point search