GRASS Programmer's Manual 6.4.1(2011)
|
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; /* IPs with very similar dist will be in the same group */ 00023 }; 00024 00025 struct seg_intersection 00026 { 00027 int with; /* second segment */ 00028 int ip; /* index of the IP */ 00029 double dist; /* distance from first point of first segment to intersection point (IP) */ 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 /* internal use */ 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 /* adds intersection point to the structure */ 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 /* O(Points->n_points) time */ 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 /* ??? is 0.001 ok ??? */ 00200 return min * 0.000001; 00201 } 00202 00203 /* currently O(n*n); future implementation O(nlogn) */ 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 /*int res2 00221 double x1_, y1_, x2_, y2_, z1_, z2_; */ 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 /*res = segment_intersection_2d_e(x[i], y[i], x[i+1], y[i+1], x[j], y[j], x[j+1], y[j+1], &x1, &y1, &x2, &y2); */ 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 /* res2 = segment_intersection_2d_e(x[i], y[i], x[i+1], y[i+1], x[j], y[j], x[j+1], y[j+1], &x1_, &y1_, &x2_, &y2_); 00249 if ((res != res2) || ((res != 0) && (x1!=x1_ || y1!=y1_)) ) { 00250 G_debug(0, "exact=%d orig=%d", res, res2); 00251 segment_intersection_2d_test(x[i], y[i], x[i+1], y[i+1], x[j], y[j], x[j+1], y[j+1], &x1, &y1, &x2, &y2); 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 /* these are not really intersection points */ 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 /* is it ok to use qsort here ? */ 00291 } 00292 } 00293 00294 /* si->ip will not be reallocated any more so we can use pointers */ 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 /* assign groups */ 00302 group = 0; /* next available group number */ 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 /* if (!almost_equal(sorted[j]->x, sorted[i]->x, 16)) */ 00309 break; 00310 if (FEQUAL(sorted[j]->y, sorted[i]->y, EPSILON)) { 00311 /* if (almost_equal(sorted[j]->y, sorted[i]->y, 16)) { */ 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 /* output contents of si */ 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 /* create's graph with n vertices and allocates memory for e edges */ 00342 /* trying to add more than e edges, produces fatal error */ 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 /* v1 and v2 must be valid */ 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 /* for internal use */ 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; /* winding is undefined if the corresponding side is not visited */ 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 /* set vertices info */ 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 /* add all edges */ 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); /* edge direction is v ---> t */ 00476 v = t; 00477 } 00478 } 00479 } 00480 00481 /* precalculate angles with 0x */ 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 I'm not sure if shrinking of the allocated memory always preserves it's physical place. 00496 That's why I don't want to do this: 00497 if (pg->ecount < pg->eallocated) { 00498 pg->eallocated = pg->ecount; 00499 pg->e = G_realloc(pg->e, (pg->ecount)*sizeof(struct pg_edge)); 00500 } 00501 */ 00502 00503 /* very time consuming */ 00504 /* 00505 for (i = 0; i < pg->vcount; i++) { 00506 if (pg->v[i].ecount < pg->v[i].eallocated) { 00507 pg->v[i].eallocated = pg->v[i].ecount; 00508 pg->v[i].edges = G_realloc(pg->v[i].edges, (pg->v[i].ecount)*sizeof(struct pg_edges)); 00509 } 00510 } 00511 */ 00512 00513 /* output pg */ 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 }