1/* $Id$ $Revision$ */
2/* vim:set shiftwidth=4 ts=8: */
3
4/*************************************************************************
5 * Copyright (c) 2011 AT&T Intellectual Property
6 * All rights reserved. This program and the accompanying materials
7 * are made available under the terms of the Eclipse Public License v1.0
8 * which accompanies this distribution, and is available at
9 * http://www.eclipse.org/legal/epl-v10.html
10 *
11 * Contributors: See CVS logs. Details at http://www.graphviz.org/
12 *************************************************************************/
13
14
15#include <stdlib.h>
16#include <stdio.h>
17#include <setjmp.h>
18#ifdef HAVE_MALLOC_H
19#include <malloc.h>
20#endif
21#include <limits.h>
22#include <math.h>
23#include "pathutil.h"
24
25#ifdef DMALLOC
26#include "dmalloc.h"
27#endif
28
29#define ISCCW 1
30#define ISCW 2
31#define ISON 3
32
33#define DQ_FRONT 1
34#define DQ_BACK 2
35
36#ifndef TRUE
37#define TRUE 1
38#define FALSE 0
39#endif
40
41#define prerror(msg) \
42 fprintf (stderr, "libpath/%s:%d: %s\n", __FILE__, __LINE__, (msg))
43
44#define POINTSIZE sizeof (Ppoint_t)
45
46typedef struct pointnlink_t {
47 Ppoint_t *pp;
48 struct pointnlink_t *link;
49} pointnlink_t;
50
51#define POINTNLINKSIZE sizeof (pointnlink_t)
52#define POINTNLINKPSIZE sizeof (pointnlink_t *)
53
54typedef struct tedge_t {
55 pointnlink_t *pnl0p;
56 pointnlink_t *pnl1p;
57 struct triangle_t *ltp;
58 struct triangle_t *rtp;
59} tedge_t;
60
61typedef struct triangle_t {
62 int mark;
63 struct tedge_t e[3];
64} triangle_t;
65
66#define TRIANGLESIZE sizeof (triangle_t)
67
68typedef struct deque_t {
69 pointnlink_t **pnlps;
70 int pnlpn, fpnlpi, lpnlpi, apex;
71} deque_t;
72
73static jmp_buf jbuf;
74static pointnlink_t *pnls, **pnlps;
75static int pnln, pnll;
76
77static triangle_t *tris;
78static int trin, tril;
79
80static deque_t dq;
81
82static Ppoint_t *ops;
83static int opn;
84
85static void triangulate(pointnlink_t **, int);
86static int isdiagonal(int, int, pointnlink_t **, int);
87static void loadtriangle(pointnlink_t *, pointnlink_t *, pointnlink_t *);
88static void connecttris(int, int);
89static int marktripath(int, int);
90
91static void add2dq(int, pointnlink_t *);
92static void splitdq(int, int);
93static int finddqsplit(pointnlink_t *);
94
95static int ccw(Ppoint_t *, Ppoint_t *, Ppoint_t *);
96static int intersects(Ppoint_t *, Ppoint_t *, Ppoint_t *, Ppoint_t *);
97static int between(Ppoint_t *, Ppoint_t *, Ppoint_t *);
98static int pointintri(int, Ppoint_t *);
99
100static void growpnls(int);
101static void growtris(int);
102static void growdq(int);
103static void growops(int);
104
105/* Pshortestpath:
106 * Find a shortest path contained in the polygon polyp going between the
107 * points supplied in eps. The resulting polyline is stored in output.
108 * Return 0 on success, -1 on bad input, -2 on memory allocation problem.
109 */
110int Pshortestpath(Ppoly_t * polyp, Ppoint_t * eps, Ppolyline_t * output)
111{
112 int pi, minpi;
113 double minx;
114 Ppoint_t p1, p2, p3;
115 int trii, trij, ftrii, ltrii;
116 int ei;
117 pointnlink_t epnls[2], *lpnlp, *rpnlp, *pnlp;
118 triangle_t *trip;
119 int splitindex;
120#ifdef DEBUG
121 int pnli;
122#endif
123
124 if (setjmp(jbuf))
125 return -2;
126 /* make space */
127 growpnls(polyp->pn);
128 pnll = 0;
129 tril = 0;
130 growdq(polyp->pn * 2);
131 dq.fpnlpi = dq.pnlpn / 2, dq.lpnlpi = dq.fpnlpi - 1;
132
133 /* make sure polygon is CCW and load pnls array */
134 for (pi = 0, minx = HUGE_VAL, minpi = -1; pi < polyp->pn; pi++) {
135 if (minx > polyp->ps[pi].x)
136 minx = polyp->ps[pi].x, minpi = pi;
137 }
138 p2 = polyp->ps[minpi];
139 p1 = polyp->ps[((minpi == 0) ? polyp->pn - 1 : minpi - 1)];
140 p3 = polyp->ps[((minpi == polyp->pn - 1) ? 0 : minpi + 1)];
141 if (((p1.x == p2.x && p2.x == p3.x) && (p3.y > p2.y)) ||
142 ccw(&p1, &p2, &p3) != ISCCW) {
143 for (pi = polyp->pn - 1; pi >= 0; pi--) {
144 if (pi < polyp->pn - 1
145 && polyp->ps[pi].x == polyp->ps[pi + 1].x
146 && polyp->ps[pi].y == polyp->ps[pi + 1].y)
147 continue;
148 pnls[pnll].pp = &polyp->ps[pi];
149 pnls[pnll].link = &pnls[pnll % polyp->pn];
150 pnlps[pnll] = &pnls[pnll];
151 pnll++;
152 }
153 } else {
154 for (pi = 0; pi < polyp->pn; pi++) {
155 if (pi > 0 && polyp->ps[pi].x == polyp->ps[pi - 1].x &&
156 polyp->ps[pi].y == polyp->ps[pi - 1].y)
157 continue;
158 pnls[pnll].pp = &polyp->ps[pi];
159 pnls[pnll].link = &pnls[pnll % polyp->pn];
160 pnlps[pnll] = &pnls[pnll];
161 pnll++;
162 }
163 }
164
165#if defined(DEBUG) && DEBUG >= 1
166 fprintf(stderr, "points\n%d\n", pnll);
167 for (pnli = 0; pnli < pnll; pnli++)
168 fprintf(stderr, "%f %f\n", pnls[pnli].pp->x, pnls[pnli].pp->y);
169#endif
170
171 /* generate list of triangles */
172 triangulate(pnlps, pnll);
173
174#if defined(DEBUG) && DEBUG >= 2
175 fprintf(stderr, "triangles\n%d\n", tril);
176 for (trii = 0; trii < tril; trii++)
177 for (ei = 0; ei < 3; ei++)
178 fprintf(stderr, "%f %f\n", tris[trii].e[ei].pnl0p->pp->x,
179 tris[trii].e[ei].pnl0p->pp->y);
180#endif
181
182 /* connect all pairs of triangles that share an edge */
183 for (trii = 0; trii < tril; trii++)
184 for (trij = trii + 1; trij < tril; trij++)
185 connecttris(trii, trij);
186
187 /* find first and last triangles */
188 for (trii = 0; trii < tril; trii++)
189 if (pointintri(trii, &eps[0]))
190 break;
191 if (trii == tril) {
192 prerror("source point not in any triangle");
193 return -1;
194 }
195 ftrii = trii;
196 for (trii = 0; trii < tril; trii++)
197 if (pointintri(trii, &eps[1]))
198 break;
199 if (trii == tril) {
200 prerror("destination point not in any triangle");
201 return -1;
202 }
203 ltrii = trii;
204
205 /* mark the strip of triangles from eps[0] to eps[1] */
206 if (!marktripath(ftrii, ltrii)) {
207 prerror("cannot find triangle path");
208 /* a straight line is better than failing */
209 growops(2);
210 output->pn = 2;
211 ops[0] = eps[0], ops[1] = eps[1];
212 output->ps = ops;
213 return 0;
214 }
215
216 /* if endpoints in same triangle, use a single line */
217 if (ftrii == ltrii) {
218 growops(2);
219 output->pn = 2;
220 ops[0] = eps[0], ops[1] = eps[1];
221 output->ps = ops;
222 return 0;
223 }
224
225 /* build funnel and shortest path linked list (in add2dq) */
226 epnls[0].pp = &eps[0], epnls[0].link = NULL;
227 epnls[1].pp = &eps[1], epnls[1].link = NULL;
228 add2dq(DQ_FRONT, &epnls[0]);
229 dq.apex = dq.fpnlpi;
230 trii = ftrii;
231 while (trii != -1) {
232 trip = &tris[trii];
233 trip->mark = 2;
234
235 /* find the left and right points of the exiting edge */
236 for (ei = 0; ei < 3; ei++)
237 if (trip->e[ei].rtp && trip->e[ei].rtp->mark == 1)
238 break;
239 if (ei == 3) { /* in last triangle */
240 if (ccw(&eps[1], dq.pnlps[dq.fpnlpi]->pp,
241 dq.pnlps[dq.lpnlpi]->pp) == ISCCW)
242 lpnlp = dq.pnlps[dq.lpnlpi], rpnlp = &epnls[1];
243 else
244 lpnlp = &epnls[1], rpnlp = dq.pnlps[dq.lpnlpi];
245 } else {
246 pnlp = trip->e[(ei + 1) % 3].pnl1p;
247 if (ccw(trip->e[ei].pnl0p->pp, pnlp->pp,
248 trip->e[ei].pnl1p->pp) == ISCCW)
249 lpnlp = trip->e[ei].pnl1p, rpnlp = trip->e[ei].pnl0p;
250 else
251 lpnlp = trip->e[ei].pnl0p, rpnlp = trip->e[ei].pnl1p;
252 }
253
254 /* update deque */
255 if (trii == ftrii) {
256 add2dq(DQ_BACK, lpnlp);
257 add2dq(DQ_FRONT, rpnlp);
258 } else {
259 if (dq.pnlps[dq.fpnlpi] != rpnlp
260 && dq.pnlps[dq.lpnlpi] != rpnlp) {
261 /* add right point to deque */
262 splitindex = finddqsplit(rpnlp);
263 splitdq(DQ_BACK, splitindex);
264 add2dq(DQ_FRONT, rpnlp);
265 /* if the split is behind the apex, then reset apex */
266 if (splitindex > dq.apex)
267 dq.apex = splitindex;
268 } else {
269 /* add left point to deque */
270 splitindex = finddqsplit(lpnlp);
271 splitdq(DQ_FRONT, splitindex);
272 add2dq(DQ_BACK, lpnlp);
273 /* if the split is in front of the apex, then reset apex */
274 if (splitindex < dq.apex)
275 dq.apex = splitindex;
276 }
277 }
278 trii = -1;
279 for (ei = 0; ei < 3; ei++)
280 if (trip->e[ei].rtp && trip->e[ei].rtp->mark == 1) {
281 trii = trip->e[ei].rtp - tris;
282 break;
283 }
284 }
285
286#if defined(DEBUG) && DEBUG >= 1
287 fprintf(stderr, "polypath");
288 for (pnlp = &epnls[1]; pnlp; pnlp = pnlp->link)
289 fprintf(stderr, " %f %f", pnlp->pp->x, pnlp->pp->y);
290 fprintf(stderr, "\n");
291#endif
292
293 for (pi = 0, pnlp = &epnls[1]; pnlp; pnlp = pnlp->link)
294 pi++;
295 growops(pi);
296 output->pn = pi;
297 for (pi = pi - 1, pnlp = &epnls[1]; pnlp; pi--, pnlp = pnlp->link)
298 ops[pi] = *pnlp->pp;
299 output->ps = ops;
300
301 return 0;
302}
303
304/* triangulate polygon */
305static void triangulate(pointnlink_t ** pnlps, int pnln)
306{
307 int pnli, pnlip1, pnlip2;
308
309 if (pnln > 3)
310 {
311 for (pnli = 0; pnli < pnln; pnli++)
312 {
313 pnlip1 = (pnli + 1) % pnln;
314 pnlip2 = (pnli + 2) % pnln;
315 if (isdiagonal(pnli, pnlip2, pnlps, pnln))
316 {
317 loadtriangle(pnlps[pnli], pnlps[pnlip1], pnlps[pnlip2]);
318 for (pnli = pnlip1; pnli < pnln - 1; pnli++)
319 pnlps[pnli] = pnlps[pnli + 1];
320 triangulate(pnlps, pnln - 1);
321 return;
322 }
323 }
324 prerror("triangulation failed");
325 }
326 else
327 loadtriangle(pnlps[0], pnlps[1], pnlps[2]);
328}
329
330/* check if (i, i + 2) is a diagonal */
331static int isdiagonal(int pnli, int pnlip2, pointnlink_t ** pnlps,
332 int pnln)
333{
334 int pnlip1, pnlim1, pnlj, pnljp1, res;
335
336 /* neighborhood test */
337 pnlip1 = (pnli + 1) % pnln;
338 pnlim1 = (pnli + pnln - 1) % pnln;
339 /* If P[pnli] is a convex vertex [ pnli+1 left of (pnli-1,pnli) ]. */
340 if (ccw(pnlps[pnlim1]->pp, pnlps[pnli]->pp, pnlps[pnlip1]->pp) ==
341 ISCCW)
342 res =
343 (ccw(pnlps[pnli]->pp, pnlps[pnlip2]->pp, pnlps[pnlim1]->pp) ==
344 ISCCW)
345 && (ccw(pnlps[pnlip2]->pp, pnlps[pnli]->pp, pnlps[pnlip1]->pp)
346 == ISCCW);
347 /* Assume (pnli - 1, pnli, pnli + 1) not collinear. */
348 else
349 res = (ccw(pnlps[pnli]->pp, pnlps[pnlip2]->pp,
350 pnlps[pnlip1]->pp) == ISCW);
351 if (!res)
352 return FALSE;
353
354 /* check against all other edges */
355 for (pnlj = 0; pnlj < pnln; pnlj++) {
356 pnljp1 = (pnlj + 1) % pnln;
357 if (!((pnlj == pnli) || (pnljp1 == pnli) ||
358 (pnlj == pnlip2) || (pnljp1 == pnlip2)))
359 if (intersects(pnlps[pnli]->pp, pnlps[pnlip2]->pp,
360 pnlps[pnlj]->pp, pnlps[pnljp1]->pp))
361 return FALSE;
362 }
363 return TRUE;
364}
365
366static void loadtriangle(pointnlink_t * pnlap, pointnlink_t * pnlbp,
367 pointnlink_t * pnlcp)
368{
369 triangle_t *trip;
370 int ei;
371
372 /* make space */
373 if (tril >= trin)
374 growtris(trin + 20);
375 trip = &tris[tril++];
376 trip->mark = 0;
377 trip->e[0].pnl0p = pnlap, trip->e[0].pnl1p = pnlbp, trip->e[0].rtp =
378 NULL;
379 trip->e[1].pnl0p = pnlbp, trip->e[1].pnl1p = pnlcp, trip->e[1].rtp =
380 NULL;
381 trip->e[2].pnl0p = pnlcp, trip->e[2].pnl1p = pnlap, trip->e[2].rtp =
382 NULL;
383 for (ei = 0; ei < 3; ei++)
384 trip->e[ei].ltp = trip;
385}
386
387/* connect a pair of triangles at their common edge (if any) */
388static void connecttris(int tri1, int tri2)
389{
390 triangle_t *tri1p, *tri2p;
391 int ei, ej;
392
393 for (ei = 0; ei < 3; ei++) {
394 for (ej = 0; ej < 3; ej++) {
395 tri1p = &tris[tri1], tri2p = &tris[tri2];
396 if ((tri1p->e[ei].pnl0p->pp == tri2p->e[ej].pnl0p->pp &&
397 tri1p->e[ei].pnl1p->pp == tri2p->e[ej].pnl1p->pp) ||
398 (tri1p->e[ei].pnl0p->pp == tri2p->e[ej].pnl1p->pp &&
399 tri1p->e[ei].pnl1p->pp == tri2p->e[ej].pnl0p->pp))
400 tri1p->e[ei].rtp = tri2p, tri2p->e[ej].rtp = tri1p;
401 }
402 }
403}
404
405/* find and mark path from trii, to trij */
406static int marktripath(int trii, int trij)
407{
408 int ei;
409
410 if (tris[trii].mark)
411 return FALSE;
412 tris[trii].mark = 1;
413 if (trii == trij)
414 return TRUE;
415 for (ei = 0; ei < 3; ei++)
416 if (tris[trii].e[ei].rtp &&
417 marktripath(tris[trii].e[ei].rtp - tris, trij))
418 return TRUE;
419 tris[trii].mark = 0;
420 return FALSE;
421}
422
423/* add a new point to the deque, either front or back */
424static void add2dq(int side, pointnlink_t * pnlp)
425{
426 if (side == DQ_FRONT) {
427 if (dq.lpnlpi - dq.fpnlpi >= 0)
428 pnlp->link = dq.pnlps[dq.fpnlpi]; /* shortest path links */
429 dq.fpnlpi--;
430 dq.pnlps[dq.fpnlpi] = pnlp;
431 } else {
432 if (dq.lpnlpi - dq.fpnlpi >= 0)
433 pnlp->link = dq.pnlps[dq.lpnlpi]; /* shortest path links */
434 dq.lpnlpi++;
435 dq.pnlps[dq.lpnlpi] = pnlp;
436 }
437}
438
439static void splitdq(int side, int index)
440{
441 if (side == DQ_FRONT)
442 dq.lpnlpi = index;
443 else
444 dq.fpnlpi = index;
445}
446
447static int finddqsplit(pointnlink_t * pnlp)
448{
449 int index;
450
451 for (index = dq.fpnlpi; index < dq.apex; index++)
452 if (ccw(dq.pnlps[index + 1]->pp, dq.pnlps[index]->pp, pnlp->pp) ==
453 ISCCW)
454 return index;
455 for (index = dq.lpnlpi; index > dq.apex; index--)
456 if (ccw(dq.pnlps[index - 1]->pp, dq.pnlps[index]->pp, pnlp->pp) ==
457 ISCW)
458 return index;
459 return dq.apex;
460}
461
462/* ccw test: CCW, CW, or co-linear */
463static int ccw(Ppoint_t * p1p, Ppoint_t * p2p, Ppoint_t * p3p)
464{
465 double d;
466
467 d = ((p1p->y - p2p->y) * (p3p->x - p2p->x)) -
468 ((p3p->y - p2p->y) * (p1p->x - p2p->x));
469 return (d > 0) ? ISCCW : ((d < 0) ? ISCW : ISON);
470}
471
472/* line to line intersection */
473static int intersects(Ppoint_t * pap, Ppoint_t * pbp,
474 Ppoint_t * pcp, Ppoint_t * pdp)
475{
476 int ccw1, ccw2, ccw3, ccw4;
477
478 if (ccw(pap, pbp, pcp) == ISON || ccw(pap, pbp, pdp) == ISON ||
479 ccw(pcp, pdp, pap) == ISON || ccw(pcp, pdp, pbp) == ISON) {
480 if (between(pap, pbp, pcp) || between(pap, pbp, pdp) ||
481 between(pcp, pdp, pap) || between(pcp, pdp, pbp))
482 return TRUE;
483 } else {
484 ccw1 = (ccw(pap, pbp, pcp) == ISCCW) ? 1 : 0;
485 ccw2 = (ccw(pap, pbp, pdp) == ISCCW) ? 1 : 0;
486 ccw3 = (ccw(pcp, pdp, pap) == ISCCW) ? 1 : 0;
487 ccw4 = (ccw(pcp, pdp, pbp) == ISCCW) ? 1 : 0;
488 return (ccw1 ^ ccw2) && (ccw3 ^ ccw4);
489 }
490 return FALSE;
491}
492
493/* is pbp between pap and pcp */
494static int between(Ppoint_t * pap, Ppoint_t * pbp, Ppoint_t * pcp)
495{
496 Ppoint_t p1, p2;
497
498 p1.x = pbp->x - pap->x, p1.y = pbp->y - pap->y;
499 p2.x = pcp->x - pap->x, p2.y = pcp->y - pap->y;
500 if (ccw(pap, pbp, pcp) != ISON)
501 return FALSE;
502 return (p2.x * p1.x + p2.y * p1.y >= 0) &&
503 (p2.x * p2.x + p2.y * p2.y <= p1.x * p1.x + p1.y * p1.y);
504}
505
506static int pointintri(int trii, Ppoint_t * pp)
507{
508 int ei, sum;
509
510 for (ei = 0, sum = 0; ei < 3; ei++)
511 if (ccw(tris[trii].e[ei].pnl0p->pp,
512 tris[trii].e[ei].pnl1p->pp, pp) != ISCW)
513 sum++;
514 return (sum == 3 || sum == 0);
515}
516
517static void growpnls(int newpnln)
518{
519 if (newpnln <= pnln)
520 return;
521 if (!pnls) {
522 if (!(pnls = (pointnlink_t *) malloc(POINTNLINKSIZE * newpnln))) {
523 prerror("cannot malloc pnls");
524 longjmp(jbuf,1);
525 }
526 if (!(pnlps = (pointnlink_t **) malloc(POINTNLINKPSIZE * newpnln))) {
527 prerror("cannot malloc pnlps");
528 longjmp(jbuf,1);
529 }
530 } else {
531 if (!(pnls = (pointnlink_t *) realloc((void *) pnls,
532 POINTNLINKSIZE * newpnln))) {
533 prerror("cannot realloc pnls");
534 longjmp(jbuf,1);
535 }
536 if (!(pnlps = (pointnlink_t **) realloc((void *) pnlps,
537 POINTNLINKPSIZE *
538 newpnln))) {
539 prerror("cannot realloc pnlps");
540 longjmp(jbuf,1);
541 }
542 }
543 pnln = newpnln;
544}
545
546static void growtris(int newtrin)
547{
548 if (newtrin <= trin)
549 return;
550 if (!tris) {
551 if (!(tris = (triangle_t *) malloc(TRIANGLESIZE * newtrin))) {
552 prerror("cannot malloc tris");
553 longjmp(jbuf,1);
554 }
555 } else {
556 if (!(tris = (triangle_t *) realloc((void *) tris,
557 TRIANGLESIZE * newtrin))) {
558 prerror("cannot realloc tris");
559 longjmp(jbuf,1);
560 }
561 }
562 trin = newtrin;
563}
564
565static void growdq(int newdqn)
566{
567 if (newdqn <= dq.pnlpn)
568 return;
569 if (!dq.pnlps) {
570 if (!
571 (dq.pnlps =
572 (pointnlink_t **) malloc(POINTNLINKPSIZE * newdqn))) {
573 prerror("cannot malloc dq.pnls");
574 longjmp(jbuf,1);
575 }
576 } else {
577 if (!(dq.pnlps = (pointnlink_t **) realloc((void *) dq.pnlps,
578 POINTNLINKPSIZE *
579 newdqn))) {
580 prerror("cannot realloc dq.pnls");
581 longjmp(jbuf,1);
582 }
583 }
584 dq.pnlpn = newdqn;
585}
586
587static void growops(int newopn)
588{
589 if (newopn <= opn)
590 return;
591 if (!ops) {
592 if (!(ops = (Ppoint_t *) malloc(POINTSIZE * newopn))) {
593 prerror("cannot malloc ops");
594 longjmp(jbuf,1);
595 }
596 } else {
597 if (!(ops = (Ppoint_t *) realloc((void *) ops,
598 POINTSIZE * newopn))) {
599 prerror("cannot realloc ops");
600 longjmp(jbuf,1);
601 }
602 }
603 opn = newopn;
604}
605