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