00001 #include <stdlib.h>
00002 #include <grass/Vect.h>
00003 #include <grass/gis.h>
00004 #include "dgraph.h"
00005 #include "e_intersect.h"
00006
00007 #define LENGTH(DX, DY) (sqrt((DX*DX)+(DY*DY)))
00008 #ifndef MIN
00009 #define MIN(X,Y) ((X<Y)?X:Y)
00010 #endif
00011 #ifndef MAX
00012 #define MAX(X,Y) ((X>Y)?X:Y)
00013 #endif
00014 #define PI M_PI
00015
00016 struct intersection_point
00017 {
00018 double x;
00019 double y;
00020 int group;
00021 };
00022
00023 struct seg_intersection
00024 {
00025 int with;
00026 int ip;
00027 double dist;
00028 };
00029
00030 struct seg_intersection_list
00031 {
00032 int count;
00033 int allocated;
00034 struct seg_intersection *a;
00035 };
00036
00037 struct seg_intersections
00038 {
00039 int ipcount;
00040 int ipallocated;
00041 struct intersection_point *ip;
00042 int ilcount;
00043 struct seg_intersection_list *il;
00044 int group_count;
00045 };
00046
00047 struct seg_intersections *create_si_struct(int segments_count)
00048 {
00049 struct seg_intersections *si;
00050
00051 int i;
00052
00053 si = G_malloc(sizeof(struct seg_intersections));
00054 si->ipcount = 0;
00055 si->ipallocated = segments_count + 16;
00056 si->ip = G_malloc((si->ipallocated) * sizeof(struct intersection_point));
00057 si->ilcount = segments_count;
00058 si->il = G_malloc(segments_count * sizeof(struct seg_intersection_list));
00059 for (i = 0; i < segments_count; i++) {
00060 si->il[i].count = 0;
00061 si->il[i].allocated = 0;
00062 si->il[i].a = NULL;
00063 }
00064
00065 return si;
00066 }
00067
00068 void destroy_si_struct(struct seg_intersections *si)
00069 {
00070 int i;
00071
00072 for (i = 0; i < si->ilcount; i++)
00073 G_free(si->il[i].a);
00074 G_free(si->il);
00075 G_free(si->ip);
00076 G_free(si);
00077
00078 return;
00079 }
00080
00081
00082 void add_ipoint1(struct seg_intersection_list *il, int with, double dist,
00083 int ip)
00084 {
00085 struct seg_intersection *s;
00086
00087 if (il->count == il->allocated) {
00088 il->allocated += 4;
00089 il->a =
00090 G_realloc(il->a,
00091 (il->allocated) * sizeof(struct seg_intersection));
00092 }
00093 s = &(il->a[il->count]);
00094 s->with = with;
00095 s->ip = ip;
00096 s->dist = dist;
00097 il->count++;
00098
00099 return;
00100 }
00101
00102
00103 void add_ipoint(struct line_pnts *Points, int first_seg, int second_seg,
00104 double x, double y, struct seg_intersections *si)
00105 {
00106 struct intersection_point *t;
00107
00108 int ip;
00109
00110 G_debug(4, "add_ipoint()");
00111
00112 if (si->ipcount == si->ipallocated) {
00113 si->ipallocated += 16;
00114 si->ip =
00115 G_realloc(si->ip,
00116 (si->ipallocated) * sizeof(struct intersection_point));
00117 }
00118 ip = si->ipcount;
00119 t = &(si->ip[ip]);
00120 t->x = x;
00121 t->y = y;
00122 t->group = -1;
00123 si->ipcount++;
00124
00125 add_ipoint1(&(si->il[first_seg]), second_seg,
00126 LENGTH((Points->x[first_seg] - x),
00127 (Points->y[first_seg] - y)), ip);
00128 if (second_seg >= 0)
00129 add_ipoint1(&(si->il[second_seg]), first_seg,
00130 LENGTH((Points->x[second_seg] - x),
00131 (Points->y[second_seg] - y)), ip);
00132 }
00133
00134 void sort_intersection_list(struct seg_intersection_list *il)
00135 {
00136 int n, i, j, min;
00137
00138 struct seg_intersection t;
00139
00140 G_debug(4, "sort_intersection_list()");
00141
00142 n = il->count;
00143 G_debug(4, " n=%d", n);
00144 for (i = 0; i < n - 1; i++) {
00145 min = i;
00146 for (j = i + 1; j < n; j++) {
00147 if (il->a[j].dist < il->a[min].dist) {
00148 min = j;
00149 }
00150 }
00151 if (min != i) {
00152 t = il->a[i];
00153 il->a[i] = il->a[min];
00154 il->a[min] = t;
00155 }
00156 }
00157
00158 return;
00159 }
00160
00161 static int compare(const void *a, const void *b)
00162 {
00163 struct intersection_point *aa, *bb;
00164
00165 aa = *((struct intersection_point **)a);
00166 bb = *((struct intersection_point **)b);
00167
00168 if (aa->x < bb->x)
00169 return -1;
00170 else if (aa->x > bb->x)
00171 return 1;
00172 else
00173 return (aa->y < bb->y) ? -1 : ((aa->y > bb->y) ? 1 : 0);
00174 }
00175
00176
00177 double get_epsilon(struct line_pnts *Points)
00178 {
00179 int i, np;
00180
00181 double min, t;
00182
00183 double *x, *y;
00184
00185 np = Points->n_points;
00186 x = Points->x;
00187 y = Points->y;
00188
00189 min = MAX(fabs(x[1] - x[0]), fabs(y[1] - y[0]));
00190 for (i = 1; i <= np - 2; i++) {
00191 t = MAX(fabs(x[i + 1] - x[i]), fabs(y[i + 1] - y[i]));
00192 if ((t > 0) && (t < min)) {
00193 min = t;
00194 }
00195 }
00196
00197
00198 return min * 0.000001;
00199 }
00200
00201
00202 struct seg_intersections *find_all_intersections(struct line_pnts *Points)
00203 {
00204 int i, j, np;
00205
00206 int group, t;
00207
00208 int looped;
00209
00210 double EPSILON = 0.00000001;
00211
00212 double *x, *y;
00213
00214 double x1, y1, x2, y2;
00215
00216 int res;
00217
00218
00219
00220 struct seg_intersections *si;
00221
00222 struct seg_intersection_list *il;
00223
00224 struct intersection_point **sorted;
00225
00226 G_debug(3, "find_all_intersections()");
00227
00228 np = Points->n_points;
00229 x = Points->x;
00230 y = Points->y;
00231
00232 si = create_si_struct(np - 1);
00233
00234 looped = ((x[0] == x[np - 1]) && (y[0] == y[np - 1]));
00235 G_debug(3, " looped=%d", looped);
00236
00237 G_debug(3, " finding intersections...");
00238 for (i = 0; i < np - 1; i++) {
00239 for (j = i + 1; j < np - 1; j++) {
00240 G_debug(4, " checking %d-%d %d-%d", i, i + 1, j, j + 1);
00241
00242 res =
00243 segment_intersection_2d(x[i], y[i], x[i + 1], y[i + 1], x[j],
00244 y[j], x[j + 1], y[j + 1], &x1, &y1,
00245 &x2, &y2);
00246
00247
00248
00249
00250
00251
00252 G_debug(4, " intersection type = %d", res);
00253 if (res == 1) {
00254 add_ipoint(Points, i, j, x1, y1, si);
00255 }
00256 else if ((res >= 2) && (res <= 5)) {
00257 add_ipoint(Points, i, j, x1, y1, si);
00258 add_ipoint(Points, i, j, x2, y2, si);
00259 }
00260 }
00261 }
00262 if (!looped) {
00263
00264 add_ipoint(Points, 0, -1, Points->x[0], Points->y[0], si);
00265 add_ipoint(Points, np - 2, -1, Points->x[np - 1], Points->y[np - 1],
00266 si);
00267 }
00268 G_debug(3, " finding intersections...done");
00269
00270 G_debug(3, " postprocessing...");
00271 if (si->ipallocated > si->ipcount) {
00272 si->ipallocated = si->ipcount;
00273 si->ip =
00274 G_realloc(si->ip,
00275 (si->ipcount) * sizeof(struct intersection_point));
00276 }
00277 for (i = 0; i < si->ilcount; i++) {
00278 il = &(si->il[i]);
00279 if (il->allocated > il->count) {
00280 il->allocated = il->count;
00281 il->a =
00282 G_realloc(il->a,
00283 (il->count) * sizeof(struct seg_intersection));
00284 }
00285
00286 if (il->count > 0) {
00287 sort_intersection_list(il);
00288
00289 }
00290 }
00291
00292
00293 sorted = G_malloc((si->ipcount) * sizeof(struct intersection_point *));
00294 for (i = 0; i < si->ipcount; i++)
00295 sorted[i] = &(si->ip[i]);
00296
00297 qsort(sorted, si->ipcount, sizeof(struct intersection_point *), compare);
00298
00299
00300 group = 0;
00301 for (i = 0; i < si->ipcount; i++) {
00302
00303 t = group;
00304 for (j = i - 1; j >= 0; j--) {
00305 if (!FEQUAL(sorted[j]->x, sorted[i]->x, EPSILON))
00306
00307 break;
00308 if (FEQUAL(sorted[j]->y, sorted[i]->y, EPSILON)) {
00309
00310 t = sorted[j]->group;
00311 break;
00312 }
00313 }
00314 G_debug(4, " group=%d, ip=%d", t,
00315 (int)(sorted[i] - &(si->ip[0])));
00316 sorted[i]->group = t;
00317 if (t == group)
00318 group++;
00319 }
00320 si->group_count = group;
00321
00322 G_debug(3, " postprocessing...done");
00323
00324
00325 for (i = 0; i < si->ilcount; i++) {
00326 G_debug(4, "%d-%d :", i, i + 1);
00327 for (j = 0; j < si->il[i].count; j++) {
00328 G_debug(4, " %d-%d, group=%d", si->il[i].a[j].with,
00329 si->il[i].a[j].with + 1, si->ip[si->il[i].a[j].ip].group);
00330 G_debug(4, " dist=%.18f", si->il[i].a[j].dist);
00331 G_debug(4, " x=%.18f, y=%.18f",
00332 si->ip[si->il[i].a[j].ip].x, si->ip[si->il[i].a[j].ip].y);
00333 }
00334 }
00335
00336 return si;
00337 }
00338
00339
00340
00341 struct planar_graph *pg_create_struct(int n, int e)
00342 {
00343 struct planar_graph *pg;
00344
00345 pg = G_malloc(sizeof(struct planar_graph));
00346 pg->vcount = n;
00347 pg->v = G_malloc(n * sizeof(struct pg_vertex));
00348 memset(pg->v, 0, n * sizeof(struct pg_vertex));
00349 pg->ecount = 0;
00350 pg->eallocated = MAX(e, 0);
00351 pg->e = NULL;
00352 pg->e = G_malloc(e * sizeof(struct pg_edge));
00353
00354 return pg;
00355 }
00356
00357 void pg_destroy_struct(struct planar_graph *pg)
00358 {
00359 int i;
00360
00361 for (i = 0; i < pg->vcount; i++) {
00362 G_free(pg->v[i].edges);
00363 G_free(pg->v[i].angles);
00364 }
00365 G_free(pg->v);
00366 G_free(pg->e);
00367 G_free(pg);
00368 }
00369
00370
00371 int pg_existsedge(struct planar_graph *pg, int v1, int v2)
00372 {
00373 struct pg_vertex *v;
00374
00375 struct pg_edge *e;
00376
00377 int i, ecount;
00378
00379 if (pg->v[v1].ecount <= pg->v[v2].ecount)
00380 v = &(pg->v[v1]);
00381 else
00382 v = &(pg->v[v2]);
00383
00384 ecount = v->ecount;
00385 for (i = 0; i < ecount; i++) {
00386 e = v->edges[i];
00387 if (((e->v1 == v1) && (e->v2 == v2)) ||
00388 ((e->v1 == v2) && (e->v2 == v1)))
00389 return 1;
00390 }
00391
00392 return 0;
00393 }
00394
00395
00396 void pg_addedge1(struct pg_vertex *v, struct pg_edge *e)
00397 {
00398 if (v->ecount == v->eallocated) {
00399 v->eallocated += 4;
00400 v->edges =
00401 G_realloc(v->edges, (v->eallocated) * sizeof(struct pg_edge *));
00402 }
00403 v->edges[v->ecount] = e;
00404 v->ecount++;
00405 }
00406
00407 void pg_addedge(struct planar_graph *pg, int v1, int v2)
00408 {
00409 struct pg_edge *e;
00410
00411 G_debug(4, "pg_addedge(), v1=%d, v2=%d", v1, v2);
00412
00413 if ((v1 == v2) || (v1 < 0) || (v1 >= pg->vcount) || (v2 < 0) ||
00414 (v2 >= pg->vcount)) {
00415 G_fatal_error(" pg_addedge(), v1 and/or v2 is invalid");
00416 return;
00417 }
00418
00419 if (pg_existsedge(pg, v1, v2))
00420 return;
00421
00422 if (pg->ecount == pg->eallocated) {
00423 G_fatal_error
00424 ("Trying to add more edges to the planar_graph than the initial allocation size allows");
00425 }
00426 e = &(pg->e[pg->ecount]);
00427 e->v1 = v1;
00428 e->v2 = v2;
00429 e->winding_left = 0;
00430 e->winding_right = 0;
00431 e->visited_left = 0;
00432 e->visited_right = 0;
00433 pg->ecount++;
00434 pg_addedge1(&(pg->v[v1]), e);
00435 pg_addedge1(&(pg->v[v2]), e);
00436
00437 return;
00438 }
00439
00440 struct planar_graph *pg_create(struct line_pnts *Points)
00441 {
00442 struct seg_intersections *si;
00443
00444 struct planar_graph *pg;
00445
00446 struct intersection_point *ip;
00447
00448 struct pg_vertex *vert;
00449
00450 struct pg_edge *edge;
00451
00452 int i, j, t, v;
00453
00454 G_debug(3, "pg_create()");
00455
00456 si = find_all_intersections(Points);
00457 pg = pg_create_struct(si->group_count, 2 * (si->ipcount));
00458
00459
00460 for (i = 0; i < si->ipcount; i++) {
00461 ip = &(si->ip[i]);
00462 t = ip->group;
00463 pg->v[t].x = ip->x;
00464 pg->v[t].y = ip->y;
00465 }
00466
00467
00468 for (i = 0; i < si->ilcount; i++) {
00469 v = si->ip[si->il[i].a[0].ip].group;
00470 for (j = 1; j < si->il[i].count; j++) {
00471 t = si->ip[si->il[i].a[j].ip].group;
00472 if (t != v) {
00473 pg_addedge(pg, v, t);
00474 v = t;
00475 }
00476 }
00477 }
00478
00479
00480 for (i = 0; i < pg->vcount; i++) {
00481 vert = &(pg->v[i]);
00482 vert->angles = G_malloc((vert->ecount) * sizeof(double));
00483 for (j = 0; j < vert->ecount; j++) {
00484 edge = vert->edges[j];
00485 t = (edge->v1 != i) ? (edge->v1) : (edge->v2);
00486 vert->angles[j] =
00487 atan2(pg->v[t].y - vert->y, pg->v[t].x - vert->x);
00488 }
00489 }
00490
00491 destroy_si_struct(si);
00492
00493
00494
00495
00496
00497
00498
00499
00500
00501
00502
00503
00504
00505
00506
00507
00508
00509
00510
00511
00512 for (i = 0; i < pg->vcount; i++) {
00513 G_debug(4, " vertex %d (%g, %g)", i, pg->v[i].x, pg->v[i].y);
00514 for (j = 0; j < pg->v[i].ecount; j++) {
00515 G_debug(4, " edge %d-%d", pg->v[i].edges[j]->v1,
00516 pg->v[i].edges[j]->v2);
00517 }
00518 }
00519
00520 return pg;
00521 }