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