/** @param smiod */
public void checkPath(StreetMobilityInfoOD smiod) {
SegmentNode temp;
RoadSegment rsTemp;
// if the current end point is equal to an end point of the second road in
// the list, then remove the first road in the list because it's redundant
if (smiod.path.size() > 1) {
temp = (SegmentNode) smiod.path.get(1);
rsTemp = (RoadSegment) segments.get(temp.segmentID);
if (smiod.rsEnd.distance(rsTemp.getStartPoint()) == 0
|| smiod.rsEnd.distance(rsTemp.getEndPoint()) == 0) {
if (DEBUG_OD) System.out.println("Removing first node in list because it is redundant!");
smiod.path.remove(0);
}
}
// case where the node is about to start moving away from the right intersection
// diagnosis: current location is equal to an end point on list
// solution: update "current rs" and "rsEnd" with next value from path
// then remove first entry in path
// Location loc = smiod.rsEnd;
// if (DEBUG_OD)System.out.println("Current location: "+loc);
// temp = (SegmentNode)smiod.path.get(1);
// rsTemp = (RoadSegment)segments.get(temp.segmentID);
// if (loc.distance(rsTemp.getEndPoint())<= StreetMobility.INTERSECTION_RESOLUTION )
// {
// if (DEBUG_OD)System.out.println("Avoiding U-turn!");
// smiod.current = rsTemp;
// smiod.rsEnd = rsTemp.getStartPoint();
// smiod.path.remove(0);
// smiod.path.remove(0);
// }
// else if (loc.distance(rsTemp.getStartPoint())<=StreetMobility.INTERSECTION_RESOLUTION)
// {
// if (DEBUG_OD)System.out.println("Avoiding U-turn!");
// smiod.current = rsTemp;
// smiod.rsEnd = rsTemp.getEndPoint();
// smiod.path.remove(0);
// smiod.path.remove(0);
// }
// case where node is already at intersection for first segment on path
// diagnosis: current location is equal to one of the points in the first segment
// temp = (SegmentNode)smiod.path.get(0);
// rsTemp = (RoadSegment)segments.get(temp.segmentID);
//
// if (loc.distance(rsTemp.getEndPoint())<=StreetMobility.INTERSECTION_RESOLUTION )
// {
// if (DEBUG_OD)System.out.println("Moving immediately to next segment!");
// smiod.current = rsTemp;
// smiod.rsEnd = rsTemp.getStartPoint();
// smiod.path.remove(0);
// }
// else if (loc.distance(rsTemp.getStartPoint())<=StreetMobility.INTERSECTION_RESOLUTION)
// {
// if (DEBUG_OD)System.out.println("Moving immediately to next segment!");
// smiod.current = rsTemp;
// smiod.rsEnd = rsTemp.getEndPoint();
// smiod.path.remove(0);
// }
temp = (SegmentNode) smiod.path.get(0);
rsTemp = (RoadSegment) segments.get(temp.segmentID);
// need to add current road onto list to perform U-turn.
if (smiod.rsEnd.distance(rsTemp.getEndPoint()) > INTERSECTION_RESOLUTION
&& smiod.rsEnd.distance(rsTemp.getStartPoint()) > INTERSECTION_RESOLUTION) {
smiod.path.addFirst(new SegmentNode(smiod.rsEnd, smiod.current.getSelfIndex(), true, false));
}
}
/**
* This method calculates the path from the origin to the destination road segment.
*
* @param smiod The SMIOD object.
*/
public boolean calculatePath(
StreetMobilityInfoOD smiod, RoadSegment origin, Location nextEnd, RoadSegment destination) {
// TODO support loading from OD pairs
// after calling constructor, objects for A* search must be created
boolean found = false;
// TODO the caller should determine direction
if (DEBUG_OD) {
System.out.println("Calculating path...");
System.out.println("Origin: " + origin.printStreetName(streets));
System.out.println("Destination: " + destination.printStreetName(streets));
System.out.println(
"Distance: " + origin.getStartPoint().distance(destination.getStartPoint()));
}
if (nextEnd == null) {
if (smiod.rsEnd.distance(origin.getEndPoint()) == 0) {
nextEnd = origin.getEndPoint();
} else {
nextEnd = origin.getStartPoint();
}
}
if (DEBUG_OD && DEBUG_VIS_OD && v != null) {
v.colorSegment(origin, Color.RED);
}
boolean endStart;
Location endPoint;
if (destination.getStartPoint().distance(nextEnd)
< destination.getEndPoint().distance(nextEnd)) {
endPoint = destination.getStartPoint();
endStart = true;
} else {
endPoint = destination.getEndPoint();
endStart = false;
}
SegmentNode startNode =
new SegmentNode(
nextEnd, origin.getSelfIndex(), origin.getStartPoint().distance(nextEnd) == 0, true);
SegmentNode endNode = new SegmentNode(endPoint, destination.getSelfIndex(), endStart, false);
sni.dest = endNode;
// try to find cached path
if (hm != null) {
LinkedList ll = (LinkedList) hm.get(nextEnd);
if (ll != null) {
ListIterator li = ll.listIterator();
while (li.hasNext()) {
LinkedList path = (LinkedList) li.next();
if (!matches(startNode, endNode, path)) {
continue;
}
smiod.path = (LinkedList) path.clone();
smiod.path.removeFirst();
if (DEBUG_OD) System.out.println("Found cached path!");
return true;
// break;
}
}
}
if (!found) {
// TODO support locations at arbitrary points on road
smiod.destinationSN = endNode;
smiod.destinationLocation = destination.getEndPoint();
AStarSearch ass = new AStarSearch(hm, this); // TODO rename variable
smiod.path = ass.findPath(startNode, endNode); // find the path
}
// no path found
if (smiod.path.get(0) == null) {
AStarSearch ass = new AStarSearch(hm, this); // TODO rename variable
smiod.path = ass.findPath(startNode, endNode); // find the path
if (v != null) {
v.colorSegments(
new RoadSegment[] {origin, destination}, new Color[] {Color.RED, Color.RED});
}
smiod.path.remove(0);
System.out.println("No path found!");
return false;
}
if (DEBUG_VIS_OD && v != null) {
showPath(smiod.path.toArray(), Color.BLUE);
}
// check for strange double entry in list
for (int i = 1; i < smiod.path.size(); i++) {
if (((SegmentNode) smiod.path.get(smiod.path.size() - i)).segmentID
== ((SegmentNode) smiod.path.get(smiod.path.size() - (i + 1))).segmentID) {
if (DEBUG_OD) System.out.println("Removed redundant entry!");
smiod.path.remove(smiod.path.size() - i);
}
}
if (DEBUG_OD) printPath(smiod.path);
if (hm != null) cachePath(smiod, startNode, endNode);
return true; // path found!
}
/* (non-Javadoc)
* @see jist.swans.field.StreetMobility#setNextRoad(jist.swans.field.StreetMobilityInfo)
*/
public void setNextRoad(StreetMobilityInfo smi) {
StreetMobilityInfoOD smiod = (StreetMobilityInfoOD) smi;
if (smiod.path.size() > 0 || smiod.config == Constants.MOBILITY_STREET_RANDOM) {
// get next intersection
smiod.nextIs = intersections.findIntersectingRoads(smiod.rsEnd);
if (smiod.path.size() == 0
|| (RECALCULATE_ALWAYS
&& JistAPI.getTime() > 30 * Constants.SECOND
&& smiod.path.size() > 2
&& smiod.speedSum > 0)) // calculate new path
{
// indicate that node should be taken off map
if (smiod.config == Constants.MOBILITY_STREET_FLOW && smiod.path.size() == 0) {
smi.nextRS = null;
return;
}
if (DEBUG_OD) System.out.println("Calculating new path...");
// recalculate path based on congestion info
if (smiod.path.size() > 2 && smiod.nextEnd != null) {
LinkedList oldPath = (LinkedList) smiod.path.clone();
SegmentNode nextOne = (SegmentNode) smiod.path.getFirst();
RoadSegment origin = (RoadSegment) segments.get(nextOne.segmentID);
SegmentNode dest = smiod.destinationSN;
RoadSegment rs = (RoadSegment) segments.get(smiod.destinationSN.segmentID);
calculatePath(smiod, smi.current, smi.rsEnd, rs);
if (!smiod.path.getLast().equals(dest))
throw new RuntimeException("Destination altered!");
if (smiod.current.equals(
(RoadSegment) segments.get(((SegmentNode) smiod.path.getFirst()).segmentID)))
smiod.path.remove();
if (smiod.path.getFirst().equals(nextOne)) ;
else {
if (DEBUG_VIS_OD && v != null) {
v.drawCircle(
20, smiod.rsEnd /*(smiod.rsEnd.distance(smi.current.getEndPoint())==0
? smi.current.getStartPoint() : smi.current.getEndPoint())*/);
oldPath.addLast(new SegmentNode(smi.rsEnd, smi.current.getSelfIndex(), true, true));
smiod.path.addLast(
new SegmentNode(smi.rsEnd, smi.current.getSelfIndex(), true, true));
showPath(oldPath.toArray(), Color.RED);
showPath(smiod.path.toArray(), Color.ORANGE);
smiod.path.removeLast();
System.out.println("Foo!");
}
}
checkPath(smiod);
if (smiod.current.equals(
(RoadSegment) segments.get(((SegmentNode) smiod.path.getFirst()).segmentID)))
smiod.path = oldPath;
} else {
// this section makes sure that a valid path is found:
// 1) there must be a path from origin to destination on the map
// 2) the path must be longer than 4 so that it's substantial
boolean valid = false;
while (!valid || smiod.path.size() < 4) {
RoadSegment nextDest = null;
int segmentSize = sni.segment.size();
while (nextDest == null
|| nextDest.getSelfIndex() == smiod.current.getSelfIndex()
|| nextDest.getEndPoint().distance(smiod.current.getEndPoint())
> threshold) // get new destination
{
int i = rnd.nextInt(segmentSize);
nextDest = (RoadSegment) sni.segment.get(i);
}
valid = calculatePath(smiod, smi.current, smi.rsEnd, nextDest);
}
checkPath(smiod);
}
if (DEBUG_OD) System.out.println("Current rsEnd: " + smi.rsEnd);
if (DEBUG_OD)
System.out.println("Current road: " + smi.current.printStreetName(sni.streets));
}
SegmentNode sn = (SegmentNode) smiod.path.get(0);
RoadSegment rs = (RoadSegment) segments.get(sn.segmentID);
updateNextRoadSegment(smiod, rs); // update next road
smiod.path.remove(0); // already used the first entry
while (smiod.path.size() > 0) {
// weird intersection type that confuses A*
// getting on a street for no distance...
sn = (SegmentNode) smiod.path.get(0); // was zero
rs = (RoadSegment) segments.get(sn.segmentID);
if (smiod.rsEnd.distance(rs.getStartPoint()) == 0
|| smiod.rsEnd.distance(rs.getEndPoint()) == 0) {
if (DEBUG_OD) System.out.println("rsEnd: " + smiod.rsEnd);
if (DEBUG_OD) System.out.println("Current street: " + rs.printStreetName(sni.streets));
if (DEBUG_OD) System.out.println("Fixing A* bug...");
updateNextRoadSegment(smiod, rs);
smiod.path.remove(0);
} else {
break;
}
}
} else if (smiod.config == Constants.MOBILITY_STREET_FLOW) {
smiod.nextRS = null;
smiod.nextEnd = null;
} else if (smiod.config == Constants.MOBILITY_STREET_CIRCUIT) {
smiod.path = (LinkedList) routes[smiod.routeIndex].clone();
SegmentNode sn = (SegmentNode) smiod.path.get(0);
RoadSegment rs = (RoadSegment) segments.get(sn.segmentID);
updateNextRoadSegment(smiod, rs); // update next road
smiod.path.remove(0); // already used the first entry
}
}