bundles/org.simantics.g2d/src/org/simantics/g2d/routing/algorithm1/StaticRouter.java

   1 /*******************************************************************************
   2  * Copyright (c) 2007, 2010 Association for Decentralized Information Management
   3  * in Industry THTH ry.
   4  * All rights reserved. This program and the accompanying materials
   5  * are made available under the terms of the Eclipse Public License v1.0
   6  * which accompanies this distribution, and is available at
   7  * http://www.eclipse.org/legal/epl-v10.html
   8  *
   9  * Contributors:
  10  *     VTT Technical Research Centre of Finland - initial API and implementation
  11  *******************************************************************************/
  12 package org.simantics.g2d.routing.algorithm1;
  13
  14 import java.awt.geom.Path2D;
  15 import java.util.ArrayList;
  16 import java.util.Arrays;
  17 import java.util.Collection;
  18 import java.util.Comparator;
  19 import java.util.HashMap;
  20 import java.util.PriorityQueue;
  21
  22 import org.simantics.g2d.routing.ConnectionDirectionUtil;
  23 import org.simantics.g2d.routing.Constants;
  24 import org.simantics.g2d.routing.Terminal;
  25 import org.simantics.g2d.routing.algorithm1.StopSet.IStopProcedure;
  26 import org.simantics.g2d.routing.algorithm1.StopSet.Line;
  27 import org.simantics.g2d.routing.algorithm1.StopSet.Stop;
  28
  29 public class StaticRouter {
  30
  31         StopSet[] stopSets = new StopSet[4];
  32         //Graphics2D g;
  33
  34         public StaticRouter(Collection<PenaltyRectangle> rectangles) {
  35                 for(int i=0;i<4;++i) {
  36                         Direction dir = Direction.directions[i];
  37                         Collection<Stop> stops= new ArrayList<Stop>(rectangles.size());
  38                         for(PenaltyRectangle rectangle : rectangles)
  39                                 stops.add(new Stop((i&1) == 0 ? rectangle.xPenalty : rectangle.yPenalty,
  40                                         dir.minSide(rectangle),
  41                                         dir.maxSide(rectangle),
  42                                         dir.front(rectangle)));
  43                         stopSets[i] = new StopSet(stops);
  44                 }
  45         }
  46
  47         /*public StaticRouter(Collection<Rectangle2D> rectangles,
  48                         Collection<Path2D> paths) {
  49                 stopSets = new StopSet[4];
  50                 Collection<Rectangle> horizontalLines = new ArrayList<Rectangle>();
  51                 Collection<Rectangle> verticalLines = new ArrayList<Rectangle>();
  52
  53                 for(Path2D path : paths) {
  54                         PathIterator it = path.getPathIterator(null);
  55                         double[] old = null;
  56                         while(!it.isDone()) {
  57                                 double[] cur = new double[2];
  58                                 it.currentSegment(cur);
  59                                 if(old != null) {
  60                                         if(old[1] == cur[1]) {
  61                                                 horizontalLines.add(new Rectangle(
  62                                                         Math.min(cur[0], old[0]),
  63                                                         cur[1]-CONNECTION_MARGINAL,
  64                                                         Math.max(cur[0], old[0]),
  65                                                         cur[1]+CONNECTION_MARGINAL
  66                                                 ));
  67                                         }
  68                                         else {
  69                                                 verticalLines.add(new Rectangle(
  70                                                         cur[0]-CONNECTION_MARGINAL,
  71                                                         Math.min(cur[1], old[1]),
  72                                                         cur[0]+CONNECTION_MARGINAL,
  73                                                         Math.max(cur[1], old[1])
  74                                                 ));
  75                                         }
  76                                 }
  77                                 it.next();
  78                                 old = cur;
  79                         }
  80                 }
  81
  82                 for(int i=0;i<4;++i) {
  83                         Direction dir = Direction.directions[i];
  84                         Collection<Stop> stops= new ArrayList<Stop>(rectangles.size()+
  85                                         horizontalLines.size()+
  86                                         verticalLines.size());
  87                         for(Rectangle2D rectangle : rectangles)
  88                                 stops.add(new Stop(obstaclePenalty,
  89                                         dir.minSide(rectangle),
  90                                         dir.maxSide(rectangle),
  91                                         dir.front(rectangle)));
  92                         for(Rectangle rectangle : horizontalLines)
  93                                 stops.add(new Stop((i&1)==0 ? connectionSidePenalty : connectionCrossPenalty,
  94                                         dir.minSide(rectangle),
  95                                         dir.maxSide(rectangle),
  96                                         dir.front(rectangle)));
  97                         for(Rectangle rectangle : verticalLines)
  98                                 stops.add(new Stop((i&1)==1 ? connectionSidePenalty : connectionCrossPenalty,
  99                                         dir.minSide(rectangle),
 100                                         dir.maxSide(rectangle),
 101                                         dir.front(rectangle)));
 102                         stopSets[i] = new StopSet(stops);
 103                 }
 104         }*/
 105
 106         static class Continuation implements Comparable<Continuation> {
 107                 Direction direction;
 108                 double min;
 109                 double max;
 110                 double front;
 111                 Line line;
 112                 int pos;
 113                 RoutePencil parent;
 114                 double penalty;
 115                 double minTurnDistance;
 116                 double priority;
 117                 double distance;
 118
 119                 public Continuation(Direction direction, double min, double max,
 120                         double front, int pos, Line line, RoutePencil parent, double penalty,
 121                         double minTurnDistance, double targetX, double targetY) {
 122                         this.direction = direction;
 123                         this.min = min;
 124                         this.max = max;
 125                         this.front = front;
 126                         this.pos = pos;
 127                         this.line = line;
 128                         this.parent = parent;
 129                         this.penalty = penalty;
 130                         this.minTurnDistance = minTurnDistance;
 131                         calcDistance(targetX, targetY);
 132                 }
 133
 134                 @Override
 135                 public int compareTo(Continuation o) {
 136                         if(priority < o.priority)
 137                                 return -1;
 138                         if(priority > o.priority)
 139                                 return 1;
 140                         if(distance < o.distance)
 141                                 return -1;
 142                         if(distance > o.distance)
 143                                 return 1;
 144                         return 0;
 145                 }
 146
 147                 @Override
 148                 public int hashCode() {
 149                         final int prime = 31;
 150                         int result = direction.getId();
 151                         long temp;
 152                         temp = Double.doubleToLongBits(front);
 153                         result = prime * result + (int) (temp ^ (temp >>> 32));
 154                         temp = Double.doubleToLongBits(max);
 155                         result = prime * result + (int) (temp ^ (temp >>> 32));
 156                         temp = Double.doubleToLongBits(min);
 157                         result = prime * result + (int) (temp ^ (temp >>> 32));
 158                         return result;
 159                 }
 160
 161                 @Override
 162                 public boolean equals(Object obj) {
 163                         if (this == obj)
 164                                 return true;
 165                         if (obj == null)
 166                                 return false;
 167                         if (getClass() != obj.getClass())
 168                                 return false;
 169                         Continuation other = (Continuation) obj;
 170                         if (direction == null) {
 171                                 if (other.direction != null)
 172                                         return false;
 173                         } else if (direction!=other.direction)
 174                                 return false;
 175                         if (Double.doubleToLongBits(front) != Double
 176                                 .doubleToLongBits(other.front))
 177                                 return false;
 178                         if (Double.doubleToLongBits(max) != Double
 179                                 .doubleToLongBits(other.max))
 180                                 return false;
 181                         if (Double.doubleToLongBits(min) != Double
 182                                 .doubleToLongBits(other.min))
 183                                 return false;
 184                         return true;
 185                 }
 186
 187                 void calcDistance(double x, double y) {
 188                         if(parent == null)
 189                                 distance = 0.0;
 190                         distance = parent.distanceConstant;
 191                         switch(direction.getId()) {
 192                         case Constants.EAST:
 193                                 if(front > parent.x1)
 194                                         distance += parent.distanceX*parent.x1 + (front-parent.x1);
 195                                 else
 196                                         distance += parent.distanceX*front;
 197                                 distance += Math.abs(x-front);
 198                                 if(y < min)
 199                                         distance += parent.distanceY*min + (min - y);
 200                                 else if(y > max)
 201                                         distance += parent.distanceY*max + (y - max);
 202                                 else
 203                                         distance += parent.distanceY*y;
 204                                 break;
 205                         case Constants.SOUTH:
 206                                 if(front > parent.y1)
 207                                         distance += parent.distanceY*parent.y1 + (front-parent.y1);
 208                                 else
 209                                         distance += parent.distanceY*front;
 210                                 distance += Math.abs(y-front);
 211                                 if(x < min)
 212                                         distance += parent.distanceX*min + (min - x);
 213                                 else if(x > max)
 214                                         distance += parent.distanceX*max + (x - max);
 215                                 else
 216                                         distance += parent.distanceX*x;
 217                                 break;
 218                         case Constants.WEST:
 219                                 if(-front < parent.x0)
 220                                         distance += parent.distanceX*parent.x0 + (parent.x0+front);
 221                                 else
 222                                         distance += parent.distanceX*(-front);
 223                                 distance += Math.abs(x+front);
 224                                 if(y < min)
 225                                         distance += parent.distanceY*min + (min - y);
 226                                 else if(y > max)
 227                                         distance += parent.distanceY*max + (y - max);
 228                                 else
 229                                         distance += parent.distanceY*y;
 230                                 break;
 231                         case Constants.NORTH:
 232                                 if(-front < parent.y0)
 233                                         distance += parent.distanceY*parent.y0 + (parent.y0+front);
 234                                 else
 235                                         distance += parent.distanceY*(-front);
 236                                 distance += Math.abs(y+front);
 237                                 if(x < min)
 238                                         distance += parent.distanceX*min + (min - x);
 239                                 else if(x > max)
 240                                         distance += parent.distanceX*max + (x - max);
 241                                 else
 242                                         distance += parent.distanceX*x;
 243                                 break;
 244                         }
 245                 }
 246
 247         }
 248
 249         static class Cost implements Comparable<Cost> {
 250                 double penalty;
 251                 double distance;
 252
 253                 public Cost(double penalty, double distance) {
 254                         this.penalty = penalty;
 255                         this.distance = distance;
 256                 }
 257
 258                 @Override
 259                 public int compareTo(Cost o) {
 260                         if(penalty < o.penalty)
 261                                 return -1;
 262                         if(penalty > o.penalty)
 263                                 return 1;
 264                         if(distance < o.distance)
 265                                 return -1;
 266                         if(distance > o.distance)
 267                                 return 1;
 268                         return 0;
 269                 }
 270         }
 271
 272         class SingleRoute {
 273                 PriorityQueue<Continuation> queue = new PriorityQueue<Continuation>();
 274                 double targetX, targetY;
 275                 RoutePencil[] targets;
 276                 int targetDirs;
 277                 RoutePencil[] solutions = new RoutePencil[4];
 278                 int requiredSolutionCount;
 279                 double bPenalty = Double.POSITIVE_INFINITY;
 280                 double bDistance = Double.POSITIVE_INFINITY;
 281                 Cost[] sortedCosts = new Cost[4];
 282                 HashMap<Continuation, Continuation> continuations = new HashMap<Continuation, Continuation>();
 283
 284                 public SingleRoute(
 285                                 double targetX, double targetY,
 286                                 RoutePencil[] targets, int targetDirs,
 287                                 int requiredSolutionCount) {
 288                         this.targetX = targetX;
 289                         this.targetY = targetY;
 290                         this.targets = targets;
 291                         this.targetDirs = targetDirs;
 292                         this.requiredSolutionCount = requiredSolutionCount;
 293
 294                         double minPenalty = Double.MAX_VALUE;
 295                         for(RoutePencil target : targets)
 296                                 if(target.penalty < minPenalty)
 297                                         minPenalty = target.penalty;
 298                         if(minPenalty > 0)
 299                                 for(RoutePencil target : targets)
 300                                         target.penalty -= minPenalty;
 301                 }
 302
 303                 void calcPriority(Continuation cont) {
 304                         cont.priority = cont.penalty;
 305                         double tDir = cont.direction.getDir(targetX, targetY);
 306                         double tSide = cont.direction.getSide(targetX, targetY);
 307                         if(tDir >= cont.front) {
 308                                 if(tSide < cont.min || tSide > cont.max)
 309                                         cont.priority += Penalty.BEND_PENALTY;
 310                         }
 311                         else
 312                                 cont.priority += 2*Penalty.BEND_PENALTY;
 313                 }
 314
 315                 void prime(Direction dir, double min, double max, double front, double minTurnDistance) {
 316                         Continuation cont = new Continuation(dir, min, max, front, 0, null,
 317                                 dir.createPencil(front, front, min, max, 0.0, null), 0.0, minTurnDistance,
 318                                 targetX, targetY);
 319                         addContinutation(cont);
 320                 }
 321
 322                 void prime(RoutePencil pencil) {
 323                         Direction dir = pencil.direction;
 324                         Continuation cont = new Continuation(
 325                                 dir,
 326                                 dir.minSide(pencil),
 327                                 dir.maxSide(pencil),
 328                                 dir.back(pencil),
 329                                 0, null, pencil, pencil.penalty, 0.0,
 330                                 targetX, targetY);
 331                         addContinutation(cont);
 332                 }
 333
 334                 void solve() {
 335                         int count = 0;
 336                         while(!queue.isEmpty() && count < 10000) {
 337                                 Continuation cont = queue.remove();
 338                                 if(cont.priority > bPenalty ||
 339                                                 (cont.priority == bPenalty && cont.distance >= bDistance)) {
 340                                         //System.out.println("solution found, next priority would be " + cont.priority);
 341                                         return;
 342                                 }
 343                                 doContinuation(cont);
 344                                 ++count;
 345                         }
 346                 }
 347
 348                 void addContinutation(Continuation cont) {
 349                         Continuation old = continuations.get(cont);
 350                         if(old != null) {
 351                                 if(old.priority < cont.priority)
 352                                         return;
 353                                 if(old.priority == cont.priority && old.distance <= cont.distance)
 354                                         return;
 355                         }
 356                         continuations.put(cont, cont);
 357                         calcPriority(cont);
 358                         queue.add(cont);
 359                 }
 360
 361                 void addBend(RoutePencil pencil) {
 362                         //System.out.println(pencil + " (" + pencil.x0 + " " + pencil.y0 + " " + pencil.x1 + " " + pencil.y1 + ")");
 363                         boolean foundSolution = false;
 364                         for(RoutePencil target : targets)
 365                                 if(pencil.intersects(target)) {
 366                                         int dir = target.direction.getId();
 367                                         if(dir==pencil.direction.getId())
 368                                                 continue;
 369                                         dir = (dir+2)%4;
 370                                         double penalty = pencil.penalty + target.penalty;
 371                                         if(dir != pencil.direction.getId())
 372                                                 penalty += Penalty.BEND_PENALTY;
 373                                         if(solutions[dir] == null || solutions[dir].penalty > penalty) {
 374                                                 //System.out.println("Solution by penalty");
 375                                                 solutions[dir] = new RoutePencil(targetX, targetY, targetX, targetY, penalty,
 376                                                         Direction.directions[dir], pencil);
 377                                                 foundSolution = true;
 378                                         }
 379                                         else if(solutions[dir].penalty == penalty) {
 380                                                 RoutePencil sol = new RoutePencil(targetX, targetY, targetX, targetY, penalty,
 381                                                         Direction.directions[dir], pencil);
 382                                                 double d1 = sol.distance();
 383                                                 double d2 = solutions[dir].distance();
 384                                                 //System.out.println("Solution by distance " + d1 + " " + d2);
 385                                                 if(d1 < d2) {
 386                                                         solutions[dir] = sol;
 387                                                         foundSolution = true;
 388                                                 }
 389                                         }
 390                                         else
 391                                                 ; /*System.out.println("Ignored solution: " +
 392                                                                 "penalty=" + penalty +
 393                                                                 ", pencil.penalty=" + pencil.penalty +
 394                                                                 ", distance=" + pencil.distance() +
 395                                                                 ", better=" + solutions[dir].penalty);*/
 396                                 }
 397                         if(foundSolution) {
 398                                 for(int i=0;i<4;++i)
 399                                         sortedCosts[i] =
 400                                                 solutions[i] == null ? Penalty.INFINITE_COST
 401                                                                 : new Cost(solutions[i].penalty, solutions[i].distance());
 402                                 Arrays.sort(sortedCosts);
 403                                 bPenalty = sortedCosts[requiredSolutionCount-1].penalty;
 404                                 bDistance = sortedCosts[requiredSolutionCount-1].distance;
 405                         }
 406                         else {
 407                                 {
 408                                         Direction left = pencil.direction.turnLeft();
 409                                         double front = left.back(pencil);
 410                                         if(front < Double.POSITIVE_INFINITY) {
 411                                                 Continuation leftCont = new Continuation(left,
 412                                                                 left.minSide(pencil),
 413                                                                 left.maxSide(pencil),
 414                                                                 front,
 415                                                                 0, null, pencil, pencil.penalty+Penalty.BEND_PENALTY, 0.0,
 416                                                                 targetX, targetY);
 417                                                 addContinutation(leftCont);
 418                                         }
 419                                 }
 420
 421                                 {
 422                                         Direction right = pencil.direction.turnRight();
 423                                         double front = right.back(pencil);
 424                                         if(front < Double.POSITIVE_INFINITY) {
 425                                                 Continuation rightCont = new Continuation(right,
 426                                                                 right.minSide(pencil),
 427                                                                 right.maxSide(pencil),
 428                                                                 right.back(pencil),
 429                                                                 0, null, pencil, pencil.penalty+Penalty.BEND_PENALTY, 0.0,
 430                                                                 targetX, targetY);
 431                                                 addContinutation(rightCont);
 432                                         }
 433                                 }
 434                         }
 435                 }
 436
 437
 438                 void doContinuation(final Continuation cont) {
 439                         //System.out.println("cont " + cont.parent + " -> " + cont.direction.getId() + " x=(" + cont.min + ", " + cont.max + ") y=" + cont.front);
 440                         IStopProcedure proc = new IStopProcedure() {
 441
 442                                 @Override
 443                                 public void blockEnd(double y) {
 444                                         double front = cont.front + cont.minTurnDistance;
 445                                         //System.out.println(cont.minTurnDistance);
 446                                         if(front <= y)
 447                                                 addBend(cont.direction.createPencil(front, y, cont.min, cont.max, cont.penalty, cont.parent));
 448                                 }
 449
 450                                 @Override
 451                                 public void continuation(double min, double max, int pos, Line line) {
 452                                         double newPenalty = cont.penalty;
 453                                         double newMinTurnDistance;
 454
 455                                         Penalty penalty = line.penalty[pos];
 456                                         if(penalty != null) {
 457                                                 newPenalty += penalty.penalty;
 458                                                 newMinTurnDistance = Math.max(penalty.minDistance,
 459                                                                 cont.minTurnDistance - line.y + cont.front);
 460                                         }
 461                                         else
 462                                                 newMinTurnDistance = Math.max(0.0,
 463                                                                 cont.minTurnDistance - line.y + cont.front);
 464                                         Continuation newCont = new Continuation(cont.direction,
 465                                                 min, max, line.y,
 466                                                 pos, line, cont.parent, newPenalty, newMinTurnDistance,
 467                                                 targetX, targetY);
 468                                         addContinutation(newCont);
 469                                 }
 470
 471                         };
 472                         if(cont.line != null)
 473                                 StopSet.continueStop(cont.pos, cont.line, cont.min, cont.max, proc);
 474                         else {
 475                                 StopSet ss = stopSets[cont.direction.getId()];
 476                                 ss.findStops(cont.min, cont.max, cont.front, proc);
 477                         }
 478                 }
 479         }
 480         static double makeFinite(double x) {
 481                 if(x == Double.POSITIVE_INFINITY)
 482                         return 10000.0;
 483                 if(x == Double.NEGATIVE_INFINITY)
 484                         return -10000.0;
 485                 return x;
 486         }
 487
 488         class TargetFinder implements IStopProcedure {
 489
 490                 Collection<RoutePencil> targets;
 491                 Direction dir;
 492                 double dd;
 493                 double side;
 494                 double penalty;
 495                 double minDistance;
 496
 497                 public TargetFinder(Direction dir, double dd, double side, double penalty,
 498                                 double minDistance,
 499                                 Collection<RoutePencil> targets) {
 500                         this.dd = dd;
 501                         this.dir = dir;
 502                         this.side = side;
 503                         this.targets = targets;
 504                         this.penalty = penalty;
 505                         this.minDistance = minDistance;
 506                 }
 507
 508                 @Override
 509                 public void blockEnd(double y) {
 510                         if(y >= dd+minDistance)
 511                                 targets.add(dir.createPencil(dd+minDistance,
 512                                                 y, side, side, penalty, null));
 513                 }
 514
 515                 @Override
 516                 public void continuation(double min, double max,
 517                                 int pos, Line line) {
 518                         Penalty p = line.penalty[pos];
 519                         if(p.penalty >= Penalty.OBSTACLE_PENALTY.penalty)
 520                                 return;
 521                         StopSet.continueStop(pos, line, side, side,
 522                                         new TargetFinder(dir, line.y, side,
 523                                                         Math.max(p.minDistance, minDistance-line.y+dd),
 524                                                         penalty+p.penalty, targets));
 525                 }
 526         }
 527
 528         public void addTargets(final Collection<RoutePencil> targets,
 529                         final double x, final double y, double minDist, int dirId) {
 530                 final Direction dir = Direction.directions[dirId];
 531                 final double side = dir.getSide(x,y);
 532                 final double dd = dir.getDir(x, y);
 533                 stopSets[dirId].findStops(side, side, dd,
 534                                 new TargetFinder(dir, dd, side, 0.0, minDist, targets));
 535         }
 536
 537         public Path2D route(Terminal begin, double[] points, Terminal end) {
 538
 539                 /* Handle begin==null (end==null) case by taking the first (last)
 540                  * route point as terminal
 541                  */
 542
 543                 if(begin == null) {
 544                         if(points.length < 2)
 545                                 return null;
 546                         begin = new Terminal(points[0], points[1], 0x1, Terminal.ZEROS, null);
 547                         points = Arrays.copyOfRange(points, 2, points.length);
 548                 }
 549                 if(end == null) {
 550                         if(points.length < 2)
 551                                 return null;
 552                         end = new Terminal(points[points.length-2], points[points.length-1], 0xf, Terminal.ZEROS, null);
 553                         points = Arrays.copyOf(points, points.length-2);
 554                 }
 555
 556                 {
 557                     double dx = end.x - begin.x;
 558                     double dy = end.y - begin.y;
 559                     if(dx < begin.minDist[0]+end.minDist[2]+20.0 && -dx < begin.minDist[2]+end.minDist[0]+20.0 &&
 560                        dy < begin.minDist[1]+end.minDist[3]+20.0 && -dy < begin.minDist[3]+end.minDist[1]+20.0 ) {
 561                         if((begin.directions & Constants.EAST_FLAG) == Constants.EAST_FLAG &&
 562                             (end.directions & Constants.WEST_FLAG) == Constants.WEST_FLAG && dx > 0.0) {
 563                             Path2D path = new Path2D.Double();
 564                             path.moveTo(begin.x, begin.y);
 565                             path.lineTo(0.5 * (begin.x + end.x), begin.y);
 566                             path.lineTo(0.5 * (begin.x + end.x), end.y);
 567                             path.lineTo(end.x, end.y);
 568                             return path;
 569                         }
 570                         else if((begin.directions & Constants.WEST_FLAG) == Constants.WEST_FLAG &&
 571                             (end.directions & Constants.EAST_FLAG) == Constants.EAST_FLAG && dx < 0.0) {
 572                             Path2D path = new Path2D.Double();
 573                             path.moveTo(begin.x, begin.y);
 574                             path.lineTo(0.5 * (begin.x + end.x), begin.y);
 575                             path.lineTo(0.5 * (begin.x + end.x), end.y);
 576                             path.lineTo(end.x, end.y);
 577                             return path;
 578                         }
 579                         else if((begin.directions & Constants.SOUTH_FLAG) == Constants.SOUTH_FLAG &&
 580                     (end.directions & Constants.NORTH_FLAG) == Constants.NORTH_FLAG && dy > 0.0) {
 581                     Path2D path = new Path2D.Double();
 582                     path.moveTo(begin.x, begin.y);
 583                     path.lineTo(begin.x, 0.5 * (begin.y + end.y));
 584                     path.lineTo(end.x, 0.5 * (begin.y + end.y));
 585                     path.lineTo(end.x, end.y);
 586                     return path;
 587                 }
 588                         else if((begin.directions & Constants.NORTH_FLAG) == Constants.NORTH_FLAG &&
 589                     (end.directions & Constants.SOUTH_FLAG) == Constants.SOUTH_FLAG && dy < 0.0) {
 590                     Path2D path = new Path2D.Double();
 591                     path.moveTo(begin.x, begin.y);
 592                     path.lineTo(begin.x, 0.5 * (begin.y + end.y));
 593                     path.lineTo(end.x, 0.5 * (begin.y + end.y));
 594                     path.lineTo(end.x, end.y);
 595                     return path;
 596                 }
 597                     }
 598                 }
 599
 600                 /*
 601                  * Route
 602                  */
 603                 SingleRoute oldRoute = null;
 604                 for(int p=0;p<=points.length;p+=2) {
 605                         /*
 606                          * Create SingleRoute-class and targets
 607                          */
 608                         SingleRoute route;
 609                         if(p==points.length) {
 610                                 Collection<RoutePencil> targets = new ArrayList<RoutePencil>(1);
 611                                 for(int i=0;i<4;++i)
 612                                         if((end.directions & (1 << i)) != 0)
 613                                                 addTargets(targets, end.x, end.y, end.minDist[i], i);
 614                                 route = new SingleRoute(end.x, end.y, targets.toArray(new RoutePencil[targets.size()]),
 615                                                 ConnectionDirectionUtil.reverseDirections(end.directions), 1);
 616                         }
 617                         else {
 618                                 double x = points[p];
 619                                 double y = points[p+1];
 620                                 Collection<RoutePencil> targets = new ArrayList<RoutePencil>(4);
 621                                 for(int i=0;i<4;++i)
 622                                         addTargets(targets, x, y, 0.0, i);
 623                                 //targets[i] = new RoutePencil(x, y, x, y, 0.0, Direction.directions[i], null);
 624                                 route = new SingleRoute(x, y, targets.toArray(new RoutePencil[targets.size()]), 0xf, 1);
 625                         }
 626
 627                         /*
 628                          * Prime route class
 629                          */
 630                         if(oldRoute == null) {
 631                                 for(int i=0;i<4;++i)
 632                                         if((begin.directions & (1 << i)) != 0) {
 633                                                 Direction dir = Direction.directions[i];
 634                                                 double side = dir.getSide(begin.x, begin.y);
 635                                                 double dd = dir.getDir(begin.x, begin.y);
 636                                                 route.prime(dir, side, side, dd, begin.minDist[i]);
 637                                         }
 638                         }
 639                         else {
 640                                 for(int i=0;i<4;++i)
 641                                         if(oldRoute.solutions[i] != null)
 642                                                 route.prime(oldRoute.solutions[i]);
 643                         }
 644
 645                         /*
 646                          * Solve
 647                          */
 648                         route.solve();
 649                         /*
 650                         System.out.println("Stage " + (p/2));
 651                         for(int i=0;i<4;++i)
 652                                 if(route.solutions[i] != null)
 653                                         System.out.println("    " + i + ": " + route.solutions[i] +
 654                                                         ", penalty: " + route.solutions[i].penalty +
 655                                                         ", distance: " + route.solutions[i].distance());
 656                         */
 657                         oldRoute = route;
 658                 }
 659
 660                 /*
 661                  * Choose the best path
 662                  */
 663                 RoutePencil[] solutions = oldRoute.solutions;
 664                 Arrays.sort(solutions, solutionComparator);
 665                 if(solutions[0] == null)
 666                         return null;
 667                 return solutions[0].createPath(end.x, end.y);
 668         }
 669
 670         final static Comparator<RoutePencil> solutionComparator = new Comparator<RoutePencil>() {
 671
 672                 @Override
 673                 public int compare(RoutePencil o1, RoutePencil o2) {
 674                         if(o1 == null) {
 675                                 if(o2 == null)
 676                                         return 0;
 677                                 else
 678                                         return 1;
 679                         }
 680                         if(o2 == null)
 681                                 return -1;
 682                         if(o1.penalty < o2.penalty)
 683                                 return -1;
 684                         if(o1.penalty > o2.penalty)
 685                                 return 1;
 686                         double d1 = o1.distance();
 687                         double d2 = o2.distance();
 688                         if(d1 < d2)
 689                                 return -1;
 690                         if(d1 > d2)
 691                                 return 1;
 692                         return 0;
 693                 }
 694
 695         };
 696 }