--- /dev/null
+/*******************************************************************************\r
+ * Copyright (c) 2007, 2010 Association for Decentralized Information Management\r
+ * in Industry THTH ry.\r
+ * All rights reserved. This program and the accompanying materials\r
+ * are made available under the terms of the Eclipse Public License v1.0\r
+ * which accompanies this distribution, and is available at\r
+ * http://www.eclipse.org/legal/epl-v10.html\r
+ *\r
+ * Contributors:\r
+ * VTT Technical Research Centre of Finland - initial API and implementation\r
+ *******************************************************************************/\r
+package org.simantics.g2d.routing.algorithm1;\r
+\r
+import java.awt.geom.Path2D;\r
+import java.util.ArrayList;\r
+import java.util.Arrays;\r
+import java.util.Collection;\r
+import java.util.Comparator;\r
+import java.util.HashMap;\r
+import java.util.PriorityQueue;\r
+\r
+import org.simantics.g2d.routing.ConnectionDirectionUtil;\r
+import org.simantics.g2d.routing.Constants;\r
+import org.simantics.g2d.routing.Terminal;\r
+import org.simantics.g2d.routing.algorithm1.StopSet.IStopProcedure;\r
+import org.simantics.g2d.routing.algorithm1.StopSet.Line;\r
+import org.simantics.g2d.routing.algorithm1.StopSet.Stop;\r
+\r
+public class StaticRouter {\r
+ \r
+ StopSet[] stopSets = new StopSet[4];\r
+ //Graphics2D g;\r
+ \r
+ public StaticRouter(Collection<PenaltyRectangle> rectangles) {\r
+ for(int i=0;i<4;++i) {\r
+ Direction dir = Direction.directions[i];\r
+ Collection<Stop> stops= new ArrayList<Stop>(rectangles.size());\r
+ for(PenaltyRectangle rectangle : rectangles)\r
+ stops.add(new Stop((i&1) == 0 ? rectangle.xPenalty : rectangle.yPenalty, \r
+ dir.minSide(rectangle),\r
+ dir.maxSide(rectangle),\r
+ dir.front(rectangle)));\r
+ stopSets[i] = new StopSet(stops);\r
+ }\r
+ }\r
+ \r
+ /*public StaticRouter(Collection<Rectangle2D> rectangles,\r
+ Collection<Path2D> paths) {\r
+ stopSets = new StopSet[4];\r
+ Collection<Rectangle> horizontalLines = new ArrayList<Rectangle>();\r
+ Collection<Rectangle> verticalLines = new ArrayList<Rectangle>();\r
+ \r
+ for(Path2D path : paths) {\r
+ PathIterator it = path.getPathIterator(null);\r
+ double[] old = null;\r
+ while(!it.isDone()) {\r
+ double[] cur = new double[2];\r
+ it.currentSegment(cur);\r
+ if(old != null) {\r
+ if(old[1] == cur[1]) {\r
+ horizontalLines.add(new Rectangle(\r
+ Math.min(cur[0], old[0]),\r
+ cur[1]-CONNECTION_MARGINAL,\r
+ Math.max(cur[0], old[0]),\r
+ cur[1]+CONNECTION_MARGINAL\r
+ )); \r
+ }\r
+ else {\r
+ verticalLines.add(new Rectangle(\r
+ cur[0]-CONNECTION_MARGINAL,\r
+ Math.min(cur[1], old[1]),\r
+ cur[0]+CONNECTION_MARGINAL,\r
+ Math.max(cur[1], old[1]) \r
+ ));\r
+ }\r
+ }\r
+ it.next();\r
+ old = cur;\r
+ }\r
+ }\r
+ \r
+ for(int i=0;i<4;++i) {\r
+ Direction dir = Direction.directions[i];\r
+ Collection<Stop> stops= new ArrayList<Stop>(rectangles.size()+\r
+ horizontalLines.size()+\r
+ verticalLines.size());\r
+ for(Rectangle2D rectangle : rectangles)\r
+ stops.add(new Stop(obstaclePenalty, \r
+ dir.minSide(rectangle),\r
+ dir.maxSide(rectangle),\r
+ dir.front(rectangle)));\r
+ for(Rectangle rectangle : horizontalLines)\r
+ stops.add(new Stop((i&1)==0 ? connectionSidePenalty : connectionCrossPenalty, \r
+ dir.minSide(rectangle),\r
+ dir.maxSide(rectangle),\r
+ dir.front(rectangle)));\r
+ for(Rectangle rectangle : verticalLines)\r
+ stops.add(new Stop((i&1)==1 ? connectionSidePenalty : connectionCrossPenalty, \r
+ dir.minSide(rectangle),\r
+ dir.maxSide(rectangle),\r
+ dir.front(rectangle)));\r
+ stopSets[i] = new StopSet(stops);\r
+ }\r
+ }*/\r
+ \r
+ static class Continuation implements Comparable<Continuation> {\r
+ Direction direction;\r
+ double min;\r
+ double max;\r
+ double front;\r
+ Line line;\r
+ int pos;\r
+ RoutePencil parent;\r
+ double penalty;\r
+ double minTurnDistance;\r
+ double priority; \r
+ double distance;\r
+\r
+ public Continuation(Direction direction, double min, double max,\r
+ double front, int pos, Line line, RoutePencil parent, double penalty,\r
+ double minTurnDistance, double targetX, double targetY) {\r
+ this.direction = direction;\r
+ this.min = min;\r
+ this.max = max;\r
+ this.front = front;\r
+ this.pos = pos;\r
+ this.line = line;\r
+ this.parent = parent;\r
+ this.penalty = penalty;\r
+ this.minTurnDistance = minTurnDistance;\r
+ calcDistance(targetX, targetY);\r
+ }\r
+\r
+ @Override\r
+ public int compareTo(Continuation o) {\r
+ if(priority < o.priority)\r
+ return -1;\r
+ if(priority > o.priority)\r
+ return 1;\r
+ if(distance < o.distance)\r
+ return -1;\r
+ if(distance > o.distance)\r
+ return 1;\r
+ return 0;\r
+ } \r
+\r
+ @Override\r
+ public int hashCode() {\r
+ final int prime = 31;\r
+ int result = direction.getId();\r
+ long temp;\r
+ temp = Double.doubleToLongBits(front);\r
+ result = prime * result + (int) (temp ^ (temp >>> 32));\r
+ temp = Double.doubleToLongBits(max);\r
+ result = prime * result + (int) (temp ^ (temp >>> 32));\r
+ temp = Double.doubleToLongBits(min);\r
+ result = prime * result + (int) (temp ^ (temp >>> 32));\r
+ return result;\r
+ }\r
+\r
+ @Override\r
+ public boolean equals(Object obj) {\r
+ if (this == obj)\r
+ return true;\r
+ if (obj == null)\r
+ return false;\r
+ if (getClass() != obj.getClass())\r
+ return false;\r
+ Continuation other = (Continuation) obj;\r
+ if (direction == null) {\r
+ if (other.direction != null)\r
+ return false;\r
+ } else if (direction!=other.direction)\r
+ return false;\r
+ if (Double.doubleToLongBits(front) != Double\r
+ .doubleToLongBits(other.front))\r
+ return false;\r
+ if (Double.doubleToLongBits(max) != Double\r
+ .doubleToLongBits(other.max))\r
+ return false;\r
+ if (Double.doubleToLongBits(min) != Double\r
+ .doubleToLongBits(other.min))\r
+ return false;\r
+ return true;\r
+ }\r
+ \r
+ void calcDistance(double x, double y) {\r
+ if(parent == null)\r
+ distance = 0.0; \r
+ distance = parent.distanceConstant;\r
+ switch(direction.getId()) {\r
+ case Constants.EAST:\r
+ if(front > parent.x1)\r
+ distance += parent.distanceX*parent.x1 + (front-parent.x1);\r
+ else\r
+ distance += parent.distanceX*front;\r
+ distance += Math.abs(x-front);\r
+ if(y < min)\r
+ distance += parent.distanceY*min + (min - y);\r
+ else if(y > max)\r
+ distance += parent.distanceY*max + (y - max);\r
+ else\r
+ distance += parent.distanceY*y;\r
+ break;\r
+ case Constants.SOUTH:\r
+ if(front > parent.y1)\r
+ distance += parent.distanceY*parent.y1 + (front-parent.y1);\r
+ else\r
+ distance += parent.distanceY*front;\r
+ distance += Math.abs(y-front);\r
+ if(x < min)\r
+ distance += parent.distanceX*min + (min - x);\r
+ else if(x > max)\r
+ distance += parent.distanceX*max + (x - max);\r
+ else\r
+ distance += parent.distanceX*x;\r
+ break;\r
+ case Constants.WEST:\r
+ if(-front < parent.x0)\r
+ distance += parent.distanceX*parent.x0 + (parent.x0+front);\r
+ else\r
+ distance += parent.distanceX*(-front);\r
+ distance += Math.abs(x+front);\r
+ if(y < min)\r
+ distance += parent.distanceY*min + (min - y);\r
+ else if(y > max)\r
+ distance += parent.distanceY*max + (y - max);\r
+ else\r
+ distance += parent.distanceY*y;\r
+ break;\r
+ case Constants.NORTH:\r
+ if(-front < parent.y0)\r
+ distance += parent.distanceY*parent.y0 + (parent.y0+front);\r
+ else\r
+ distance += parent.distanceY*(-front);\r
+ distance += Math.abs(y+front);\r
+ if(x < min)\r
+ distance += parent.distanceX*min + (min - x);\r
+ else if(x > max)\r
+ distance += parent.distanceX*max + (x - max);\r
+ else\r
+ distance += parent.distanceX*x;\r
+ break;\r
+ }\r
+ }\r
+ \r
+ }\r
+ \r
+ static class Cost implements Comparable<Cost> {\r
+ double penalty;\r
+ double distance; \r
+ \r
+ public Cost(double penalty, double distance) {\r
+ this.penalty = penalty;\r
+ this.distance = distance;\r
+ }\r
+ \r
+ @Override\r
+ public int compareTo(Cost o) {\r
+ if(penalty < o.penalty)\r
+ return -1;\r
+ if(penalty > o.penalty)\r
+ return 1;\r
+ if(distance < o.distance)\r
+ return -1;\r
+ if(distance > o.distance)\r
+ return 1;\r
+ return 0;\r
+ } \r
+ }\r
+ \r
+ class SingleRoute {\r
+ PriorityQueue<Continuation> queue = new PriorityQueue<Continuation>();\r
+ double targetX, targetY;\r
+ RoutePencil[] targets;\r
+ int targetDirs;\r
+ RoutePencil[] solutions = new RoutePencil[4];\r
+ int requiredSolutionCount; \r
+ double bPenalty = Double.POSITIVE_INFINITY;\r
+ double bDistance = Double.POSITIVE_INFINITY;\r
+ Cost[] sortedCosts = new Cost[4];\r
+ HashMap<Continuation, Continuation> continuations = new HashMap<Continuation, Continuation>();\r
+\r
+ public SingleRoute(\r
+ double targetX, double targetY, \r
+ RoutePencil[] targets, int targetDirs,\r
+ int requiredSolutionCount) {\r
+ this.targetX = targetX;\r
+ this.targetY = targetY;\r
+ this.targets = targets;\r
+ this.targetDirs = targetDirs;\r
+ this.requiredSolutionCount = requiredSolutionCount;\r
+ \r
+ double minPenalty = Double.MAX_VALUE;\r
+ for(RoutePencil target : targets)\r
+ if(target.penalty < minPenalty)\r
+ minPenalty = target.penalty;\r
+ if(minPenalty > 0)\r
+ for(RoutePencil target : targets)\r
+ target.penalty -= minPenalty;\r
+ }\r
+\r
+ void calcPriority(Continuation cont) {\r
+ cont.priority = cont.penalty;\r
+ double tDir = cont.direction.getDir(targetX, targetY);\r
+ double tSide = cont.direction.getSide(targetX, targetY);\r
+ if(tDir >= cont.front) {\r
+ if(tSide < cont.min || tSide > cont.max)\r
+ cont.priority += Penalty.BEND_PENALTY;\r
+ }\r
+ else\r
+ cont.priority += 2*Penalty.BEND_PENALTY;\r
+ }\r
+ \r
+ void prime(Direction dir, double min, double max, double front, double minTurnDistance) {\r
+ Continuation cont = new Continuation(dir, min, max, front, 0, null, \r
+ dir.createPencil(front, front, min, max, 0.0, null), 0.0, minTurnDistance,\r
+ targetX, targetY);\r
+ addContinutation(cont);\r
+ }\r
+ \r
+ void prime(RoutePencil pencil) {\r
+ Direction dir = pencil.direction;\r
+ Continuation cont = new Continuation(\r
+ dir, \r
+ dir.minSide(pencil),\r
+ dir.maxSide(pencil),\r
+ dir.back(pencil),\r
+ 0, null, pencil, pencil.penalty, 0.0,\r
+ targetX, targetY);\r
+ addContinutation(cont);\r
+ }\r
+ \r
+ void solve() {\r
+ int count = 0;\r
+ while(!queue.isEmpty() && count < 10000) {\r
+ Continuation cont = queue.remove();\r
+ if(cont.priority > bPenalty ||\r
+ (cont.priority == bPenalty && cont.distance >= bDistance)) {\r
+ //System.out.println("solution found, next priority would be " + cont.priority);\r
+ return;\r
+ }\r
+ doContinuation(cont); \r
+ ++count; \r
+ }\r
+ }\r
+ \r
+ void addContinutation(Continuation cont) { \r
+ Continuation old = continuations.get(cont);\r
+ if(old != null) {\r
+ if(old.priority < cont.priority)\r
+ return;\r
+ if(old.priority == cont.priority && old.distance <= cont.distance)\r
+ return;\r
+ }\r
+ continuations.put(cont, cont);\r
+ calcPriority(cont);\r
+ queue.add(cont);\r
+ }\r
+ \r
+ void addBend(RoutePencil pencil) { \r
+ //System.out.println(pencil + " (" + pencil.x0 + " " + pencil.y0 + " " + pencil.x1 + " " + pencil.y1 + ")");\r
+ boolean foundSolution = false;\r
+ for(RoutePencil target : targets) \r
+ if(pencil.intersects(target)) { \r
+ int dir = target.direction.getId();\r
+ if(dir==pencil.direction.getId())\r
+ continue;\r
+ dir = (dir+2)%4;\r
+ double penalty = pencil.penalty + target.penalty;\r
+ if(dir != pencil.direction.getId())\r
+ penalty += Penalty.BEND_PENALTY;\r
+ if(solutions[dir] == null || solutions[dir].penalty > penalty) {\r
+ //System.out.println("Solution by penalty");\r
+ solutions[dir] = new RoutePencil(targetX, targetY, targetX, targetY, penalty,\r
+ Direction.directions[dir], pencil);\r
+ foundSolution = true;\r
+ }\r
+ else if(solutions[dir].penalty == penalty) { \r
+ RoutePencil sol = new RoutePencil(targetX, targetY, targetX, targetY, penalty,\r
+ Direction.directions[dir], pencil);\r
+ double d1 = sol.distance();\r
+ double d2 = solutions[dir].distance();\r
+ //System.out.println("Solution by distance " + d1 + " " + d2);\r
+ if(d1 < d2) {\r
+ solutions[dir] = sol;\r
+ foundSolution = true;\r
+ }\r
+ }\r
+ else\r
+ ; /*System.out.println("Ignored solution: " +\r
+ "penalty=" + penalty + \r
+ ", pencil.penalty=" + pencil.penalty + \r
+ ", distance=" + pencil.distance() + \r
+ ", better=" + solutions[dir].penalty);*/\r
+ } \r
+ if(foundSolution) { \r
+ for(int i=0;i<4;++i)\r
+ sortedCosts[i] =\r
+ solutions[i] == null ? Penalty.INFINITE_COST \r
+ : new Cost(solutions[i].penalty, solutions[i].distance());\r
+ Arrays.sort(sortedCosts);\r
+ bPenalty = sortedCosts[requiredSolutionCount-1].penalty;\r
+ bDistance = sortedCosts[requiredSolutionCount-1].distance;\r
+ } \r
+ else {\r
+ {\r
+ Direction left = pencil.direction.turnLeft();\r
+ double front = left.back(pencil);\r
+ if(front < Double.POSITIVE_INFINITY) {\r
+ Continuation leftCont = new Continuation(left, \r
+ left.minSide(pencil),\r
+ left.maxSide(pencil),\r
+ front,\r
+ 0, null, pencil, pencil.penalty+Penalty.BEND_PENALTY, 0.0,\r
+ targetX, targetY);\r
+ addContinutation(leftCont);\r
+ }\r
+ }\r
+\r
+ {\r
+ Direction right = pencil.direction.turnRight();\r
+ double front = right.back(pencil);\r
+ if(front < Double.POSITIVE_INFINITY) {\r
+ Continuation rightCont = new Continuation(right, \r
+ right.minSide(pencil),\r
+ right.maxSide(pencil),\r
+ right.back(pencil),\r
+ 0, null, pencil, pencil.penalty+Penalty.BEND_PENALTY, 0.0,\r
+ targetX, targetY);\r
+ addContinutation(rightCont);\r
+ }\r
+ }\r
+ }\r
+ }\r
+\r
+\r
+ void doContinuation(final Continuation cont) {\r
+ //System.out.println("cont " + cont.parent + " -> " + cont.direction.getId() + " x=(" + cont.min + ", " + cont.max + ") y=" + cont.front);\r
+ IStopProcedure proc = new IStopProcedure() {\r
+\r
+ @Override\r
+ public void blockEnd(double y) { \r
+ double front = cont.front + cont.minTurnDistance;\r
+ //System.out.println(cont.minTurnDistance);\r
+ if(front <= y)\r
+ addBend(cont.direction.createPencil(front, y, cont.min, cont.max, cont.penalty, cont.parent));\r
+ }\r
+\r
+ @Override\r
+ public void continuation(double min, double max, int pos, Line line) {\r
+ double newPenalty = cont.penalty;\r
+ double newMinTurnDistance; \r
+ \r
+ Penalty penalty = line.penalty[pos]; \r
+ if(penalty != null) {\r
+ newPenalty += penalty.penalty;\r
+ newMinTurnDistance = Math.max(penalty.minDistance, \r
+ cont.minTurnDistance - line.y + cont.front);\r
+ }\r
+ else\r
+ newMinTurnDistance = Math.max(0.0, \r
+ cont.minTurnDistance - line.y + cont.front);\r
+ Continuation newCont = new Continuation(cont.direction,\r
+ min, max, line.y,\r
+ pos, line, cont.parent, newPenalty, newMinTurnDistance,\r
+ targetX, targetY);\r
+ addContinutation(newCont); \r
+ }\r
+ \r
+ };\r
+ if(cont.line != null)\r
+ StopSet.continueStop(cont.pos, cont.line, cont.min, cont.max, proc);\r
+ else {\r
+ StopSet ss = stopSets[cont.direction.getId()];\r
+ ss.findStops(cont.min, cont.max, cont.front, proc);\r
+ }\r
+ }\r
+ }\r
+ static double makeFinite(double x) {\r
+ if(x == Double.POSITIVE_INFINITY)\r
+ return 10000.0;\r
+ if(x == Double.NEGATIVE_INFINITY)\r
+ return -10000.0;\r
+ return x;\r
+ }\r
+ \r
+ class TargetFinder implements IStopProcedure {\r
+ \r
+ Collection<RoutePencil> targets;\r
+ Direction dir;\r
+ double dd;\r
+ double side; \r
+ double penalty;\r
+ double minDistance;\r
+ \r
+ public TargetFinder(Direction dir, double dd, double side, double penalty,\r
+ double minDistance,\r
+ Collection<RoutePencil> targets) {\r
+ this.dd = dd;\r
+ this.dir = dir;\r
+ this.side = side;\r
+ this.targets = targets;\r
+ this.penalty = penalty;\r
+ this.minDistance = minDistance;\r
+ }\r
+\r
+ @Override\r
+ public void blockEnd(double y) {\r
+ if(y >= dd+minDistance)\r
+ targets.add(dir.createPencil(dd+minDistance, \r
+ y, side, side, penalty, null));\r
+ }\r
+\r
+ @Override\r
+ public void continuation(double min, double max,\r
+ int pos, Line line) {\r
+ Penalty p = line.penalty[pos];\r
+ if(p.penalty >= Penalty.OBSTACLE_PENALTY.penalty)\r
+ return;\r
+ StopSet.continueStop(pos, line, side, side, \r
+ new TargetFinder(dir, line.y, side, \r
+ Math.max(p.minDistance, minDistance-line.y+dd), \r
+ penalty+p.penalty, targets)); \r
+ }\r
+ }\r
+ \r
+ public void addTargets(final Collection<RoutePencil> targets, \r
+ final double x, final double y, double minDist, int dirId) {\r
+ final Direction dir = Direction.directions[dirId]; \r
+ final double side = dir.getSide(x,y);\r
+ final double dd = dir.getDir(x, y);\r
+ stopSets[dirId].findStops(side, side, dd, \r
+ new TargetFinder(dir, dd, side, 0.0, minDist, targets)); \r
+ } \r
+ \r
+ public Path2D route(Terminal begin, double[] points, Terminal end) {\r
+ \r
+ /* Handle begin==null (end==null) case by taking the first (last) \r
+ * route point as terminal \r
+ */\r
+ \r
+ if(begin == null) {\r
+ if(points.length < 2)\r
+ return null;\r
+ begin = new Terminal(points[0], points[1], 0x1, Terminal.ZEROS, null);\r
+ points = Arrays.copyOfRange(points, 2, points.length);\r
+ }\r
+ if(end == null) {\r
+ if(points.length < 2)\r
+ return null;\r
+ end = new Terminal(points[points.length-2], points[points.length-1], 0xf, Terminal.ZEROS, null);\r
+ points = Arrays.copyOf(points, points.length-2);\r
+ }\r
+ \r
+ {\r
+ double dx = end.x - begin.x;\r
+ double dy = end.y - begin.y;\r
+ if(dx < begin.minDist[0]+end.minDist[2]+20.0 && -dx < begin.minDist[2]+end.minDist[0]+20.0 &&\r
+ dy < begin.minDist[1]+end.minDist[3]+20.0 && -dy < begin.minDist[3]+end.minDist[1]+20.0 ) {\r
+ if((begin.directions & Constants.EAST_FLAG) == Constants.EAST_FLAG &&\r
+ (end.directions & Constants.WEST_FLAG) == Constants.WEST_FLAG && dx > 0.0) {\r
+ Path2D path = new Path2D.Double();\r
+ path.moveTo(begin.x, begin.y);\r
+ path.lineTo(0.5 * (begin.x + end.x), begin.y);\r
+ path.lineTo(0.5 * (begin.x + end.x), end.y);\r
+ path.lineTo(end.x, end.y);\r
+ return path;\r
+ }\r
+ else if((begin.directions & Constants.WEST_FLAG) == Constants.WEST_FLAG &&\r
+ (end.directions & Constants.EAST_FLAG) == Constants.EAST_FLAG && dx < 0.0) {\r
+ Path2D path = new Path2D.Double();\r
+ path.moveTo(begin.x, begin.y);\r
+ path.lineTo(0.5 * (begin.x + end.x), begin.y);\r
+ path.lineTo(0.5 * (begin.x + end.x), end.y);\r
+ path.lineTo(end.x, end.y);\r
+ return path;\r
+ }\r
+ else if((begin.directions & Constants.SOUTH_FLAG) == Constants.SOUTH_FLAG &&\r
+ (end.directions & Constants.NORTH_FLAG) == Constants.NORTH_FLAG && dy > 0.0) {\r
+ Path2D path = new Path2D.Double();\r
+ path.moveTo(begin.x, begin.y);\r
+ path.lineTo(begin.x, 0.5 * (begin.y + end.y));\r
+ path.lineTo(end.x, 0.5 * (begin.y + end.y)); \r
+ path.lineTo(end.x, end.y);\r
+ return path;\r
+ }\r
+ else if((begin.directions & Constants.NORTH_FLAG) == Constants.NORTH_FLAG &&\r
+ (end.directions & Constants.SOUTH_FLAG) == Constants.SOUTH_FLAG && dy < 0.0) {\r
+ Path2D path = new Path2D.Double();\r
+ path.moveTo(begin.x, begin.y);\r
+ path.lineTo(begin.x, 0.5 * (begin.y + end.y));\r
+ path.lineTo(end.x, 0.5 * (begin.y + end.y)); \r
+ path.lineTo(end.x, end.y);\r
+ return path;\r
+ }\r
+ }\r
+ }\r
+ \r
+ /*\r
+ * Route\r
+ */\r
+ SingleRoute oldRoute = null;\r
+ for(int p=0;p<=points.length;p+=2) {\r
+ /*\r
+ * Create SingleRoute-class and targets\r
+ */\r
+ SingleRoute route; \r
+ if(p==points.length) {\r
+ Collection<RoutePencil> targets = new ArrayList<RoutePencil>(1);\r
+ for(int i=0;i<4;++i) \r
+ if((end.directions & (1 << i)) != 0)\r
+ addTargets(targets, end.x, end.y, end.minDist[i], i);\r
+ route = new SingleRoute(end.x, end.y, targets.toArray(new RoutePencil[targets.size()]), \r
+ ConnectionDirectionUtil.reverseDirections(end.directions), 1);\r
+ }\r
+ else {\r
+ double x = points[p];\r
+ double y = points[p+1];\r
+ Collection<RoutePencil> targets = new ArrayList<RoutePencil>(4); \r
+ for(int i=0;i<4;++i)\r
+ addTargets(targets, x, y, 0.0, i);\r
+ //targets[i] = new RoutePencil(x, y, x, y, 0.0, Direction.directions[i], null);\r
+ route = new SingleRoute(x, y, targets.toArray(new RoutePencil[targets.size()]), 0xf, 1);\r
+ }\r
+ \r
+ /*\r
+ * Prime route class\r
+ */\r
+ if(oldRoute == null) {\r
+ for(int i=0;i<4;++i)\r
+ if((begin.directions & (1 << i)) != 0) {\r
+ Direction dir = Direction.directions[i];\r
+ double side = dir.getSide(begin.x, begin.y);\r
+ double dd = dir.getDir(begin.x, begin.y);\r
+ route.prime(dir, side, side, dd, begin.minDist[i]);\r
+ }\r
+ }\r
+ else {\r
+ for(int i=0;i<4;++i)\r
+ if(oldRoute.solutions[i] != null)\r
+ route.prime(oldRoute.solutions[i]);\r
+ }\r
+ \r
+ /*\r
+ * Solve\r
+ */ \r
+ route.solve();\r
+ /*\r
+ System.out.println("Stage " + (p/2));\r
+ for(int i=0;i<4;++i)\r
+ if(route.solutions[i] != null)\r
+ System.out.println(" " + i + ": " + route.solutions[i] + \r
+ ", penalty: " + route.solutions[i].penalty + \r
+ ", distance: " + route.solutions[i].distance());\r
+ */\r
+ oldRoute = route; \r
+ } \r
+ \r
+ /*\r
+ * Choose the best path\r
+ */\r
+ RoutePencil[] solutions = oldRoute.solutions;\r
+ Arrays.sort(solutions, solutionComparator);\r
+ if(solutions[0] == null)\r
+ return null; \r
+ return solutions[0].createPath(end.x, end.y);\r
+ }\r
+\r
+ final static Comparator<RoutePencil> solutionComparator = new Comparator<RoutePencil>() {\r
+\r
+ @Override\r
+ public int compare(RoutePencil o1, RoutePencil o2) {\r
+ if(o1 == null) {\r
+ if(o2 == null)\r
+ return 0;\r
+ else\r
+ return 1;\r
+ }\r
+ if(o2 == null)\r
+ return -1;\r
+ if(o1.penalty < o2.penalty)\r
+ return -1;\r
+ if(o1.penalty > o2.penalty)\r
+ return 1;\r
+ double d1 = o1.distance();\r
+ double d2 = o2.distance();\r
+ if(d1 < d2)\r
+ return -1;\r
+ if(d1 > d2)\r
+ return 1;\r
+ return 0;\r
+ }\r
+ \r
+ };\r
+}\r