/misc/src/release/graphviz-2.18-1/src/graphviz-2.18/lib/pathplan/visibility.c

Go to the documentation of this file.
00001 /* $Id: visibility.c,v 1.2 2005/02/10 23:16:10 erg Exp $ $Revision: 1.2 $ */
00002 /* vim:set shiftwidth=4 ts=8: */
00003 
00004 /**********************************************************
00005 *      This software is part of the graphviz package      *
00006 *                http://www.graphviz.org/                 *
00007 *                                                         *
00008 *            Copyright (c) 1994-2004 AT&T Corp.           *
00009 *                and is licensed under the                *
00010 *            Common Public License, Version 1.0           *
00011 *                      by AT&T Corp.                      *
00012 *                                                         *
00013 *        Information and Software Systems Research        *
00014 *              AT&T Research, Florham Park NJ             *
00015 **********************************************************/
00016 
00017 
00018 #include <vis.h>
00019 
00020 #ifdef DMALLOC
00021 #include "dmalloc.h"
00022 #endif
00023 
00024         /* TRANSPARENT means router sees past colinear obstacles */
00025 #ifdef TRANSPARENT
00026 #define INTERSECT(a,b,c,d,e) intersect1((a),(b),(c),(d),(e))
00027 #else
00028 #define INTERSECT(a,b,c,d,e) intersect((a),(b),(c),(d))
00029 #endif
00030 
00031 /* allocArray:
00032  * Allocate a VxV array of COORD values.
00033  * (array2 is a pointer to an array of pointers; the array is
00034  * accessed in row-major order.)
00035  * The values in the array are initialized to 0.
00036  * Add extra rows.
00037  */
00038 static array2 allocArray(int V, int extra)
00039 {
00040     int i, k;
00041     array2 arr;
00042     COORD *p;
00043 
00044     arr = (COORD **) malloc((V + extra) * sizeof(COORD *));
00045     for (i = 0; i < V; i++) {
00046         p = (COORD *) malloc(V * sizeof(COORD));
00047         arr[i] = p;
00048         for (k = 0; k < V; k++) {
00049             *p++ = 0;
00050         }
00051     }
00052     for (i = V; i < V + extra; i++)
00053         arr[i] = (COORD *) 0;
00054 
00055     return arr;
00056 }
00057 
00058 /* area2:
00059  * Returns twice the area of triangle abc.
00060  */
00061 COORD area2(Ppoint_t a, Ppoint_t b, Ppoint_t c)
00062 {
00063     return ((a.y - b.y) * (c.x - b.x) - (c.y - b.y) * (a.x - b.x));
00064 }
00065 
00066 /* wind:
00067  * Returns 1, 0, -1 if the points abc are counterclockwise,
00068  * collinear, or clockwise.
00069  */
00070 int wind(Ppoint_t a, Ppoint_t b, Ppoint_t c)
00071 {
00072     COORD w;
00073 
00074     w = ((a.y - b.y) * (c.x - b.x) - (c.y - b.y) * (a.x - b.x));
00075     /* need to allow for small math errors.  seen with "gcc -O2 -mcpu=i686 -ffast-math" */
00076     return (w > .0001) ? 1 : ((w < -.0001) ? -1 : 0);
00077 }
00078 
00079 #if 0                           /* NOT USED */
00080 /* open_intersect:
00081  * Returns true iff segment ab intersects segment cd.
00082  * NB: segments are considered open sets
00083  */
00084 static int open_intersect(Ppoint_t a, Ppoint_t b, Ppoint_t c, Ppoint_t d)
00085 {
00086     return (((area2(a, b, c) > 0 && area2(a, b, d) < 0) ||
00087              (area2(a, b, c) < 0 && area2(a, b, d) > 0))
00088             &&
00089             ((area2(c, d, a) > 0 && area2(c, d, b) < 0) ||
00090              (area2(c, d, a) < 0 && area2(c, d, b) > 0)));
00091 }
00092 #endif
00093 
00094 /* inBetween:
00095  * Return true if c is in (a,b), assuming a,b,c are collinear.
00096  */
00097 int inBetween(Ppoint_t a, Ppoint_t b, Ppoint_t c)
00098 {
00099     if (a.x != b.x)             /* not vertical */
00100         return (((a.x < c.x) && (c.x < b.x))
00101                 || ((b.x < c.x) && (c.x < a.x)));
00102     else
00103         return (((a.y < c.y) && (c.y < b.y))
00104                 || ((b.y < c.y) && (c.y < a.y)));
00105 }
00106 
00107         /* TRANSPARENT means router sees past colinear obstacles */
00108 #ifdef TRANSPARENT
00109 /* intersect1:
00110  * Returns true if the polygon segment [q,n) blocks a and b from seeing
00111  * each other.
00112  * More specifically, returns true iff the two segments intersect as open
00113  * sets, or if q lies on (a,b) and either n and p lie on
00114  * different sides of (a,b), i.e., wind(a,b,n)*wind(a,b,p) < 0, or the polygon
00115  * makes a left turn at q, i.e., wind(p,q,n) > 0.
00116  *
00117  * We are assuming the p,q,n are three consecutive vertices of a barrier
00118  * polygon with the polygon interior to the right of p-q-n.
00119  *
00120  * Note that given the constraints of our problem, we could probably
00121  * simplify this code even more. For example, if abq are collinear, but
00122  * q is not in (a,b), we could return false since n will not be in (a,b)
00123  * nor will the (a,b) intersect (q,n).
00124  *
00125  * Also note that we are computing w_abq twice in a tour of a polygon,
00126  * once for each edge of which it is a vertex.
00127  */
00128 static int intersect1(Ppoint_t a, Ppoint_t b, Ppoint_t q, Ppoint_t n,
00129                       Ppoint_t p)
00130 {
00131     int w_abq;
00132     int w_abn;
00133     int w_qna;
00134     int w_qnb;
00135 
00136     w_abq = wind(a, b, q);
00137     w_abn = wind(a, b, n);
00138 
00139     /* If q lies on (a,b),... */
00140     if ((w_abq == 0) && inBetween(a, b, q)) {
00141         return ((w_abn * wind(a, b, p) < 0) || (wind(p, q, n) > 0));
00142     } else {
00143         w_qna = wind(q, n, a);
00144         w_qnb = wind(q, n, b);
00145         /* True if q and n are on opposite sides of ab,
00146          * and a and b are on opposite sides of qn.
00147          */
00148         return (((w_abq * w_abn) < 0) && ((w_qna * w_qnb) < 0));
00149     }
00150 }
00151 #else
00152 
00153 /* intersect:
00154  * Returns true if the segment [c,d] blocks a and b from seeing each other.
00155  * More specifically, returns true iff c or d lies on (a,b) or the two
00156  * segments intersect as open sets.
00157  */
00158 int intersect(Ppoint_t a, Ppoint_t b, Ppoint_t c, Ppoint_t d)
00159 {
00160     int a_abc;
00161     int a_abd;
00162     int a_cda;
00163     int a_cdb;
00164 
00165     a_abc = wind(a, b, c);
00166     if ((a_abc == 0) && inBetween(a, b, c)) {
00167         return 1;
00168     }
00169     a_abd = wind(a, b, d);
00170     if ((a_abd == 0) && inBetween(a, b, d)) {
00171         return 1;
00172     }
00173     a_cda = wind(c, d, a);
00174     a_cdb = wind(c, d, b);
00175 
00176     /* True if c and d are on opposite sides of ab,
00177      * and a and b are on opposite sides of cd.
00178      */
00179     return (((a_abc * a_abd) < 0) && ((a_cda * a_cdb) < 0));
00180 }
00181 #endif
00182 
00183 /* in_cone:
00184  * Returns true iff point b is in the cone a0,a1,a2
00185  * NB: the cone is considered a closed set
00186  */
00187 static int in_cone(Ppoint_t a0, Ppoint_t a1, Ppoint_t a2, Ppoint_t b)
00188 {
00189     int m = wind(b, a0, a1);
00190     int p = wind(b, a1, a2);
00191 
00192     if (wind(a0, a1, a2) > 0)
00193         return (m >= 0 && p >= 0);      /* convex at a */
00194     else
00195         return (m >= 0 || p >= 0);      /* reflex at a */
00196 }
00197 
00198 #if 0                           /* NOT USED */
00199 /* in_open_cone:
00200  * Returns true iff point b is in the cone a0,a1,a2
00201  * NB: the cone is considered an open set
00202  */
00203 static int in_open_cone(Ppoint_t a0, Ppoint_t a1, Ppoint_t a2, Ppoint_t b)
00204 {
00205     int m = wind(b, a0, a1);
00206     int p = wind(b, a1, a2);
00207 
00208     if (wind(a0, a1, a2) >= 0)
00209         return (m > 0 && p > 0);        /* convex at a */
00210     else
00211         return (m > 0 || p > 0);        /* reflex at a */
00212 }
00213 #endif
00214 
00215 /* dist2:
00216  * Returns the square of the distance between points a and b.
00217  */
00218 COORD dist2(Ppoint_t a, Ppoint_t b)
00219 {
00220     COORD delx = a.x - b.x;
00221     COORD dely = a.y - b.y;
00222 
00223     return (delx * delx + dely * dely);
00224 }
00225 
00226 /* dist:
00227  * Returns the distance between points a and b.
00228  */
00229 static COORD dist(Ppoint_t a, Ppoint_t b)
00230 {
00231     return sqrt(dist2(a, b));
00232 }
00233 
00234 static int inCone(int i, int j, Ppoint_t pts[], int nextPt[], int prevPt[])
00235 {
00236     return in_cone(pts[prevPt[i]], pts[i], pts[nextPt[i]], pts[j]);
00237 }
00238 
00239 /* clear:
00240  * Return true if no polygon line segment non-trivially intersects
00241  * the segment [pti,ptj], ignoring segments in [start,end).
00242  */
00243 static int clear(Ppoint_t pti, Ppoint_t ptj,
00244                  int start, int end,
00245                  int V, Ppoint_t pts[], int nextPt[], int prevPt[])
00246 {
00247     int k;
00248 
00249     for (k = 0; k < start; k++) {
00250         if (INTERSECT(pti, ptj, pts[k], pts[nextPt[k]], pts[prevPt[k]]))
00251             return 0;
00252     }
00253     for (k = end; k < V; k++) {
00254         if (INTERSECT(pti, ptj, pts[k], pts[nextPt[k]], pts[prevPt[k]]))
00255             return 0;
00256     }
00257     return 1;
00258 }
00259 
00260 /* compVis:
00261  * Compute visibility graph of vertices of polygons.
00262  * Only do polygons from index startp to end.
00263  * If two nodes cannot see each other, the matrix entry is 0.
00264  * If two nodes can see each other, the matrix entry is the distance
00265  * between them.
00266  */
00267 static void compVis(vconfig_t * conf, int start)
00268 {
00269     int V = conf->N;
00270     Ppoint_t *pts = conf->P;
00271     int *nextPt = conf->next;
00272     int *prevPt = conf->prev;
00273     array2 wadj = conf->vis;
00274     int j, i, previ;
00275     COORD d;
00276 
00277     for (i = start; i < V; i++) {
00278         /* add edge between i and previ.
00279          * Note that this works for the cases of polygons of 1 and 2
00280          * vertices, though needless work is done.
00281          */
00282         previ = prevPt[i];
00283         d = dist(pts[i], pts[previ]);
00284         wadj[i][previ] = d;
00285         wadj[previ][i] = d;
00286 
00287         /* Check remaining, earlier vertices */
00288         if (previ == i - 1)
00289             j = i - 2;
00290         else
00291             j = i - 1;
00292         for (; j >= 0; j--) {
00293             if (inCone(i, j, pts, nextPt, prevPt) &&
00294                 inCone(j, i, pts, nextPt, prevPt) &&
00295                 clear(pts[i], pts[j], V, V, V, pts, nextPt, prevPt)) {
00296                 /* if i and j see each other, add edge */
00297                 d = dist(pts[i], pts[j]);
00298                 wadj[i][j] = d;
00299                 wadj[j][i] = d;
00300             }
00301         }
00302     }
00303 }
00304 
00305 /* visibility:
00306  * Given a vconfig_t conf, representing polygonal barriers,
00307  * compute the visibility graph of the vertices of conf. 
00308  * The graph is stored in conf->vis. 
00309  */
00310 void visibility(vconfig_t * conf)
00311 {
00312     conf->vis = allocArray(conf->N, 2);
00313     compVis(conf, 0);
00314 }
00315 
00316 /* polyhit:
00317  * Given a vconfig_t conf, as above, and a point,
00318  * return the index of the polygon that contains
00319  * the point, or else POLYID_NONE.
00320  */
00321 static int polyhit(vconfig_t * conf, Ppoint_t p)
00322 {
00323     int i;
00324     Ppoly_t poly;
00325 
00326     for (i = 0; i < conf->Npoly; i++) {
00327         poly.ps = &(conf->P[conf->start[i]]);
00328         poly.pn = conf->start[i + 1] - conf->start[i];
00329         if (in_poly(poly, p))
00330             return i;
00331     }
00332     return POLYID_NONE;
00333 }
00334 
00335 /* ptVis:
00336  * Given a vconfig_t conf, representing polygonal barriers,
00337  * and a point within one of the polygons, compute the point's
00338  * visibility vector relative to the vertices of the remaining
00339  * polygons, i.e., pretend the argument polygon is invisible.
00340  *
00341  * If pp is NIL, ptVis computes the visibility vector for p
00342  * relative to all barrier vertices.
00343  */
00344 COORD *ptVis(vconfig_t * conf, int pp, Ppoint_t p)
00345 {
00346     int V = conf->N;
00347     Ppoint_t *pts = conf->P;
00348     int *nextPt = conf->next;
00349     int *prevPt = conf->prev;
00350     int k;
00351     int start, end;
00352     COORD *vadj;
00353     Ppoint_t pk;
00354     COORD d;
00355 
00356     vadj = (COORD *) malloc((V + 2) * sizeof(COORD));
00357 
00358 
00359     if (pp == POLYID_UNKNOWN)
00360         pp = polyhit(conf, p);
00361     if (pp >= 0) {
00362         start = conf->start[pp];
00363         end = conf->start[pp + 1];
00364     } else {
00365         start = V;
00366         end = V;
00367     }
00368 
00369     for (k = 0; k < start; k++) {
00370         pk = pts[k];
00371         if (in_cone(pts[prevPt[k]], pk, pts[nextPt[k]], p) &&
00372             clear(p, pk, start, end, V, pts, nextPt, prevPt)) {
00373             /* if p and pk see each other, add edge */
00374             d = dist(p, pk);
00375             vadj[k] = d;
00376         } else
00377             vadj[k] = 0;
00378     }
00379 
00380     for (k = start; k < end; k++)
00381         vadj[k] = 0;
00382 
00383     for (k = end; k < V; k++) {
00384         pk = pts[k];
00385         if (in_cone(pts[prevPt[k]], pk, pts[nextPt[k]], p) &&
00386             clear(p, pk, start, end, V, pts, nextPt, prevPt)) {
00387             /* if p and pk see each other, add edge */
00388             d = dist(p, pk);
00389             vadj[k] = d;
00390         } else
00391             vadj[k] = 0;
00392     }
00393     vadj[V] = 0;
00394     vadj[V + 1] = 0;
00395 
00396     return vadj;
00397 
00398 }
00399 
00400 /* directVis:
00401  * Given two points, return true if the points can directly see each other.
00402  * If a point is associated with a polygon, the edges of the polygon
00403  * are ignored when checking visibility.
00404  */
00405 int directVis(Ppoint_t p, int pp, Ppoint_t q, int qp, vconfig_t * conf)
00406 {
00407     int V = conf->N;
00408     Ppoint_t *pts = conf->P;
00409     int *nextPt = conf->next;
00410     /* int*   prevPt = conf->prev; */
00411     int k;
00412     int s1, e1;
00413     int s2, e2;
00414 
00415     if (pp < 0) {
00416         s1 = 0;
00417         e1 = 0;
00418         if (qp < 0) {
00419             s2 = 0;
00420             e2 = 0;
00421         } else {
00422             s2 = conf->start[qp];
00423             e2 = conf->start[qp + 1];
00424         }
00425     } else if (qp < 0) {
00426         s1 = 0;
00427         e1 = 0;
00428         s2 = conf->start[pp];
00429         e2 = conf->start[pp + 1];
00430     } else if (pp <= qp) {
00431         s1 = conf->start[pp];
00432         e1 = conf->start[pp + 1];
00433         s2 = conf->start[qp];
00434         e2 = conf->start[qp + 1];
00435     } else {
00436         s1 = conf->start[qp];
00437         e1 = conf->start[qp + 1];
00438         s2 = conf->start[pp];
00439         e2 = conf->start[pp + 1];
00440     }
00441 
00442     for (k = 0; k < s1; k++) {
00443         if (INTERSECT(p, q, pts[k], pts[nextPt[k]], pts[prevPt[k]]))
00444             return 0;
00445     }
00446     for (k = e1; k < s2; k++) {
00447         if (INTERSECT(p, q, pts[k], pts[nextPt[k]], pts[prevPt[k]]))
00448             return 0;
00449     }
00450     for (k = e2; k < V; k++) {
00451         if (INTERSECT(p, q, pts[k], pts[nextPt[k]], pts[prevPt[k]]))
00452             return 0;
00453     }
00454     return 1;
00455 }

Generated on Mon Mar 31 19:03:28 2008 for Graphviz by  doxygen 1.5.1