Source code: com/port80/graph/dot/impl/ShortestPath.java
1 //
2 // Copyright(c) 2002, Chris Leung
3 //
4
5 package com.port80.graph.dot.impl;
6
7 import com.port80.util.msg;
8
9 public class ShortestPath {
10
11 ////////////////////////////////////////////////////////////////////////
12
13 private static final String NAME = "ShortestPath";
14 private static final boolean DEBUG = false;
15
16 private static final int ISCCW = 1;
17 private static final int ISCW = 2;
18 private static final int ISON = 3;
19
20 private static final int DQ_FRONT = 1;
21 private static final int DQ_BACK = 2;
22
23 ////////////////////////////////////////////////////////////////////////
24
25 static class PointLink {
26 DotPoint pt;
27 /** link points to the previous point on the shortest path.*/
28 PointLink link;
29 public PointLink(DotPoint pt) {
30 this.pt = pt;
31 }
32 public String toString() {
33 return pt.toString();
34 }
35 }
36
37 ////////////////////////////////////////
38
39 static class TriEdge {
40 PointLink p0;
41 PointLink p1;
42 Triangle right; /** Neighbour triangle.*/
43 public TriEdge(PointLink p0, PointLink p1, Triangle right) {
44 this.p0 = p0;
45 this.p1 = p1;
46 this.right = right;
47 }
48 }
49
50 static class Triangle {
51 int mark; /** mark=0,1,2.*/
52 int index; /** Index in triangles.*/
53 TriEdge[] e;
54
55 public Triangle(PointLink a, PointLink b, PointLink c) {
56 e = new TriEdge[3];
57 e[0] = new TriEdge(a, b, null);
58 e[1] = new TriEdge(b, c, null);
59 e[2] = new TriEdge(c, a, null);
60 }
61 public boolean contains(DotPoint pt) {
62 int sum = 0;
63 for (int i = 0; i < 3; i++)
64 if (ccw(e[i].p0.pt, e[i].p1.pt, pt) != ISCW)
65 sum++;
66 return (sum == 3 || sum == 0);
67 }
68 public String toString() {
69 return e[0].p0.pt.toString() + " " + e[1].p0.pt.toString() + " " + e[2].p0.pt.toString();
70 }
71 }
72
73 ////////////////////////////////////////
74
75 static class Dequeue {
76 PointLink[] pts;
77 int first;
78 int last;
79 int pivot;
80
81 public Dequeue() {
82 this(8);
83 }
84 public Dequeue(int n) {
85 pts = new PointLink[n];
86 first = n / 2;
87 last = first - 1;
88 }
89 /** Grow pts array by size 'n'. */
90 public void grow(int n) {
91 growTo(pts.length + n);
92 }
93 /** Grow pts array to size 'n'. */
94 public void growTo(int n) {
95 if (n <= pts.length)
96 return;
97 PointLink[] a = new PointLink[n];
98 int offset = (n - pts.length) * (n - last) / (first + n - last);
99 System.arraycopy(pts, first, a, first + offset, last - first + 1);
100 pts = a;
101 first += offset;
102 last += offset;
103 pivot += offset;
104 }
105 public int size() {
106 return last - first + 1;
107 }
108 /** Add a point to the front. */
109 public void prepend(PointLink pt) {
110 if (last >= first)
111 pt.link = pts[first]; /* shortest path links */
112 pts[--first] = pt;
113 }
114 /** Add a point to the back. */
115 public void append(PointLink pt) {
116 if (last >= first)
117 pt.link = pts[last]; /* shortest path links */
118 pts[++last] = pt;
119 }
120 public void removeAfter(int index) {
121 last = index;
122 if (pivot > last)
123 pivot = last;
124 }
125 public void removeBefore(int index) {
126 first = index;
127 if (pivot < first)
128 pivot = first;
129 }
130 public int splitIndexOf(PointLink pt) {
131 for (int i = first; i < pivot; ++i)
132 if (ccw(pts[i + 1].pt, pts[i].pt, pt.pt) == ISCCW)
133 return i;
134 for (int i = last; i > pivot; --i)
135 if (ccw(pts[i - 1].pt, pts[i].pt, pt.pt) == ISCW)
136 return i;
137 return pivot;
138 }
139
140 }
141
142 ////////////////////////////////////////////////////////////////////////
143
144 private PointLink[] ptList;
145 private Triangle[] triangles;
146 private int nTri;
147 private Dequeue dq;
148
149 // Static methods //////////////////////////////////////////////////////
150 //
151
152 public static int find(DotPolyline bound, DotPoint[] endpts, DotPolyline ret) {
153 return new ShortestPath().shortestPath(bound, endpts, ret);
154 }
155
156 // Instance methods ////////////////////////////////////////////////////
157 //
158
159 public int shortestPath(DotPolyline bound, DotPoint[] endpts, DotPolyline ret) {
160
161 if(DEBUG) msg.println(NAME+".shortestPath(): bound=\n"+bound.toGeneralPath());
162 /* make space */
163 dq = new Dequeue(bound.size * 2);
164 triangles = new Triangle[bound.size];
165 nTri = 0;
166
167 /* make sure polygon is CCW and load ptList array */
168 ptList = new PointLink[bound.size];
169 for (int i = 0; i < bound.size; i++) {
170 ptList[i] = new PointLink(bound.pts[i]);
171 }
172
173 /* generate list of triangles */
174 PointLink[] pts = new PointLink[bound.size + 2];
175 System.arraycopy(ptList, 0, pts, 0, bound.size);
176 pts[bound.size] = pts[0];
177 pts[bound.size + 1] = pts[1];
178 triangulate(pts, bound.size);
179 if (DEBUG) {
180 msg.println(NAME + ".find(): triangles: " + nTri);
181 for (int i = 0; i < nTri; ++i)
182 msg.println(triangles[i].toString());
183 }
184
185 /* connect all pairs of triangles that share an edge */
186 for (int i = 0; i < nTri; i++)
187 for (int j = i + 1; j < nTri; j++)
188 connectTriangles(triangles[i], triangles[j]);
189
190 /* find first and last triangles */
191 int fsttri = -1;
192 int lsttri = -1;
193 for (int i = 0; i < nTri; i++) {
194 if (triangles[i].contains(endpts[0])) {
195 fsttri = i;
196 break;
197 }
198 }
199 if (fsttri < 0) {
200 msg.err(NAME + ".find(): source point not in any triangle: " + endpts[0].toString());
201 return -1;
202 }
203 for (int i = 0; i < nTri; i++) {
204 if (triangles[i].contains(endpts[1])) {
205 lsttri = i;
206 break;
207 }
208 }
209 if (lsttri < 0) {
210 msg.err(NAME + ".find(): destination point not in any triangle: " + endpts[1].toString());
211 return -1;
212 }
213
214 /* mark the strip of triangles from endpts[0] to endpts[1] */
215 if (!markTrianglePath(triangles, fsttri, lsttri)) {
216 msg.fatal(NAME + ".find(): cannot find triangle path: fsttri=" + fsttri + ", lsttri=" + lsttri);
217 }
218
219 /* if endpoints in same triangle,use a single line */
220 if (fsttri == lsttri) {
221 ret.add(endpts[0]);
222 ret.add(endpts[1]);
223 if (DEBUG)
224 msg.println(NAME + ".find(): return straight line.");
225 return 0;
226 }
227
228 /* build funnel and shortest path linked list. */
229 PointLink[] eptList = new PointLink[2];
230 eptList[0] = new PointLink(endpts[0]);
231 eptList[1] = new PointLink(endpts[1]);
232 dq.prepend(eptList[0]);
233 dq.pivot = dq.first;
234
235 PointLink lpt, rpt;
236 for (int ti = fsttri; ti >= 0;) {
237 Triangle tri = triangles[ti];
238 tri.mark = 2;
239 // Find the left and right points of the exiting edge.
240 {
241 int ei;
242 for (ei = 0; ei < 3; ei++)
243 if (tri.e[ei].right != null && tri.e[ei].right.mark == 1)
244 break;
245 if (ei == 3) { // In last triangle.
246 if (ccw(endpts[1], dq.pts[dq.first].pt, dq.pts[dq.last].pt) == ISCCW) {
247 lpt = dq.pts[dq.last];
248 rpt = eptList[1];
249 } else {
250 lpt = eptList[1];
251 rpt = dq.pts[dq.last];
252 }
253 } else {
254 PointLink pt = tri.e[(ei + 1) % 3].p1;
255 if (ccw(tri.e[ei].p0.pt, pt.pt, tri.e[ei].p1.pt) == ISCCW) {
256 lpt = tri.e[ei].p1;
257 rpt = tri.e[ei].p0;
258 } else {
259 lpt = tri.e[ei].p0;
260 rpt = tri.e[ei].p1;
261 }
262 }
263 }
264
265 /* update deque */
266 if (ti == fsttri) {
267 dq.append(lpt);
268 dq.prepend(rpt);
269 } else if (dq.pts[dq.first] != rpt && dq.pts[dq.last] != rpt) {
270 /* add right point to deque */
271 dq.removeBefore(dq.splitIndexOf(rpt));
272 dq.prepend(rpt);
273 } else {
274 /* add left point to deque */
275 dq.removeAfter(dq.splitIndexOf(lpt));
276 dq.append(lpt);
277 }
278 ti = -1;
279 for (int i = 0; i < 3; i++) {
280 if (tri.e[i].right != null && tri.e[i].right.mark == 1) {
281 ti = tri.e[i].right.index;
282 break;
283 }
284 }
285 }
286
287 // Backtrace the shortest path.
288 {
289 int n;
290 PointLink pt = eptList[1];
291 for (n = 0; pt != null; pt = pt.link)
292 n++;
293 ret.growTo(n);
294 ret.size = n;
295 for (pt = eptList[1]; pt != null; pt = pt.link) {
296 ret.pts[--n] = pt.pt;
297 }
298 if (n != 0)
299 msg.err(NAME + ".find(): n=" + n);
300 else if (DEBUG)
301 msg.println(NAME + ".find(): ret=" + ret.toString());
302 }
303 return 0;
304 }
305
306 /* triangulate polygon */
307 private void triangulate(PointLink[] pts, int max) {
308 if (false)
309 msg.println(
310 NAME
311 + ".triangulate(): max="
312 + max
313 + ": p0="
314 + pts[0]
315 + ", p1="
316 + pts[1]
317 + ", p2="
318 + pts[2]);
319 if (max <= 3) {
320 loadTriangle(pts[0], pts[1], pts[2]);
321 return;
322 }
323 for (int i = 0; i <max; i++) {
324 int ip1 = i + 1;
325 int ip2 = i + 2;
326 if (isDiagonal(i, ip2, pts, max)) {
327 loadTriangle(pts[i], pts[ip1], pts[ip2]);
328 for (i = ip1; i < max - 1; i++)
329 pts[i] = pts[i + 1];
330 triangulate(pts, max - 1);
331 return;
332 }
333 }
334 msg.fatal(NAME + ".triangulate(): max=" + max);
335 }
336
337 /* check if(i,i+2) is a diagonal */
338 private boolean isDiagonal(int i, int ip2, PointLink[] pts, int max) {
339
340 /* neighborhood test */
341 int ip1 = (i + 1) % max;
342 int im1 = (i + max - 1) % max;
343 boolean res;
344
345 /* If P[i] is a convex vertex [ i+1 left of(i-1,i) ]. */
346 if (ccw(pts[im1].pt, pts[i].pt, pts[ip1].pt) == ISCCW)
347 res =
348 (ccw(pts[i].pt, pts[ip2].pt, pts[im1].pt) == ISCCW)
349 && (ccw(pts[ip2].pt, pts[i].pt, pts[ip1].pt) == ISCCW);
350 /* Assume(i-1,i,i+1) not collinear. */
351 else
352 res = (ccw(pts[i].pt, pts[ip2].pt, pts[ip1].pt) == ISCW);
353
354 if (!res)
355 return false;
356
357 /* check against all other edges */
358 for (int j = 0; j < max; j++) {
359 int jp1 = (j + 1) % max;
360 if (!((j == i) || (j == ip2) || (jp1 == i) || (jp1 == ip2)))
361 if (intersects(pts[i].pt, pts[ip2].pt, pts[j].pt, pts[jp1].pt))
362 return false;
363 }
364 if (false)
365 msg.println(NAME + ".isDiagonal(): i=" + i);
366 return true;
367 }
368
369 private void loadTriangle(PointLink a, PointLink b, PointLink c) {
370 if (nTri >= triangles.length) {
371 Triangle[] tmp = new Triangle[triangles.length + 20];
372 System.arraycopy(triangles, 0, tmp, 0, nTri);
373 triangles = tmp;
374 }
375 Triangle tri = new Triangle(a, b, c);
376 tri.index = nTri;
377 triangles[nTri++] = tri;
378 if (false)
379 msg.println(NAME + ".loadTriangle(): " + tri.toString());
380 }
381
382 /* connect a pair of triangles at their common edge(if any) */
383 private void connectTriangles(Triangle t1, Triangle t2) {
384 for (int i = 0; i < 3; i++) {
385 for (int j = 0; j < 3; j++) {
386 if ((t1.e[i].p0.pt == t2.e[j].p0.pt)
387 && (t1.e[i].p1.pt == t2.e[j].p1.pt)
388 || (t1.e[i].p0.pt == t2.e[j].p1.pt)
389 && (t1.e[i].p1.pt == t2.e[j].p0.pt)) {
390 t1.e[i].right = t2;
391 t2.e[j].right = t1;
392 }
393 }
394 }
395 }
396
397 /** Find and mark triangle path from 'first' to 'last'. Search in
398 * all direction until last is reached. Backtrace to 'first',
399 * and unmark branches that do not lead to 'last'.
400 */
401 private boolean markTrianglePath(Triangle[] a, int first, int last) {
402 if (a[first].mark != 0)
403 return false;
404 a[first].mark = 1;
405 if (first == last)
406 return true;
407 for (int i = 0; i < 3; i++) {
408 if (a[first].e[i].right != null && markTrianglePath(a, a[first].e[i].right.index, last))
409 return true;
410 }
411 a[first].mark = 0;
412 return false;
413 }
414
415 ////////////////////////////////////////////////////////////////////////
416
417 /* ccw test: CCW,CW,or co-linear */
418 private static int ccw(DotPoint p1, DotPoint p2, DotPoint p3) {
419 //double d=(p1.y-p2.y)*(p3.x-p2.x) - (p3.y-p2.y)*(p1.x-p2.x);
420 double d = (p2.y - p1.y) * (p3.x - p2.x) - (p2.y - p3.y) * (p1.x - p2.x);
421 if (d > 0)
422 return ISCCW;
423 if (d < 0)
424 return ISCW;
425 return ISON;
426 }
427
428 /* line (a,b) to line (c,d) intersection */
429 private static boolean intersects(DotPoint a, DotPoint b, DotPoint c, DotPoint d) {
430 if (ccw(a, b, c) == ISON || ccw(a, b, d) == ISON || ccw(c, d, a) == ISON || ccw(c, d, b) == ISON) {
431 if (between(a, b, c) || between(a, b, d) || between(c, d, a) || between(c, d, b))
432 return true;
433 } else {
434 int ccw1 = (ccw(a, b, c) == ISCCW) ? 1 : 0;
435 int ccw2 = (ccw(a, b, d) == ISCCW) ? 1 : 0;
436 int ccw3 = (ccw(c, d, a) == ISCCW) ? 1 : 0;
437 int ccw4 = (ccw(c, d, b) == ISCCW) ? 1 : 0;
438 return ((ccw1 ^ ccw2) & (ccw3 ^ ccw4)) != 0;
439 }
440 return false;
441 }
442
443 /* is b between a and c */
444 private static boolean between(DotPoint a, DotPoint b, DotPoint c) {
445 double p1x = b.x - a.x;
446 double p1y = b.y - a.y;
447 double p2x = c.x - a.x;
448 double p2y = c.y - a.y;
449 if (ccw(a, b, c) != ISON)
450 return false;
451 return (p2x * p1x + p2y * p1y >= 0) && (p2x * p2x + p2y * p2y <= p1x * p1x + p1y * p1y);
452 }
453
454 ////////////////////////////////////////////////////////////////////////
455
456 }