diff --git a/OsmAnd-java/src/net/osmand/router/RoutePlannerFrontEnd.java b/OsmAnd-java/src/net/osmand/router/RoutePlannerFrontEnd.java index 4c03aa413c..ea5fa7da5a 100644 --- a/OsmAnd-java/src/net/osmand/router/RoutePlannerFrontEnd.java +++ b/OsmAnd-java/src/net/osmand/router/RoutePlannerFrontEnd.java @@ -11,6 +11,7 @@ import net.osmand.binary.BinaryMapRouteReaderAdapter; import net.osmand.binary.BinaryMapRouteReaderAdapter.RouteRegion; import net.osmand.binary.RouteDataObject; import net.osmand.data.LatLon; +import net.osmand.data.QuadPoint; import net.osmand.router.BinaryRoutePlanner.RouteSegment; import net.osmand.util.MapUtils; @@ -25,20 +26,7 @@ public class RoutePlannerFrontEnd { this.useOldVersion = useOldVersion; } - private static double squareRootDist(int x1, int y1, int x2, int y2) { - // translate into meters - double dy = MapUtils.convert31YToMeters(y1, y2); - double dx = MapUtils.convert31XToMeters(x1, x2); - return Math.sqrt(dx * dx + dy * dy); -// return measuredDist(x1, y1, x2, y2); - } - private static double calculateProjection(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) + - MapUtils.convert31YToMeters(yB, yA) * MapUtils.convert31YToMeters(yC, yA); - return multiple; - } private static double squareDist(int x1, int y1, int x2, int y2) { // translate into meters double dy = MapUtils.convert31YToMeters(y1, y2); @@ -56,38 +44,18 @@ public class RoutePlannerFrontEnd { } RouteSegment road = null; double sdist = 0; - int foundProjX = 0; - int foundProjY = 0; for (RouteDataObject r : dataObjects) { if (r.getPointsLength() > 1) { for (int j = 1; j < r.getPointsLength(); j++) { - double mDist = squareRootDist(r.getPoint31XTile(j), r.getPoint31YTile(j), r.getPoint31XTile(j - 1), - r.getPoint31YTile(j - 1)); - int prx = r.getPoint31XTile(j); - int pry = r.getPoint31YTile(j); - double projection = calculateProjection(r.getPoint31XTile(j - 1), r.getPoint31YTile(j - 1), r.getPoint31XTile(j), - r.getPoint31YTile(j), px, py); - if (projection < 0) { - prx = r.getPoint31XTile(j - 1); - pry = r.getPoint31YTile(j - 1); - } else if (projection >= mDist * mDist) { - prx = r.getPoint31XTile(j); - pry = r.getPoint31YTile(j); - } else { - prx = (int) (r.getPoint31XTile(j - 1) + (r.getPoint31XTile(j) - r.getPoint31XTile(j - 1)) - * (projection / (mDist * mDist))); - pry = (int) (r.getPoint31YTile(j - 1) + (r.getPoint31YTile(j) - r.getPoint31YTile(j - 1)) - * (projection / (mDist * mDist))); - } - double currentsDist = squareDist(prx, pry, px, py); + QuadPoint pr = MapUtils.getProjectionPoint31(px, py, r.getPoint31XTile(j - 1), + r.getPoint31YTile(j - 1), r.getPoint31XTile(j ), r.getPoint31YTile(j )); + double currentsDist = squareDist((int) pr.x, (int)pr.y, px, py); if (road == null || currentsDist < sdist) { RouteDataObject ro = new RouteDataObject(r); road = new RouteSegment(ro, j); - ro.insert(j, prx, pry); + ro.insert(j, (int) pr.x, (int)pr.y); sdist = currentsDist; - foundProjX = prx; - foundProjY = pry; } } } @@ -175,7 +143,7 @@ public class RoutePlannerFrontEnd { ctx.calculationProgress.distanceFromEnd = 0; ctx.calculationProgress.reverseSegmentQueueSize = 0; ctx.calculationProgress.directSegmentQueueSize = 0; - float rd = (float) squareRootDist(ctx.startX, ctx.startY, ctx.targetX, ctx.targetY); + float rd = (float) MapUtils.squareRootDist31(ctx.startX, ctx.startY, ctx.targetX, ctx.targetY); float speed = 0.9f * ctx.config.router.getMaxDefaultSpeed(); ctx.calculationProgress.totalEstimatedDistance = (float) (rd / speed); } diff --git a/OsmAnd-java/src/net/osmand/util/MapUtils.java b/OsmAnd-java/src/net/osmand/util/MapUtils.java index c1a755900a..4da7658df3 100644 --- a/OsmAnd-java/src/net/osmand/util/MapUtils.java +++ b/OsmAnd-java/src/net/osmand/util/MapUtils.java @@ -7,6 +7,7 @@ import java.util.List; import net.osmand.data.LatLon; import net.osmand.data.MapObject; +import net.osmand.data.QuadPoint; /** @@ -421,9 +422,41 @@ public class MapUtils { } + public static QuadPoint getProjectionPoint31(int px, int py, int st31x, int st31y,int end31x, int end31y) { + double mDist = squareRootDist31(end31x, end31y, st31x, + st31y); + int prx = end31x; + int pry = end31y; + double projection = calculateProjection31TileMetric(st31x, st31y, end31x, + end31y, px, py); + if (projection < 0) { + prx = st31x; + pry = st31y; + } else if (projection >= mDist * mDist) { + prx = end31x; + pry = end31y; + } else { + prx = (int) (st31x + (end31x - st31x) + * (projection / (mDist * mDist))); + pry = (int) (st31y + (end31y - st31y) + * (projection / (mDist * mDist))); + } + return new QuadPoint(prx, pry); + } + + + public static double squareRootDist31(int x1, int y1, int x2, int y2) { + // translate into meters + double dy = MapUtils.convert31YToMeters(y1, y2); + double dx = MapUtils.convert31XToMeters(x1, x2); + return Math.sqrt(dx * dx + dy * dy); +// return measuredDist(x1, y1, x2, y2); + } + public static double calculateProjection31TileMetric(int xA, int yA, int xB, int yB, int xC, int yC) { // Scalar multiplication between (AB, AC) - double multiple = convert31XToMeters(xB, xA) * convert31XToMeters(xC, xA) + convert31YToMeters(yB, yA) * convert31YToMeters(yC, yA); + double multiple = MapUtils.convert31XToMeters(xB, xA) * MapUtils.convert31XToMeters(xC, xA) + + MapUtils.convert31YToMeters(yB, yA) * MapUtils.convert31YToMeters(yC, yA); return multiple; } diff --git a/OsmAnd/src/net/osmand/plus/OsmAndLocationProvider.java b/OsmAnd/src/net/osmand/plus/OsmAndLocationProvider.java index bfda046fbf..5640fb742d 100644 --- a/OsmAnd/src/net/osmand/plus/OsmAndLocationProvider.java +++ b/OsmAnd/src/net/osmand/plus/OsmAndLocationProvider.java @@ -9,6 +9,7 @@ import java.util.List; import net.osmand.GeoidAltitudeCorrection; import net.osmand.PlatformUtil; import net.osmand.access.NavigationInfo; +import net.osmand.binary.BinaryMapDataObject; import net.osmand.binary.RouteDataObject; import net.osmand.data.LatLon; import net.osmand.plus.OsmandSettings.OsmandPreference; @@ -94,14 +95,43 @@ public class OsmAndLocationProvider implements SensorEventListener { private Listener gpsStatusListener; private float[] mRotationM = new float[9]; private OsmandPreference USE_MAGNETIC_FIELD_SENSOR_COMPASS; - private OsmandPreference uSE_FILTER_FOR_COMPASS; + private OsmandPreference USE_FILTER_FOR_COMPASS; + + + private class SimulationSegment { + private List simulateRoads = new ArrayList(); + private float currentSimulationLength = 0; + private long simulationStarted = 0; + private float simulationSpeedMS = 0; + + + public void startSimulation(List roads, + Location currentLocation) { + simulationStarted = currentLocation.getTime(); + simulationSpeedMS = currentLocation.getSpeed(); + double orthDistance = 10000; + int currentRoad = 0; + int currentSegment = 0; + for(int i = 0; i < roads.size(); i++) { + BinaryMapDataObject road = roads.get(i); + for(int j = 1; j < road.getPointsLength(); j++) { + + } + } + + } + + public boolean isSimulatedDataAvailable() { + return simulationStarted > 0 && simulationSpeedMS > 0; + } + } public OsmAndLocationProvider(OsmandApplication app) { this.app = app; navigationInfo = new NavigationInfo(app); settings = app.getSettings(); USE_MAGNETIC_FIELD_SENSOR_COMPASS = settings.USE_MAGNETIC_FIELD_SENSOR_COMPASS; - uSE_FILTER_FOR_COMPASS = settings.USE_KALMAN_FILTER_FOR_COMPASS; + USE_FILTER_FOR_COMPASS = settings.USE_KALMAN_FILTER_FOR_COMPASS; currentPositionHelper = new CurrentPositionHelper(app); locationSimulation = new OsmAndLocationSimulation(app, this); } @@ -320,7 +350,7 @@ public class OsmAndLocationProvider implements SensorEventListener { lastValSin = (float) Math.sin(valRad); lastValCos = (float) Math.cos(valRad); // lastHeadingCalcTime = System.currentTimeMillis(); - boolean filter = uSE_FILTER_FOR_COMPASS.get(); //USE_MAGNETIC_FIELD_SENSOR_COMPASS.get(); + boolean filter = USE_FILTER_FOR_COMPASS.get(); //USE_MAGNETIC_FIELD_SENSOR_COMPASS.get(); if (filter) { filterCompassValue(); } else {