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

   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
   8  *\r
   9  * Contributors:\r
  10  *     VTT Technical Research Centre of Finland - initial API and implementation\r
  11  *******************************************************************************/\r
  12 package org.simantics.g2d.routing.algorithm1;\r
  13 \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
  21 \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
  28 \r
  29 public class StaticRouter {\r
  30                 \r
  31         StopSet[] stopSets = new StopSet[4];\r
  32         //Graphics2D g;\r
  33         \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
  44                 }\r
  45         }\r
  46                 \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
  52                                 \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
  59                                 if(old != null) {\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
  66                                                 ));                                             \r
  67                                         }\r
  68                                         else {\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
  74                                                 ));\r
  75                                         }\r
  76                                 }\r
  77                                 it.next();\r
  78                                 old = cur;\r
  79                         }\r
  80                 }\r
  81                 \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
 103                 }\r
 104         }*/\r
 105         \r
 106         static class Continuation implements Comparable<Continuation> {\r
 107                 Direction direction;\r
 108                 double min;\r
 109                 double max;\r
 110                 double front;\r
 111                 Line line;\r
 112                 int pos;\r
 113                 RoutePencil parent;\r
 114                 double penalty;\r
 115                 double minTurnDistance;\r
 116                 double priority;        \r
 117                 double distance;\r
 118 \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
 123                         this.min = min;\r
 124                         this.max = max;\r
 125                         this.front = front;\r
 126                         this.pos = pos;\r
 127                         this.line = line;\r
 128                         this.parent = parent;\r
 129                         this.penalty = penalty;\r
 130                         this.minTurnDistance = minTurnDistance;\r
 131                         calcDistance(targetX, targetY);\r
 132                 }\r
 133 \r
 134                 @Override\r
 135                 public int compareTo(Continuation o) {\r
 136                         if(priority < o.priority)\r
 137                                 return -1;\r
 138                         if(priority > o.priority)\r
 139                                 return 1;\r
 140                         if(distance < o.distance)\r
 141                                 return -1;\r
 142                         if(distance > o.distance)\r
 143                                 return 1;\r
 144                         return 0;\r
 145                 }               \r
 146 \r
 147                 @Override\r
 148                 public int hashCode() {\r
 149                         final int prime = 31;\r
 150                         int result = direction.getId();\r
 151                         long temp;\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
 158                         return result;\r
 159                 }\r
 160 \r
 161                 @Override\r
 162                 public boolean equals(Object obj) {\r
 163                         if (this == obj)\r
 164                                 return true;\r
 165                         if (obj == null)\r
 166                                 return false;\r
 167                         if (getClass() != obj.getClass())\r
 168                                 return false;\r
 169                         Continuation other = (Continuation) obj;\r
 170                         if (direction == null) {\r
 171                                 if (other.direction != null)\r
 172                                         return false;\r
 173                         } else if (direction!=other.direction)\r
 174                                 return false;\r
 175                         if (Double.doubleToLongBits(front) != Double\r
 176                                 .doubleToLongBits(other.front))\r
 177                                 return false;\r
 178                         if (Double.doubleToLongBits(max) != Double\r
 179                                 .doubleToLongBits(other.max))\r
 180                                 return false;\r
 181                         if (Double.doubleToLongBits(min) != Double\r
 182                                 .doubleToLongBits(other.min))\r
 183                                 return false;\r
 184                         return true;\r
 185                 }\r
 186                 \r
 187                 void calcDistance(double x, double y) {\r
 188                         if(parent == null)\r
 189                                 distance = 0.0;                 \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
 195                                 else\r
 196                                         distance += parent.distanceX*front;\r
 197                                 distance += Math.abs(x-front);\r
 198                                 if(y < min)\r
 199                                         distance += parent.distanceY*min + (min - y);\r
 200                                 else if(y > max)\r
 201                                         distance += parent.distanceY*max + (y - max);\r
 202                                 else\r
 203                                         distance += parent.distanceY*y;\r
 204                                 break;\r
 205                         case Constants.SOUTH:\r
 206                                 if(front > parent.y1)\r
 207                                         distance += parent.distanceY*parent.y1 + (front-parent.y1);\r
 208                                 else\r
 209                                         distance += parent.distanceY*front;\r
 210                                 distance += Math.abs(y-front);\r
 211                                 if(x < min)\r
 212                                         distance += parent.distanceX*min + (min - x);\r
 213                                 else if(x > max)\r
 214                                         distance += parent.distanceX*max + (x - max);\r
 215                                 else\r
 216                                         distance += parent.distanceX*x;\r
 217                                 break;\r
 218                         case Constants.WEST:\r
 219                                 if(-front < parent.x0)\r
 220                                         distance += parent.distanceX*parent.x0 + (parent.x0+front);\r
 221                                 else\r
 222                                         distance += parent.distanceX*(-front);\r
 223                                 distance += Math.abs(x+front);\r
 224                                 if(y < min)\r
 225                                         distance += parent.distanceY*min + (min - y);\r
 226                                 else if(y > max)\r
 227                                         distance += parent.distanceY*max + (y - max);\r
 228                                 else\r
 229                                         distance += parent.distanceY*y;\r
 230                                 break;\r
 231                         case Constants.NORTH:\r
 232                                 if(-front < parent.y0)\r
 233                                         distance += parent.distanceY*parent.y0 + (parent.y0+front);\r
 234                                 else\r
 235                                         distance += parent.distanceY*(-front);\r
 236                                 distance += Math.abs(y+front);\r
 237                                 if(x < min)\r
 238                                         distance += parent.distanceX*min + (min - x);\r
 239                                 else if(x > max)\r
 240                                         distance += parent.distanceX*max + (x - max);\r
 241                                 else\r
 242                                         distance += parent.distanceX*x;\r
 243                                 break;\r
 244                         }\r
 245                 }\r
 246                 \r
 247         }\r
 248         \r
 249         static class Cost implements Comparable<Cost> {\r
 250                 double penalty;\r
 251                 double distance;                \r
 252                 \r
 253                 public Cost(double penalty, double distance) {\r
 254                         this.penalty = penalty;\r
 255                         this.distance = distance;\r
 256                 }\r
 257                 \r
 258                 @Override\r
 259                 public int compareTo(Cost o) {\r
 260                         if(penalty < o.penalty)\r
 261                                 return -1;\r
 262                         if(penalty > o.penalty)\r
 263                                 return 1;\r
 264                         if(distance < o.distance)\r
 265                                 return -1;\r
 266                         if(distance > o.distance)\r
 267                                 return 1;\r
 268                         return 0;\r
 269                 }                       \r
 270         }\r
 271         \r
 272         class SingleRoute {\r
 273                 PriorityQueue<Continuation> queue = new PriorityQueue<Continuation>();\r
 274                 double targetX, targetY;\r
 275                 RoutePencil[] targets;\r
 276                 int targetDirs;\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
 283 \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
 293                         \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
 298                         if(minPenalty > 0)\r
 299                                 for(RoutePencil target : targets)\r
 300                                         target.penalty -= minPenalty;\r
 301                 }\r
 302 \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
 310                         }\r
 311                         else\r
 312                                 cont.priority += 2*Penalty.BEND_PENALTY;\r
 313                 }\r
 314                 \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
 318                                 targetX, targetY);\r
 319                         addContinutation(cont);\r
 320                 }\r
 321                 \r
 322                 void prime(RoutePencil pencil) {\r
 323                         Direction dir = pencil.direction;\r
 324                         Continuation cont = new Continuation(\r
 325                                 dir, \r
 326                                 dir.minSide(pencil),\r
 327                                 dir.maxSide(pencil),\r
 328                                 dir.back(pencil),\r
 329                                 0, null, pencil, pencil.penalty, 0.0,\r
 330                                 targetX, targetY);\r
 331                         addContinutation(cont);\r
 332                 }\r
 333                 \r
 334                 void solve() {\r
 335                         int count = 0;\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
 341                                         return;\r
 342                                 }\r
 343                                 doContinuation(cont);                           \r
 344                                 ++count;                                \r
 345                         }\r
 346                 }\r
 347                 \r
 348                 void addContinutation(Continuation cont) {      \r
 349                         Continuation old = continuations.get(cont);\r
 350                         if(old != null) {\r
 351                                 if(old.priority < cont.priority)\r
 352                                         return;\r
 353                                 if(old.priority == cont.priority && old.distance <= cont.distance)\r
 354                                         return;\r
 355                         }\r
 356                         continuations.put(cont, cont);\r
 357                         calcPriority(cont);\r
 358                         queue.add(cont);\r
 359                 }\r
 360                 \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
 368                                                 continue;\r
 369                                         dir = (dir+2)%4;\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
 378                                         }\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
 385                                                 if(d1 < d2) {\r
 386                                                         solutions[dir] = sol;\r
 387                                                         foundSolution = true;\r
 388                                                 }\r
 389                                         }\r
 390                                         else\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
 396                                 }                       \r
 397                         if(foundSolution) {                             \r
 398                                 for(int i=0;i<4;++i)\r
 399                                         sortedCosts[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
 405                         }                       \r
 406                         else {\r
 407                                 {\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
 414                                                                 front,\r
 415                                                                 0, null, pencil, pencil.penalty+Penalty.BEND_PENALTY, 0.0,\r
 416                                                                 targetX, targetY);\r
 417                                                 addContinutation(leftCont);\r
 418                                         }\r
 419                                 }\r
 420 \r
 421                                 {\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
 430                                                                 targetX, targetY);\r
 431                                                 addContinutation(rightCont);\r
 432                                         }\r
 433                                 }\r
 434                         }\r
 435                 }\r
 436 \r
 437 \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
 441 \r
 442                                 @Override\r
 443                                 public void blockEnd(double y) {                                        \r
 444                                         double front = cont.front + cont.minTurnDistance;\r
 445                                         //System.out.println(cont.minTurnDistance);\r
 446                                         if(front <= y)\r
 447                                                 addBend(cont.direction.createPencil(front, y, cont.min, cont.max, cont.penalty, cont.parent));\r
 448                                 }\r
 449 \r
 450                                 @Override\r
 451                                 public void continuation(double min, double max, int pos, Line line) {\r
 452                                         double newPenalty = cont.penalty;\r
 453                                         double newMinTurnDistance; \r
 454                                                 \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
 460                                         }\r
 461                                         else\r
 462                                                 newMinTurnDistance = Math.max(0.0, \r
 463                                                                 cont.minTurnDistance - line.y + cont.front);\r
 464                                         Continuation newCont = new Continuation(cont.direction,\r
 465                                                 min, max, line.y,\r
 466                                                 pos, line, cont.parent, newPenalty, newMinTurnDistance,\r
 467                                                 targetX, targetY);\r
 468                                         addContinutation(newCont);                                      \r
 469                                 }\r
 470                                 \r
 471                         };\r
 472                         if(cont.line != null)\r
 473                                 StopSet.continueStop(cont.pos, cont.line, cont.min, cont.max, proc);\r
 474                         else {\r
 475                                 StopSet ss = stopSets[cont.direction.getId()];\r
 476                                 ss.findStops(cont.min, cont.max, cont.front, proc);\r
 477                         }\r
 478                 }\r
 479         }\r
 480         static double makeFinite(double x) {\r
 481                 if(x == Double.POSITIVE_INFINITY)\r
 482                         return 10000.0;\r
 483                 if(x == Double.NEGATIVE_INFINITY)\r
 484                         return -10000.0;\r
 485                 return x;\r
 486         }\r
 487         \r
 488         class TargetFinder implements IStopProcedure {\r
 489                 \r
 490                 Collection<RoutePencil> targets;\r
 491                 Direction dir;\r
 492                 double dd;\r
 493                 double side;            \r
 494                 double penalty;\r
 495                 double minDistance;\r
 496                 \r
 497                 public TargetFinder(Direction dir, double dd, double side, double penalty,\r
 498                                 double minDistance,\r
 499                                 Collection<RoutePencil> targets) {\r
 500                         this.dd = dd;\r
 501                         this.dir = dir;\r
 502                         this.side = side;\r
 503                         this.targets = targets;\r
 504                         this.penalty = penalty;\r
 505                         this.minDistance = minDistance;\r
 506                 }\r
 507 \r
 508                 @Override\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
 513                 }\r
 514 \r
 515                 @Override\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
 520                                 return;\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
 525                 }\r
 526         }\r
 527         \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
 535         }       \r
 536         \r
 537         public Path2D route(Terminal begin, double[] points, Terminal end) {\r
 538                 \r
 539                 /* Handle begin==null (end==null) case by taking the first (last) \r
 540                  * route point as terminal \r
 541                  */\r
 542                 \r
 543                 if(begin == null) {\r
 544                         if(points.length < 2)\r
 545                                 return null;\r
 546                         begin = new Terminal(points[0], points[1], 0x1, Terminal.ZEROS, null);\r
 547                         points = Arrays.copyOfRange(points, 2, points.length);\r
 548                 }\r
 549                 if(end == null) {\r
 550                         if(points.length < 2)\r
 551                                 return null;\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
 554                 }\r
 555                 \r
 556                 {\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
 568                             return path;\r
 569                         }\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
 577                             return path;\r
 578                         }\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
 586                     return path;\r
 587                 }\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
 595                     return path;\r
 596                 }\r
 597                     }\r
 598                 }\r
 599                 \r
 600                 /*\r
 601                  * Route\r
 602                  */\r
 603                 SingleRoute oldRoute = null;\r
 604                 for(int p=0;p<=points.length;p+=2) {\r
 605                         /*\r
 606                          * Create SingleRoute-class and targets\r
 607                          */\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
 616                         }\r
 617                         else {\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
 625                         }\r
 626                         \r
 627                         /*\r
 628                          * Prime route class\r
 629                          */\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
 637                                         }\r
 638                         }\r
 639                         else {\r
 640                                 for(int i=0;i<4;++i)\r
 641                                         if(oldRoute.solutions[i] != null)\r
 642                                                 route.prime(oldRoute.solutions[i]);\r
 643                         }\r
 644                         \r
 645                         /*\r
 646                          * Solve\r
 647                          */                     \r
 648                         route.solve();\r
 649                         /*\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
 656                         */\r
 657                         oldRoute = route;                       \r
 658                 }               \r
 659                 \r
 660                 /*\r
 661                  * Choose the best path\r
 662                  */\r
 663                 RoutePencil[] solutions = oldRoute.solutions;\r
 664                 Arrays.sort(solutions, solutionComparator);\r
 665                 if(solutions[0] == null)\r
 666                         return null;            \r
 667                 return solutions[0].createPath(end.x, end.y);\r
 668         }\r
 669 \r
 670         final static Comparator<RoutePencil> solutionComparator = new Comparator<RoutePencil>() {\r
 671 \r
 672                 @Override\r
 673                 public int compare(RoutePencil o1, RoutePencil o2) {\r
 674                         if(o1 == null) {\r
 675                                 if(o2 == null)\r
 676                                         return 0;\r
 677                                 else\r
 678                                         return 1;\r
 679                         }\r
 680                         if(o2 == null)\r
 681                                 return -1;\r
 682                         if(o1.penalty < o2.penalty)\r
 683                                 return -1;\r
 684                         if(o1.penalty > o2.penalty)\r
 685                                 return 1;\r
 686                         double d1 = o1.distance();\r
 687                         double d2 = o2.distance();\r
 688                         if(d1 < d2)\r
 689                                 return -1;\r
 690                         if(d1 > d2)\r
 691                                 return 1;\r
 692                         return 0;\r
 693                 }\r
 694                 \r
 695         };\r
 696 }\r