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

Go to the documentation of this file.
00001 /* $Id: cvt.c,v 1.2 2007/04/23 18:12:39 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 #include <stdio.h>
00020 
00021 #ifdef DMALLOC
00022 #include "dmalloc.h"
00023 #endif
00024 
00025 typedef Ppoint_t ilcoord_t;
00026 
00027 #ifdef DEBUG
00028 static void printVconfig(vconfig_t * cp);
00029 static void printVis(char *lbl, COORD * vis, int n);
00030 static void printDad(int *vis, int n);
00031 #endif
00032 
00033 #ifdef GASP
00034 static void gasp_print_obstacles(vconfig_t * conf);
00035 static void gasp_print_point(Ppoint_t p);
00036 static void gasp_print_polyline(Ppolyline_t * route);
00037 static void gasp_print_bezier(Ppolyline_t * route);
00038 #endif
00039 
00040 #if 0                           /* not used */
00041 static void *myrealloc(void *p, size_t newsize)
00042 {
00043     void *rv;
00044 
00045     if (p == (void *) 0)
00046         rv = malloc(newsize);
00047     else
00048         rv = realloc(p, newsize);
00049     return rv;
00050 }
00051 #endif
00052 
00053 static void *mymalloc(size_t newsize)
00054 {
00055     void *rv;
00056 
00057     if (newsize > 0)
00058         rv = malloc(newsize);
00059     else
00060         rv = (void *) 0;
00061     return rv;
00062 }
00063 
00064 
00065 vconfig_t *Pobsopen(Ppoly_t ** obs, int n_obs)
00066 {
00067     vconfig_t *rv;
00068     int poly_i, pt_i, i, n;
00069     int start, end;
00070 
00071     rv = malloc(sizeof(vconfig_t));
00072 
00073     /* get storage */
00074     n = 0;
00075     for (poly_i = 0; poly_i < n_obs; poly_i++)
00076         n = n + obs[poly_i]->pn;
00077     rv->P = mymalloc(n * sizeof(Ppoint_t));
00078     rv->start = mymalloc((n_obs + 1) * sizeof(int));
00079     rv->next = mymalloc(n * sizeof(int));
00080     rv->prev = mymalloc(n * sizeof(int));
00081     rv->N = n;
00082     rv->Npoly = n_obs;
00083 
00084     /* build arrays */
00085     i = 0;
00086     for (poly_i = 0; poly_i < n_obs; poly_i++) {
00087         start = i;
00088         rv->start[poly_i] = start;
00089         end = start + obs[poly_i]->pn - 1;
00090         for (pt_i = 0; pt_i < obs[poly_i]->pn; pt_i++) {
00091             rv->P[i] = obs[poly_i]->ps[pt_i];
00092             rv->next[i] = i + 1;
00093             rv->prev[i] = i - 1;
00094             i++;
00095         }
00096         rv->next[end] = start;
00097         rv->prev[start] = end;
00098     }
00099     rv->start[poly_i] = i;
00100     visibility(rv);
00101     return rv;
00102 }
00103 
00104 void Pobsclose(vconfig_t * config)
00105 {
00106     free(config->P);
00107     free(config->start);
00108     free(config->next);
00109     free(config->prev);
00110     free(config->vis);
00111     free(config);
00112 }
00113 
00114 int Pobspath(vconfig_t * config, Ppoint_t p0, int poly0, Ppoint_t p1,
00115              int poly1, Ppolyline_t * output_route)
00116 {
00117     int i, j, *dad;
00118     int opn;
00119     Ppoint_t *ops;
00120     COORD *ptvis0, *ptvis1;
00121 
00122 #ifdef GASP
00123     gasp_print_obstacles(config);
00124 #endif
00125     ptvis0 = ptVis(config, poly0, p0);
00126     ptvis1 = ptVis(config, poly1, p1);
00127 
00128 #ifdef GASP
00129     gasp_print_point(p0);
00130     gasp_print_point(p1);
00131 #endif
00132     dad = makePath(p0, poly0, ptvis0, p1, poly1, ptvis1, config);
00133 
00134     opn = 1;
00135     for (i = dad[config->N]; i != config->N + 1; i = dad[i])
00136         opn++;
00137     opn++;
00138     ops = malloc(opn * sizeof(Ppoint_t));
00139 
00140     j = opn - 1;
00141     ops[j--] = p1;
00142     for (i = dad[config->N]; i != config->N + 1; i = dad[i])
00143         ops[j--] = config->P[i];
00144     ops[j] = p0;
00145     assert(j == 0);
00146 
00147 #ifdef DEBUG
00148     printVconfig(config);
00149     printVis("p", ptvis0, config->N + 1);
00150     printVis("q", ptvis1, config->N + 1);
00151     printDad(dad, config->N + 1);
00152 #endif
00153 
00154     if (ptvis0)
00155         free(ptvis0);
00156     if (ptvis1)
00157         free(ptvis1);
00158 
00159     output_route->pn = opn;
00160     output_route->ps = ops;
00161 #ifdef GASP
00162     gasp_print_polyline(output_route);
00163 #endif
00164     return TRUE;
00165 }
00166 
00167 int Pobsbarriers(vconfig_t * config, Pedge_t ** barriers, int *n_barriers)
00168 {
00169     int i, j;
00170 
00171     *barriers = malloc(config->N * sizeof(Pedge_t));
00172     *n_barriers = config->N;
00173 
00174     for (i = 0; i < config->N; i++) {
00175         barriers[i]->a.x = config->P[i].x;
00176         barriers[i]->a.y = config->P[i].y;
00177         j = config->next[i];
00178         barriers[i]->b.x = config->P[j].x;
00179         barriers[i]->b.y = config->P[j].y;
00180     }
00181     return 1;
00182 }
00183 
00184 #ifdef DEBUG
00185 static void printVconfig(vconfig_t * cp)
00186 {
00187     int i, j;
00188     int *next, *prev;
00189     Ppoint_t *pts;
00190     array2 arr;
00191 
00192     next = cp->next;
00193     prev = cp->prev;
00194     pts = cp->P;
00195     arr = cp->vis;
00196 
00197     printf("this next prev point\n");
00198     for (i = 0; i < cp->N; i++)
00199         printf("%3d  %3d  %3d    (%3g,%3g)\n", i, next[i], prev[i],
00200                pts[i].x, pts[i].y);
00201 
00202     printf("\n\n");
00203 
00204     for (i = 0; i < cp->N; i++) {
00205         for (j = 0; j < cp->N; j++)
00206             printf("%4.1f ", arr[i][j]);
00207         printf("\n");
00208     }
00209 }
00210 
00211 static void printVis(char *lbl, COORD * vis, int n)
00212 {
00213     int i;
00214 
00215     printf("%s: ", lbl);
00216     for (i = 0; i < n; i++)
00217         printf("%4.1f ", vis[i]);
00218     printf("\n");
00219 }
00220 
00221 static void printDad(int *vis, int n)
00222 {
00223     int i;
00224 
00225     printf("     ");
00226     for (i = 0; i < n; i++) {
00227         printf("%3d ", i);
00228     }
00229     printf("\n");
00230     printf("dad: ");
00231     for (i = 0; i < n; i++) {
00232         printf("%3d ", vis[i]);
00233     }
00234     printf("\n");
00235 }
00236 #endif
00237 
00238 static Ppoint_t Bezpt[1000];
00239 static int Bezctr;
00240 
00241 static void addpt(Ppoint_t p)
00242 {
00243     if ((Bezctr == 0) ||
00244         (Bezpt[Bezctr - 1].x != p.x) || (Bezpt[Bezctr - 1].y != p.y))
00245         Bezpt[Bezctr++] = p;
00246 }
00247 
00248 #define W_DEGREE 5
00249 static ilcoord_t Bezier(ilcoord_t * V, int degree, double t,
00250                         ilcoord_t * Left, ilcoord_t * Right)
00251 {
00252     int i, j;                   /* Index variables  */
00253     ilcoord_t Vtemp[W_DEGREE + 1][W_DEGREE + 1];
00254 
00255     /* Copy control points  */
00256     for (j = 0; j <= degree; j++) {
00257         Vtemp[0][j] = V[j];
00258     }
00259 
00260     /* Triangle computation */
00261     for (i = 1; i <= degree; i++) {
00262         for (j = 0; j <= degree - i; j++) {
00263             Vtemp[i][j].x =
00264                 (1.0 - t) * Vtemp[i - 1][j].x + t * Vtemp[i - 1][j + 1].x;
00265             Vtemp[i][j].y =
00266                 (1.0 - t) * Vtemp[i - 1][j].y + t * Vtemp[i - 1][j + 1].y;
00267         }
00268     }
00269 
00270     if (Left != NIL(ilcoord_t *))
00271         for (j = 0; j <= degree; j++)
00272             Left[j] = Vtemp[j][0];
00273     if (Right != NIL(ilcoord_t *))
00274         for (j = 0; j <= degree; j++)
00275             Right[j] = Vtemp[degree - j][j];
00276     return (Vtemp[degree][0]);
00277 }
00278 
00279 static void append_bezier(Ppoint_t * bezier)
00280 {
00281     double a;
00282     ilcoord_t left[4], right[4];
00283 
00284     a = fabs(area2(bezier[0], bezier[1], bezier[2]))
00285         + fabs(area2(bezier[2], bezier[3], bezier[0]));
00286     if (a < .5) {
00287         addpt(bezier[0]);
00288         addpt(bezier[3]);
00289     } else {
00290         (void) Bezier(bezier, 3, .5, left, right);
00291         append_bezier(left);
00292         append_bezier(right);
00293     }
00294 }
00295 
00296 #ifdef GASP
00297 
00298 FILE *GASPout = stderr;
00299 
00300 static void gasp_print_point(Ppoint_t p)
00301 {
00302     fprintf(GASPout, "%3g %3g\n", p.x, p.y);
00303 }
00304 
00305 void gasp_print_obstacles(vconfig_t * conf)
00306 {
00307     int i, j;
00308     Ppoly_t poly;
00309 
00310     fprintf(GASPout, "%d\n", conf->Npoly);
00311     for (i = 0; i < conf->Npoly; i++) {
00312         poly.ps = &(conf->P[conf->start[i]]);
00313         poly.pn = conf->start[i + 1] - conf->start[i];
00314         fprintf(GASPout, "%d\n", poly.pn);
00315         for (j = 0; j < poly.pn; j++)
00316             gasp_print_point(poly.ps[j]);
00317     }
00318 }
00319 
00320 void gasp_print_bezier(Ppolyline_t * route)
00321 {
00322     int i;
00323 
00324     Bezctr = 0;
00325     for (i = 0; i + 3 < route->pn; i += 3)
00326         append_bezier(route->ps + i);
00327     fprintf(GASPout, "%d\n", Bezctr);
00328     for (i = 0; i < Bezctr; i++)
00329         gasp_print_point(Bezpt[i]);
00330     Bezctr = 0;
00331 }
00332 
00333 void gasp_print_polyline(Ppolyline_t * route)
00334 {
00335     int i;
00336 
00337     fprintf(GASPout, "%d\n", route->pn);
00338     for (i = 0; i < route->pn; i++)
00339         gasp_print_point(route->ps[i]);
00340 }
00341 #endif

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