-/*******************************************************************************\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
+/*******************************************************************************
+ * 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 gnu.trove.list.array.TIntArrayList;
+
+import java.util.Arrays;
+import java.util.Collection;
+import java.util.Comparator;
+
+public class StopSet {
+ public static class Line {
+ /**
+ * The size of xs is even. The array is strictly ordered.
+ * For all n, (xs[2n], xs[2n+1]) is an obstacle.
+ */
+ double[] xs;
+ public double y;
+
+ /* Following tables are indexed by intervals 0..xs.length such that
+ interval i is (xs[i-1],xs[i]), where xs[-1] = -INFINITY and
+ xs[xs.length] = INFINITY
+ */
+ /**
+ * Penalty of crossing the stop (null, if no penalty)
+ */
+ public Penalty[] penalty;
+ /**
+ * Next line of stop with positive penalty
+ */
+ Line[] nextLine;
+ /**
+ * Index of the first interval in the nextLine[i]
+ * that blocks fronts from this interval.
+ */
+ int[] nextPosition;
+
+ public Line(double[] xs, double y, Penalty[] penalty, Line[] nextLine, int[] nextPosition) {
+ this.xs = xs;
+ this.y = y;
+ this.penalty = penalty;
+ this.nextLine = nextLine;
+ this.nextPosition = nextPosition;
+ }
+ }
+
+ Line[] lines;
+
+ public static class Stop {
+ double x0, x1;
+ double y;
+ Penalty penalty;
+
+ public Stop(Penalty penalty, double x0, double x1, double y) {
+ this.penalty = penalty;
+ this.x0 = x0;
+ this.x1 = x1;
+ this.y = y;
+ }
+ }
+
+ public static final Comparator<Stop> stopComparator = new Comparator<Stop>() {
+
+ @Override
+ public int compare(Stop o1, Stop o2) {
+ if(o1.y < o2.y)
+ return -1;
+ else if(o1.y > o2.y)
+ return 1;
+ else if(o1.x0 < o2.x0)
+ return -1;
+ else if(o1.x0 > o2.x0)
+ return 1;
+ else
+ return 0;
+ }
+
+ };
+
+ public StopSet(Collection<Stop> _stops) {
+ if(_stops.isEmpty())
+ return;
+ Stop[] stops = _stops.toArray(new Stop[_stops.size()]);
+ Arrays.sort(stops, stopComparator);
+
+ TIntArrayList lineBegins = new TIntArrayList();
+ lineBegins.add(0);
+ for(int i=1;i<stops.length;++i)
+ if(stops[i-1].y < stops[i].y)
+ lineBegins.add(i);
+
+ lines = new Line[lineBegins.size()];
+ Line prev = null;
+ int from = stops.length;
+ for(int i=lineBegins.size()-1;i>=0;--i) {
+ int to = from;
+ from = lineBegins.get(i);
+
+ // Calculate xs and penalty
+
+ double[] xs = new double[(to-from)*2];
+ Penalty[] penalty = new Penalty[(to-from)*2+1];
+ int pointCount = 0;
+ for(int j=from;j<to;++j) {
+ Stop stop = stops[j];
+ if(pointCount == 0 || xs[pointCount-1] < stop.x0) {
+ penalty[pointCount] = null;
+ xs[pointCount++] = stop.x0;
+ penalty[pointCount] = stop.penalty;
+ xs[pointCount++] = stop.x1;
+ }
+ else if(stop.penalty.equals(penalty[pointCount-1])) {
+ if(stop.x1 > xs[pointCount-1])
+ xs[pointCount-1] = stop.x1;
+ }
+ else {
+ if(stop.x0 == xs[pointCount-1]) {
+ penalty[pointCount] = stop.penalty;
+ xs[pointCount++] = stop.x1;
+ }
+ else if(stop.x1 > xs[pointCount-1]) {
+ if(stop.penalty.penalty > penalty[pointCount-1].penalty) {
+ if(xs[pointCount-2] == stop.x0)
+ --pointCount;
+ else
+ xs[pointCount-1] = stop.x0;
+ }
+ penalty[pointCount] = stop.penalty;
+ xs[pointCount++] = stop.x1;
+ }
+ else if(stop.penalty.penalty > penalty[pointCount-1].penalty) {
+ // FIXME Not correct if more than two intersecting stops
+ }
+ }
+ }
+ penalty[pointCount] = null;
+ if(pointCount < xs.length) {
+ xs = Arrays.copyOf(xs, pointCount);
+ penalty = Arrays.copyOf(penalty, pointCount+1);
+ }
+
+ // Calculate nextLine and nextPosition
+
+ Line[] nextLine = new Line[pointCount + 1];
+ int[] nextPosition = new int[pointCount + 1];
+ if(prev != null) {
+ int prevI = 0;
+ double[] prevXs = prev.xs;
+ for(int j=0;j<=pointCount;++j) {
+ double min = j==0 ? Double.NEGATIVE_INFINITY : xs[j-1];
+ double max = j==pointCount ? Double.POSITIVE_INFINITY : xs[j];
+
+ while(prevI < prevXs.length && prevXs[prevI] <= min)
+ ++prevI;
+
+ Line prev2 = prev;
+ int prevI2 = prevI;
+ double[] prevXs2 = prevXs;
+ while(prev2.penalty[prevI2]==null && (prevI2==prevXs2.length || prevXs2[prevI2] >= max)) {
+ int temp = prev2.nextPosition[prevI2];
+ prev2 = prev2.nextLine[prevI2];
+ if(prev2 == null)
+ break;
+ prevI2 = temp;
+ prevXs2 = prev2.xs;
+ while(prevI2 < prevXs2.length && prevXs2[prevI2] <= min)
+ ++prevI2;
+ }
+ nextLine[j] = prev2;
+ nextPosition[j] = prevI2;
+ }
+ }
+ lines[i] = prev = new Line(xs, stops[from].y, penalty, nextLine, nextPosition);
+ }
+ }
+
+ public static interface IStopProcedure {
+ void blockEnd(double y);
+ void continuation(double min, double max, int pos, Line line);
+ }
+
+ public void findStops(double min, double max, double y, IStopProcedure proc) {
+ if(lines == null) {
+ proc.blockEnd(Double.POSITIVE_INFINITY);
+ return;
+ }
+ int minJ = -1;
+ int maxJ = lines.length;
+ while(maxJ > minJ+1) {
+ int middleJ = (minJ+maxJ) >>> 1;
+ double middleY = lines[middleJ].y;
+ if(middleY < y)
+ minJ = middleJ;
+ else if(middleY > y)
+ maxJ = middleJ;
+ else {
+ minJ = maxJ = middleJ;
+ break;
+ }
+ }
+
+ if(maxJ == lines.length)
+ proc.blockEnd(Double.POSITIVE_INFINITY);
+ else {
+ double[] xs = lines[maxJ].xs;
+ int minI = -1;
+ int maxI = xs.length;
+ while(maxI > minI+1) {
+ int middleI = (minI+maxI) >>> 1;
+ double middleX = xs[middleI];
+ if(middleX < min)
+ minI = middleI;
+ else if(middleX > min)
+ maxI = middleI;
+ else {
+ minI = maxI = middleI;
+ }
+ }
+ findStops(maxI, lines[maxJ], min, max, y, proc);
+ }
+
+ }
+
+ static private void findStops(int pos, Line line, double min, double max, double y, IStopProcedure proc) {
+ double[] xs = line.xs;
+ while(line.penalty[pos]==null && (pos==xs.length || xs[pos] >= max)) {
+ int temp = line.nextPosition[pos];
+ line = line.nextLine[pos];
+ if(line == null)
+ break;
+ xs = line.xs;
+ pos = temp;
+ while(pos < xs.length && xs[pos] <= min)
+ ++pos;
+ }
+ if(line == null)
+ proc.blockEnd(Double.POSITIVE_INFINITY);
+ else {
+ if(line.y > y)
+ proc.blockEnd(line.y);
+ if(pos==xs.length || xs[pos] >= max)
+ proc.continuation(min, max, pos, line);
+ else {
+ proc.continuation(min, xs[pos], pos, line);
+ while(pos+1 < xs.length && xs[pos+1] < max) {
+ ++pos;
+ proc.continuation(xs[pos-1], xs[pos], pos, line);
+ }
+ proc.continuation(xs[pos], max, pos+1, line);
+ }
+ }
+ }
+
+ static public void continueStop(final int pos, final Line line, double min, double max, IStopProcedure proc) {
+ Line newLine = line.nextLine[pos];
+ if(newLine == null)
+ proc.blockEnd(Double.POSITIVE_INFINITY);
+ else {
+ int newPos = line.nextPosition[pos];
+ double[] xs = newLine.xs;
+ while(newPos < xs.length && xs[newPos] <= min)
+ ++newPos;
+ findStops(newPos, newLine, min, max, line.y, proc);
+ }
+ }
+
+}
Code Review / simantics / platform.git / blobdiff