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