Fixed various problems in nearestNeighbor and assignmentSweep and cleaned up the code
Merge branch 'vdev' into develop