Fixed various problems in nearestNeighbor and assignmentSweep and cleaned up the code
-first working path from pickup-deliver
creating my evaluation squeme
reorganized some code and made some tests
Merge branch 'vdev' into develop