1 /*******************************************************************************
\r
2 * Copyright (c) 2007, 2010 Association for Decentralized Information Management
\r
3 * in Industry THTH ry.
\r
4 * All rights reserved. This program and the accompanying materials
\r
5 * are made available under the terms of the Eclipse Public License v1.0
\r
6 * which accompanies this distribution, and is available at
\r
7 * http://www.eclipse.org/legal/epl-v10.html
\r
10 * VTT Technical Research Centre of Finland - initial API and implementation
\r
11 *******************************************************************************/
\r
12 package org.simantics.g2d.routing.algorithm1;
\r
14 import java.awt.geom.Path2D;
\r
15 import java.util.ArrayList;
\r
16 import java.util.Arrays;
\r
17 import java.util.Collection;
\r
18 import java.util.Comparator;
\r
19 import java.util.HashMap;
\r
20 import java.util.PriorityQueue;
\r
22 import org.simantics.g2d.routing.ConnectionDirectionUtil;
\r
23 import org.simantics.g2d.routing.Constants;
\r
24 import org.simantics.g2d.routing.Terminal;
\r
25 import org.simantics.g2d.routing.algorithm1.StopSet.IStopProcedure;
\r
26 import org.simantics.g2d.routing.algorithm1.StopSet.Line;
\r
27 import org.simantics.g2d.routing.algorithm1.StopSet.Stop;
\r
29 public class StaticRouter {
\r
31 StopSet[] stopSets = new StopSet[4];
\r
34 public StaticRouter(Collection<PenaltyRectangle> rectangles) {
\r
35 for(int i=0;i<4;++i) {
\r
36 Direction dir = Direction.directions[i];
\r
37 Collection<Stop> stops= new ArrayList<Stop>(rectangles.size());
\r
38 for(PenaltyRectangle rectangle : rectangles)
\r
39 stops.add(new Stop((i&1) == 0 ? rectangle.xPenalty : rectangle.yPenalty,
\r
40 dir.minSide(rectangle),
\r
41 dir.maxSide(rectangle),
\r
42 dir.front(rectangle)));
\r
43 stopSets[i] = new StopSet(stops);
\r
47 /*public StaticRouter(Collection<Rectangle2D> rectangles,
\r
48 Collection<Path2D> paths) {
\r
49 stopSets = new StopSet[4];
\r
50 Collection<Rectangle> horizontalLines = new ArrayList<Rectangle>();
\r
51 Collection<Rectangle> verticalLines = new ArrayList<Rectangle>();
\r
53 for(Path2D path : paths) {
\r
54 PathIterator it = path.getPathIterator(null);
\r
55 double[] old = null;
\r
56 while(!it.isDone()) {
\r
57 double[] cur = new double[2];
\r
58 it.currentSegment(cur);
\r
60 if(old[1] == cur[1]) {
\r
61 horizontalLines.add(new Rectangle(
\r
62 Math.min(cur[0], old[0]),
\r
63 cur[1]-CONNECTION_MARGINAL,
\r
64 Math.max(cur[0], old[0]),
\r
65 cur[1]+CONNECTION_MARGINAL
\r
69 verticalLines.add(new Rectangle(
\r
70 cur[0]-CONNECTION_MARGINAL,
\r
71 Math.min(cur[1], old[1]),
\r
72 cur[0]+CONNECTION_MARGINAL,
\r
73 Math.max(cur[1], old[1])
\r
82 for(int i=0;i<4;++i) {
\r
83 Direction dir = Direction.directions[i];
\r
84 Collection<Stop> stops= new ArrayList<Stop>(rectangles.size()+
\r
85 horizontalLines.size()+
\r
86 verticalLines.size());
\r
87 for(Rectangle2D rectangle : rectangles)
\r
88 stops.add(new Stop(obstaclePenalty,
\r
89 dir.minSide(rectangle),
\r
90 dir.maxSide(rectangle),
\r
91 dir.front(rectangle)));
\r
92 for(Rectangle rectangle : horizontalLines)
\r
93 stops.add(new Stop((i&1)==0 ? connectionSidePenalty : connectionCrossPenalty,
\r
94 dir.minSide(rectangle),
\r
95 dir.maxSide(rectangle),
\r
96 dir.front(rectangle)));
\r
97 for(Rectangle rectangle : verticalLines)
\r
98 stops.add(new Stop((i&1)==1 ? connectionSidePenalty : connectionCrossPenalty,
\r
99 dir.minSide(rectangle),
\r
100 dir.maxSide(rectangle),
\r
101 dir.front(rectangle)));
\r
102 stopSets[i] = new StopSet(stops);
\r
106 static class Continuation implements Comparable<Continuation> {
\r
107 Direction direction;
\r
113 RoutePencil parent;
\r
115 double minTurnDistance;
\r
119 public Continuation(Direction direction, double min, double max,
\r
120 double front, int pos, Line line, RoutePencil parent, double penalty,
\r
121 double minTurnDistance, double targetX, double targetY) {
\r
122 this.direction = direction;
\r
125 this.front = front;
\r
128 this.parent = parent;
\r
129 this.penalty = penalty;
\r
130 this.minTurnDistance = minTurnDistance;
\r
131 calcDistance(targetX, targetY);
\r
135 public int compareTo(Continuation o) {
\r
136 if(priority < o.priority)
\r
138 if(priority > o.priority)
\r
140 if(distance < o.distance)
\r
142 if(distance > o.distance)
\r
148 public int hashCode() {
\r
149 final int prime = 31;
\r
150 int result = direction.getId();
\r
152 temp = Double.doubleToLongBits(front);
\r
153 result = prime * result + (int) (temp ^ (temp >>> 32));
\r
154 temp = Double.doubleToLongBits(max);
\r
155 result = prime * result + (int) (temp ^ (temp >>> 32));
\r
156 temp = Double.doubleToLongBits(min);
\r
157 result = prime * result + (int) (temp ^ (temp >>> 32));
\r
162 public boolean equals(Object obj) {
\r
167 if (getClass() != obj.getClass())
\r
169 Continuation other = (Continuation) obj;
\r
170 if (direction == null) {
\r
171 if (other.direction != null)
\r
173 } else if (direction!=other.direction)
\r
175 if (Double.doubleToLongBits(front) != Double
\r
176 .doubleToLongBits(other.front))
\r
178 if (Double.doubleToLongBits(max) != Double
\r
179 .doubleToLongBits(other.max))
\r
181 if (Double.doubleToLongBits(min) != Double
\r
182 .doubleToLongBits(other.min))
\r
187 void calcDistance(double x, double y) {
\r
190 distance = parent.distanceConstant;
\r
191 switch(direction.getId()) {
\r
192 case Constants.EAST:
\r
193 if(front > parent.x1)
\r
194 distance += parent.distanceX*parent.x1 + (front-parent.x1);
\r
196 distance += parent.distanceX*front;
\r
197 distance += Math.abs(x-front);
\r
199 distance += parent.distanceY*min + (min - y);
\r
201 distance += parent.distanceY*max + (y - max);
\r
203 distance += parent.distanceY*y;
\r
205 case Constants.SOUTH:
\r
206 if(front > parent.y1)
\r
207 distance += parent.distanceY*parent.y1 + (front-parent.y1);
\r
209 distance += parent.distanceY*front;
\r
210 distance += Math.abs(y-front);
\r
212 distance += parent.distanceX*min + (min - x);
\r
214 distance += parent.distanceX*max + (x - max);
\r
216 distance += parent.distanceX*x;
\r
218 case Constants.WEST:
\r
219 if(-front < parent.x0)
\r
220 distance += parent.distanceX*parent.x0 + (parent.x0+front);
\r
222 distance += parent.distanceX*(-front);
\r
223 distance += Math.abs(x+front);
\r
225 distance += parent.distanceY*min + (min - y);
\r
227 distance += parent.distanceY*max + (y - max);
\r
229 distance += parent.distanceY*y;
\r
231 case Constants.NORTH:
\r
232 if(-front < parent.y0)
\r
233 distance += parent.distanceY*parent.y0 + (parent.y0+front);
\r
235 distance += parent.distanceY*(-front);
\r
236 distance += Math.abs(y+front);
\r
238 distance += parent.distanceX*min + (min - x);
\r
240 distance += parent.distanceX*max + (x - max);
\r
242 distance += parent.distanceX*x;
\r
249 static class Cost implements Comparable<Cost> {
\r
253 public Cost(double penalty, double distance) {
\r
254 this.penalty = penalty;
\r
255 this.distance = distance;
\r
259 public int compareTo(Cost o) {
\r
260 if(penalty < o.penalty)
\r
262 if(penalty > o.penalty)
\r
264 if(distance < o.distance)
\r
266 if(distance > o.distance)
\r
272 class SingleRoute {
\r
273 PriorityQueue<Continuation> queue = new PriorityQueue<Continuation>();
\r
274 double targetX, targetY;
\r
275 RoutePencil[] targets;
\r
277 RoutePencil[] solutions = new RoutePencil[4];
\r
278 int requiredSolutionCount;
\r
279 double bPenalty = Double.POSITIVE_INFINITY;
\r
280 double bDistance = Double.POSITIVE_INFINITY;
\r
281 Cost[] sortedCosts = new Cost[4];
\r
282 HashMap<Continuation, Continuation> continuations = new HashMap<Continuation, Continuation>();
\r
284 public SingleRoute(
\r
285 double targetX, double targetY,
\r
286 RoutePencil[] targets, int targetDirs,
\r
287 int requiredSolutionCount) {
\r
288 this.targetX = targetX;
\r
289 this.targetY = targetY;
\r
290 this.targets = targets;
\r
291 this.targetDirs = targetDirs;
\r
292 this.requiredSolutionCount = requiredSolutionCount;
\r
294 double minPenalty = Double.MAX_VALUE;
\r
295 for(RoutePencil target : targets)
\r
296 if(target.penalty < minPenalty)
\r
297 minPenalty = target.penalty;
\r
299 for(RoutePencil target : targets)
\r
300 target.penalty -= minPenalty;
\r
303 void calcPriority(Continuation cont) {
\r
304 cont.priority = cont.penalty;
\r
305 double tDir = cont.direction.getDir(targetX, targetY);
\r
306 double tSide = cont.direction.getSide(targetX, targetY);
\r
307 if(tDir >= cont.front) {
\r
308 if(tSide < cont.min || tSide > cont.max)
\r
309 cont.priority += Penalty.BEND_PENALTY;
\r
312 cont.priority += 2*Penalty.BEND_PENALTY;
\r
315 void prime(Direction dir, double min, double max, double front, double minTurnDistance) {
\r
316 Continuation cont = new Continuation(dir, min, max, front, 0, null,
\r
317 dir.createPencil(front, front, min, max, 0.0, null), 0.0, minTurnDistance,
\r
319 addContinutation(cont);
\r
322 void prime(RoutePencil pencil) {
\r
323 Direction dir = pencil.direction;
\r
324 Continuation cont = new Continuation(
\r
326 dir.minSide(pencil),
\r
327 dir.maxSide(pencil),
\r
329 0, null, pencil, pencil.penalty, 0.0,
\r
331 addContinutation(cont);
\r
336 while(!queue.isEmpty() && count < 10000) {
\r
337 Continuation cont = queue.remove();
\r
338 if(cont.priority > bPenalty ||
\r
339 (cont.priority == bPenalty && cont.distance >= bDistance)) {
\r
340 //System.out.println("solution found, next priority would be " + cont.priority);
\r
343 doContinuation(cont);
\r
348 void addContinutation(Continuation cont) {
\r
349 Continuation old = continuations.get(cont);
\r
351 if(old.priority < cont.priority)
\r
353 if(old.priority == cont.priority && old.distance <= cont.distance)
\r
356 continuations.put(cont, cont);
\r
357 calcPriority(cont);
\r
361 void addBend(RoutePencil pencil) {
\r
362 //System.out.println(pencil + " (" + pencil.x0 + " " + pencil.y0 + " " + pencil.x1 + " " + pencil.y1 + ")");
\r
363 boolean foundSolution = false;
\r
364 for(RoutePencil target : targets)
\r
365 if(pencil.intersects(target)) {
\r
366 int dir = target.direction.getId();
\r
367 if(dir==pencil.direction.getId())
\r
370 double penalty = pencil.penalty + target.penalty;
\r
371 if(dir != pencil.direction.getId())
\r
372 penalty += Penalty.BEND_PENALTY;
\r
373 if(solutions[dir] == null || solutions[dir].penalty > penalty) {
\r
374 //System.out.println("Solution by penalty");
\r
375 solutions[dir] = new RoutePencil(targetX, targetY, targetX, targetY, penalty,
\r
376 Direction.directions[dir], pencil);
\r
377 foundSolution = true;
\r
379 else if(solutions[dir].penalty == penalty) {
\r
380 RoutePencil sol = new RoutePencil(targetX, targetY, targetX, targetY, penalty,
\r
381 Direction.directions[dir], pencil);
\r
382 double d1 = sol.distance();
\r
383 double d2 = solutions[dir].distance();
\r
384 //System.out.println("Solution by distance " + d1 + " " + d2);
\r
386 solutions[dir] = sol;
\r
387 foundSolution = true;
\r
391 ; /*System.out.println("Ignored solution: " +
\r
392 "penalty=" + penalty +
\r
393 ", pencil.penalty=" + pencil.penalty +
\r
394 ", distance=" + pencil.distance() +
\r
395 ", better=" + solutions[dir].penalty);*/
\r
397 if(foundSolution) {
\r
398 for(int i=0;i<4;++i)
\r
400 solutions[i] == null ? Penalty.INFINITE_COST
\r
401 : new Cost(solutions[i].penalty, solutions[i].distance());
\r
402 Arrays.sort(sortedCosts);
\r
403 bPenalty = sortedCosts[requiredSolutionCount-1].penalty;
\r
404 bDistance = sortedCosts[requiredSolutionCount-1].distance;
\r
408 Direction left = pencil.direction.turnLeft();
\r
409 double front = left.back(pencil);
\r
410 if(front < Double.POSITIVE_INFINITY) {
\r
411 Continuation leftCont = new Continuation(left,
\r
412 left.minSide(pencil),
\r
413 left.maxSide(pencil),
\r
415 0, null, pencil, pencil.penalty+Penalty.BEND_PENALTY, 0.0,
\r
417 addContinutation(leftCont);
\r
422 Direction right = pencil.direction.turnRight();
\r
423 double front = right.back(pencil);
\r
424 if(front < Double.POSITIVE_INFINITY) {
\r
425 Continuation rightCont = new Continuation(right,
\r
426 right.minSide(pencil),
\r
427 right.maxSide(pencil),
\r
428 right.back(pencil),
\r
429 0, null, pencil, pencil.penalty+Penalty.BEND_PENALTY, 0.0,
\r
431 addContinutation(rightCont);
\r
438 void doContinuation(final Continuation cont) {
\r
439 //System.out.println("cont " + cont.parent + " -> " + cont.direction.getId() + " x=(" + cont.min + ", " + cont.max + ") y=" + cont.front);
\r
440 IStopProcedure proc = new IStopProcedure() {
\r
443 public void blockEnd(double y) {
\r
444 double front = cont.front + cont.minTurnDistance;
\r
445 //System.out.println(cont.minTurnDistance);
\r
447 addBend(cont.direction.createPencil(front, y, cont.min, cont.max, cont.penalty, cont.parent));
\r
451 public void continuation(double min, double max, int pos, Line line) {
\r
452 double newPenalty = cont.penalty;
\r
453 double newMinTurnDistance;
\r
455 Penalty penalty = line.penalty[pos];
\r
456 if(penalty != null) {
\r
457 newPenalty += penalty.penalty;
\r
458 newMinTurnDistance = Math.max(penalty.minDistance,
\r
459 cont.minTurnDistance - line.y + cont.front);
\r
462 newMinTurnDistance = Math.max(0.0,
\r
463 cont.minTurnDistance - line.y + cont.front);
\r
464 Continuation newCont = new Continuation(cont.direction,
\r
466 pos, line, cont.parent, newPenalty, newMinTurnDistance,
\r
468 addContinutation(newCont);
\r
472 if(cont.line != null)
\r
473 StopSet.continueStop(cont.pos, cont.line, cont.min, cont.max, proc);
\r
475 StopSet ss = stopSets[cont.direction.getId()];
\r
476 ss.findStops(cont.min, cont.max, cont.front, proc);
\r
480 static double makeFinite(double x) {
\r
481 if(x == Double.POSITIVE_INFINITY)
\r
483 if(x == Double.NEGATIVE_INFINITY)
\r
488 class TargetFinder implements IStopProcedure {
\r
490 Collection<RoutePencil> targets;
\r
495 double minDistance;
\r
497 public TargetFinder(Direction dir, double dd, double side, double penalty,
\r
498 double minDistance,
\r
499 Collection<RoutePencil> targets) {
\r
503 this.targets = targets;
\r
504 this.penalty = penalty;
\r
505 this.minDistance = minDistance;
\r
509 public void blockEnd(double y) {
\r
510 if(y >= dd+minDistance)
\r
511 targets.add(dir.createPencil(dd+minDistance,
\r
512 y, side, side, penalty, null));
\r
516 public void continuation(double min, double max,
\r
517 int pos, Line line) {
\r
518 Penalty p = line.penalty[pos];
\r
519 if(p.penalty >= Penalty.OBSTACLE_PENALTY.penalty)
\r
521 StopSet.continueStop(pos, line, side, side,
\r
522 new TargetFinder(dir, line.y, side,
\r
523 Math.max(p.minDistance, minDistance-line.y+dd),
\r
524 penalty+p.penalty, targets));
\r
528 public void addTargets(final Collection<RoutePencil> targets,
\r
529 final double x, final double y, double minDist, int dirId) {
\r
530 final Direction dir = Direction.directions[dirId];
\r
531 final double side = dir.getSide(x,y);
\r
532 final double dd = dir.getDir(x, y);
\r
533 stopSets[dirId].findStops(side, side, dd,
\r
534 new TargetFinder(dir, dd, side, 0.0, minDist, targets));
\r
537 public Path2D route(Terminal begin, double[] points, Terminal end) {
\r
539 /* Handle begin==null (end==null) case by taking the first (last)
\r
540 * route point as terminal
\r
543 if(begin == null) {
\r
544 if(points.length < 2)
\r
546 begin = new Terminal(points[0], points[1], 0x1, Terminal.ZEROS, null);
\r
547 points = Arrays.copyOfRange(points, 2, points.length);
\r
550 if(points.length < 2)
\r
552 end = new Terminal(points[points.length-2], points[points.length-1], 0xf, Terminal.ZEROS, null);
\r
553 points = Arrays.copyOf(points, points.length-2);
\r
557 double dx = end.x - begin.x;
\r
558 double dy = end.y - begin.y;
\r
559 if(dx < begin.minDist[0]+end.minDist[2]+20.0 && -dx < begin.minDist[2]+end.minDist[0]+20.0 &&
\r
560 dy < begin.minDist[1]+end.minDist[3]+20.0 && -dy < begin.minDist[3]+end.minDist[1]+20.0 ) {
\r
561 if((begin.directions & Constants.EAST_FLAG) == Constants.EAST_FLAG &&
\r
562 (end.directions & Constants.WEST_FLAG) == Constants.WEST_FLAG && dx > 0.0) {
\r
563 Path2D path = new Path2D.Double();
\r
564 path.moveTo(begin.x, begin.y);
\r
565 path.lineTo(0.5 * (begin.x + end.x), begin.y);
\r
566 path.lineTo(0.5 * (begin.x + end.x), end.y);
\r
567 path.lineTo(end.x, end.y);
\r
570 else if((begin.directions & Constants.WEST_FLAG) == Constants.WEST_FLAG &&
\r
571 (end.directions & Constants.EAST_FLAG) == Constants.EAST_FLAG && dx < 0.0) {
\r
572 Path2D path = new Path2D.Double();
\r
573 path.moveTo(begin.x, begin.y);
\r
574 path.lineTo(0.5 * (begin.x + end.x), begin.y);
\r
575 path.lineTo(0.5 * (begin.x + end.x), end.y);
\r
576 path.lineTo(end.x, end.y);
\r
579 else if((begin.directions & Constants.SOUTH_FLAG) == Constants.SOUTH_FLAG &&
\r
580 (end.directions & Constants.NORTH_FLAG) == Constants.NORTH_FLAG && dy > 0.0) {
\r
581 Path2D path = new Path2D.Double();
\r
582 path.moveTo(begin.x, begin.y);
\r
583 path.lineTo(begin.x, 0.5 * (begin.y + end.y));
\r
584 path.lineTo(end.x, 0.5 * (begin.y + end.y));
\r
585 path.lineTo(end.x, end.y);
\r
588 else if((begin.directions & Constants.NORTH_FLAG) == Constants.NORTH_FLAG &&
\r
589 (end.directions & Constants.SOUTH_FLAG) == Constants.SOUTH_FLAG && dy < 0.0) {
\r
590 Path2D path = new Path2D.Double();
\r
591 path.moveTo(begin.x, begin.y);
\r
592 path.lineTo(begin.x, 0.5 * (begin.y + end.y));
\r
593 path.lineTo(end.x, 0.5 * (begin.y + end.y));
\r
594 path.lineTo(end.x, end.y);
\r
603 SingleRoute oldRoute = null;
\r
604 for(int p=0;p<=points.length;p+=2) {
\r
606 * Create SingleRoute-class and targets
\r
608 SingleRoute route;
\r
609 if(p==points.length) {
\r
610 Collection<RoutePencil> targets = new ArrayList<RoutePencil>(1);
\r
611 for(int i=0;i<4;++i)
\r
612 if((end.directions & (1 << i)) != 0)
\r
613 addTargets(targets, end.x, end.y, end.minDist[i], i);
\r
614 route = new SingleRoute(end.x, end.y, targets.toArray(new RoutePencil[targets.size()]),
\r
615 ConnectionDirectionUtil.reverseDirections(end.directions), 1);
\r
618 double x = points[p];
\r
619 double y = points[p+1];
\r
620 Collection<RoutePencil> targets = new ArrayList<RoutePencil>(4);
\r
621 for(int i=0;i<4;++i)
\r
622 addTargets(targets, x, y, 0.0, i);
\r
623 //targets[i] = new RoutePencil(x, y, x, y, 0.0, Direction.directions[i], null);
\r
624 route = new SingleRoute(x, y, targets.toArray(new RoutePencil[targets.size()]), 0xf, 1);
\r
628 * Prime route class
\r
630 if(oldRoute == null) {
\r
631 for(int i=0;i<4;++i)
\r
632 if((begin.directions & (1 << i)) != 0) {
\r
633 Direction dir = Direction.directions[i];
\r
634 double side = dir.getSide(begin.x, begin.y);
\r
635 double dd = dir.getDir(begin.x, begin.y);
\r
636 route.prime(dir, side, side, dd, begin.minDist[i]);
\r
640 for(int i=0;i<4;++i)
\r
641 if(oldRoute.solutions[i] != null)
\r
642 route.prime(oldRoute.solutions[i]);
\r
650 System.out.println("Stage " + (p/2));
\r
651 for(int i=0;i<4;++i)
\r
652 if(route.solutions[i] != null)
\r
653 System.out.println(" " + i + ": " + route.solutions[i] +
\r
654 ", penalty: " + route.solutions[i].penalty +
\r
655 ", distance: " + route.solutions[i].distance());
\r
661 * Choose the best path
\r
663 RoutePencil[] solutions = oldRoute.solutions;
\r
664 Arrays.sort(solutions, solutionComparator);
\r
665 if(solutions[0] == null)
\r
667 return solutions[0].createPath(end.x, end.y);
\r
670 final static Comparator<RoutePencil> solutionComparator = new Comparator<RoutePencil>() {
\r
673 public int compare(RoutePencil o1, RoutePencil o2) {
\r
682 if(o1.penalty < o2.penalty)
\r
684 if(o1.penalty > o2.penalty)
\r
686 double d1 = o1.distance();
\r
687 double d2 = o2.distance();
\r