Graphviz  2.35.20130930.0449
shortest.c
Go to the documentation of this file.
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 
46 typedef struct pointnlink_t {
48  struct pointnlink_t *link;
49 } pointnlink_t;
50 
51 #define POINTNLINKSIZE sizeof (pointnlink_t)
52 #define POINTNLINKPSIZE sizeof (pointnlink_t *)
53 
54 typedef struct tedge_t {
57  struct triangle_t *ltp;
58  struct triangle_t *rtp;
59 } tedge_t;
60 
61 typedef struct triangle_t {
62  int mark;
63  struct tedge_t e[3];
64 } triangle_t;
65 
66 #define TRIANGLESIZE sizeof (triangle_t)
67 
68 typedef struct deque_t {
71 } deque_t;
72 
73 static jmp_buf jbuf;
74 static pointnlink_t *pnls, **pnlps;
75 static int pnln, pnll;
76 
77 static triangle_t *tris;
78 static int trin, tril;
79 
80 static deque_t dq;
81 
82 static Ppoint_t *ops;
83 static int opn;
84 
85 static void triangulate(pointnlink_t **, int);
86 static int isdiagonal(int, int, pointnlink_t **, int);
87 static void loadtriangle(pointnlink_t *, pointnlink_t *, pointnlink_t *);
88 static void connecttris(int, int);
89 static int marktripath(int, int);
90 
91 static void add2dq(int, pointnlink_t *);
92 static void splitdq(int, int);
93 static int finddqsplit(pointnlink_t *);
94 
95 static int ccw(Ppoint_t *, Ppoint_t *, Ppoint_t *);
96 static int intersects(Ppoint_t *, Ppoint_t *, Ppoint_t *, Ppoint_t *);
97 static int between(Ppoint_t *, Ppoint_t *, Ppoint_t *);
98 static int pointintri(int, Ppoint_t *);
99 
100 static void growpnls(int);
101 static void growtris(int);
102 static void growdq(int);
103 static 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  */
110 int 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 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 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 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 */
305 static 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 */
331 static 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 
366 static 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) */
388 static 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 */
406 static 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 */
424 static 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 
439 static void splitdq(int side, int index)
440 {
441  if (side == DQ_FRONT)
442  dq.lpnlpi = index;
443  else
444  dq.fpnlpi = index;
445 }
446 
447 static 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 */
463 static 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 */
473 static 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 */
494 static 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 
506 static 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 
517 static 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,
538  newpnln))) {
539  prerror("cannot realloc pnlps");
540  longjmp(jbuf,1);
541  }
542  }
543  pnln = newpnln;
544 }
545 
546 static 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 
565 static 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,
579  newdqn))) {
580  prerror("cannot realloc dq.pnls");
581  longjmp(jbuf,1);
582  }
583  }
584  dq.pnlpn = newdqn;
585 }
586 
587 static 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 }