/*
* This program 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; version 3 of the License.
*
* This program 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.
*
* Author: Damian Waradzyn
*/
package com.mapmidlet.routing;
import henson.midp.Float11;
import java.util.Vector;
import com.mapmidlet.CloudGps;
import com.mapmidlet.gps.GpsState;
import com.mapmidlet.options.Options;
import com.mapmidlet.projection.*;
import com.mapmidlet.routing.Route.RouteDirection;
import com.mapmidlet.tile.ui.*;
/**
* This class is responsible for calculating optimized route and finding current
* turn-by-turn direction by current GPS location.
*
* Optimized route means route optimized for drawing purposes. This class
* removes not visible route segments and uses a simple algorithm to eliminate
* segments such that resulting route will be similar in shape to original but
* with significantly less segments.
*/
public class Navigation {
private static final int INTERNAL_ZOOM = 17;
private static final int INTERNAL_TILESIZE = 256;
private static double snapx;
private static double snapy;
public static void navigate(GpsState gpsState) {
Options options = Options.getInstance();
if (options.routingStatus == Options.ROUTING_CALCULATING) {
return;
}
optimizeWaypoints();
if (gpsState.state >= GpsState.CONNECTED_FIX && options.route != null) {
double distance = snapToRoute(gpsState);
if (distance > options.snapTolerance) {
if ((options.navContext.offRouteStartMilis > 0
&& System.currentTimeMillis() - options.navContext.offRouteStartMilis > options.offRouteRecalculateMilis && options.routingStatus != Options.ROUTING_CALCULATING)
|| distance > options.offRouteRecalculateMeters) {
// too long off road or too far away from route - need to
// calculate new route from current position
CloudGps.calculateRoute(true);
options.navContext.offRouteStartMilis = System.currentTimeMillis();
}
if (options.navContext.offRouteStartMilis < 0) {
options.navContext.offRouteStartMilis = System.currentTimeMillis();
}
} else {
options.navContext.offRouteStartMilis = -1;
}
}
}
// 1. determine visible route segments
// 2. if there are too many route try to remove some by merging
public static void optimizeWaypoints() {
TileCanvas canvas = CloudGps.getTileCanvas();
Options options = Options.getInstance();
OptimizedRoute or = options.navContext.optimizedRoute;
if (or != null && or.visibleSegments != null && or.zoom == options.zoom
&& or.horizontalTilesCount == canvas.horizontalTilesCount
&& or.verticalTilesCount == canvas.verticalTilesCount && or.latitude == canvas.latitude
&& or.longitude == canvas.longitude && or.tileSize == canvas.tileSize) {
return;
}
if (or == null) {
or = new OptimizedRoute();
options.navContext.optimizedRoute = or;
}
or.zoom = options.zoom;
or.horizontalTilesCount = canvas.horizontalTilesCount;
or.verticalTilesCount = canvas.verticalTilesCount;
or.latitude = canvas.latitude;
or.longitude = canvas.longitude;
or.tileSize = canvas.tileSize;
Route r = options.route;
or.segmentsSCmaxZoom = null;
or.visibleSegments = null;
or.visibleSegmentDirections = null;
Vector visibleSegments = new Vector(100, 200);
Vector visibleSegmentDirections = new Vector(100, 200);
int directionIdx = 0;
if (r != null && r.waypoints.size() > 1) {
int count = 1;
if (or.waypointsTC == null || ((TileCoordinate) or.waypointsTC.elementAt(0)).zoom != options.zoom) {
or.waypointsTC = new Vector(r.waypoints.size());
for (int i = 0; i < r.waypoints.size(); i++) {
or.waypointsTC.addElement(ProjectionTool.toTileCoordinate((WorldCoordinate) r.waypoints
.elementAt(i), options.zoom, options.getTileFactory().getTileSize()));
}
}
TileCoordinate t1 = (TileCoordinate) or.waypointsTC.elementAt(0), t2;
ScreenCoordinate s1 = toSC(t1, or.latitude, or.longitude, or.tileSize), s2;
int maxX = options.getTileFactory().getTileSize() * or.horizontalTilesCount;
int maxY = options.getTileFactory().getTileSize() * or.verticalTilesCount;
for (int i = 1; i < or.waypointsTC.size(); i++) {
t2 = (TileCoordinate) or.waypointsTC.elementAt(i);
s2 = toSC(t2, or.latitude, or.longitude, or.tileSize);
int bx1 = getBoundary(s1.x, 0, maxX);
int bx2 = getBoundary(s2.x, 0, maxX);
int by1 = getBoundary(s1.y, 0, maxY);
int by2 = getBoundary(s2.y, 0, maxY);
boolean visible = true;
if (bx1 != 0 && bx1 == bx2 && bx1 == by1 && by1 == by2) {
visible = false;
}
if (bx1 == bx2 && (bx1 != 0)) {
visible = false;
}
if (by1 == by2 && (by1 != 0)) {
visible = false;
}
if (directionIdx < r.routeDirections.size() - 1
&& ((RouteDirection) r.routeDirections.elementAt(directionIdx + 1)).offset <= i) {
directionIdx++;
}
if (visible) {
visibleSegments.addElement(t1);
visibleSegments.addElement(t2);
visibleSegmentDirections.addElement(new Integer(directionIdx));
count++;
}
s1 = s2;
t1 = t2;
}
// Now remove some unnecessary segments.
ScreenCoordinate s3, s4;
boolean removeMore = true;
double tolerance = options.navContext.tolerancePixels * options.navContext.tolerancePixels;
while (removeMore) {
System.out.println("visible segments:" + visibleSegments.size() / 2);
Vector newVisibleSegmentDirections = new Vector(visibleSegmentDirections.size());
Vector newVisibleSegments = new Vector(visibleSegments.size());
int i;
for (i = 0; i < visibleSegments.size() - 4; i += 4) {
s1 = toSC((TileCoordinate) visibleSegments.elementAt(i), or.latitude, or.longitude, or.tileSize);
s2 = toSC((TileCoordinate) visibleSegments.elementAt(i + 1), or.latitude, or.longitude, or.tileSize);
s3 = toSC((TileCoordinate) visibleSegments.elementAt(i + 2), or.latitude, or.longitude, or.tileSize);
s4 = toSC((TileCoordinate) visibleSegments.elementAt(i + 3), or.latitude, or.longitude, or.tileSize);
if (s2.x == s3.x && s2.y == s3.y) {
// continuous line - a candidate to remove s2 and s3
newVisibleSegments.addElement(visibleSegments.elementAt(i));
Object e1 = visibleSegmentDirections.elementAt(i / 2);
Object e2 = visibleSegmentDirections.elementAt(i / 2 + 1);
newVisibleSegmentDirections.addElement(visibleSegmentDirections.elementAt(i / 2));
if (!e1.equals(e2) || distancePointFromLine(s2.x, s2.y, s1.x, s1.y, s4.x, s4.y) > tolerance) {
newVisibleSegmentDirections.addElement(e2);
newVisibleSegments.addElement(visibleSegments.elementAt(i + 1));
newVisibleSegments.addElement(visibleSegments.elementAt(i + 2));
}
newVisibleSegments.addElement(visibleSegments.elementAt(i + 3));
}
}
for (int j = Math.max(i - 4, 0); j < visibleSegments.size(); j++) {
newVisibleSegments.addElement(visibleSegments.elementAt(j));
if (j % 2 == 0) {
newVisibleSegmentDirections.addElement(visibleSegmentDirections.elementAt(j / 2));
}
}
int removedCount = visibleSegments.size() - newVisibleSegments.size();
double removedPercent = removedCount * 100 / (double) visibleSegments.size();
if (removedCount < options.navContext.minRemovedCount
|| removedPercent < options.navContext.minRemovedPercent) {
removeMore = false;
}
visibleSegments = newVisibleSegments;
visibleSegmentDirections = newVisibleSegmentDirections;
}
or.visibleSegmentDirections = visibleSegmentDirections;
or.visibleSegments = visibleSegments;
System.out.println("visible " + count + " out of " + or.waypointsTC.size() + " lines, after lod change "
+ or.visibleSegments.size() / 2 + ", visibleDirections:" + or.visibleSegmentDirections.size());
}
}
private static ScreenCoordinate toSC(TileCoordinate c, int latitude, int longitude, int tileSize) {
ScreenCoordinate result = new ScreenCoordinate();
result.x = (int) ((c.latitude - latitude) * tileSize + c.x);
result.y = (int) ((c.longitude - longitude) * tileSize + c.y);
return result;
}
private static int getBoundary(int x, int lower, int upper) {
if (x < lower) {
return -1;
}
if (x > upper) {
return 1;
}
return 0;
}
private static double snapToRoute(GpsState gpsState) {
double minDistance = Double.POSITIVE_INFINITY;
int minIdx = -1;
Options o = Options.getInstance();
OptimizedRoute or = o.navContext.optimizedRoute;
Vector segments = or.visibleSegments;
double sx = 0, sy = 0;
if (segments != null && !segments.isEmpty()) {
if (or.segmentsSCmaxZoom == null) {
or.segmentsSCmaxZoom = new Vector(segments.size());
for (int i = 0; i < segments.size(); i++) {
or.segmentsSCmaxZoom.addElement(internalToSC((TileCoordinate) segments.elementAt(i)));
}
}
Vector v = or.segmentsSCmaxZoom;
ScreenCoordinate sc = internalToSC(ProjectionTool.toTileCoordinate(gpsState.worldCoordinate, INTERNAL_ZOOM,
INTERNAL_TILESIZE));
for (int i = 0; i < v.size(); i += 2) {
double distance = distancePointFromLine(sc.x, sc.y, ((ScreenCoordinate) v.elementAt(i)).x,
((ScreenCoordinate) v.elementAt(i)).y, ((ScreenCoordinate) v.elementAt(i + 1)).x,
((ScreenCoordinate) v.elementAt(i + 1)).y);
if (minDistance > distance) {
minDistance = distance;
minIdx = i;
sx = snapx;
sy = snapy;
}
}
}
ScreenCoordinate snap = new ScreenCoordinate();
snap.x = (int) sx;
snap.y = (int) sy;
WorldCoordinate snapWC = internalToWc(snap);
double distance = ProjectionTool.getDistance(gpsState.worldCoordinate, snapWC);
if (distance < o.snapTolerance) {
o.navContext.directionIdx = ((Integer) or.visibleSegmentDirections.elementAt(minIdx / 2)).intValue();
if (o.navContext.directionIdx < or.visibleSegmentDirections.size() - 1) {
o.navContext.directionIdx++;
}
if (o.snapToRoad) {
gpsState.worldCoordinate.latitude = snapWC.latitude;
gpsState.worldCoordinate.longitude = snapWC.longitude;
}
return distance;
}
o.navContext.directionIdx = -1;
return distance;
}
private static ScreenCoordinate internalToSC(TileCoordinate tc) {
ScreenCoordinate sc = new ScreenCoordinate();
sc.x = (int) (tc.latitude * INTERNAL_TILESIZE + tc.x);
sc.y = (int) (tc.longitude * INTERNAL_TILESIZE + tc.y);
if (tc.zoom != INTERNAL_ZOOM) {
double pow = Float11.pow(2, INTERNAL_ZOOM - tc.zoom);
sc.x *= pow;
sc.y *= pow;
}
return sc;
}
private static WorldCoordinate internalToWc(ScreenCoordinate sc) {
TileCoordinate tc = new TileCoordinate();
tc.zoom = INTERNAL_ZOOM;
tc.latitude = sc.x / INTERNAL_TILESIZE;
tc.longitude = sc.y / INTERNAL_TILESIZE;
tc.x = sc.x - tc.latitude * INTERNAL_TILESIZE;
tc.y = sc.y - tc.longitude * INTERNAL_TILESIZE;
return ProjectionTool.toWorldCoordinate(tc, INTERNAL_TILESIZE);
}
public static double distancePointFromLine(double px, double py, double lx1, double ly1, double lx2, double ly2) {
double xd = lx2 - lx1, yd = ly2 - ly1;
if (xd == 0 && yd == 0) {
return distancePointFromPoint(px, py, lx1, ly1);
}
double u = ((px - lx1) * xd + (py - ly1) * yd) / (xd * xd + yd * yd);
if (u < 0) {
snapx = lx1;
snapy = ly1;
} else if (u > 1) {
snapx = lx2;
snapy = ly2;
} else {
snapx = lx1 + u * xd;
snapy = ly1 + u * yd;
}
return distancePointFromPoint(px, py, snapx, snapy);
}
private static double distancePointFromPoint(double x1, double y1, double x2, double y2) {
return (x2 - x1) * (x2 - x1) + (y2 - y1) * (y2 - y1);
// return Math.sqrt((x2 - x1) * (x2 - x1) + (y2 - y1) * (y2 - y1));
}
}