Fixed various problems in nearestNeighbor and assignmentSweep and cleaned up the code