Path calculation bug gefixed, waypoint objecten methodes sneller gemaakt
(door vaker HashMaps te gebruiken)