From 6c7cd3d3c295bdf4db4c75317309f9d12085b02e Mon Sep 17 00:00:00 2001 From: vshcherb Date: Tue, 29 Oct 2013 21:13:57 +0100 Subject: [PATCH] Simulate position in tunnels --- .../net/osmand/binary/RouteDataObject.java | 14 ++ .../net/osmand/router/RouteSegmentResult.java | 2 +- OsmAnd-java/src/net/osmand/util/MapUtils.java | 13 +- .../osmand/plus/OsmAndLocationProvider.java | 129 +++++++++++++----- .../plus/routing/RouteCalculationResult.java | 39 ++++++ .../osmand/plus/routing/RoutingHelper.java | 6 +- 6 files changed, 163 insertions(+), 40 deletions(-) diff --git a/OsmAnd-java/src/net/osmand/binary/RouteDataObject.java b/OsmAnd-java/src/net/osmand/binary/RouteDataObject.java index 263adec68e..798c53343f 100644 --- a/OsmAnd-java/src/net/osmand/binary/RouteDataObject.java +++ b/OsmAnd-java/src/net/osmand/binary/RouteDataObject.java @@ -176,6 +176,20 @@ public class RouteDataObject { return false; } + public boolean tunnel(){ + int sz = types.length; + for(int i=0; i[] old = this.attachedRoutes ; + List[] old = this.attachedRoutes; this.attachedRoutes = new List[capacity]; if(old != null){ System.arraycopy(old, 0, this.attachedRoutes, 0, Math.min(old.length, this.attachedRoutes.length)); diff --git a/OsmAnd-java/src/net/osmand/util/MapUtils.java b/OsmAnd-java/src/net/osmand/util/MapUtils.java index e8ea99d425..d4cfdaa1ac 100644 --- a/OsmAnd-java/src/net/osmand/util/MapUtils.java +++ b/OsmAnd-java/src/net/osmand/util/MapUtils.java @@ -405,12 +405,6 @@ public class MapUtils { } - public static double squareDist31TileMetric(int x1, int y1, int x2, int y2) { - // translate into meters - double dy = convert31YToMeters(y1, y2); - double dx = convert31XToMeters(x1, x2); - return dx * dx + dy * dy; - } public static double convert31YToMeters(float y1, float y2) { // translate into meters return (y1 - y2) * 0.01863d; @@ -453,6 +447,13 @@ public class MapUtils { // return measuredDist(x1, y1, x2, y2); } + public static double squareDist31TileMetric(int x1, int y1, int x2, int y2) { + // translate into meters + double dy = convert31YToMeters(y1, y2); + double dx = convert31XToMeters(x1, x2); + return dx * dx + dy * dy; + } + public static double calculateProjection31TileMetric(int xA, int yA, int xB, int yB, int xC, int yC) { // Scalar multiplication between (AB, AC) double multiple = MapUtils.convert31XToMeters(xB, xA) * MapUtils.convert31XToMeters(xC, xA) + diff --git a/OsmAnd/src/net/osmand/plus/OsmAndLocationProvider.java b/OsmAnd/src/net/osmand/plus/OsmAndLocationProvider.java index d1f6fa7277..8926cda133 100644 --- a/OsmAnd/src/net/osmand/plus/OsmAndLocationProvider.java +++ b/OsmAnd/src/net/osmand/plus/OsmAndLocationProvider.java @@ -15,6 +15,7 @@ import net.osmand.data.LatLon; import net.osmand.data.QuadPoint; import net.osmand.plus.OsmandSettings.OsmandPreference; import net.osmand.plus.routing.RoutingHelper; +import net.osmand.router.RouteSegmentResult; import net.osmand.util.MapUtils; import android.content.Context; import android.hardware.GeomagneticField; @@ -43,8 +44,11 @@ public class OsmAndLocationProvider implements SensorEventListener { } private static final int INTERVAL_TO_CLEAR_SET_LOCATION = 30 * 1000; - private static final int LOST_LOCATION_MSG_ID = 10; + private static final int LOST_LOCATION_MSG_ID = 53; + private static final int START_SIMULATE_LOCATION_MSG_ID = 54; + private static final int RUN_SIMULATE_LOCATION_MSG_ID = 55; private static final long LOST_LOCATION_CHECK_DELAY = 18000; + private static final long START_LOCATION_SIMULATION_DELAY = 4000; private static final float ACCURACY_FOR_GPX_AND_ROUTING = 50; @@ -54,6 +58,7 @@ public class OsmAndLocationProvider implements SensorEventListener { private long lastTimeGPSLocationFixed = 0; private boolean gpsSignalLost; + private SimulationProvider simulatePosition = null; private boolean sensorRegistered = false; private float[] mGravs = new float[3]; @@ -105,19 +110,17 @@ public class OsmAndLocationProvider implements SensorEventListener { return dx * dx + dy * dy; } - public class SimulationSegment { - private List simulateRoads = new ArrayList(); - private float currentSimulationLength = 0; + public class SimulationProvider { private int currentRoad; private int currentSegment; - private double currentProjection; + private QuadPoint currentPoint; private net.osmand.Location startLocation; - private List roads; + private List roads; - public void startSimulation(List roads, + public void startSimulation(List roads, net.osmand.Location currentLocation) { - roads = roads; + this.roads = roads; startLocation = new net.osmand.Location(currentLocation); long ms = System.currentTimeMillis(); if(ms - startLocation.getTime() > 5000 || @@ -129,54 +132,74 @@ public class OsmAndLocationProvider implements SensorEventListener { int py = MapUtils.get31TileNumberY(currentLocation.getLatitude()); double dist = 1000; for(int i = 0; i < roads.size(); i++) { - BinaryMapDataObject road = roads.get(i); - for(int j = 1; j < road.getPointsLength(); j++) { - QuadPoint proj = MapUtils.getProjectionPoint31(px, py, road.getPoint31XTile(j-1), road.getPoint31YTile(j-1), - road.getPoint31XTile(j), road.getPoint31YTile(j)); + RouteSegmentResult road = roads.get(i); + boolean plus = road.getStartPointIndex() < road.getEndPointIndex(); + for(int j = road.getStartPointIndex() + 1; j <= road.getEndPointIndex(); ) { + RouteDataObject obj = road.getObject(); + QuadPoint proj = MapUtils.getProjectionPoint31(px, py, obj.getPoint31XTile(j-1), obj.getPoint31YTile(j-1), + obj.getPoint31XTile(j), obj.getPoint31YTile(j)); double dd = squareDist((int)proj.x, (int)proj.y, px, py); if (dd < dist) { dist = dd; currentRoad = i; currentSegment = j; - currentProjection = MapUtils.calculateProjection31TileMetric(road.getPoint31XTile(j-1), road.getPoint31YTile(j-1), - road.getPoint31XTile(j), road.getPoint31YTile(j), px, py); + currentPoint = proj; } + j += plus ? 1 : -1; } } } - private void proceedMeters() { - /*for(int i = 0; i < roads.size(); i++) { - BinaryMapDataObject road = roads.get(i); - for(int j = 1; j < road.getPointsLength(); j++) { - QuadPoint proj = MapUtils.getProjectionPoint31(px, py, road.getPoint31XTile(j-1), road.getPoint31YTile(j-1), - road.getPoint31XTile(j), road.getPoint31YTile(j)); - double dd = squareDist((int)proj.x, (int)proj.y, px, py); - if (dd < dist) { - dist = dd; - currentRoad = i; - currentSegment = j; - currentProjection = MapUtils.calculateProjection31TileMetric(road.getPoint31XTile(j-1), road.getPoint31YTile(j-1), - road.getPoint31XTile(j), road.getPoint31YTile(j), px, py); + private float proceedMeters(float meters, net.osmand.Location l) { + for(int i = currentRoad; i < roads.size(); i++) { + RouteSegmentResult road = roads.get(i); + boolean firstRoad = i == currentRoad; + boolean plus = road.getStartPointIndex() < road.getEndPointIndex(); + for(int j = firstRoad ? currentSegment : road.getStartPointIndex() + 1; j <= road.getEndPointIndex(); ) { + RouteDataObject obj = road.getObject(); + int st31x = obj.getPoint31XTile(j-1); + int st31y = obj.getPoint31YTile(j-1); + int end31x = obj.getPoint31XTile(j); + int end31y = obj.getPoint31YTile(j); + boolean last = i == roads.size() - 1 && j == road.getEndPointIndex(); + boolean first = firstRoad && j == currentSegment; + if(first) { + st31x = (int) currentPoint.x; + st31y = (int) currentPoint.y; } + double dd = MapUtils.squareRootDist31(st31x, st31y, end31x, end31y); + if(meters > dd && !last){ + meters -= dd; + } else { + int prx = (int) (st31x + (end31x - st31x) * (meters / dd)); + int pry = (int) (st31y + (end31y - st31y) * (meters / dd)); + l.setLongitude(MapUtils.get31LongitudeX(prx)); + l.setLatitude(MapUtils.get31LatitudeY(pry)); + return (float) Math.max(meters - dd, 0); + } + j += plus ? 1 : -1; } - }*/ + } + return -1; } /** * @return null if it is not available of far from boundaries */ - public Location getSimulatedLocation() { + public net.osmand.Location getSimulatedLocation() { if(!isSimulatedDataAvailable()) { return null; } - Location loc = new Location("OsmAnd"); + net.osmand.Location loc = new net.osmand.Location("OsmAnd"); loc.setSpeed(startLocation.getSpeed()); loc.setAltitude(startLocation.getAltitude()); loc.setTime(System.currentTimeMillis()); float meters = (System.currentTimeMillis() - startLocation.getTime()) / startLocation.getSpeed(); - + float proc = proceedMeters(meters, loc); + if(proc < 0 || proc >= 100){ + return null; + } return loc; } @@ -621,7 +644,7 @@ public class OsmAndLocationProvider implements SensorEventListener { } - private void scheduleCheckIfGpsLost(net.osmand.Location location) { + private void scheduleCheckIfGpsLost(final net.osmand.Location location) { final RoutingHelper routingHelper = app.getRoutingHelper(); if (location != null) { final long fixTime = location.getTime(); @@ -641,8 +664,47 @@ public class OsmAndLocationProvider implements SensorEventListener { setLocation(null); } }, LOST_LOCATION_CHECK_DELAY); + if (routingHelper.isFollowingMode() && routingHelper.getLeftDistance() > 0 && simulatePosition == null) { + app.runMessageInUIThreadAndCancelPrevious(START_SIMULATE_LOCATION_MSG_ID, new Runnable() { + + @Override + public void run() { + net.osmand.Location lastKnown = getLastKnownLocation(); + if (lastKnown != null && lastKnown.getTime() > fixTime) { + // false positive case, still strange how we got here with removeMessages + return; + } + List tunnel = routingHelper.getUpcomingTunnel(1000); + if(tunnel != null) { + simulatePosition = new SimulationProvider(); + simulatePosition.startSimulation(tunnel, location); + simulatePosition(); + } + } + }, START_LOCATION_SIMULATION_DELAY); + } } } + + public void simulatePosition() { + app.runMessageInUIThreadAndCancelPrevious(RUN_SIMULATE_LOCATION_MSG_ID, new Runnable() { + + @Override + public void run() { + if(simulatePosition != null){ + net.osmand.Location loc = simulatePosition.getSimulatedLocation(); + if(loc != null){ + setLocation(loc); + simulatePosition(); + } else { + simulatePosition = null; + } + } + } + }, 1000); + } + + public void setLocationFromService(net.osmand.Location location, boolean continuous) { // if continuous notify about lost location if (continuous) { @@ -664,6 +726,9 @@ public class OsmAndLocationProvider implements SensorEventListener { if(location == null){ updateGPSInfo(null); } + if(location != null) { + simulatePosition = null; + } enhanceLocation(location); scheduleCheckIfGpsLost(location); final RoutingHelper routingHelper = app.getRoutingHelper(); diff --git a/OsmAnd/src/net/osmand/plus/routing/RouteCalculationResult.java b/OsmAnd/src/net/osmand/plus/routing/RouteCalculationResult.java index b4341fc08e..861c621a1a 100644 --- a/OsmAnd/src/net/osmand/plus/routing/RouteCalculationResult.java +++ b/OsmAnd/src/net/osmand/plus/routing/RouteCalculationResult.java @@ -678,6 +678,45 @@ public class RouteCalculationResult { return null; } + public List getUpcomingTunnel(float distToStart) { + int cs = currentRoute > 0 ? currentRoute - 1 : 0; + if(cs < segments.size()) { + RouteSegmentResult prev = null; + boolean tunnel = false; + while(cs < segments.size() && distToStart > 0) { + RouteSegmentResult segment = segments.get(cs); + if(segment != prev ) { + if(segment.getObject().tunnel()){ + tunnel = true; + break; + } else { + distToStart -= segment.getDistance(); + prev = segment; + } + } + cs++; + } + if(tunnel) { + List list = new ArrayList(); + while(cs < segments.size()) { + RouteSegmentResult segment = segments.get(cs); + if(segment != prev ) { + if(segment.getObject().tunnel()) { + list.add(segment); + } else { + break; + } + prev = segment; + } + cs++; + } + return list; + } + } + + return null; + } + public float getCurrentMaxSpeed() { RouteSegmentResult res = getCurrentSegmentResult(); if(res != null) { diff --git a/OsmAnd/src/net/osmand/plus/routing/RoutingHelper.java b/OsmAnd/src/net/osmand/plus/routing/RoutingHelper.java index a057219633..187ea23cbe 100644 --- a/OsmAnd/src/net/osmand/plus/routing/RoutingHelper.java +++ b/OsmAnd/src/net/osmand/plus/routing/RoutingHelper.java @@ -669,7 +669,11 @@ public class RoutingHelper { public RouteSegmentResult getCurrentSegmentResult() { return route.getCurrentSegmentResult(); } - + + public List getUpcomingTunnel(float distToStart) { + return route.getUpcomingTunnel(distToStart); + } + public synchronized NextDirectionInfo getNextRouteDirectionInfoAfter(NextDirectionInfo previous, NextDirectionInfo to, boolean toSpeak){ NextDirectionInfo i = route.getNextRouteDirectionInfoAfter(previous, to, toSpeak); if(i != null) {