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#include <multispline.h>
15#include <delaunay.h>
16#include <neatoprocs.h>
17#include <math.h>
18
19
20static boolean spline_merge(node_t * n)
21{
22 return FALSE;
23}
24
25static boolean swap_ends_p(edge_t * e)
26{
27 return FALSE;
28}
29
30static splineInfo sinfo = { swap_ends_p, spline_merge };
31
32typedef struct {
33 int i, j;
34} ipair;
35
36typedef struct _tri {
37 ipair v;
38 struct _tri *nxttri;
39} tri;
40
41typedef struct {
42 Ppoly_t poly; /* base polygon used for routing an edge */
43 tri **triMap; /* triMap[j] is list of all opposite sides of
44 triangles containing vertex j, represented
45 as the indices of the two points in poly */
46} tripoly_t;
47
48/*
49 * Support for map from I x I -> I
50 */
51typedef struct {
52 Dtlink_t link; /* cdt data */
53 int a[2]; /* key */
54 int t;
55} item;
56
57static int cmpItem(Dt_t * d, int p1[], int p2[], Dtdisc_t * disc)
58{
59 NOTUSED(d);
60 NOTUSED(disc);
61
62 if (p1[0] < p2[0])
63 return -1;
64 else if (p1[0] > p2[0])
65 return 1;
66 else if (p1[1] < p2[1])
67 return -1;
68 else if (p1[1] > p2[1])
69 return 1;
70 else
71 return 0;
72}
73
74/* newItem:
75 */
76static void *newItem(Dt_t * d, item * objp, Dtdisc_t * disc)
77{
78 item *newp = NEW(item);
79
80 NOTUSED(disc);
81 newp->a[0] = objp->a[0];
82 newp->a[1] = objp->a[1];
83 newp->t = objp->t;
84
85 return newp;
86}
87
88static void freeItem(Dt_t * d, item * obj, Dtdisc_t * disc)
89{
90 free(obj);
91}
92
93static Dtdisc_t itemdisc = {
94 offsetof(item, a),
95 2 * sizeof(int),
96 offsetof(item, link),
97 (Dtmake_f) newItem,
98 (Dtfree_f) freeItem,
99 (Dtcompar_f) cmpItem,
100 NIL(Dthash_f),
101 NIL(Dtmemory_f),
102 NIL(Dtevent_f)
103};
104
105static void addMap(Dt_t * map, int a, int b, int t)
106{
107 item it;
108 int tmp;
109 if (a > b) {
110 tmp = a;
111 a = b;
112 b = tmp;
113 }
114 it.a[0] = a;
115 it.a[1] = b;
116 it.t = t;
117 dtinsert(map, &it);
118}
119
120/* mapSegToTri:
121 * Create mapping from indices of side endpoints to triangle id
122 * We use a set rather than a bag because the segments used for lookup
123 * will always be a side of a polygon and hence have a unique triangle.
124 */
125static Dt_t *mapSegToTri(surface_t * sf)
126{
127 Dt_t *map = dtopen(&itemdisc, Dtoset);
128 int i, a, b, c;
129 int *ps = sf->faces;
130 for (i = 0; i < sf->nfaces; i++) {
131 a = *ps++;
132 b = *ps++;
133 c = *ps++;
134 addMap(map, a, b, i);
135 addMap(map, b, c, i);
136 addMap(map, c, a, i);
137 }
138 return map;
139}
140
141static int findMap(Dt_t * map, int a, int b)
142{
143 item it;
144 item *ip;
145 if (a > b) {
146 int tmp = a;
147 a = b;
148 b = tmp;
149 }
150 it.a[0] = a;
151 it.a[1] = b;
152 ip = (item *) dtsearch(map, &it);
153 assert(ip);
154 return ip->t;
155}
156
157/*
158 * Support for map from I -> I
159 */
160
161typedef struct {
162 Dtlink_t link; /* cdt data */
163 int i; /* key */
164 int j;
165} Ipair;
166
167static int cmpIpair(Dt_t * d, int *p1, int *p2, Dtdisc_t * disc)
168{
169 NOTUSED(d);
170 NOTUSED(disc);
171
172 return (*p1 - *p2);
173}
174
175static void *newIpair(Dt_t * d, Ipair * objp, Dtdisc_t * disc)
176{
177 Ipair *newp = NEW(Ipair);
178
179 NOTUSED(disc);
180 newp->i = objp->i;
181 newp->j = objp->j;
182
183 return newp;
184}
185
186static void freeIpair(Dt_t * d, Ipair * obj, Dtdisc_t * disc)
187{
188 free(obj);
189}
190
191static Dtdisc_t ipairdisc = {
192 offsetof(Ipair, i),
193 sizeof(int),
194 offsetof(Ipair, link),
195 (Dtmake_f) newIpair,
196 (Dtfree_f) freeIpair,
197 (Dtcompar_f) cmpIpair,
198 NIL(Dthash_f),
199 NIL(Dtmemory_f),
200 NIL(Dtevent_f)
201};
202
203static void vmapAdd(Dt_t * map, int i, int j)
204{
205 Ipair obj;
206 obj.i = i;
207 obj.j = j;
208 dtinsert(map, &obj);
209}
210
211static int vMap(Dt_t * map, int i)
212{
213 Ipair *ip;
214 ip = (Ipair *) dtmatch(map, &i);
215 return ip->j;
216}
217
218/* mapTri:
219 * Map vertex indices from router_t to tripoly_t coordinates.
220 */
221static void mapTri(Dt_t * map, tri * tp)
222{
223 for (; tp; tp = tp->nxttri) {
224 tp->v.i = vMap(map, tp->v.i);
225 tp->v.j = vMap(map, tp->v.j);
226 }
227}
228
229/* addTri:
230 */
231static tri *
232addTri(int i, int j, tri * oldp)
233{
234 tri *tp = NEW(tri);
235 tp->v.i = i;
236 tp->v.j = j;
237 tp->nxttri = oldp;
238 return tp;
239}
240
241/* bisect:
242 * Return the angle bisecting the angle pp--cp--np
243 */
244static double bisect(pointf pp, pointf cp, pointf np)
245{
246 double theta, phi;
247 theta = atan2(np.y - cp.y, np.x - cp.x);
248 phi = atan2(pp.y - cp.y, pp.x - cp.x);
249 return (theta + phi) / 2.0;
250}
251
252/* raySeg:
253 * Check if ray v->w intersects segment a--b.
254 */
255static int raySeg(pointf v, pointf w, pointf a, pointf b)
256{
257 int wa = wind(v, w, a);
258 int wb = wind(v, w, b);
259 if (wa == wb)
260 return 0;
261 if (wa == 0) {
262 return (wind(v, b, w) * wind(v, b, a) >= 0);
263 } else {
264 return (wind(v, a, w) * wind(v, a, b) >= 0);
265 }
266}
267
268/* raySegIntersect:
269 * Find the point p where ray v->w intersects segment ai-bi, if any.
270 * Return 1 on success, 0 on failure
271 */
272static int
273raySegIntersect(pointf v, pointf w, pointf a, pointf b, pointf * p)
274{
275 if (raySeg(v, w, a, b))
276 return line_intersect(v, w, a, b, p);
277 else
278 return 0;
279}
280
281#ifdef DEVDBG
282#include <psdbg.c>
283#endif
284
285/* triPoint:
286 * Given the triangle vertex v, and point w so that v->w points
287 * into the polygon, return where the ray v->w intersects the
288 * polygon. The search uses all of the opposite sides of triangles
289 * with v as vertex.
290 * Return 0 on success; 1 on failure.
291 */
292static int
293triPoint(tripoly_t * trip, int vx, pointf v, pointf w, pointf * ip)
294{
295 tri *tp;
296
297 for (tp = trip->triMap[vx]; tp; tp = tp->nxttri) {
298 if (raySegIntersect
299 (v, w, trip->poly.ps[tp->v.i], trip->poly.ps[tp->v.j], ip))
300 return 0;
301 }
302#ifdef DEVDBG
303 psInit();
304 psComment ("Failure in triPoint");
305 psColor("0 0 1");
306 psSeg (v, w);
307 for (tp = trip->triMap[vx]; tp; tp = tp->nxttri) {
308 psTri(v, trip->poly.ps[tp->v.i], trip->poly.ps[tp->v.j]);
309 }
310 psOut(stderr);
311#endif
312 return 1;
313}
314
315/* ctrlPtIdx:
316 * Find the index of v in the points polys->ps.
317 * We start at 1 since the point corresponding to 0
318 * will never be used as v.
319 */
320static int ctrlPtIdx(pointf v, Ppoly_t * polys)
321{
322 pointf w;
323 int i;
324 for (i = 1; i < polys->pn; i++) {
325 w = polys->ps[i];
326 if ((w.x == v.x) && (w.y == v.y))
327 return i;
328 }
329 return -1;
330}
331
332#define SEP 15
333
334/* mkCtrlPts:
335 * Generate mult points associated with v.
336 * The points will lie on the ray bisecting the angle prev--v--nxt.
337 * The first point will aways be v.
338 * The rest are positioned equally spaced with maximum spacing SEP.
339 * In addition, they all lie within the polygon trip->poly.
340 * Parameter s gives the index after which a vertex lies on the
341 * opposite side. This is necessary to get the "curvature" of the
342 * path correct.
343 */
344static pointf *mkCtrlPts(int s, int mult, pointf prev, pointf v,
345 pointf nxt, tripoly_t * trip)
346{
347 pointf *ps;
348 int idx = ctrlPtIdx(v, &(trip->poly));
349 int i;
350 double d, sep, theta, sinTheta, cosTheta;
351 pointf q, w;
352
353 if (idx < 0)
354 return NULL;
355
356 ps = N_GNEW(mult, pointf);
357 theta = bisect(prev, v, nxt);
358 sinTheta = sin(theta);
359 cosTheta = cos(theta);
360 w.x = v.x + 100 * cosTheta;
361 w.y = v.y + 100 * sinTheta;
362 if (idx > s) {
363 if (wind(prev, v, w) != 1) {
364 sinTheta *= -1;
365 cosTheta *= -1;
366 w.x = v.x + 100 * cosTheta;
367 w.y = v.y + 100 * sinTheta;
368 }
369 } else if (wind(prev, v, w) != -1) {
370 sinTheta *= -1;
371 cosTheta *= -1;
372 w.x = v.x + 100 * cosTheta;
373 w.y = v.y + 100 * sinTheta;
374 }
375
376 if (triPoint(trip, idx, v, w, &q)) {
377 return 0;
378 }
379
380 d = DIST(q, v);
381 if (d >= mult * SEP)
382 sep = SEP;
383 else
384 sep = d / mult;
385 if (idx < s) {
386 for (i = 0; i < mult; i++) {
387 ps[i].x = v.x + i * sep * cosTheta;
388 ps[i].y = v.y + i * sep * sinTheta;
389 }
390 } else {
391 for (i = 0; i < mult; i++) {
392 ps[mult - i - 1].x = v.x + i * sep * cosTheta;
393 ps[mult - i - 1].y = v.y + i * sep * sinTheta;
394 }
395 }
396 return ps;
397}
398
399/*
400 * Simple graph structure for recording the triangle graph.
401 */
402
403typedef struct {
404 int ne; /* no. of edges. */
405 int *edges; /* indices of edges adjacent to node. */
406 pointf ctr; /* center of triangle. */
407} tnode;
408
409typedef struct {
410 int t, h; /* indices of head and tail nodes */
411 ipair seg; /* indices of points forming shared segment */
412 double dist; /* length of edge; usually distance between centers */
413} tedge;
414
415typedef struct {
416 tnode *nodes;
417 tedge *edges;
418 int nedges; /* no. of edges; no. of nodes is stored in router_t */
419} tgraph;
420
421struct router_s {
422 int pn; /* no. of points */
423 pointf *ps; /* all points in configuration */
424 int *obs; /* indices in obstacle i are obs[i]...obs[i+1]-1 */
425 int *tris; /* indices of triangle i are tris[3*i]...tris[3*i+2] */
426 Dt_t *trimap; /* map from obstacle side (a,b) to index of adj. triangle */
427 int tn; /* no. of nodes in tg */
428 tgraph *tg; /* graph of triangles */
429};
430
431/* triCenter:
432 * Given an array of points and 3 integer indices,
433 * compute and return the center of the triangle.
434 */
435static pointf triCenter(pointf * pts, int *idxs)
436{
437 pointf a = pts[*idxs++];
438 pointf b = pts[*idxs++];
439 pointf c = pts[*idxs++];
440 pointf p;
441 p.x = (a.x + b.x + c.x) / 3.0;
442 p.y = (a.y + b.y + c.y) / 3.0;
443 return p;
444}
445
446#define MARGIN 32
447
448/* bbox:
449 * Compute bounding box of polygons, and return it
450 * with an added margin of MARGIN.
451 * Store total number of points in *np.
452 */
453static boxf bbox(Ppoly_t** obsp, int npoly, int *np)
454{
455 boxf bb;
456 int i, j, cnt = 0;
457 pointf p;
458 Ppoly_t* obs;
459
460 bb.LL.x = bb.LL.y = MAXDOUBLE;
461 bb.UR.x = bb.UR.y = -MAXDOUBLE;
462
463 for (i = 0; i < npoly; i++) {
464 obs = *obsp++;
465 for (j = 0; j < obs->pn; j++) {
466 p = obs->ps[j];
467 if (p.x < bb.LL.x)
468 bb.LL.x = p.x;
469 if (p.x > bb.UR.x)
470 bb.UR.x = p.x;
471 if (p.y < bb.LL.y)
472 bb.LL.y = p.y;
473 if (p.y > bb.UR.y)
474 bb.UR.y = p.y;
475 cnt++;
476 }
477 }
478
479 *np = cnt;
480
481 bb.LL.x -= MARGIN;
482 bb.LL.y -= MARGIN;
483 bb.UR.x += MARGIN;
484 bb.UR.y += MARGIN;
485
486 return bb;
487}
488
489static int *mkTriIndices(surface_t * sf)
490{
491 int *tris = N_GNEW(3 * sf->nfaces, int);
492 memcpy(tris, sf->faces, 3 * sf->nfaces * sizeof(int));
493 return tris;
494}
495
496/* degT:
497 * Given a pointer to an array of 3 integers, return
498 * the number not equal to -1
499 */
500static int degT(int *ip)
501{
502 ip++;
503 if (*ip++ == -1)
504 return 1;
505 if (*ip == -1)
506 return 2;
507 else
508 return 3;
509}
510
511/* sharedEdge:
512 * Returns a pair of integer (x,y), x < y, where x and y are the
513 * indices of the two vertices of the shared edge.
514 */
515static ipair sharedEdge(int *p, int *q)
516{
517 ipair pt;
518 int tmp, p1, p2;
519 p1 = *p;
520 p2 = *(p + 1);
521 if (p1 == *q) {
522 if ((p2 != *(q + 1)) && (p2 != *(q + 2))) {
523 p2 = *(p + 2);
524 }
525 } else if (p1 == *(q + 1)) {
526 if ((p2 != *q) && (p2 != *(q + 2))) {
527 p2 = *(p + 2);
528 }
529 } else if (p1 == *(q + 2)) {
530 if ((p2 != *q) && (p2 != *(q + 1))) {
531 p2 = *(p + 2);
532 }
533 } else {
534 p1 = *(p + 2);
535 }
536
537 if (p1 > p2) {
538 tmp = p1;
539 p1 = p2;
540 p2 = tmp;
541 }
542 pt.i = p1;
543 pt.j = p2;
544 return pt;
545}
546
547/* addTriEdge:
548 * Add an edge to g, with tail t, head h, distance d, and shared
549 * segment seg.
550 */
551static void addTriEdge(tgraph * g, int t, int h, double d, ipair seg)
552{
553 tedge *ep = g->edges + g->nedges;
554 tnode *tp = g->nodes + t;
555 tnode *hp = g->nodes + h;
556
557 ep->t = t;
558 ep->h = h;
559 ep->dist = DIST(tp->ctr, hp->ctr);
560 ep->seg = seg;
561
562 tp->edges[tp->ne++] = g->nedges;
563 hp->edges[hp->ne++] = g->nedges;
564
565 g->nedges++;
566}
567
568static void freeTriGraph(tgraph * tg)
569{
570 free(tg->nodes->edges);
571 free(tg->nodes);
572 free(tg->edges);
573 free(tg);
574}
575
576/* mkTriGraph:
577 * Generate graph with triangles as nodes and an edge iff two triangles
578 * share an edge.
579 */
580static tgraph *mkTriGraph(surface_t * sf, int maxv, pointf * pts)
581{
582 tgraph *g;
583 tnode *np;
584 int j, i, ne = 0;
585 int *edgei;
586 int *jp;
587
588 /* ne is twice no. of edges */
589 for (i = 0; i < 3 * sf->nfaces; i++)
590 if (sf->neigh[i] != -1)
591 ne++;
592
593 g = GNEW(tgraph);
594
595 /* plus 2 for nodes added as endpoints of an edge */
596 g->nodes = N_GNEW(sf->nfaces + 2, tnode);
597
598 /* allow 1 possible extra edge per triangle, plus
599 * obstacles can have at most maxv triangles touching
600 */
601 edgei = N_GNEW(sf->nfaces + ne + 2 * maxv, int);
602 g->edges = N_GNEW(ne/2 + 2 * maxv, tedge);
603 g->nedges = 0;
604
605 for (i = 0; i < sf->nfaces; i++) {
606 np = g->nodes + i;
607 np->ne = 0;
608 np->edges = edgei;
609 np->ctr = triCenter(pts, sf->faces + 3 * i);
610 edgei += degT(sf->neigh + 3 * i) + 1;
611 }
612 /* initialize variable nodes */
613 np = g->nodes + i;
614 np->ne = 0;
615 np->edges = edgei;
616 np++;
617 np->ne = 0;
618 np->edges = edgei + maxv;
619
620 for (i = 0; i < sf->nfaces; i++) {
621 np = g->nodes + i;
622 jp = sf->neigh + 3 * i;
623 ne = 0;
624 while ((ne < 3) && ((j = *jp++) != -1)) {
625 if (i < j) {
626 double dist = DIST(np->ctr, (g->nodes + j)->ctr);
627 ipair seg =
628 sharedEdge(sf->faces + 3 * i, sf->faces + 3 * j);
629 addTriEdge(g, i, j, dist, seg);
630 }
631 ne++;
632 }
633 }
634
635 return g;
636}
637
638void freeRouter(router_t * rtr)
639{
640 free(rtr->ps);
641 free(rtr->obs);
642 free(rtr->tris);
643 dtclose(rtr->trimap);
644 freeTriGraph(rtr->tg);
645 free(rtr);
646}
647
648#ifdef DEVDBG
649static void
650prTriPoly (tripoly_t *poly, int si, int ei)
651{
652 FILE* fp = fopen ("dumppoly","w");
653
654 psInit();
655 psPoly (&(poly->poly));
656 psPoint (poly->poly.ps[si]);
657 psPoint (poly->poly.ps[ei]);
658 psOut(fp);
659 fclose(fp);
660}
661
662static void
663prTriGraph (router_t* rtr, int n)
664{
665 FILE* fp = fopen ("dump","w");
666 int i;
667 pointf* pts = rtr->ps;
668 tnode* nodes = rtr->tg->nodes;
669 char buf[BUFSIZ];
670
671 psInit();
672 for (i=0;i < rtr->tn; i++) {
673 pointf a = pts[rtr->tris[3*i]];
674 pointf b = pts[rtr->tris[3*i+1]];
675 pointf c = pts[rtr->tris[3*i+2]];
676 psTri (a, b,c);
677 sprintf (buf, "%d", i);
678 psTxt (buf, nodes[i].ctr);
679 }
680 for (i=rtr->tn;i < n; i++) {
681 sprintf (buf, "%d", i);
682 psTxt (buf, nodes[i].ctr);
683 }
684 psColor ("1 0 0");
685 for (i=0;i < rtr->tg->nedges; i++) {
686 tedge* e = rtr->tg->edges+i;
687 psSeg (nodes[e->t].ctr, nodes[e->h].ctr);
688 }
689 psOut(fp);
690 fclose(fp);
691}
692#endif
693
694router_t *mkRouter(Ppoly_t** obsp, int npoly)
695{
696 router_t *rtr = NEW(router_t);
697 Ppoly_t* obs;
698 boxf bb;
699 pointf *pts;
700 int npts;
701 surface_t *sf;
702 int *segs;
703 double *x;
704 double *y;
705 int maxv = 4; /* default max. no. of vertices in an obstacle; set below */
706 /* points in obstacle i have indices obsi[i] through obsi[i+1]-1 in pts
707 */
708 int *obsi = N_NEW(npoly + 1, int);
709 int i, j, ix = 4, six = 0;
710
711 bb = bbox(obsp, npoly, &npts);
712 npts += 4; /* 4 points of bounding box */
713 pts = N_GNEW(npts, pointf); /* all points are stored in pts */
714 segs = N_GNEW(2 * npts, int); /* indices of points forming segments */
715
716 /* store bounding box in CCW order */
717 pts[0] = bb.LL;
718 pts[1].x = bb.UR.x;
719 pts[1].y = bb.LL.y;
720 pts[2] = bb.UR;
721 pts[3].x = bb.LL.x;
722 pts[3].y = bb.UR.y;
723 for (i = 1; i <= 4; i++) {
724 segs[six++] = i - 1;
725 if (i < 4)
726 segs[six++] = i;
727 else
728 segs[six++] = 0;
729 }
730
731 /* store obstacles in CW order and generate constraint segments */
732 for (i = 0; i < npoly; i++) {
733 obsi[i] = ix;
734 obs = *obsp++;
735 for (j = 1; j <= obs->pn; j++) {
736 segs[six++] = ix;
737 if (j < obs->pn)
738 segs[six++] = ix + 1;
739 else
740 segs[six++] = obsi[i];
741 pts[ix++] = obs->ps[j - 1];
742 }
743 if (obs->pn > maxv)
744 maxv = obs->pn;
745 }
746 obsi[i] = ix;
747
748 /* copy points into coordinate arrays */
749 x = N_GNEW(npts, double);
750 y = N_GNEW(npts, double);
751 for (i = 0; i < npts; i++) {
752 x[i] = pts[i].x;
753 y[i] = pts[i].y;
754 }
755 sf = mkSurface(x, y, npts, segs, npts);
756 free(x);
757 free(y);
758 free(segs);
759
760 rtr->ps = pts;
761 rtr->pn = npts;
762 rtr->obs = obsi;
763 rtr->tris = mkTriIndices(sf);
764 rtr->trimap = mapSegToTri(sf);
765 rtr->tn = sf->nfaces;
766 rtr->tg = mkTriGraph(sf, maxv, pts);
767
768 freeSurface(sf);
769 return rtr;
770}
771
772/* finishEdge:
773 * Finish edge generation, clipping to nodes and adding arrowhead
774 * if necessary, and adding edge labels
775 */
776static void
777finishEdge (graph_t* g, edge_t* e, Ppoly_t spl, int flip, pointf p, pointf q)
778{
779 int j;
780 pointf *spline = N_GNEW(spl.pn, pointf);
781 pointf p1, q1;
782
783 if (flip) {
784 for (j = 0; j < spl.pn; j++) {
785 spline[spl.pn - 1 - j] = spl.ps[j];
786 }
787 p1 = q;
788 q1 = p;
789 }
790 else {
791 for (j = 0; j < spl.pn; j++) {
792 spline[j] = spl.ps[j];
793 }
794 p1 = p;
795 q1 = q;
796 }
797 if (Verbose > 1)
798 fprintf(stderr, "spline %s %s\n", agnameof(agtail(e)), agnameof(aghead(e)));
799 clip_and_install(e, aghead(e), spline, spl.pn, &sinfo);
800 free(spline);
801
802 addEdgeLabels(g, e, p1, q1);
803}
804
805#define EQPT(p,q) (((p).x==(q).x)&&((p).y==(q).y))
806
807/* tweakEnd:
808 * Hack because path routing doesn't know about the interiors
809 * of polygons. If the first or last segment of the shortest path
810 * lies along one of the polygon boundaries, the path may flip
811 * inside the polygon. To avoid this, we shift the point a bit.
812 *
813 * If the edge p(=poly.ps[s])-q of the shortest path is also an
814 * edge of the border polygon, move p slightly inside the polygon
815 * and return it. If prv and nxt are the two vertices adjacent to
816 * p in the polygon, let m be the midpoint of prv--nxt. We then
817 * move a tiny bit along the ray p->m.
818 *
819 * Otherwise, return p unchanged.
820 */
821static Ppoint_t
822tweakEnd (Ppoly_t poly, int s, Ppolyline_t pl, Ppoint_t q)
823{
824 Ppoint_t prv, nxt, p;
825
826 p = poly.ps[s];
827 nxt = poly.ps[(s + 1) % poly.pn];
828 if (s == 0)
829 prv = poly.ps[poly.pn-1];
830 else
831 prv = poly.ps[s - 1];
832 if (EQPT(q, nxt) || EQPT(q, prv) ){
833 Ppoint_t m;
834 double d;
835 m.x = (nxt.x + prv.x)/2.0 - p.x;
836 m.y = (nxt.y + prv.y)/2.0 - p.y;
837 d = LEN(m.x,m.y);
838 p.x += 0.1*m.x/d;
839 p.y += 0.1*m.y/d;
840 }
841 return p;
842}
843
844static void
845tweakPath (Ppoly_t poly, int s, int t, Ppolyline_t pl)
846{
847 pl.ps[0] = tweakEnd (poly, s, pl, pl.ps[1]);
848 pl.ps[pl.pn-1] = tweakEnd (poly, t, pl, pl.ps[pl.pn-2]);
849}
850
851
852/* genroute:
853 * Generate splines for e and cohorts.
854 * Edges go from s to t.
855 * Return 0 on success.
856 */
857static int
858genroute(graph_t* g, tripoly_t * trip, int s, int t, edge_t * e, int doPolyline)
859{
860 pointf eps[2];
861 Pvector_t evs[2];
862 pointf **cpts = NULL; /* lists of control points */
863 Ppoly_t poly;
864 Ppolyline_t pl, spl;
865 int i, j;
866 Ppolyline_t mmpl;
867 Pedge_t *medges = N_GNEW(trip->poly.pn, Pedge_t);
868 int pn;
869 int mult = ED_count(e);
870 node_t* head = aghead(e);
871 int rv = 0;
872
873 poly.ps = NULL;
874 pl.pn = 0;
875 eps[0].x = trip->poly.ps[s].x, eps[0].y = trip->poly.ps[s].y;
876 eps[1].x = trip->poly.ps[t].x, eps[1].y = trip->poly.ps[t].y;
877 if (Pshortestpath(&(trip->poly), eps, &pl) < 0) {
878 agerr(AGWARN, "Could not create control points for multiple spline for edge (%s,%s)\n", agnameof(agtail(e)), agnameof(aghead(e)));
879 rv = 1;
880 goto finish;
881 }
882
883 if (pl.pn == 2) {
884 makeStraightEdge(agraphof(head), e, doPolyline, &sinfo);
885 goto finish;
886 }
887
888 evs[0].x = evs[0].y = 0;
889 evs[1].x = evs[1].y = 0;
890
891 if ((mult == 1) || Concentrate) {
892 poly = trip->poly;
893 for (j = 0; j < poly.pn; j++) {
894 medges[j].a = poly.ps[j];
895 medges[j].b = poly.ps[(j + 1) % poly.pn];
896 }
897 tweakPath (poly, s, t, pl);
898 if (Proutespline(medges, poly.pn, pl, evs, &spl) < 0) {
899 agerr(AGWARN, "Could not create control points for multiple spline for edge (%s,%s)\n", agnameof(agtail(e)), agnameof(aghead(e)));
900 rv = 1;
901 goto finish;
902 }
903 finishEdge (g, e, spl, aghead(e) != head, eps[0], eps[1]);
904 free(medges);
905
906 return 0;
907 }
908
909 pn = 2 * (pl.pn - 1);
910
911 cpts = N_NEW(pl.pn - 2, pointf *);
912 for (i = 0; i < pl.pn - 2; i++) {
913 cpts[i] =
914 mkCtrlPts(t, mult+1, pl.ps[i], pl.ps[i + 1], pl.ps[i + 2], trip);
915 if (!cpts[i]) {
916 agerr(AGWARN, "Could not create control points for multiple spline for edge (%s,%s)\n", agnameof(agtail(e)), agnameof(aghead(e)));
917 rv = 1;
918 goto finish;
919 }
920 }
921
922 poly.ps = N_GNEW(pn, pointf);
923 poly.pn = pn;
924
925 for (i = 0; i < mult; i++) {
926 poly.ps[0] = eps[0];
927 for (j = 1; j < pl.pn - 1; j++) {
928 poly.ps[j] = cpts[j - 1][i];
929 }
930 poly.ps[pl.pn - 1] = eps[1];
931 for (j = 1; j < pl.pn - 1; j++) {
932 poly.ps[pn - j] = cpts[j - 1][i + 1];
933 }
934 if (Pshortestpath(&poly, eps, &mmpl) < 0) {
935 agerr(AGWARN, "Could not create control points for multiple spline for edge (%s,%s)\n", agnameof(agtail(e)), agnameof(aghead(e)));
936 rv = 1;
937 goto finish;
938 }
939
940 if (doPolyline) {
941 make_polyline (mmpl, &spl);
942 }
943 else {
944 for (j = 0; j < poly.pn; j++) {
945 medges[j].a = poly.ps[j];
946 medges[j].b = poly.ps[(j + 1) % poly.pn];
947 }
948 tweakPath (poly, 0, pl.pn-1, mmpl);
949 if (Proutespline(medges, poly.pn, mmpl, evs, &spl) < 0) {
950 agerr(AGWARN, "Could not create control points for multiple spline for edge (%s,%s)\n",
951 agnameof(agtail(e)), agnameof(aghead(e)));
952 rv = 1;
953 goto finish;
954 }
955 }
956 finishEdge (g, e, spl, aghead(e) != head, eps[0], eps[1]);
957
958 e = ED_to_virt(e);
959 }
960
961finish :
962 if (cpts) {
963 for (i = 0; i < pl.pn - 2; i++)
964 free(cpts[i]);
965 free(cpts);
966 }
967 free(medges);
968 free(poly.ps);
969 return rv;
970}
971
972#define NSMALL -0.0000000001
973
974/* inCone:
975 * Returns true iff q is in the convex cone a-b-c
976 */
977static int
978inCone (pointf a, pointf b, pointf c, pointf q)
979{
980 return ((area2(q,a,b) >= NSMALL) && (area2(q,b,c) >= NSMALL));
981}
982
983static pointf north = {0, 1};
984static pointf northeast = {1, 1};
985static pointf east = {1, 0};
986static pointf southeast = {1, -1};
987static pointf south = {0, -1};
988static pointf southwest = {-1, -1};
989static pointf west = {-1, 0};
990static pointf northwest = {-1, 1};
991
992/* addEndpoint:
993 * Add node to graph representing spline end point p inside obstruction obs_id.
994 * For each side of obstruction, add edge from p to corresponding triangle.
995 * The node id of the new node in the graph is v_id.
996 * If p lies on the side of its node (sides != 0), we limit the triangles
997 * to those within 45 degrees of each side of the natural direction of p.
998 */
999static void addEndpoint(router_t * rtr, pointf p, node_t* v, int v_id, int sides)
1000{
1001 int obs_id = ND_lim(v);
1002 int starti = rtr->obs[obs_id];
1003 int endi = rtr->obs[obs_id + 1];
1004 pointf* pts = rtr->ps;
1005 int i, t;
1006 double d;
1007 pointf vr, v0, v1;
1008
1009 switch (sides) {
1010 case TOP :
1011 vr = add_pointf (p, north);
1012 v0 = add_pointf (p, northwest);
1013 v1 = add_pointf (p, northeast);
1014 break;
1015 case TOP|RIGHT :
1016 vr = add_pointf (p, northeast);
1017 v0 = add_pointf (p, north);
1018 v1 = add_pointf (p, east);
1019 break;
1020 case RIGHT :
1021 vr = add_pointf (p, east);
1022 v0 = add_pointf (p, northeast);
1023 v1 = add_pointf (p, southeast);
1024 break;
1025 case BOTTOM|RIGHT :
1026 vr = add_pointf (p, southeast);
1027 v0 = add_pointf (p, east);
1028 v1 = add_pointf (p, south);
1029 break;
1030 case BOTTOM :
1031 vr = add_pointf (p, south);
1032 v0 = add_pointf (p, southeast);
1033 v1 = add_pointf (p, southwest);
1034 break;
1035 case BOTTOM|LEFT :
1036 vr = add_pointf (p, southwest);
1037 v0 = add_pointf (p, south);
1038 v1 = add_pointf (p, west);
1039 break;
1040 case LEFT :
1041 vr = add_pointf (p, west);
1042 v0 = add_pointf (p, southwest);
1043 v1 = add_pointf (p, northwest);
1044 break;
1045 case TOP|LEFT :
1046 vr = add_pointf (p, northwest);
1047 v0 = add_pointf (p, west);
1048 v1 = add_pointf (p, north);
1049 break;
1050 case 0 :
1051 break;
1052 default :
1053 assert (0);
1054 break;
1055 }
1056
1057 rtr->tg->nodes[v_id].ne = 0;
1058 rtr->tg->nodes[v_id].ctr = p;
1059 for (i = starti; i < endi; i++) {
1060 ipair seg;
1061 seg.i = i;
1062 if (i < endi - 1)
1063 seg.j = i + 1;
1064 else
1065 seg.j = starti;
1066 t = findMap(rtr->trimap, seg.i, seg.j);
1067 if (sides && !inCone (v0, p, v1, pts[seg.i]) && !inCone (v0, p, v1, pts[seg.j]) && !raySeg(p,vr,pts[seg.i],pts[seg.j]))
1068 continue;
1069 d = DIST(p, (rtr->tg->nodes + t)->ctr);
1070 addTriEdge(rtr->tg, v_id, t, d, seg);
1071 }
1072}
1073
1074/* edgeToSeg:
1075 * Given edge from i to j, find segment associated
1076 * with the edge.
1077 *
1078 * This lookup could be made faster by modifying the
1079 * shortest path algorithm to store the edges rather than
1080 * the nodes.
1081 */
1082static ipair edgeToSeg(tgraph * tg, int i, int j)
1083{
1084 ipair ip;
1085 tnode *np = tg->nodes + i;
1086 tedge *ep;
1087
1088 for (i = 0; i < np->ne; i++) {
1089 ep = tg->edges + np->edges[i];
1090 if ((ep->t == j) || (ep->h == j))
1091 return (ep->seg);
1092 }
1093
1094 assert(0);
1095 return ip;
1096}
1097
1098static void
1099freeTripoly (tripoly_t* trip)
1100{
1101 int i;
1102 tri* tp;
1103 tri* nxt;
1104
1105 free (trip->poly.ps);
1106 for (i = 0; i < trip->poly.pn; i++) {
1107 for (tp = trip->triMap[i]; tp; tp = nxt) {
1108 nxt = tp->nxttri;
1109 free (tp);
1110 }
1111 }
1112 free (trip->triMap);
1113 free (trip);
1114}
1115
1116/* Auxiliary data structure used to translate a path of rectangles
1117 * into a polygon. Each side_t represents a vertex on one side of
1118 * the polygon. v is the index of the vertex in the global router_t,
1119 * and ts is a linked list of the indices of segments of sides opposite
1120 * to v in some triangle on the path. These lists will be translated
1121 * to polygon indices by mapTri, and stored in tripoly_t.triMap.
1122 */
1123typedef struct {
1124 int v;
1125 tri *ts;
1126} side_t;
1127
1128/* mkPoly:
1129 * Construct simple polygon from shortest path from t to s in g.
1130 * dad gives the indices of the triangles on path.
1131 * sx used to store index of s in points.
1132 * index of t is always 0
1133 */
1134static tripoly_t *mkPoly(router_t * rtr, int *dad, int s, int t,
1135 pointf p_s, pointf p_t, int *sx)
1136{
1137 tripoly_t *ps;
1138 int nxt;
1139 ipair p;
1140 int nt = 0;
1141 side_t *side1;
1142 side_t *side2;
1143 int i, idx;
1144 int cnt1 = 0;
1145 int cnt2 = 0;
1146 pointf *pts;
1147 pointf *pps;
1148 /* maps vertex index used in router_t to vertex index used in tripoly */
1149 Dt_t *vmap;
1150 tri **trim;
1151
1152 /* count number of triangles in path */
1153 for (nxt = dad[t]; nxt != s; nxt = dad[nxt])
1154 nt++;
1155
1156 side1 = N_NEW(nt + 4, side_t);
1157 side2 = N_NEW(nt + 4, side_t);
1158
1159 nxt = dad[t];
1160 p = edgeToSeg(rtr->tg, nxt, t);
1161 side1[cnt1].ts = addTri(-1, p.j, NULL);
1162 side1[cnt1++].v = p.i;
1163 side2[cnt2].ts = addTri(-1, p.i, NULL);
1164 side2[cnt2++].v = p.j;
1165
1166 t = nxt;
1167 for (nxt = dad[t]; nxt >= 0; nxt = dad[nxt]) {
1168 p = edgeToSeg(rtr->tg, t, nxt);
1169 if (p.i == side1[cnt1 - 1].v) {
1170 side1[cnt1 - 1].ts =
1171 addTri(side2[cnt2 - 1].v, p.j, side1[cnt1 - 1].ts);
1172 side2[cnt2 - 1].ts =
1173 addTri(side1[cnt1 - 1].v, p.j, side2[cnt2 - 1].ts);
1174 side2[cnt2].ts =
1175 addTri(side2[cnt2 - 1].v, side1[cnt1 - 1].v, NULL);
1176 side2[cnt2++].v = p.j;
1177 } else if (p.i == side2[cnt2 - 1].v) {
1178 side1[cnt1 - 1].ts =
1179 addTri(side2[cnt2 - 1].v, p.j, side1[cnt1 - 1].ts);
1180 side2[cnt2 - 1].ts =
1181 addTri(side1[cnt1 - 1].v, p.j, side2[cnt2 - 1].ts);
1182 side1[cnt1].ts =
1183 addTri(side2[cnt2 - 1].v, side1[cnt1 - 1].v, NULL);
1184 side1[cnt1++].v = p.j;
1185 } else if (p.j == side1[cnt1 - 1].v) {
1186 side1[cnt1 - 1].ts =
1187 addTri(side2[cnt2 - 1].v, p.i, side1[cnt1 - 1].ts);
1188 side2[cnt2 - 1].ts =
1189 addTri(side1[cnt1 - 1].v, p.i, side2[cnt2 - 1].ts);
1190 side2[cnt2].ts =
1191 addTri(side2[cnt2 - 1].v, side1[cnt1 - 1].v, NULL);
1192 side2[cnt2++].v = p.i;
1193 } else {
1194 side1[cnt1 - 1].ts =
1195 addTri(side2[cnt2 - 1].v, p.i, side1[cnt1 - 1].ts);
1196 side2[cnt2 - 1].ts =
1197 addTri(side1[cnt1 - 1].v, p.i, side2[cnt2 - 1].ts);
1198 side1[cnt1].ts =
1199 addTri(side2[cnt2 - 1].v, side1[cnt1 - 1].v, NULL);
1200 side1[cnt1++].v = p.i;
1201 }
1202 t = nxt;
1203 }
1204 side1[cnt1 - 1].ts = addTri(-2, side2[cnt2 - 1].v, side1[cnt1 - 1].ts);
1205 side2[cnt2 - 1].ts = addTri(-2, side1[cnt1 - 1].v, side2[cnt2 - 1].ts);
1206
1207 /* store points in pts starting with t in 0,
1208 * then side1, then s, then side2
1209 */
1210 vmap = dtopen(&ipairdisc, Dtoset);
1211 vmapAdd(vmap, -1, 0);
1212 vmapAdd(vmap, -2, cnt1 + 1);
1213 pps = pts = N_GNEW(nt + 4, pointf);
1214 trim = N_NEW(nt + 4, tri *);
1215 *pps++ = p_t;
1216 idx = 1;
1217 for (i = 0; i < cnt1; i++) {
1218 vmapAdd(vmap, side1[i].v, idx);
1219 *pps++ = rtr->ps[side1[i].v];
1220 trim[idx++] = side1[i].ts;
1221 }
1222 *pps++ = p_s;
1223 idx++;
1224 for (i = cnt2 - 1; i >= 0; i--) {
1225 vmapAdd(vmap, side2[i].v, idx);
1226 *pps++ = rtr->ps[side2[i].v];
1227 trim[idx++] = side2[i].ts;
1228 }
1229
1230 for (i = 0; i < nt + 4; i++) {
1231 mapTri(vmap, trim[i]);
1232 }
1233
1234 ps = NEW(tripoly_t);
1235 ps->poly.pn = nt + 4; /* nt triangles gives nt+2 points plus s and t */
1236 ps->poly.ps = pts;
1237 ps->triMap = trim;
1238
1239 free (side1);
1240 free (side2);
1241 dtclose(vmap);
1242 *sx = cnt1 + 1; /* index of s in ps */
1243 return ps;
1244}
1245
1246/* resetGraph:
1247 * Remove edges and nodes added for current edge routing
1248 */
1249static void resetGraph(tgraph * g, int ncnt, int ecnt)
1250{
1251 int i;
1252 tnode *np = g->nodes;
1253 g->nedges = ecnt;
1254 for (i = 0; i < ncnt; i++) {
1255 if (np->edges + np->ne == (np + 1)->edges)
1256 np->ne--;
1257 np++;
1258 }
1259}
1260
1261#define PQTYPE int
1262#define PQVTYPE float
1263
1264#define PQ_TYPES
1265#include "fPQ.h"
1266#undef PQ_TYPES
1267
1268typedef struct {
1269 PQ pq;
1270 PQVTYPE *vals;
1271 int *idxs;
1272} PPQ;
1273
1274#define N_VAL(pq,n) ((PPQ*)pq)->vals[n]
1275#define N_IDX(pq,n) ((PPQ*)pq)->idxs[n]
1276
1277#define PQ_CODE
1278#include "fPQ.h"
1279#undef PQ_CODE
1280
1281#define N_DAD(n) dad[n]
1282#define E_WT(e) (e->dist)
1283#define UNSEEN -MAXFLOAT
1284
1285/* triPath:
1286 * Find the shortest path with lengths in g from
1287 * v0 to v1. The returned vector (dad) encodes the
1288 * shorted path from v1 to v0. That path is given by
1289 * v1, dad[v1], dad[dad[v1]], ..., v0.
1290 */
1291static int *
1292triPath(tgraph * g, int n, int v0, int v1, PQ * pq)
1293{
1294 int i, j, adjn;
1295 double d;
1296 tnode *np;
1297 tedge *e;
1298 int *dad = N_NEW(n, int);
1299
1300 for (i = 0; i < pq->PQsize; i++)
1301 N_VAL(pq, i) = UNSEEN;
1302
1303 PQinit(pq);
1304 N_DAD(v0) = -1;
1305 N_VAL(pq, v0) = 0;
1306 if (PQinsert(pq, v0))
1307 return NULL;
1308
1309 while ((i = PQremove(pq)) != -1) {
1310 N_VAL(pq, i) *= -1;
1311 if (i == v1)
1312 break;
1313 np = g->nodes + i;
1314 for (j = 0; j < np->ne; j++) {
1315 e = g->edges + np->edges[j];
1316 if (e->t == i)
1317 adjn = e->h;
1318 else
1319 adjn = e->t;
1320 if (N_VAL(pq, adjn) < 0) {
1321 d = -(N_VAL(pq, i) + E_WT(e));
1322 if (N_VAL(pq, adjn) == UNSEEN) {
1323 N_VAL(pq, adjn) = d;
1324 N_DAD(adjn) = i;
1325 if (PQinsert(pq, adjn)) return NULL;
1326 } else if (N_VAL(pq, adjn) < d) {
1327 PQupdate(pq, adjn, d);
1328 N_DAD(adjn) = i;
1329 }
1330 }
1331 }
1332 }
1333 return dad;
1334}
1335
1336/* makeMultiSpline:
1337 * FIX: we don't really use the shortest path provided by ED_path,
1338 * so avoid in neato spline code.
1339 * Return 0 on success.
1340 */
1341int makeMultiSpline(graph_t* g, edge_t* e, router_t * rtr, int doPolyline)
1342{
1343 Ppolyline_t line = ED_path(e);
1344 node_t *t = agtail(e);
1345 node_t *h = aghead(e);
1346 pointf t_p = line.ps[0];
1347 pointf h_p = line.ps[line.pn - 1];
1348 tripoly_t *poly;
1349 int idx;
1350 int *sp;
1351 int t_id = rtr->tn;
1352 int h_id = rtr->tn + 1;
1353 int ecnt = rtr->tg->nedges;
1354 PPQ pq;
1355 PQTYPE *idxs;
1356 PQVTYPE *vals;
1357 int ret;
1358
1359 /* Add endpoints to triangle graph */
1360 addEndpoint(rtr, t_p, t, t_id, ED_tail_port(e).side);
1361 addEndpoint(rtr, h_p, h, h_id, ED_head_port(e).side);
1362
1363 /* Initialize priority queue */
1364 PQgen(&pq.pq, rtr->tn + 2, -1);
1365 idxs = N_GNEW(pq.pq.PQsize + 1, PQTYPE);
1366 vals = N_GNEW(pq.pq.PQsize + 1, PQVTYPE);
1367 vals[0] = 0;
1368 pq.vals = vals + 1;
1369 pq.idxs = idxs + 1;
1370
1371 /* Find shortest path of triangles */
1372 sp = triPath(rtr->tg, rtr->tn+2, h_id, t_id, (PQ *) & pq);
1373
1374 free(vals);
1375 free(idxs);
1376 PQfree(&(pq.pq), 0);
1377
1378 /* Use path of triangles to generate guiding polygon */
1379 if (sp) {
1380 poly = mkPoly(rtr, sp, h_id, t_id, h_p, t_p, &idx);
1381 free(sp);
1382
1383 /* Generate multiple splines using polygon */
1384 ret = genroute(g, poly, 0, idx, e, doPolyline);
1385 freeTripoly (poly);
1386 }
1387 else ret = -1;
1388
1389 resetGraph(rtr->tg, rtr->tn, ecnt);
1390 return ret;
1391}
1392