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 | |
20 | static boolean spline_merge(node_t * n) |
21 | { |
22 | return FALSE; |
23 | } |
24 | |
25 | static boolean swap_ends_p(edge_t * e) |
26 | { |
27 | return FALSE; |
28 | } |
29 | |
30 | static splineInfo sinfo = { swap_ends_p, spline_merge }; |
31 | |
32 | typedef struct { |
33 | int i, j; |
34 | } ipair; |
35 | |
36 | typedef struct _tri { |
37 | ipair v; |
38 | struct _tri *nxttri; |
39 | } tri; |
40 | |
41 | typedef 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 | */ |
51 | typedef struct { |
52 | Dtlink_t link; /* cdt data */ |
53 | int a[2]; /* key */ |
54 | int t; |
55 | } item; |
56 | |
57 | static 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 | */ |
76 | static 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 | |
88 | static void freeItem(Dt_t * d, item * obj, Dtdisc_t * disc) |
89 | { |
90 | free(obj); |
91 | } |
92 | |
93 | static 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 | |
105 | static 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 | */ |
125 | static 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 | |
141 | static 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 | |
161 | typedef struct { |
162 | Dtlink_t link; /* cdt data */ |
163 | int i; /* key */ |
164 | int j; |
165 | } Ipair; |
166 | |
167 | static 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 | |
175 | static 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 | |
186 | static void freeIpair(Dt_t * d, Ipair * obj, Dtdisc_t * disc) |
187 | { |
188 | free(obj); |
189 | } |
190 | |
191 | static 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 | |
203 | static 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 | |
211 | static 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 | */ |
221 | static 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 | */ |
231 | static tri * |
232 | addTri(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 | */ |
244 | static 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 | */ |
255 | static 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 | */ |
272 | static int |
273 | raySegIntersect(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 | */ |
292 | static int |
293 | triPoint(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 | */ |
320 | static 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 | */ |
344 | static 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 | |
403 | typedef 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 | |
409 | typedef 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 | |
415 | typedef struct { |
416 | tnode *nodes; |
417 | tedge *edges; |
418 | int nedges; /* no. of edges; no. of nodes is stored in router_t */ |
419 | } tgraph; |
420 | |
421 | struct 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 | */ |
435 | static 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 | */ |
453 | static 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 | |
489 | static 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 | */ |
500 | static 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 | */ |
515 | static 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 | */ |
551 | static 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 | |
568 | static 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 | */ |
580 | static 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 | |
638 | void 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 |
649 | static void |
650 | prTriPoly (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 | |
662 | static void |
663 | prTriGraph (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 | |
694 | router_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 | */ |
776 | static void |
777 | finishEdge (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 | */ |
821 | static Ppoint_t |
822 | tweakEnd (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 | |
844 | static void |
845 | tweakPath (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 | */ |
857 | static int |
858 | genroute(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 | |
961 | finish : |
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 | */ |
977 | static int |
978 | inCone (pointf a, pointf b, pointf c, pointf q) |
979 | { |
980 | return ((area2(q,a,b) >= NSMALL) && (area2(q,b,c) >= NSMALL)); |
981 | } |
982 | |
983 | static pointf north = {0, 1}; |
984 | static pointf northeast = {1, 1}; |
985 | static pointf east = {1, 0}; |
986 | static pointf southeast = {1, -1}; |
987 | static pointf south = {0, -1}; |
988 | static pointf southwest = {-1, -1}; |
989 | static pointf west = {-1, 0}; |
990 | static 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 | */ |
999 | static 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 | */ |
1082 | static 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 | |
1098 | static void |
1099 | freeTripoly (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 | */ |
1123 | typedef 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 | */ |
1134 | static 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 | */ |
1249 | static 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 | |
1268 | typedef 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 | */ |
1291 | static int * |
1292 | triPath(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 | */ |
1341 | int 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 | |