--- /dev/null
+/* $Id$Revision: */
+/* vim:set shiftwidth=4 ts=8: */
+
+/**********************************************************
+* This software is part of the graphviz package *
+* http://www.graphviz.org/ *
+* *
+* Copyright (c) 1994-2004 AT&T Corp. *
+* and is licensed under the *
+* Common Public License, Version 1.0 *
+* by AT&T Corp. *
+* *
+* Information and Software Systems Research *
+* AT&T Research, Florham Park NJ *
+**********************************************************/
+
+#include <multispline.h>
+#include <delaunay.h>
+#include <neatoprocs.h>
+#include <math.h>
+
+static int _cnt;
+static boolean spline_merge(node_t * n)
+{
+ return FALSE;
+}
+
+static boolean swap_ends_p(edge_t * e)
+{
+ return FALSE;
+}
+
+static splineInfo sinfo = { swap_ends_p, spline_merge };
+
+typedef struct {
+ int i, j;
+} ipair;
+
+typedef struct _tri {
+ ipair v;
+ struct _tri *nxttri;
+} tri;
+
+typedef struct {
+ Ppoly_t poly; /* base polygon used for routing an edge */
+ tri **triMap; /* triMap[j] is list of all opposite sides of
+ triangles containing vertex j, represented
+ as the indices of the two points in poly */
+} tripoly_t;
+
+/*
+ * Support for map from I x I -> I
+ */
+typedef struct {
+ Dtlink_t link; /* cdt data */
+ int a[2]; /* key */
+ int t;
+} item;
+
+static int cmpItem(Dt_t * d, int p1[], int p2[], Dtdisc_t * disc)
+{
+ NOTUSED(d);
+ NOTUSED(disc);
+
+ if (p1[0] < p2[0])
+ return -1;
+ else if (p1[0] > p2[0])
+ return 1;
+ else if (p1[1] < p2[1])
+ return -1;
+ else if (p1[1] > p2[1])
+ return 1;
+ else
+ return 0;
+}
+
+/* newItem:
+ */
+static void *newItem(Dt_t * d, item * objp, Dtdisc_t * disc)
+{
+ item *newp = NEW(item);
+
+ NOTUSED(disc);
+ newp->a[0] = objp->a[0];
+ newp->a[1] = objp->a[1];
+ newp->t = objp->t;
+
+ return newp;
+}
+
+static void freeItem(Dt_t * d, item * obj, Dtdisc_t * disc)
+{
+ free(obj);
+}
+
+static Dtdisc_t itemdisc = {
+ offsetof(item, a),
+ 2 * sizeof(int),
+ offsetof(item, link),
+ (Dtmake_f) newItem,
+ (Dtfree_f) freeItem,
+ (Dtcompar_f) cmpItem,
+ NIL(Dthash_f),
+ NIL(Dtmemory_f),
+ NIL(Dtevent_f)
+};
+
+static void addMap(Dt_t * map, int a, int b, int t)
+{
+ item it;
+ int tmp;
+ if (a > b) {
+ tmp = a;
+ a = b;
+ b = tmp;
+ }
+ it.a[0] = a;
+ it.a[1] = b;
+ it.t = t;
+ dtinsert(map, &it);
+}
+
+/* mapSegToTri:
+ * Create mapping from indices of side endpoints to triangle id
+ * We use a set rather than a bag because the segments used for lookup
+ * will always be a side of a polygon and hence have a unique triangle.
+ */
+static Dt_t *mapSegToTri(surface_t * sf)
+{
+ Dt_t *map = dtopen(&itemdisc, Dtoset);
+ int i, a, b, c;
+ int *ps = sf->faces;
+ for (i = 0; i < sf->nfaces; i++) {
+ a = *ps++;
+ b = *ps++;
+ c = *ps++;
+ addMap(map, a, b, i);
+ addMap(map, b, c, i);
+ addMap(map, c, a, i);
+ }
+ return map;
+}
+
+static int findMap(Dt_t * map, int a, int b)
+{
+ item it;
+ item *ip;
+ if (a > b) {
+ int tmp = a;
+ a = b;
+ b = tmp;
+ }
+ it.a[0] = a;
+ it.a[1] = b;
+ ip = (item *) dtsearch(map, &it);
+ assert(ip);
+ return ip->t;
+}
+
+/*
+ * Support for map from I -> I
+ */
+
+typedef struct {
+ Dtlink_t link; /* cdt data */
+ int i; /* key */
+ int j;
+} Ipair;
+
+static int cmpIpair(Dt_t * d, int *p1, int *p2, Dtdisc_t * disc)
+{
+ NOTUSED(d);
+ NOTUSED(disc);
+
+ return (*p1 - *p2);
+}
+
+static void *newIpair(Dt_t * d, Ipair * objp, Dtdisc_t * disc)
+{
+ Ipair *newp = NEW(Ipair);
+
+ NOTUSED(disc);
+ newp->i = objp->i;
+ newp->j = objp->j;
+
+ return newp;
+}
+
+static void freeIpair(Dt_t * d, Ipair * obj, Dtdisc_t * disc)
+{
+ free(obj);
+}
+
+static Dtdisc_t ipairdisc = {
+ offsetof(Ipair, i),
+ sizeof(int),
+ offsetof(Ipair, link),
+ (Dtmake_f) newIpair,
+ (Dtfree_f) freeIpair,
+ (Dtcompar_f) cmpIpair,
+ NIL(Dthash_f),
+ NIL(Dtmemory_f),
+ NIL(Dtevent_f)
+};
+
+static void vmapAdd(Dt_t * map, int i, int j)
+{
+ Ipair obj;
+ obj.i = i;
+ obj.j = j;
+ dtinsert(map, &obj);
+}
+
+static int vMap(Dt_t * map, int i)
+{
+ Ipair *ip;
+ ip = (Ipair *) dtmatch(map, &i);
+ return ip->j;
+}
+
+/* mapTri:
+ * Map vertex indices from router_t to tripoly_t coordinates.
+ */
+static void mapTri(Dt_t * map, tri * tp)
+{
+ for (; tp; tp = tp->nxttri) {
+ tp->v.i = vMap(map, tp->v.i);
+ tp->v.j = vMap(map, tp->v.j);
+ }
+}
+
+/* addTri:
+ */
+static tri *
+addTri(int i, int j, tri * oldp)
+{
+ tri *tp = NEW(tri);
+ tp->v.i = i;
+ tp->v.j = j;
+ tp->nxttri = oldp;
+ return tp;
+}
+
+/* bisect:
+ * Return the angle bisecting the angle pp--cp--np
+ */
+static double bisect(pointf pp, pointf cp, pointf np)
+{
+ double theta, phi;
+ theta = atan2(np.y - cp.y, np.x - cp.x);
+ phi = atan2(pp.y - cp.y, pp.x - cp.x);
+ return (theta + phi) / 2.0;
+}
+
+#define dot(v,w) (v.x*w.x+v.y*w.y)
+#define SMALL 0.0000000001
+
+/* lintersect:
+ * Compute the intersection of segments (a,b) and (c,d).
+ * Store the point in p.
+ * Return 1 on success, 0 on failure.
+ */
+static int
+lintersect(pointf a, pointf b, pointf c, pointf d, pointf * p)
+{
+ pointf mu = a;
+ pointf mv = subPt(b, a);
+ pointf lu = c;
+ pointf lv = subPt(d, c);
+ pointf ln = perp(lv);
+ double lc = -(dot(ln, lu));
+ double dd = dot(ln, mv);
+ if (fabs(dd) < SMALL)
+ return 0;
+ *p = subPt(mu, scale((dot(ln, mu) + lc) / dd, mv));
+ return 1;
+}
+
+/* raySeg:
+ * Check if ray v->w intersects segment a--b.
+ */
+static int raySeg(pointf v, pointf w, pointf a, pointf b)
+{
+ int wa = wind(v, w, a);
+ int wb = wind(v, w, b);
+ if (wa == wb)
+ return 0;
+ if (wa == 0) {
+ return (wind(v, b, w) * wind(v, b, a) >= 0);
+ } else {
+ return (wind(v, a, w) * wind(v, a, b) >= 0);
+ }
+}
+
+/* raySegIntersect:
+ * Find the point p where ray v->w intersects segment ai-bi, if any.
+ * Return 1 on success, 0 on failure
+ */
+static int
+raySegIntersect(pointf v, pointf w, pointf a, pointf b,
+ pointf * p)
+{
+ if (raySeg(v, w, a, b))
+ return lintersect(v, w, a, b, p);
+ else
+ return 0;
+}
+
+/* triPoint:
+ * Given the triangle vertex v, and point w so that v->w points
+ * into the polygon, return where the ray v->w intersects the
+ * polygon. The search uses all of the opposite sides of triangles
+ * with v as vertex.
+ * Return 0 on success; 1 on failure.
+ */
+static int
+triPoint(tripoly_t * trip, int vx, pointf v, pointf w, pointf * ip)
+{
+ tri *tp;
+
+ for (tp = trip->triMap[vx]; tp; tp = tp->nxttri) {
+ if (raySegIntersect
+ (v, w, trip->poly.ps[tp->v.i], trip->poly.ps[tp->v.j], ip))
+ return 0;
+ }
+#ifdef DEBUG
+ fprintf(stderr, "%%Failure in triPoint\n");
+ fprintf(stderr, "0 0 1 setrgbcolor\n");
+ drawLine(v, w);
+ for (tp = trip->triMap[vx]; tp; tp = tp->nxttri) {
+ drawTri(v, trip->poly.ps[tp->v.i], trip->poly.ps[tp->v.j]);
+ }
+#endif
+
+ return 1;
+}
+
+/* ctrlPtIdx:
+ * Find the index of v in the points polys->ps.
+ * We start at 1 since the point corresponding to 0
+ * will never be used as v.
+ */
+static int ctrlPtIdx(pointf v, Ppoly_t * polys)
+{
+ pointf w;
+ int i;
+ for (i = 1; i < polys->pn; i++) {
+ w = polys->ps[i];
+ if ((w.x == v.x) && (w.y == v.y))
+ return i;
+ }
+ return -1;
+}
+
+#define SEP 15
+
+/* mkCtrlPts:
+ * Generate mult points associated with v.
+ * The points will lie on the ray bisecting the angle prev--v--nxt.
+ * The first point will aways be v.
+ * The rest are positioned equally spaced with maximum spacing SEP.
+ * In addition, they all lie within the polygon trip->poly.
+ * Parameter s gives the index after which a vertex likes on the
+ * opposite side. This is necessary to get the "curvature" of the
+ * path correct.
+ */
+static pointf *mkCtrlPts(int s, int mult, pointf prev, pointf v,
+ pointf nxt, tripoly_t * trip)
+{
+ pointf *ps;
+ int idx = ctrlPtIdx(v, &(trip->poly));
+ int i;
+ double d, sep, theta, sinTheta, cosTheta;
+ pointf q, w;
+
+ if (idx < 0)
+ return NULL;
+
+ ps = N_GNEW(mult, pointf);
+ theta = bisect(prev, v, nxt);
+ sinTheta = sin(theta);
+ cosTheta = cos(theta);
+ w.x = v.x + 100 * cosTheta;
+ w.y = v.y + 100 * sinTheta;
+ if ((idx > s) && (wind(prev, v, w) != 1)) {
+ sinTheta *= -1;
+ cosTheta *= -1;
+ w.x = v.x + 100 * cosTheta;
+ w.y = v.y + 100 * sinTheta;
+ } else if (wind(prev, v, w) != -1) {
+ sinTheta *= -1;
+ cosTheta *= -1;
+ w.x = v.x + 100 * cosTheta;
+ w.y = v.y + 100 * sinTheta;
+ }
+
+ if (triPoint(trip, idx, v, w, &q)) {
+ return 0;
+ }
+
+ d = DIST(q, v);
+ if (d >= mult * SEP)
+ sep = SEP;
+ else
+ sep = d / mult;
+ if (idx < s) {
+ for (i = 0; i < mult; i++) {
+ ps[i].x = v.x + i * sep * cosTheta;
+ ps[i].y = v.y + i * sep * sinTheta;
+ }
+ } else {
+ for (i = 0; i < mult; i++) {
+ ps[mult - i - 1].x = v.x + i * sep * cosTheta;
+ ps[mult - i - 1].y = v.y + i * sep * sinTheta;
+ }
+ }
+ return ps;
+}
+
+/*
+ * Simple graph structure for recording the triangle graph.
+ */
+
+typedef struct {
+ int ne; /* no. of edges. */
+ int *edges; /* indices of edges adjacent to node. */
+ pointf ctr; /* center of triangle. */
+} tnode;
+
+typedef struct {
+ int t, h; /* indices of head and tail nodes */
+ ipair seg; /* indices of points forming shared segment */
+ double dist; /* length of edge; usually distance between centers */
+} tedge;
+
+typedef struct {
+ tnode *nodes;
+ tedge *edges;
+ int nedges; /* no. of edges; no. of nodes is stored in router_t */
+} tgraph;
+
+struct router_s {
+ int pn; /* no. of points */
+ pointf *ps; /* all points in configuration */
+ int *obs; /* indices in obstacle i are obs[i]...obs[i+1]-1 */
+ int *tris; /* indices of triangle i are tris[3*i]...tris[3*i+2] */
+ Dt_t *trimap; /* map from obstacle side (a,b) to index of adj. triangle */
+ int tn; /* no. of nodes in tg */
+ tgraph *tg; /* graph of triangles */
+};
+
+/* triCenter:
+ * Given an array of points and 3 integer indices,
+ * compute and return the center of the triangle.
+ */
+static pointf triCenter(pointf * pts, int *idxs)
+{
+ pointf a = pts[*idxs++];
+ pointf b = pts[*idxs++];
+ pointf c = pts[*idxs++];
+ pointf p;
+ p.x = (a.x + b.x + c.x) / 3.0;
+ p.y = (a.y + b.y + c.y) / 3.0;
+ return p;
+}
+
+#define MARGIN 32
+
+/* bbox:
+ * Compute bounding box of polygons, and return it
+ * with an added margin of MARGIN.
+ * Store total number of points in *np.
+ */
+static boxf bbox(Ppoly_t** obsp, int npoly, int *np)
+{
+ boxf bb;
+ int i, j, cnt = 0;
+ pointf p;
+ Ppoly_t* obs;
+
+ bb.LL.x = bb.LL.y = MAXDOUBLE;
+ bb.UR.x = bb.UR.y = -MAXDOUBLE;
+
+ for (i = 0; i < npoly; i++) {
+ obs = *obsp++;
+ for (j = 0; j < obs->pn; j++) {
+ p = obs->ps[j];
+ if (p.x < bb.LL.x)
+ bb.LL.x = p.x;
+ if (p.x > bb.UR.x)
+ bb.UR.x = p.x;
+ if (p.y < bb.LL.y)
+ bb.LL.y = p.y;
+ if (p.y > bb.UR.y)
+ bb.UR.y = p.y;
+ cnt++;
+ }
+ }
+
+ *np = cnt;
+
+ bb.LL.x -= MARGIN;
+ bb.LL.y -= MARGIN;
+ bb.UR.x += MARGIN;
+ bb.UR.y += MARGIN;
+
+ return bb;
+}
+
+static int *mkTriIndices(surface_t * sf)
+{
+ int *tris = N_GNEW(3 * sf->nfaces, int);
+ memcpy(tris, sf->faces, 3 * sf->nfaces * sizeof(int));
+ return tris;
+}
+
+/* degT:
+ * Given a pointer to an array of 3 integers, return
+ * the number not equal to -1
+ */
+static int degT(int *ip)
+{
+ ip++;
+ if (*ip++ == -1)
+ return 1;
+ if (*ip == -1)
+ return 2;
+ else
+ return 3;
+}
+
+/* sharedEdge:
+ * Returns a pair of integer (x,y), x < y, where x and y are the
+ * indices of the two vertices of the shared edge.
+ */
+static ipair sharedEdge(int *p, int *q)
+{
+ ipair pt;
+ int tmp, p1, p2;
+ p1 = *p;
+ p2 = *(p + 1);
+ if (p1 == *q) {
+ if ((p2 != *(q + 1)) && (p2 != *(q + 2))) {
+ p2 = *(p + 2);
+ }
+ } else if (p1 == *(q + 1)) {
+ if ((p2 != *q) && (p2 != *(q + 2))) {
+ p2 = *(p + 2);
+ }
+ } else if (p1 == *(q + 2)) {
+ if ((p2 != *q) && (p2 != *(q + 1))) {
+ p2 = *(p + 2);
+ }
+ } else {
+ p1 = *(p + 2);
+ }
+
+ if (p1 > p2) {
+ tmp = p1;
+ p1 = p2;
+ p2 = tmp;
+ }
+ pt.i = p1;
+ pt.j = p2;
+ return pt;
+}
+
+/* addTriEdge:
+ * Add an edge to g, with tail t, head h, distance d, and shared
+ * segment seg.
+ */
+static void addTriEdge(tgraph * g, int t, int h, double d, ipair seg)
+{
+ tedge *ep = g->edges + g->nedges;
+ tnode *tp = g->nodes + t;
+ tnode *hp = g->nodes + h;
+
+ ep->t = t;
+ ep->h = h;
+ ep->dist = DIST(tp->ctr, hp->ctr);
+ ep->seg = seg;
+
+ tp->edges[tp->ne++] = g->nedges;
+ hp->edges[hp->ne++] = g->nedges;
+
+ g->nedges++;
+}
+
+static void freeTriGraph(tgraph * tg)
+{
+ free(tg->nodes->edges);
+ free(tg->nodes);
+ free(tg->edges);
+ free(tg);
+}
+
+/* mkTriGraph:
+ * Generate graph with triangles as nodes and an edge iff two triangles
+ * share an edge.
+ */
+static tgraph *mkTriGraph(surface_t * sf, int maxv, pointf * pts)
+{
+ tgraph *g;
+ tnode *np;
+ int j, i, ne = 0;
+ int *edgei;
+ int *jp;
+
+ /* ne is twice no. of edges */
+ for (i = 0; i < 3 * sf->nfaces; i++)
+ if (sf->neigh[i] != -1)
+ ne++;
+
+ g = GNEW(tgraph);
+
+ /* plus 2 for nodes added as endpoints of an edge */
+ g->nodes = N_GNEW(sf->nfaces + 2, tnode);
+
+ /* allow 1 possible extra edge per triangle, plus
+ * obstacles can have at most maxv triangles touching
+ */
+ edgei = N_GNEW(sf->nfaces + ne + 2 * maxv, int);
+ g->edges = N_GNEW(ne/2 + 2 * maxv, tedge);
+ g->nedges = 0;
+
+ for (i = 0; i < sf->nfaces; i++) {
+ np = g->nodes + i;
+ np->ne = 0;
+ np->edges = edgei;
+ np->ctr = triCenter(pts, sf->faces + 3 * i);
+ edgei += degT(sf->neigh + 3 * i) + 1;
+ }
+ /* initialize variable nodes */
+ np++;
+ np->ne = 0;
+ np->edges = edgei;
+ np++;
+ np->ne = 0;
+ np->edges = edgei + maxv;
+
+ for (i = 0; i < sf->nfaces; i++) {
+ np = g->nodes + i;
+ jp = sf->neigh + 3 * i;
+ ne = 0;
+ while ((ne < 3) && ((j = *jp++) != -1)) {
+ if (i < j) {
+ double dist = DIST(np->ctr, (g->nodes + j)->ctr);
+ ipair seg =
+ sharedEdge(sf->faces + 3 * i, sf->faces + 3 * j);
+ addTriEdge(g, i, j, dist, seg);
+ }
+ ne++;
+ }
+ }
+
+ return g;
+}
+
+void freeRouter(router_t * rtr)
+{
+ free(rtr->ps);
+ free(rtr->obs);
+ free(rtr->tris);
+ dtclose(rtr->trimap);
+ freeTriGraph(rtr->tg);
+ free(rtr);
+}
+
+#include <psdbg.c>
+#define DEBUG
+#ifdef DEBUG
+static void
+prTriGraph (router_t* rtr, int n)
+{
+ FILE* fp = fopen ("dump","w");
+ int i;
+ pointf* pts = rtr->ps;
+ tnode* nodes = rtr->tg->nodes;
+ char buf[BUFSIZ];
+
+ psInit();
+ for (i=0;i < rtr->tn; i++) {
+ pointf a = pts[rtr->tris[3*i]];
+ pointf b = pts[rtr->tris[3*i+1]];
+ pointf c = pts[rtr->tris[3*i+2]];
+ psTri (a, b,c);
+ sprintf (buf, "%d", i);
+ psTxt (buf, nodes[i].ctr);
+ }
+ for (i=rtr->tn;i < n; i++) {
+ psTxt (buf, nodes[i].ctr);
+ }
+ psColor ("1 0 0");
+ for (i=0;i < rtr->tg->nedges; i++) {
+ tedge* e = rtr->tg->edges+i;
+ psSeg (nodes[e->t].ctr, nodes[e->h].ctr);
+ }
+ psOut(fp);
+ fclose(fp);
+}
+#endif
+
+router_t *mkRouter(Ppoly_t** obsp, int npoly)
+{
+ router_t *rtr = NEW(router_t);
+ Ppoly_t* obs;
+ boxf bb;
+ pointf *pts;
+ int npts;
+ surface_t *sf;
+ int *segs;
+ double *x;
+ double *y;
+ int maxv = 4; /* default max. no. of vertices in an obstacle; set below */
+ /* points in obstacle i have indices obsi[i] through obsi[i+1]-1 in pts
+ */
+ int *obsi = N_NEW(npoly + 1, int);
+ int i, j, ix = 4, six = 0;
+
+ /* psInit(); */
+ bb = bbox(obsp, npoly, &npts);
+ npts += 4; /* 4 points of bounding box */
+ pts = N_GNEW(npts, pointf); /* all points are stored in pts */
+ segs = N_GNEW(2 * npts, int); /* indices of points forming segments */
+
+ /* store bounding box in CCW order */
+ pts[0] = bb.LL;
+ pts[1].x = bb.UR.x;
+ pts[1].y = bb.LL.y;
+ pts[2] = bb.UR;
+ pts[3].x = bb.LL.x;
+ pts[3].y = bb.UR.y;
+ for (i = 1; i <= 4; i++) {
+ segs[six++] = i - 1;
+ if (i < 4)
+ segs[six++] = i;
+ else
+ segs[six++] = 0;
+ }
+
+/* psColor("0.8 0.8 0.8"); */
+ /* store obstacles in CW order and generate constraint segments */
+ for (i = 0; i < npoly; i++) {
+ obsi[i] = ix;
+ obs = *obsp++;
+/* psPolyf (obs); */
+ for (j = 1; j <= obs->pn; j++) {
+ segs[six++] = ix;
+ if (j < obs->pn)
+ segs[six++] = ix + 1;
+ else
+ segs[six++] = obsi[i];
+ pts[ix++] = obs->ps[j - 1];
+ }
+ if (obs->pn > maxv)
+ maxv = obs->pn;
+ }
+ obsi[i] = ix;
+
+ /* copy points into coordinate arrays */
+ x = N_GNEW(npts, double);
+ y = N_GNEW(npts, double);
+ for (i = 0; i < npts; i++) {
+ x[i] = pts[i].x;
+ y[i] = pts[i].y;
+ }
+ sf = mkSurface(x, y, npts, segs, npts);
+ free(x);
+ free(y);
+ free(segs);
+
+ rtr->ps = pts;
+ rtr->pn = npts;
+ rtr->obs = obsi;
+ rtr->tris = mkTriIndices(sf);
+ rtr->trimap = mapSegToTri(sf);
+ rtr->tn = sf->nfaces;
+ rtr->tg = mkTriGraph(sf, maxv, pts);
+
+/*
+psColor("0 0 0");
+for (i = 0; i < rtr->tn; i++) {
+ psTri(pts[rtr->tris[3*i]], pts[rtr->tris[3*i+1]],pts[rtr->tris[3*i+2]]);
+}
+psOut(stderr);
+*/
+ freeSurface(sf);
+ return rtr;
+}
+
+/* finishEdge:
+ * Finish edge generation, clipping to nodes and adding arrowhead
+ * if necessary, and adding edge labels
+ */
+static void
+finishEdge (edge_t* e, Ppoly_t spl, int flip, pointf p, pointf q)
+{
+ int j;
+ point *ispline = N_GNEW(spl.pn, point);
+ point p1, q1;
+
+
+ if (flip) {
+ for (j = 0; j < spl.pn; j++) {
+ PF2P (spl.ps[j], ispline[spl.pn - 1 - j]);
+ }
+ PF2P(q, p1);
+ PF2P(p, q1);
+ }
+ else {
+ for (j = 0; j < spl.pn; j++) {
+ PF2P (spl.ps[j], ispline[j]);
+ }
+ PF2P(p, p1);
+ PF2P(q, q1);
+ }
+ if (Verbose > 1)
+ fprintf(stderr, "spline %s %s\n", e->tail->name, e->head->name);
+ clip_and_install(e, e->head, ispline, spl.pn, &sinfo);
+ free(ispline);
+
+ addEdgeLabels(e, p1, q1);
+}
+
+#define LEN(x,y) sqrt((x)*(x)+(y)*(y))
+#define EQPT(p,q) (((p).x==(q).x)&&((p).y==(q).y))
+
+/* tweakEnd:
+ * Hack because path routing doesn't know about the interiors
+ * of polygons. If the first or last segment of the shortest path
+ * lies along one of the polygon boundaries, the path may flip
+ * inside the polygon. To avoid this, we shift the point a bit.
+ *
+ * If the edge p(=poly.ps[s])-q of the shortest path is also an
+ * edge of the border polygon, move p slightly inside the polygon
+ * and return it. If prv and nxt are the two vertices adjacent to
+ * p in the polygon, let m be the midpoint of prv--nxt. We then
+ * move a tiny bit along the ray p->m.
+ *
+ * Otherwise, return p unchanged.
+ */
+static Ppoint_t
+tweakEnd (Ppoly_t poly, int s, Ppolyline_t pl, Ppoint_t q)
+{
+ Ppoint_t prv, nxt, p;
+
+ p = poly.ps[s];
+ nxt = poly.ps[(s + 1) % poly.pn];
+ if (s == 0)
+ prv = poly.ps[poly.pn-1];
+ else
+ prv = poly.ps[s - 1];
+ if (EQPT(q, nxt) || EQPT(q, prv) ){
+ Ppoint_t m;
+ double d;
+ m.x = (nxt.x + prv.x)/2.0 - p.x;
+ m.y = (nxt.y + prv.y)/2.0 - p.y;
+ d = LEN(m.x,m.y);
+ p.x += 0.1*m.x/d;
+ p.y += 0.1*m.y/d;
+ }
+ return p;
+}
+
+static void
+tweakPath (Ppoly_t poly, int s, int t, Ppolyline_t pl)
+{
+ pl.ps[0] = tweakEnd (poly, s, pl, pl.ps[1]);
+ pl.ps[pl.pn-1] = tweakEnd (poly, t, pl, pl.ps[pl.pn-2]);
+}
+
+
+/* genroute:
+ * Generate splines for e and cohorts.
+ * Edges go from s to t.
+ * Return 0 on success.
+ */
+static int
+genroute(tripoly_t * trip, int s, int t, edge_t * e, int doPolyline)
+{
+ pointf eps[2];
+ Pvector_t evs[2];
+ pointf **cpts; /* lists of control points */
+ Ppoly_t poly;
+ Ppolyline_t pl, spl;
+ int i, j;
+ Ppolyline_t mmpl;
+ Pedge_t *medges = N_GNEW(trip->poly.pn, Pedge_t);
+ int pn;
+ int mult = ED_count(e);
+ node_t* head = e->head;
+
+ eps[0].x = trip->poly.ps[s].x, eps[0].y = trip->poly.ps[s].y;
+ eps[1].x = trip->poly.ps[t].x, eps[1].y = trip->poly.ps[t].y;
+ Pshortestpath(&(trip->poly), eps, &pl);
+
+/*
+if (_cnt == 2) {
+ psInit();
+ psPoly (&(trip->poly));
+ psColor("1 0 0");
+ psPolyline(&pl);
+}
+*/
+
+ if (pl.pn == 2) {
+ makeStraightEdge(head->graph, e, doPolyline);
+ return 0;
+ }
+
+ evs[0].x = evs[0].y = 0;
+ evs[1].x = evs[1].y = 0;
+
+ if (mult == 1) {
+ poly = trip->poly;
+ for (j = 0; j < poly.pn; j++) {
+ medges[j].a = poly.ps[j];
+ medges[j].b = poly.ps[(j + 1) % poly.pn];
+ }
+ tweakPath (poly, s, t, pl);
+ Proutespline(medges, poly.pn, pl, evs, &spl);
+ finishEdge (e, spl, e->head != head, eps[0], eps[1]);
+ free(medges);
+
+ return 0;
+ }
+
+ pn = 2 * (pl.pn - 1);
+
+ cpts = N_NEW(pl.pn - 2, pointf *);
+ for (i = 0; i < pl.pn - 2; i++) {
+ cpts[i] =
+ mkCtrlPts(t, mult+1, pl.ps[i], pl.ps[i + 1], pl.ps[i + 2], trip);
+ if (!cpts[i]) {
+ agerr(AGWARN, "Could not create control points for multiple spline for edge (%s,%s)\n", e->tail->name, e->head->name);
+ return 1;
+ }
+ }
+
+ poly.ps = N_GNEW(pn, pointf);
+ poly.pn = pn;
+
+ for (i = 0; i < mult; i++) {
+ poly.ps[0] = eps[0];
+ for (j = 1; j < pl.pn - 1; j++) {
+ poly.ps[j] = cpts[j - 1][i];
+ }
+ poly.ps[pl.pn - 1] = eps[1];
+ for (j = 1; j < pl.pn - 1; j++) {
+ poly.ps[pn - j] = cpts[j - 1][i + 1];
+ }
+/* if (_cnt == 2) psPoly(&poly); */
+ Pshortestpath(&poly, eps, &mmpl);
+
+ if (doPolyline) {
+ make_polyline (mmpl, &spl);
+ }
+ else {
+ for (j = 0; j < poly.pn; j++) {
+ medges[j].a = poly.ps[j];
+ medges[j].b = poly.ps[(j + 1) % poly.pn];
+ }
+ tweakPath (poly, 0, pl.pn-1, mmpl);
+ Proutespline(medges, poly.pn, mmpl, evs, &spl);
+ }
+ finishEdge (e, spl, e->head != head, eps[0], eps[1]);
+
+ e = ED_to_virt(e);
+ }
+/* if (_cnt == 2) psOut(stderr); */
+
+ for (i = 0; i < pl.pn - 2; i++)
+ free(cpts[i]);
+ free(cpts);
+ free(medges);
+ free(poly.ps);
+ return 0;
+}
+
+#define NSMALL -0.0000000001
+
+/* inCone:
+ * Returns true iff q is in the convex cone a-b-c
+ */
+static int
+inCone (pointf a, pointf b, pointf c, pointf q)
+{
+ return ((area2(q,a,b) >= NSMALL) && (area2(q,b,c) >= NSMALL));
+}
+
+static pointf north = {0, 1};
+static pointf northeast = {1, 1};
+static pointf east = {1, 0};
+static pointf southeast = {1, -1};
+static pointf south = {0, -1};
+static pointf southwest = {-1, -1};
+static pointf west = {-1, 0};
+static pointf northwest = {-1, 1};
+
+/* addEndpoint:
+ * Add node to graph representing spline end point p inside obstruction obs_id.
+ * For each side of obstruction, add edge from p to corresponding triangle.
+ * The node id of the new node in the graph is v_id.
+ * If p lies on the side of its node (sides != 0), we limit the triangles
+ * to those within 45 degrees of each side of the natural direction of p.
+ */
+static void addEndpoint(router_t * rtr, pointf p, node_t* v, int v_id, int sides)
+{
+ int obs_id = ND_lim(v);
+ int starti = rtr->obs[obs_id];
+ int endi = rtr->obs[obs_id + 1];
+ pointf* pts = rtr->ps;
+ int i, t;
+ double d;
+ pointf v0, v1;
+
+ switch (sides) {
+ case TOP :
+ v0 = addPt (p, northwest);
+ v1 = addPt (p, northeast);
+ break;
+ case TOP|RIGHT :
+ v0 = addPt (p, north);
+ v1 = addPt (p, east);
+ break;
+ case RIGHT :
+ v0 = addPt (p, northeast);
+ v1 = addPt (p, southeast);
+ break;
+ case BOTTOM|RIGHT :
+ v0 = addPt (p, east);
+ v1 = addPt (p, south);
+ break;
+ case BOTTOM :
+ v0 = addPt (p, southeast);
+ v1 = addPt (p, southwest);
+ break;
+ case BOTTOM|LEFT :
+ v0 = addPt (p, south);
+ v1 = addPt (p, west);
+ break;
+ case LEFT :
+ v0 = addPt (p, southwest);
+ v1 = addPt (p, northwest);
+ break;
+ case TOP|LEFT :
+ v0 = addPt (p, west);
+ v1 = addPt (p, north);
+ break;
+ case 0 :
+ break;
+ default :
+ assert (0);
+ break;
+ }
+
+ rtr->tg->nodes[v_id].ne = 0;
+ rtr->tg->nodes[v_id].ctr = p;
+ for (i = starti; i < endi; i++) {
+ ipair seg;
+ seg.i = i;
+ if (i < endi - 1)
+ seg.j = i + 1;
+ else
+ seg.j = starti;
+ t = findMap(rtr->trimap, seg.i, seg.j);
+ if (sides && !inCone (v0, p, v1, pts[seg.i]) && !inCone (v0, p, v1, pts[seg.j]))
+ continue;
+ d = DIST(p, (rtr->tg->nodes + t)->ctr);
+ addTriEdge(rtr->tg, v_id, t, d, seg);
+ }
+}
+
+/* edgeToSeg:
+ * Given edge from i to j, find segment associated
+ * with the edge.
+ *
+ * This lookup could be made faster by modifying the
+ * shortest path algorithm to store the edges rather than
+ * the nodes.
+ */
+static ipair edgeToSeg(tgraph * tg, int i, int j)
+{
+ ipair ip;
+ tnode *np = tg->nodes + i;
+ tedge *ep;
+
+ for (i = 0; i < np->ne; i++) {
+ ep = tg->edges + np->edges[i];
+ if ((ep->t == j) || (ep->h == j))
+ return (ep->seg);
+ }
+
+ assert(0);
+ return ip;
+}
+
+static void
+freeTripoly (tripoly_t* trip)
+{
+ int i;
+ tri* tp;
+ tri* nxt;
+
+ free (trip->poly.ps);
+ for (i = 0; i < trip->poly.pn; i++) {
+ for (tp = trip->triMap[i]; tp; tp = nxt) {
+ nxt = tp->nxttri;
+ free (tp);
+ }
+ }
+ free (trip->triMap);
+ free (trip);
+}
+
+/* Auxiliary data structure used to translate a path of rectangles
+ * into a polygon. Each side_t represents a vertex on one side of
+ * the polygon. v is the index of the vertex in the global router_t,
+ * and ts is a linked list of the indices of segments of sides opposite
+ * to v in some triangle on the path. These lists will be translated
+ * to polygon indices by mapTri, and stored in tripoly_t.triMap.
+ */
+typedef struct {
+ int v;
+ tri *ts;
+} side_t;
+
+/* mkPoly:
+ * Construct simple polygon from shortest path from t to s in g.
+ * dad gives the indices of the triangles on path.
+ * sx used to store index of s in points.
+ * index of t is always 0
+ */
+static tripoly_t *mkPoly(router_t * rtr, int *dad, int s, int t,
+ pointf p_s, pointf p_t, int *sx)
+{
+ tripoly_t *ps;
+ int nxt;
+ ipair p;
+ int nt = 0;
+ side_t *side1;
+ side_t *side2;
+ int i, idx;
+ int cnt1 = 0;
+ int cnt2 = 0;
+ pointf *pts;
+ pointf *pps;
+ /* maps vertex index used in router_t to vertex index used in tripoly */
+ Dt_t *vmap;
+ tri **trim;
+
+ /* count number of triangles in path */
+ for (nxt = dad[t]; nxt != s; nxt = dad[nxt])
+ nt++;
+
+ side1 = N_NEW(nt + 4, side_t);
+ side2 = N_NEW(nt + 4, side_t);
+
+ nxt = dad[t];
+ p = edgeToSeg(rtr->tg, nxt, t);
+ side1[cnt1].ts = addTri(-1, p.j, NULL);
+ side1[cnt1++].v = p.i;
+ side2[cnt2].ts = addTri(-1, p.i, NULL);
+ side2[cnt2++].v = p.j;
+
+ t = nxt;
+ for (nxt = dad[t]; nxt >= 0; nxt = dad[nxt]) {
+ p = edgeToSeg(rtr->tg, t, nxt);
+ if (p.i == side1[cnt1 - 1].v) {
+ side1[cnt1 - 1].ts =
+ addTri(side2[cnt2 - 1].v, p.j, side1[cnt1 - 1].ts);
+ side2[cnt2 - 1].ts =
+ addTri(side1[cnt1 - 1].v, p.j, side2[cnt2 - 1].ts);
+ side2[cnt2].ts =
+ addTri(side2[cnt2 - 1].v, side1[cnt1 - 1].v, NULL);
+ side2[cnt2++].v = p.j;
+ } else if (p.i == side2[cnt2 - 1].v) {
+ side1[cnt1 - 1].ts =
+ addTri(side2[cnt2 - 1].v, p.j, side1[cnt1 - 1].ts);
+ side2[cnt2 - 1].ts =
+ addTri(side1[cnt1 - 1].v, p.j, side2[cnt2 - 1].ts);
+ side1[cnt1].ts =
+ addTri(side2[cnt2 - 1].v, side1[cnt1 - 1].v, NULL);
+ side1[cnt1++].v = p.j;
+ } else if (p.j == side1[cnt1 - 1].v) {
+ side1[cnt1 - 1].ts =
+ addTri(side2[cnt2 - 1].v, p.i, side1[cnt1 - 1].ts);
+ side2[cnt2 - 1].ts =
+ addTri(side1[cnt1 - 1].v, p.i, side2[cnt2 - 1].ts);
+ side2[cnt2].ts =
+ addTri(side2[cnt2 - 1].v, side1[cnt1 - 1].v, NULL);
+ side2[cnt2++].v = p.i;
+ } else {
+ side1[cnt1 - 1].ts =
+ addTri(side2[cnt2 - 1].v, p.i, side1[cnt1 - 1].ts);
+ side2[cnt2 - 1].ts =
+ addTri(side1[cnt1 - 1].v, p.i, side2[cnt2 - 1].ts);
+ side1[cnt1].ts =
+ addTri(side2[cnt2 - 1].v, side1[cnt1 - 1].v, NULL);
+ side1[cnt1++].v = p.i;
+ }
+ t = nxt;
+ }
+ side1[cnt1 - 1].ts = addTri(-2, side2[cnt2 - 1].v, side1[cnt1 - 1].ts);
+ side2[cnt2 - 1].ts = addTri(-2, side1[cnt1 - 1].v, side2[cnt2 - 1].ts);
+
+ /* store points in pts starting with t in 0,
+ * then side1, then s, then side2
+ */
+ vmap = dtopen(&ipairdisc, Dtoset);
+ vmapAdd(vmap, -1, 0);
+ vmapAdd(vmap, -2, cnt1 + 1);
+ pps = pts = N_GNEW(nt + 4, pointf);
+ trim = N_NEW(nt + 4, tri *);
+ *pps++ = p_t;
+ idx = 1;
+ for (i = 0; i < cnt1; i++) {
+ vmapAdd(vmap, side1[i].v, idx);
+ *pps++ = rtr->ps[side1[i].v];
+ trim[idx++] = side1[i].ts;
+ }
+ *pps++ = p_s;
+ idx++;
+ for (i = cnt2 - 1; i >= 0; i--) {
+ vmapAdd(vmap, side2[i].v, idx);
+ *pps++ = rtr->ps[side2[i].v];
+ trim[idx++] = side2[i].ts;
+ }
+
+ for (i = 0; i < nt + 4; i++) {
+ mapTri(vmap, trim[i]);
+ }
+
+ ps = NEW(tripoly_t);
+ ps->poly.pn = nt + 4; /* nt triangles gives nt+2 points plus s and t */
+ ps->poly.ps = pts;
+ ps->triMap = trim;
+
+ free (side1);
+ free (side2);
+ dtclose(vmap);
+ *sx = cnt1 + 1; /* index of s in ps */
+ return ps;
+}
+
+/* resetGraph:
+ * Remove edges and nodes added for current edge routing
+ */
+static void resetGraph(tgraph * g, int ncnt, int ecnt)
+{
+ int i;
+ tnode *np = g->nodes;
+ g->nedges = ecnt;
+ for (i = 0; i < ncnt; i++) {
+ if (np->edges + np->ne == (np + 1)->edges)
+ np->ne--;
+ np++;
+ }
+}
+
+#define PQTYPE int
+#define PQVTYPE float
+
+#define PQ_TYPES
+#include <fPQ.h>
+#undef PQ_TYPES
+
+typedef struct {
+ PQ pq;
+ PQVTYPE *vals;
+ int *idxs;
+} PPQ;
+
+#define N_VAL(pq,n) ((PPQ*)pq)->vals[n]
+#define N_IDX(pq,n) ((PPQ*)pq)->idxs[n]
+
+#define PQ_CODE
+#include <fPQ.h>
+#undef PQ_CODE
+
+#define N_DAD(n) dad[n]
+#define E_WT(e) (e->dist)
+#define UNSEEN -MAXFLOAT
+
+/* triPath:
+ * Find the shortest path with lengths in g from
+ * v0 to v1. The returned vector (dad) encodes the
+ * shorted path from v1 to v0. That path is given by
+ * v1, dad[v1], dad[dad[v1]], ..., v0.
+ */
+static int *
+triPath(tgraph * g, int n, int v0, int v1, PQ * pq)
+{
+ int i, j, adjn;
+ double d;
+ tnode *np;
+ tedge *e;
+ int *dad = N_NEW(n, int);
+
+ for (i = 0; i < pq->PQsize; i++)
+ N_VAL(pq, i) = UNSEEN;
+
+ PQinit(pq);
+ N_DAD(v0) = -1;
+ N_VAL(pq, v0) = 0;
+ PQinsert(pq, v0);
+
+ while ((i = PQremove(pq)) != -1) {
+ N_VAL(pq, i) *= -1;
+ if (i == v1)
+ break;
+ np = g->nodes + i;
+ for (j = 0; j < np->ne; j++) {
+ e = g->edges + np->edges[j];
+ if (e->t == i)
+ adjn = e->h;
+ else
+ adjn = e->t;
+ if (N_VAL(pq, adjn) < 0) {
+ d = -(N_VAL(pq, i) + E_WT(e));
+ if (N_VAL(pq, adjn) == UNSEEN) {
+ N_VAL(pq, adjn) = d;
+ N_DAD(adjn) = i;
+ PQinsert(pq, adjn);
+ } else if (N_VAL(pq, adjn) < d) {
+ PQupdate(pq, adjn, d);
+ N_DAD(adjn) = i;
+ }
+ }
+ }
+ }
+ return dad;
+}
+
+/* makeMultiSpline:
+ * FIX: we don't really use the shortest path provided by ED_path,
+ * so avoid in neato spline code.
+ * Return 0 on success.
+ */
+int makeMultiSpline(edge_t* e, router_t * rtr, int doPolyline)
+{
+ Ppolyline_t line = ED_path(e);
+ node_t *t = e->tail;
+ node_t *h = e->head;
+ pointf t_p = line.ps[0];
+ pointf h_p = line.ps[line.pn - 1];
+ tripoly_t *poly;
+ int idx;
+ int *sp;
+ int t_id = rtr->tn;
+ int h_id = rtr->tn + 1;
+ int ecnt = rtr->tg->nedges;
+ PPQ pq;
+ PQTYPE *idxs;
+ PQVTYPE *vals;
+ int ret;
+
+_cnt++;
+
+ /* Add endpoints to triangle graph */
+ addEndpoint(rtr, t_p, t, t_id, ED_tail_port(e).side);
+ addEndpoint(rtr, h_p, h, h_id, ED_head_port(e).side);
+/* if (_cnt == 1) prTriGraph (rtr, rtr->tn+2); */
+
+ /* Initialize priority queue */
+ PQgen(&pq.pq, rtr->tn + 2, -1);
+ idxs = N_GNEW(pq.pq.PQsize + 1, PQTYPE);
+ vals = N_GNEW(pq.pq.PQsize + 1, PQVTYPE);
+ vals[0] = 0;
+ pq.vals = vals + 1;
+ pq.idxs = idxs + 1;
+
+ /* Find shortest path of triangles */
+ sp = triPath(rtr->tg, rtr->tn+2, h_id, t_id, (PQ *) & pq);
+
+ free(vals);
+ free(idxs);
+ PQfree(&(pq.pq), 0);
+
+ /* Use path of triangles to generate guiding polygon */
+ poly = mkPoly(rtr, sp, h_id, t_id, h_p, t_p, &idx);
+ free(sp);
+
+ /* Generate multiple splines using polygon */
+ ret = genroute(poly, 0, idx, e, doPolyline);
+ freeTripoly (poly);
+
+ resetGraph(rtr->tg, rtr->tn, ecnt);
+ return ret;
+}
projects / graphviz / commitdiff