+/*******************************************************************************\r
+ * Copyright (c) 2007, 2010 Association for Decentralized Information Management\r
+ * in Industry THTH ry.\r
+ * All rights reserved. This program and the accompanying materials\r
+ * are made available under the terms of the Eclipse Public License v1.0\r
+ * which accompanies this distribution, and is available at\r
+ * http://www.eclipse.org/legal/epl-v10.html\r
+ *\r
+ * Contributors:\r
+ * VTT Technical Research Centre of Finland - initial API and implementation\r
+ *******************************************************************************/\r
+package org.simantics.g2d.routing.algorithm1;\r
+\r
+import gnu.trove.list.array.TIntArrayList;\r
+\r
+import java.util.Arrays;\r
+import java.util.Collection;\r
+import java.util.Comparator;\r
+\r
+public class StopSet {\r
+ public static class Line {\r
+ /**\r
+ * The size of xs is even. The array is strictly ordered. \r
+ * For all n, (xs[2n], xs[2n+1]) is an obstacle. \r
+ */\r
+ double[] xs;\r
+ public double y;\r
+ \r
+ /* Following tables are indexed by intervals 0..xs.length such that\r
+ interval i is (xs[i-1],xs[i]), where xs[-1] = -INFINITY and\r
+ xs[xs.length] = INFINITY \r
+ */\r
+ /**\r
+ * Penalty of crossing the stop (null, if no penalty) \r
+ */\r
+ public Penalty[] penalty;\r
+ /**\r
+ * Next line of stop with positive penalty\r
+ */\r
+ Line[] nextLine; \r
+ /**\r
+ * Index of the first interval in the nextLine[i] \r
+ * that blocks fronts from this interval.\r
+ */\r
+ int[] nextPosition; \r
+ \r
+ public Line(double[] xs, double y, Penalty[] penalty, Line[] nextLine, int[] nextPosition) {\r
+ this.xs = xs;\r
+ this.y = y;\r
+ this.penalty = penalty;\r
+ this.nextLine = nextLine;\r
+ this.nextPosition = nextPosition;\r
+ } \r
+ }\r
+ \r
+ Line[] lines;\r
+ \r
+ public static class Stop {\r
+ double x0, x1;\r
+ double y;\r
+ Penalty penalty;\r
+ \r
+ public Stop(Penalty penalty, double x0, double x1, double y) {\r
+ this.penalty = penalty;\r
+ this.x0 = x0;\r
+ this.x1 = x1;\r
+ this.y = y;\r
+ } \r
+ }\r
+\r
+ public static final Comparator<Stop> stopComparator = new Comparator<Stop>() {\r
+\r
+ @Override\r
+ public int compare(Stop o1, Stop o2) {\r
+ if(o1.y < o2.y)\r
+ return -1;\r
+ else if(o1.y > o2.y)\r
+ return 1;\r
+ else if(o1.x0 < o2.x0)\r
+ return -1;\r
+ else if(o1.x0 > o2.x0)\r
+ return 1;\r
+ else\r
+ return 0;\r
+ }\r
+ \r
+ };\r
+ \r
+ public StopSet(Collection<Stop> _stops) {\r
+ if(_stops.isEmpty())\r
+ return;\r
+ Stop[] stops = _stops.toArray(new Stop[_stops.size()]);\r
+ Arrays.sort(stops, stopComparator);\r
+ \r
+ TIntArrayList lineBegins = new TIntArrayList();\r
+ lineBegins.add(0);\r
+ for(int i=1;i<stops.length;++i)\r
+ if(stops[i-1].y < stops[i].y)\r
+ lineBegins.add(i);\r
+ \r
+ lines = new Line[lineBegins.size()];\r
+ Line prev = null;\r
+ int from = stops.length;\r
+ for(int i=lineBegins.size()-1;i>=0;--i) {\r
+ int to = from;\r
+ from = lineBegins.get(i);\r
+ \r
+ // Calculate xs and penalty\r
+ \r
+ double[] xs = new double[(to-from)*2];\r
+ Penalty[] penalty = new Penalty[(to-from)*2+1];\r
+ int pointCount = 0;\r
+ for(int j=from;j<to;++j) {\r
+ Stop stop = stops[j];\r
+ if(pointCount == 0 || xs[pointCount-1] < stop.x0) {\r
+ penalty[pointCount] = null;\r
+ xs[pointCount++] = stop.x0;\r
+ penalty[pointCount] = stop.penalty;\r
+ xs[pointCount++] = stop.x1; \r
+ }\r
+ else if(stop.penalty.equals(penalty[pointCount-1])) {\r
+ if(stop.x1 > xs[pointCount-1])\r
+ xs[pointCount-1] = stop.x1;\r
+ }\r
+ else {\r
+ if(stop.x0 == xs[pointCount-1]) {\r
+ penalty[pointCount] = stop.penalty;\r
+ xs[pointCount++] = stop.x1;\r
+ }\r
+ else if(stop.x1 > xs[pointCount-1]) {\r
+ if(stop.penalty.penalty > penalty[pointCount-1].penalty) {\r
+ if(xs[pointCount-2] == stop.x0)\r
+ --pointCount;\r
+ else\r
+ xs[pointCount-1] = stop.x0;\r
+ }\r
+ penalty[pointCount] = stop.penalty;\r
+ xs[pointCount++] = stop.x1;\r
+ }\r
+ else if(stop.penalty.penalty > penalty[pointCount-1].penalty) {\r
+ // FIXME Not correct if more than two intersecting stops \r
+ }\r
+ }\r
+ }\r
+ penalty[pointCount] = null;\r
+ if(pointCount < xs.length) {\r
+ xs = Arrays.copyOf(xs, pointCount);\r
+ penalty = Arrays.copyOf(penalty, pointCount+1);\r
+ }\r
+ \r
+ // Calculate nextLine and nextPosition\r
+ \r
+ Line[] nextLine = new Line[pointCount + 1];\r
+ int[] nextPosition = new int[pointCount + 1];\r
+ if(prev != null) {\r
+ int prevI = 0;\r
+ double[] prevXs = prev.xs;\r
+ for(int j=0;j<=pointCount;++j) {\r
+ double min = j==0 ? Double.NEGATIVE_INFINITY : xs[j-1];\r
+ double max = j==pointCount ? Double.POSITIVE_INFINITY : xs[j];\r
+ \r
+ while(prevI < prevXs.length && prevXs[prevI] <= min)\r
+ ++prevI;\r
+ \r
+ Line prev2 = prev;\r
+ int prevI2 = prevI;\r
+ double[] prevXs2 = prevXs;\r
+ while(prev2.penalty[prevI2]==null && (prevI2==prevXs2.length || prevXs2[prevI2] >= max)) {\r
+ int temp = prev2.nextPosition[prevI2];\r
+ prev2 = prev2.nextLine[prevI2];\r
+ if(prev2 == null)\r
+ break;\r
+ prevI2 = temp;\r
+ prevXs2 = prev2.xs; \r
+ while(prevI2 < prevXs2.length && prevXs2[prevI2] <= min)\r
+ ++prevI2;\r
+ }\r
+ nextLine[j] = prev2;\r
+ nextPosition[j] = prevI2; \r
+ }\r
+ }\r
+ lines[i] = prev = new Line(xs, stops[from].y, penalty, nextLine, nextPosition); \r
+ }\r
+ }\r
+ \r
+ public static interface IStopProcedure {\r
+ void blockEnd(double y);\r
+ void continuation(double min, double max, int pos, Line line);\r
+ }\r
+ \r
+ public void findStops(double min, double max, double y, IStopProcedure proc) {\r
+ if(lines == null) {\r
+ proc.blockEnd(Double.POSITIVE_INFINITY);\r
+ return;\r
+ }\r
+ int minJ = -1;\r
+ int maxJ = lines.length;\r
+ while(maxJ > minJ+1) {\r
+ int middleJ = (minJ+maxJ) >>> 1;\r
+ double middleY = lines[middleJ].y;\r
+ if(middleY < y)\r
+ minJ = middleJ;\r
+ else if(middleY > y)\r
+ maxJ = middleJ;\r
+ else {\r
+ minJ = maxJ = middleJ;\r
+ break;\r
+ }\r
+ }\r
+ \r
+ if(maxJ == lines.length)\r
+ proc.blockEnd(Double.POSITIVE_INFINITY);\r
+ else {\r
+ double[] xs = lines[maxJ].xs;\r
+ int minI = -1;\r
+ int maxI = xs.length;\r
+ while(maxI > minI+1) {\r
+ int middleI = (minI+maxI) >>> 1;\r
+ double middleX = xs[middleI];\r
+ if(middleX < min)\r
+ minI = middleI;\r
+ else if(middleX > min)\r
+ maxI = middleI;\r
+ else {\r
+ minI = maxI = middleI;\r
+ }\r
+ }\r
+ findStops(maxI, lines[maxJ], min, max, y, proc);\r
+ }\r
+ \r
+ }\r
+ \r
+ static private void findStops(int pos, Line line, double min, double max, double y, IStopProcedure proc) {\r
+ double[] xs = line.xs;\r
+ while(line.penalty[pos]==null && (pos==xs.length || xs[pos] >= max)) {\r
+ int temp = line.nextPosition[pos];\r
+ line = line.nextLine[pos];\r
+ if(line == null)\r
+ break;\r
+ xs = line.xs; \r
+ pos = temp;\r
+ while(pos < xs.length && xs[pos] <= min)\r
+ ++pos;\r
+ }\r
+ if(line == null)\r
+ proc.blockEnd(Double.POSITIVE_INFINITY);\r
+ else {\r
+ if(line.y > y)\r
+ proc.blockEnd(line.y);\r
+ if(pos==xs.length || xs[pos] >= max)\r
+ proc.continuation(min, max, pos, line);\r
+ else {\r
+ proc.continuation(min, xs[pos], pos, line);\r
+ while(pos+1 < xs.length && xs[pos+1] < max) {\r
+ ++pos;\r
+ proc.continuation(xs[pos-1], xs[pos], pos, line);\r
+ }\r
+ proc.continuation(xs[pos], max, pos+1, line);\r
+ }\r
+ }\r
+ }\r
+ \r
+ static public void continueStop(final int pos, final Line line, double min, double max, IStopProcedure proc) {\r
+ Line newLine = line.nextLine[pos];\r
+ if(newLine == null)\r
+ proc.blockEnd(Double.POSITIVE_INFINITY);\r
+ else {\r
+ int newPos = line.nextPosition[pos];\r
+ double[] xs = newLine.xs;\r
+ while(newPos < xs.length && xs[newPos] <= min)\r
+ ++newPos;\r
+ findStops(newPos, newLine, min, max, line.y, proc); \r
+ }\r
+ }\r
+\r
+}\r