Adapted road finding

src/de/tudarmstadt/maki/simonstrator/api/component/vehicular/roadnetwork/routing/BreadthFirstSearch.java

@@ -27,6 +27,7 @@ import java.util.List;
 import java.util.PriorityQueue;
 import java.util.Queue;
 
+import de.tudarmstadt.maki.simonstrator.api.component.vehicular.information.RoadInformation;
 import de.tudarmstadt.maki.simonstrator.api.component.vehicular.roadnetwork.RoadNetwork;
 import de.tudarmstadt.maki.simonstrator.api.component.vehicular.roadnetwork.RoadNetworkEdge;
 import de.tudarmstadt.maki.simonstrator.api.component.vehicular.roadnetwork.RoadNetworkRoute;
@@ -185,4 +186,11 @@ public class BreadthFirstSearch implements RoutingAlgorithm {
 		}
 	}
 
+	@Override
+	public RoadNetworkRoute findRoute(RoadNetwork pNetwork, RoadNetworkEdge pCurrentPosition,
+			RoadNetworkEdge pDestination, List<RoadNetworkRoute> pKnownRoutes, List<RoadNetworkEdge> pEdgesToAvoid,
+			List<RoadInformation> pInfo) {
+		return null;
+	}
+
 }

src/de/tudarmstadt/maki/simonstrator/api/component/vehicular/roadnetwork/routing/DijkstraAlgorithm.java

@@ -23,10 +23,14 @@ package de.tudarmstadt.maki.simonstrator.api.component.vehicular.roadnetwork.rou
 import java.util.ArrayList;
 import java.util.Collections;
 import java.util.Comparator;
+import java.util.HashMap;
 import java.util.List;
+import java.util.Map;
 import java.util.PriorityQueue;
 import java.util.Queue;
+import java.util.concurrent.atomic.DoubleAdder;
 
+import de.tudarmstadt.maki.simonstrator.api.component.vehicular.information.RoadInformation;
 import de.tudarmstadt.maki.simonstrator.api.component.vehicular.roadnetwork.RoadNetwork;
 import de.tudarmstadt.maki.simonstrator.api.component.vehicular.roadnetwork.RoadNetworkEdge;
 import de.tudarmstadt.maki.simonstrator.api.component.vehicular.roadnetwork.RoadNetworkRoute;
@@ -49,6 +53,14 @@ public class DijkstraAlgorithm implements RoutingAlgorithm {
 	@Override
 	public RoadNetworkRoute findRoute(RoadNetwork pNetwork, RoadNetworkEdge pCurrentPosition, RoadNetworkEdge pDestination,
 			List<RoadNetworkRoute> pKnownRoutes, List<RoadNetworkEdge> pEdgesToAvoid) {
+		return findRoute(pNetwork, pCurrentPosition, pDestination, pKnownRoutes, pEdgesToAvoid,
+				Collections.emptyList());
+	}
+
+	@Override
+	public RoadNetworkRoute findRoute(RoadNetwork pNetwork, RoadNetworkEdge pCurrentPosition,
+			RoadNetworkEdge pDestination, List<RoadNetworkRoute> pKnownRoutes, List<RoadNetworkEdge> pEdgesToAvoid,
+			List<RoadInformation> pInfo) {
 		if (pCurrentPosition.equals(pDestination)) {
 			List<RoadNetworkEdge> segments = new ArrayList<>();
 			segments.add(pCurrentPosition);
@@ -69,7 +81,9 @@ public class DijkstraAlgorithm implements RoutingAlgorithm {
 			}
 		}
 
-		outer:while (buffer.size() > 0) {
+		Map<RoadNetworkEdge, DoubleAdder> visited = new HashMap<>();
+
+		outer: while (buffer.size() > 0) {
 			PathInformation pathInformation = buffer.poll();
 
 			if (pathInformation.getHops() >= maxDepth) {
@@ -81,6 +95,13 @@ public class DijkstraAlgorithm implements RoutingAlgorithm {
 			}
 
 			RoadNetworkEdge edge = pathInformation.getEdge();
+			if (visited.containsKey(edge) && visited.get(edge).intValue() > pKnownRoutes.size()) {
+				continue;
+			}
+			if (!visited.containsKey(edge)) {
+				visited.put(edge, new DoubleAdder());
+			}
+			visited.get(edge).add(1);
 
 			if (pEdgesToAvoid.contains(edge)) {
 				continue;
@@ -88,9 +109,12 @@ public class DijkstraAlgorithm implements RoutingAlgorithm {
 
 			List<RoadNetworkEdge> accessibleEdges = edge.getAccessibleEdges();
 
+			List<RoadNetworkEdge> currentRouteList = new ArrayList<>();
+			extractRoute(currentRouteList, pathInformation);
+			double edgeLength = edge.calculateStandardEdgeCosts();
+			edgeLength += calculateAdditionalEdgeCosts(edge, pInfo,
+					pathInformation.getLength() - pCurrentPosition.calculateStandardEdgeCosts(), currentRouteList);
 			for (RoadNetworkEdge roadNetworkEdge : accessibleEdges) {
-				double edgeLength = edge.calculateStandardEdgeCosts();
-
 				if (roadNetworkEdge.equals(pDestination)) {
 					PathInformation childPathInformation = new PathInformation(roadNetworkEdge, pathInformation,
 							pathInformation.getHops(), pathInformation.getLength());
@@ -122,6 +146,19 @@ public class DijkstraAlgorithm implements RoutingAlgorithm {
 		return currentShortestRoute;
 	}
 
+	protected double calculateAdditionalEdgeCosts(RoadNetworkEdge pEdge, List<RoadInformation> pInfo,
+			double pCurrentPathLength, List<RoadNetworkEdge> pCurrentRouteList) {
+		if (pInfo != null) {
+			for (RoadInformation roadInformation : pInfo) {
+				if (roadInformation.getEdge().equals(pEdge)) {
+					double additionalCostIfKnown = pEdge.calculateAdditionalCostIfKnown(roadInformation.getValue());
+					return additionalCostIfKnown;
+				}
+			}
+		}
+		return 0d;
+	}
+
 	private void extractRoute(List<RoadNetworkEdge> pEdges, PathInformation pPathInformation) {
 		if (pPathInformation.getPreviousEdge() != null) {
 			extractRoute(pEdges, pPathInformation.getPreviousEdge());

src/de/tudarmstadt/maki/simonstrator/api/component/vehicular/roadnetwork/routing/GlobalKnowledgeDijkstraAlgorithm.java

+/*
+ * Copyright (c) 2005-2010 KOM – Multimedia Communications Lab
+ *
+ * This file is part of Simonstrator.KOM.
+ * 
+ * Simonstrator.KOM is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * any later version.
+ * 
+ * PeerfactSim.KOM is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ * 
+ * You should have received a copy of the GNU General Public License
+ * along with PeerfactSim.KOM.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ */
+
+package de.tudarmstadt.maki.simonstrator.api.component.vehicular.roadnetwork.routing;
+
+import java.util.List;
+
+import de.tudarmstadt.maki.simonstrator.api.Time;
+import de.tudarmstadt.maki.simonstrator.api.component.vehicular.information.RoadInformation;
+import de.tudarmstadt.maki.simonstrator.api.component.vehicular.roadnetwork.RoadNetworkEdge;
+
+public class GlobalKnowledgeDijkstraAlgorithm extends DijkstraAlgorithm {
+
+	protected double calculateAdditionalEdgeCosts(RoadNetworkEdge pEdge, List<RoadInformation> pInfo,
+			double pCurrentPathLength, List<RoadNetworkEdge> pCurrentRouteList) {
+		if (pInfo != null) {
+			for (RoadInformation roadInformation : pInfo) {
+				if (roadInformation.getEdge().equals(pEdge) && roadInformation.getDetectionDate() / Time.SECOND
+						+ roadInformation.getValue().getDuration() / Time.SECOND
+						- (Time.getCurrentTime() / Time.SECOND + pCurrentPathLength) > 0) {
+					double additionalCostIfKnown = pEdge.calculateAdditionalCostIfKnown(roadInformation.getValue());
+					return additionalCostIfKnown;
+				}
+			}
+		}
+		return 0d;
+	}
+}

src/de/tudarmstadt/maki/simonstrator/api/component/vehicular/roadnetwork/routing/RoutingAlgorithm.java

@@ -22,6 +22,7 @@ package de.tudarmstadt.maki.simonstrator.api.component.vehicular.roadnetwork.rou
 
 import java.util.List;
 
+import de.tudarmstadt.maki.simonstrator.api.component.vehicular.information.RoadInformation;
 import de.tudarmstadt.maki.simonstrator.api.component.vehicular.roadnetwork.RoadNetwork;
 import de.tudarmstadt.maki.simonstrator.api.component.vehicular.roadnetwork.RoadNetworkEdge;
 import de.tudarmstadt.maki.simonstrator.api.component.vehicular.roadnetwork.RoadNetworkRoute;
@@ -37,4 +38,7 @@ public interface RoutingAlgorithm {
 	 * @return
 	 */
 	RoadNetworkRoute findRoute(RoadNetwork pNetwork, RoadNetworkEdge pCurrentPosition, RoadNetworkEdge pDestination);
+
+	RoadNetworkRoute findRoute(RoadNetwork pNetwork, RoadNetworkEdge pCurrentPosition, RoadNetworkEdge pDestination,
+			List<RoadNetworkRoute> pKnownRoutes, List<RoadNetworkEdge> pEdgesToAvoid, List<RoadInformation> pInfo);
 }