Mobility: Preparations for RouteSensor integration

- unified routing via RealWorldMovementPoints
- reduced overhead due to PositionVector calculations

src/de/tud/kom/p2psim/impl/topology/movement/local/OnlineMapQuestMovement.java

@@ -53,7 +53,7 @@ public class OnlineMapQuestMovement extends AbstractLocalMovementStrategy {
 	
 	private PositionVector worldDimensions;
 	
-	private static HashMap<Integer, RealWorldMovementPoints> movementPoints = new HashMap<>();
+	private static HashMap<SimLocationActuator, RealWorldMovementPoints> movementPoints = new HashMap<>();
 	
 	private String movementType; // {Fastest, Shortest, Pedestrian, Bicycle}
 	private String mapQuestKey;
@@ -82,8 +82,8 @@ public class OnlineMapQuestMovement extends AbstractLocalMovementStrategy {
 			newPosition = destination.clone();
 		} else {
 			//if not set already for this node or new destination is different than last one
-			PointList pointList;
-			if(!movementPoints.containsKey(comp.hashCode()) || destination.distanceTo(movementPoints.get(comp.hashCode()).getDestination()) > 1.0) {
+			RealWorldMovementPoints trajectory = movementPoints.get(comp);
+			if(trajectory == null || destination.distanceTo(trajectory.getDestination()) > 1.0) {
 				double[] startPosition = transformOwnWorldWindowToGPS(comp.getRealPosition().getX(), comp.getRealPosition().getY());
 				double[] destinationPosition = transformOwnWorldWindowToGPS(destination.getX(), destination.getY());
 				
@@ -110,7 +110,7 @@ public class OnlineMapQuestMovement extends AbstractLocalMovementStrategy {
 				}
 				
 				//read in all waypoints
-				pointList = new PointList();
+				PointList pointList = new PointList();
 				if(allDirections != null) {
 					for(int i = 0; i < allDirections.length(); i++) {
 						try {
@@ -123,25 +123,12 @@ public class OnlineMapQuestMovement extends AbstractLocalMovementStrategy {
 						}
 					}
 				}
-				movementPoints.put(comp.hashCode(), new RealWorldMovementPoints(comp.getRealPosition(), destination, pointList, 0));
-			}
-			else {
-				pointList = movementPoints.get(comp.hashCode()).getPointList();
+				trajectory = new RealWorldMovementPoints(comp.getRealPosition(), destination, pointList);
+				movementPoints.put(comp, trajectory);
 			}
 			
-			int actualIndex = movementPoints.get(comp.hashCode()).getActualIndex();
-			int i = 0;
-			for(GHPoint3D temp : pointList) {
-				if(i==actualIndex) {
-					PositionVector nextPoint = transformGPSWindowToOwnWorld(temp.getLat(), temp.getLon());
-					newPosition = comp.getRealPosition().moveStep(nextPoint, getMovementSpeed(comp));
-					
-					if(nextPoint.distanceTo(comp.getRealPosition()) < maxDistanceNextPoint) actualIndex++;
-				}
-				i++;
-			}
+			newPosition = trajectory.updateCurrentLocation(comp, getMovementSpeed(comp), 1.0);
 			
-			movementPoints.put(comp.hashCode(), new RealWorldMovementPoints(movementPoints.get(comp.hashCode()).getStart(), destination, pointList, actualIndex));
 		}
 		return new Left<PositionVector, Boolean>(newPosition);
 	}
@@ -159,18 +146,6 @@ public class OnlineMapQuestMovement extends AbstractLocalMovementStrategy {
 		return gps_coordinates;
 	}
 	
-	/**
-	 * Projects the gps coordinates in the given gps window to the world-coordinates given in world-dimensions
-	 * @param lat
-	 * @param lon
-	 * @return The projected position in world-dimensions
-	 */
-	private PositionVector transformGPSWindowToOwnWorld(double lat, double lon) {
-		double x = worldDimensions.getX() * (lon - lonLeft)/(lonRight - lonLeft);
-		double y = worldDimensions.getY() - worldDimensions.getY() * (lat - latLeft)/(latRight - latLeft);
-		return new PositionVector(x, y);
-	}
-	
 	public void setMovementType(String movementType) {
 		this.movementType = movementType;
 	}

src/de/tud/kom/p2psim/impl/topology/movement/local/RealWorldMovementPoints.java

@@ -20,43 +20,91 @@
 
 package de.tud.kom.p2psim.impl.topology.movement.local;
 
+import java.util.LinkedList;
+import java.util.List;
+
 import com.graphhopper.util.PointList;
+import com.graphhopper.util.shapes.GHPoint3D;
 
+import de.tud.kom.p2psim.api.topology.movement.SimLocationActuator;
 import de.tud.kom.p2psim.impl.topology.PositionVector;
 
+/**
+ * This class contains one trajectory from a start to a destination. It saves
+ * the respective points (coordinates) along the path with a given movement
+ * speed and also (if enabled) the assigned IDs of the roads and walkways that
+ * are traversed.
+ * 
+ * @author Bjoern Richerzhagen
+ * @version 1.0, Aug 9, 2017
+ */
 public class RealWorldMovementPoints {
 
 	private PositionVector start;
+
 	private PositionVector destination;
+
 	private PointList pointList;
-	private int actualIndex;
 	
-	public RealWorldMovementPoints(PositionVector start, PositionVector destination, PointList pointList, int actualIndex) {
+	private final List<PositionVector> positionsList;
+
+	private int currentIndex;
+	
+	private int routeLength;
+
+	public RealWorldMovementPoints(PositionVector start,
+			PositionVector destination, PointList pointList) {
 		this.start = start;
 		this.destination = destination;
 		this.pointList = pointList;
-		this.actualIndex = actualIndex;
+		this.positionsList = new LinkedList<>();
+		// Create Position vectors ONCE
+		for (GHPoint3D temp : pointList) {
+			positionsList.add(RealWorldStreetsMovement.transformGPSWindowToOwnWorld(temp.getLat(), temp.getLon()));
+		}
+		routeLength = positionsList.size();
 	}
 	
 	public PositionVector getStart() {
 		return start;
 	}
-	public void setStart(PositionVector start) {
-		this.start = start;
-	}
+
 	public PositionVector getDestination() {
 		return destination;
 	}
-	public void setDestination(PositionVector destination) {
-		this.destination = destination;
+	
+	/**
+	 * Update the internal location with the given movement speed
+	 * 
+	 * @param speed
+	 * @param realPosition
+	 * @return
+	 */
+	public PositionVector updateCurrentLocation(SimLocationActuator comp, double speed, double tolerance) {
+		List<PositionVector> sublist = positionsList.subList(currentIndex, routeLength);
+		PositionVector newPosition = null;
+		for (PositionVector candidate : sublist) {
+			newPosition = comp.getRealPosition().moveStep(candidate, speed);
+			if (candidate
+					.distanceTo(comp.getRealPosition()) < tolerance) {
+				currentIndex++;
+			} else {
+				break;
+			}
+		}
+		return newPosition;
+	}
+	
+	@Override
+	public String toString() {
+		return "\n\tfrom "+start.toString()+" to "+destination.toString()+ "\n\tstep: "+currentIndex + " length: "+routeLength;
 	}
+
 	public PointList getPointList() {
 		return pointList;
 	}
-	public void setPointList(PointList pointList) {
-		this.pointList = pointList;
-	}
-	public int getActualIndex() {
-		return this.actualIndex;
+
+	public int getCurrentIndex() {
+		return this.currentIndex;
 	}
 }

src/de/tud/kom/p2psim/impl/topology/movement/local/RealWorldStreetsMovement.java

@@ -23,8 +23,12 @@ package de.tud.kom.p2psim.impl.topology.movement.local;
 import com.graphhopper.GHRequest;
 import com.graphhopper.GHResponse;
 import com.graphhopper.GraphHopper;
+import com.graphhopper.routing.util.EdgeFilter;
 import com.graphhopper.routing.util.EncodingManager;
+import com.graphhopper.storage.index.LocationIndex;
+import com.graphhopper.storage.index.QueryResult;
 import com.graphhopper.util.DistanceCalc2D;
+import com.graphhopper.util.EdgeIteratorState;
 import com.graphhopper.util.PointList;
 import com.graphhopper.util.shapes.GHPoint;
 import com.graphhopper.util.shapes.GHPoint3D;
@@ -52,8 +56,9 @@ import java.util.*;
  */
 public class RealWorldStreetsMovement extends AbstractLocalMovementStrategy {
 	
-	private PositionVector worldDimensions;
+	private static PositionVector worldDimensions;
 	private GraphHopper hopper;
+	private LocationIndex index;
 	private boolean init = false;
 	
 	private static HashMap<SimLocationActuator, RealWorldMovementPoints> movementPoints = new HashMap<>();
@@ -63,10 +68,10 @@ public class RealWorldStreetsMovement extends AbstractLocalMovementStrategy {
 	private String movementType; //car, bike or foot
 	private String defaultMovement;
 	private String navigationalType; //fastest,
-	private double latLower; //Values from -90 to 90; always smaller than latUpper
-	private double latUpper; //Values from -90 to 90
-	private double lonLeft; //Values from -180 to 180; Always smaller than lonRight
-	private double lonRight; //Values from -180 to 180
+	private static double latLower; //Values from -90 to 90; always smaller than latUpper
+	private static double latUpper; //Values from -90 to 90
+	private static double lonLeft; //Values from -180 to 180; Always smaller than lonRight
+	private static double lonRight; //Values from -180 to 180
 	private boolean uniqueFolders;
 
 	/**
@@ -76,7 +81,7 @@ public class RealWorldStreetsMovement extends AbstractLocalMovementStrategy {
 	private double tolerance = 1;
 
 	public RealWorldStreetsMovement() {
-		this.worldDimensions = Binder.getComponentOrNull(Topology.class)
+		worldDimensions = Binder.getComponentOrNull(Topology.class)
 				.getWorldDimensions();
 		latLower = GPSCalculation.getLatLower();
 		latUpper = GPSCalculation.getLatUpper();
@@ -101,6 +106,7 @@ public class RealWorldStreetsMovement extends AbstractLocalMovementStrategy {
 		}
 		hopper.setEncodingManager(new EncodingManager(movementType));
 		hopper.importOrLoad();
+		index = hopper.getLocationIndex();
 
 		init = true;
 	}
@@ -121,8 +127,8 @@ public class RealWorldStreetsMovement extends AbstractLocalMovementStrategy {
         if (destination.distanceTo(comp.getRealPosition()) > getMovementSpeed(comp))
         {
 			//if not set already for this node or new destination is different than last one
-			PointList pointList;
-			if(!movementPoints.containsKey(comp) || destination.distanceTo(movementPoints.get(comp).getDestination()) > 1.0) {
+			RealWorldMovementPoints trajectory = movementPoints.get(comp);
+			if(trajectory == null || destination.distanceTo(trajectory.getDestination()) > 1.0) {
 				double[] startPosition = transformOwnWorldWindowToGPS(comp.getRealPosition().getX(), comp.getRealPosition().getY());
 				double[] destinationPosition = transformOwnWorldWindowToGPS(destination.getX(), destination.getY());
 				GHRequest req = new GHRequest(startPosition[0], startPosition[1], destinationPosition[0], destinationPosition[1]).
@@ -136,35 +142,31 @@ public class RealWorldStreetsMovement extends AbstractLocalMovementStrategy {
 							"%f,%f), destination (%f,%f) and type %s failed with error: %s.", comp.getHost().getId().valueAsString(),startPosition[0], startPosition[1],
 							destinationPosition[0], destinationPosition[1], movementType, rsp.getErrors());
 					
-					pointList = new PointList();
+					PointList pointList = new PointList();
 					pointList.add(new GHPoint(destinationPosition[0], destinationPosition[1]));
-					movementPoints.put(comp, new RealWorldMovementPoints(comp.getRealPosition(), destination, pointList, 0));
+					trajectory = new RealWorldMovementPoints(comp.getRealPosition(), destination, pointList);
+					movementPoints.put(comp, trajectory);
+				} else {
+					PointList pointList = rsp.getBest().getPoints();
+					trajectory = new RealWorldMovementPoints(comp.getRealPosition(), destination, pointList);
+					/*
+					 * TODO obtain route IDs along the path.
+					 */
+//					for (GHPoint point : pointList) {
+//						QueryResult qr = index.findClosest(point.getLat(), point.getLon(), EdgeFilter.ALL_EDGES);
+//						EdgeIteratorState nearEdge = qr.getClosestEdge();
+//						nearEdge.
+//						int channelId = -1;
+//						if (nearEdge != null) {
+//							channelId = nearEdge.getEdge();
+//						}
+//						currentRoute.add(new PointIdTuple(point, channelId));
+//					}
+						
+					movementPoints.put(comp, trajectory);
 				}
-				else {
-					pointList = rsp.getBest().getPoints();
-					movementPoints.put(comp, new RealWorldMovementPoints(comp.getRealPosition(), destination, pointList, 0));
-				}
-			}
-			else {
-				pointList = movementPoints.get(comp).getPointList();
-			}
-			
-			int actualIndex = movementPoints.get(comp).getActualIndex();
-			int i = 0;
-			for(GHPoint3D temp : pointList) {
-				if(i==actualIndex) {
-					PositionVector nextPoint = transformGPSWindowToOwnWorld(temp.getLat(), temp.getLon());
-					newPosition = comp.getRealPosition().moveStep(nextPoint, getMovementSpeed(comp));
-					
-					if (nextPoint
-							.distanceTo(comp.getRealPosition()) < tolerance) {
-						actualIndex++;
-					}
-				}
-				i++;
 			}
-			
-			movementPoints.put(comp, new RealWorldMovementPoints(movementPoints.get(comp).getStart(), destination, pointList, actualIndex));
+			newPosition = trajectory.updateCurrentLocation(comp, getMovementSpeed(comp), tolerance);
 		}
 		return new Left<PositionVector, Boolean>(newPosition);
 	}
@@ -188,7 +190,7 @@ public class RealWorldStreetsMovement extends AbstractLocalMovementStrategy {
 	 * @param lon
 	 * @return The projected position in world-dimensions
 	 */
-	private PositionVector transformGPSWindowToOwnWorld(double lat, double lon) {
+	public static PositionVector transformGPSWindowToOwnWorld(double lat, double lon) {
 		double x = worldDimensions.getX() * (lon - lonLeft)/(lonRight - lonLeft);
 		double y = worldDimensions.getY() - worldDimensions.getY() * (lat - latLower)/(latUpper - latLower);
 		x = Math.max(0, x);