Initial srtm routing

OsmAnd-java/src/net/osmand/binary/BinaryMapRouteReaderAdapter.java

@@ -218,6 +218,7 @@ public class BinaryMapRouteReaderAdapter {
 			return "Routing";
 		}
 		
+
 		public int getFieldNumber() {
 			return OsmandOdb.OsmAndStructure.ROUTINGINDEX_FIELD_NUMBER;
 		}

OsmAnd-java/src/net/osmand/binary/RouteDataObject.java

@@ -16,6 +16,7 @@ import net.sf.junidecode.Junidecode;
 public class RouteDataObject {
 	/*private */static final int RESTRICTION_SHIFT = 3;
 	/*private */static final int RESTRICTION_MASK = 7;
+	public static int HEIGHT_UNDEFINED = -80000;
 	
 	public final RouteRegion region;
 	// all these arrays supposed to be immutable!
@@ -31,6 +32,8 @@ public class RouteDataObject {
 	public TIntObjectHashMap<String> names;
 	public final static float NONE_MAX_SPEED = 40f;
 	public int[] nameIds;
+	// mixed array [0, height, cumulative_distance height, cumulative_distance, height, ...] - length is length(points)*2
+	public float[] heightDistanceArray = null;
 	
 	public RouteDataObject(RouteRegion region) {
 		this.region = region;
@@ -60,6 +63,73 @@ public class RouteDataObject {
 		this.id = copy.id;
 	}
 	
+	public float[] calculateHeightArray() {
+		if(heightDistanceArray != null) {
+			return heightDistanceArray;
+		}
+		int startHeight = Algorithms.parseIntSilently(getValue("osmand_ele_start"), HEIGHT_UNDEFINED);
+		int endHeight = Algorithms.parseIntSilently(getValue("osmand_ele_end"), startHeight);
+		if(startHeight == HEIGHT_UNDEFINED) {
+			heightDistanceArray = new float[0];
+			return heightDistanceArray;
+		}
+		
+		heightDistanceArray = new float[2*getPointsLength()]; 
+		double plon = 0;
+		double plat = 0;
+		int prevHeight = startHeight;
+		for(int k = 0; k < getPointsLength(); k++) {
+			double lon = MapUtils.get31LongitudeX(getPoint31XTile(k));
+			double lat = MapUtils.get31LatitudeY(getPoint31YTile(k));
+			if(k > 0) {
+				double dd = MapUtils.getDistance(plat, plon, lat, lon);
+				int height = HEIGHT_UNDEFINED;
+				if(k == getPointsLength() - 1) {
+					height = endHeight;
+				} else {
+					int[] tps = getPointTypes(k);
+					if (tps != null) {
+						for (int id : tps) {
+							RouteTypeRule rt = region.quickGetEncodingRule(id);
+							if (rt.getTag().equals("osmand_ele_asc")) {
+								height = (int) (prevHeight + Float.parseFloat(rt.getValue()));
+								break;
+							} else if (rt.getTag().equals("osmand_ele_desc")) {
+								height = (int) (prevHeight - Float.parseFloat(rt.getValue()));
+								break;
+							}
+						}
+					}
+				}
+				heightDistanceArray[2*k] = (float) dd;
+				heightDistanceArray[2*k+1] = height;
+				if(height != HEIGHT_UNDEFINED) {
+					// interpolate undefined
+					double totalDistance = dd;
+					int startUndefined = k;
+					while(startUndefined - 1 >= 0 && heightDistanceArray[2*(startUndefined - 1)+1] == HEIGHT_UNDEFINED) {
+						startUndefined --;
+						totalDistance += heightDistanceArray[2*(startUndefined)];
+					}
+					if(totalDistance > 0) {
+						double angle = (height - prevHeight) / totalDistance;
+						for(int j = startUndefined; j < k; j++) {
+							heightDistanceArray[2*j+1] = (float) ((heightDistanceArray[2*j] * angle) + heightDistanceArray[2*j-1]);
+						}
+					}
+					prevHeight = height;
+				}
+				
+			} else {
+				heightDistanceArray[0] = 0;
+				heightDistanceArray[1] = startHeight;
+			}
+			plat = lat;
+			plon = lon;
+		}
+		return heightDistanceArray;
+	}
+
 	public long getId() {
 		return id;
 	}

OsmAnd-java/src/net/osmand/router/BinaryRoutePlanner.java

@@ -445,10 +445,16 @@ public class BinaryRoutePlanner {
 			if (obstacle < 0) {
 				directionAllowed = false;
 				continue;
+			}
+			double heightObstacle = ctx.getRouter().defineHeightObstacle(road, !reverseWaySearch ? prevInd : segmentPoint, 
+					!reverseWaySearch ? segmentPoint : prevInd, segmentDist);
+			if(heightObstacle > 0) {
+				
 			}
 			boolean alreadyVisited = checkIfOppositeSegmentWasVisited(ctx, reverseWaySearch, graphSegments, segment, oppositeSegments,
 					segmentPoint, segmentDist, obstaclesTime);
 			obstaclesTime += obstacle;
+			obstaclesTime += heightObstacle;
 			if (alreadyVisited) {
 				directionAllowed = false;
 				continue;

OsmAnd-java/src/net/osmand/router/GeneralRouter.java

@@ -25,6 +25,7 @@ public class GeneralRouter implements VehicleRouter {
 	
 	private static final float CAR_SHORTEST_DEFAULT_SPEED = 55/3.6f;
 	public static final String USE_SHORTEST_WAY = "short_way";
+	public static final String USE_HEIGHT_OBSTACLES = "height_obstacles";
 	public static final String AVOID_FERRIES = "avoid_ferries";
 	public static final String AVOID_TOLL = "avoid_toll";
 	public static final String AVOID_MOTORWAY = "avoid_motorway";
@@ -39,6 +40,7 @@ public class GeneralRouter implements VehicleRouter {
 	private final Map<String, BitSet> tagRuleMask;
 	private final ArrayList<Object> ruleToValue;
 	private boolean shortestRoute;
+	private boolean heightObstacles;
 	
 	private Map<RouteRegion, Map<Integer, Integer>> regionConvert = new LinkedHashMap<RouteRegion, Map<Integer,Integer>>();
 	
@@ -129,6 +131,7 @@ public class GeneralRouter implements VehicleRouter {
 			objectAttributes[i] = new RouteAttributeContext(parent.objectAttributes[i], params);
 		}
 		shortestRoute = params.containsKey(USE_SHORTEST_WAY) && parseSilentBoolean(params.get(USE_SHORTEST_WAY), false);
+		heightObstacles = params.containsKey(USE_HEIGHT_OBSTACLES) && parseSilentBoolean(params.get(USE_HEIGHT_OBSTACLES), false); 
 		if(shortestRoute) {
 			maxDefaultSpeed = Math.min(CAR_SHORTEST_DEFAULT_SPEED, maxDefaultSpeed);
 		}
@@ -284,6 +287,39 @@ public class GeneralRouter implements VehicleRouter {
 		return 0;
 	}
 	
+	@Override
+	public double defineHeightObstacle(RouteDataObject road, short startIndex, short endIndex, float distance) {
+		if(!heightObstacles) {
+			return 0;
+		}
+		float[] heightArray = road.calculateHeightArray();
+		if(heightArray == null || heightArray.length == 0 ) {
+			return 0;
+		}
+		
+		double sum = 0;
+		int knext;
+		int[] types = new int[0];
+		RouteAttributeContext objContext = getObjContext(RouteDataObjectAttribute.OBSTACLE_SRTM_ALT_SPEED);
+		for(int k = startIndex; k != endIndex; k = knext) {
+			knext = startIndex < endIndex ? k + 1 : k - 1;
+			double dist = Math.abs(heightArray[2 * k] - heightArray[2 * knext]) ;
+			double diff = heightArray[2 * knext + 1] - heightArray[2 * k + 1] ;
+			if(diff > 0 && dist > 0) {
+				double incl = diff / dist;
+				int percentIncl = (int) (incl * 100);
+				percentIncl = (percentIncl + 2)/ 3 * 3 - 2; // 1, 4, 7, 10, .   
+				if(percentIncl > 0) {
+					// IMPROVEMENT: register with value and cache parsed value
+					objContext.paramContext.vars.put("incline", percentIncl + "");
+					sum += objContext.evaluateFloat(road.region, types, 0) * diff;
+				}
+			}
+		}
+		return sum;
+	}
+	
+	
 	@Override
 	public int isOneWay(RouteDataObject road) {
 		return getObjContext(RouteDataObjectAttribute.ONEWAY).evaluateInt(road, 0);
@@ -299,6 +335,7 @@ public class GeneralRouter implements VehicleRouter {
 		return Math.min(defineVehicleSpeed(road), maxDefaultSpeed);
 	}
 	
+	
 	@Override
 	public float defineVehicleSpeed(RouteDataObject road) {
 		return getObjContext(RouteDataObjectAttribute.ROAD_SPEED) .evaluateFloat(road, getMinDefaultSpeed());

OsmAnd-java/src/net/osmand/router/RouteSegmentResult.java

@@ -1,8 +1,6 @@
 package net.osmand.router;
 
 
-import gnu.trove.list.array.TIntArrayList;
-
 import java.util.ArrayList;
 import java.util.Collections;
 import java.util.List;
@@ -27,7 +25,6 @@ public class RouteSegmentResult {
 	private String description = "";
 	// this make not possible to make turns in between segment result for now
 	private TurnType turnType;
-	public static int HEIGHT_UNDEFINED = -80000;
 	
 	
 	public RouteSegmentResult(RouteDataObject object, int startPointIndex, int endPointIndex) {
@@ -38,70 +35,13 @@ public class RouteSegmentResult {
 	}
 	
 	public float[] getHeightValues() {
-		int startHeight = Algorithms.parseIntSilently(object.getValue("osmand_ele_start"), HEIGHT_UNDEFINED);
+		float[] pf = object.calculateHeightArray();
-		int endHeight = Algorithms.parseIntSilently(object.getValue("osmand_ele_end"), startHeight);
+		if(pf == null || pf.length == 0) {
-		if(startHeight == HEIGHT_UNDEFINED) {
 			return new float[0];
 		}
-		TIntArrayList list = new TIntArrayList();
-		float[] pf = new float[2*object.getPointsLength()]; 
-		double dist = 0;
-		double plon = 0;
-		double plat = 0;
-		int prevHeight = startHeight;
-		for(int k = 0; k < object.getPointsLength(); k++) {
-			double lon = MapUtils.get31LongitudeX(object.getPoint31XTile(k));
-			double lat = MapUtils.get31LatitudeY(object.getPoint31YTile(k));
-			if(k > 0) {
-				double dd = MapUtils.getDistance(plat, plon, lat, lon);
-				int height = HEIGHT_UNDEFINED;
-				if(k == object.getPointsLength() - 1) {
-					height = endHeight;
-				} else {
-					int[] tps = object.getPointTypes(k);
-					if (tps != null) {
-						for (int id : tps) {
-							RouteTypeRule rt = object.region.quickGetEncodingRule(id);
-							if (rt.getTag().equals("osmand_ele_asc")) {
-								height = (int) (prevHeight + Float.parseFloat(rt.getValue()));
-								break;
-							} else if (rt.getTag().equals("osmand_ele_desc")) {
-								height = (int) (prevHeight - Float.parseFloat(rt.getValue()));
-								break;
-							}
-						}
-					}
-				}
-				pf[2*k] = (float) dd;
-				pf[2*k+1] = height;
-				if(height != HEIGHT_UNDEFINED) {
-					// interpolate undefined
-					double totalDistance = dd;
-					int startUndefined = k;
-					while(startUndefined - 1 >= 0 && pf[2*(startUndefined - 1)+1] == HEIGHT_UNDEFINED) {
-						startUndefined --;
-						totalDistance += pf[2*(startUndefined)];
-					}
-					if(totalDistance > 0) {
-						double angle = (height - prevHeight) / totalDistance;
-						for(int j = startUndefined; j < k; j++) {
-							pf[2*j+1] = (float) ((pf[2*j] * angle) + pf[2*j-1]);
-						}
-					}
-					prevHeight = height;
-				}
-				
-			} else {
-				pf[0] = 0;
-				pf[1] = startHeight;
-			}
-			plat = lat;
-			plon = lon;
-		}
 		boolean reverse = startPointIndex > endPointIndex;
 		int st = Math.min(startPointIndex, endPointIndex);
 		int end = Math.max(startPointIndex, endPointIndex);
-		
 		float[] res = new float[(end - st + 1) * 2];
 		for (int k = 0; k < res.length / 2; k++) {
 			if (k == 0) {

OsmAnd-java/src/net/osmand/router/VehicleRouter.java

@@ -32,6 +32,11 @@ public interface VehicleRouter {
 	 */
 	public float defineObstacle(RouteDataObject road, int point);
 	
+	/**
+	 * return delay in seconds for height obstacles
+	 */
+	public double defineHeightObstacle(RouteDataObject road, short startIndex, short endIndex, float distance);
+	
 	/**
 	 * return delay in seconds (0 no obstacles)
 	 */
@@ -79,4 +84,6 @@ public interface VehicleRouter {
 		
 	public VehicleRouter build(Map<String, String> params);
 
+	
+	
 }