78 static int cmp_cross(
const void *pa,
const void *pb);
79 static void add_cross(
int asegment,
double adistance,
int bsegment,
80 double bdistance,
double x,
double y);
81 static double dist2(
double x1,
double y1,
double x2,
double y2);
83 static int debug_level = -1;
86 static int ident(
double x1,
double y1,
double x2,
double y2,
double thresh);
88 static int snap_cross(
int asegment,
double *adistance,
int bsegment,
89 double *bdistance,
double *xc,
double *yc);
90 static int cross_seg(
int i,
int j,
int b);
91 static int find_cross(
int i,
int j,
int b);
93 struct line_pnts *BPoints,
int with_z,
int all);
106 static int a_cross = 0;
108 static CROSS *cross =
NULL;
109 static int *use_cross =
NULL;
113 static double d_ulp(
double a,
double b)
128 result = frexp(dmax, &exp);
130 result = ldexp(result, exp);
135 static void add_cross(
int asegment,
double adistance,
int bsegment,
136 double bdistance,
double x,
double y)
138 if (n_cross == a_cross) {
142 (a_cross + 101) *
sizeof(CROSS));
145 (a_cross + 101) *
sizeof(
int));
150 " add new cross: aseg/dist = %d/%f bseg/dist = %d/%f, x = %f y = %f",
151 asegment, adistance, bsegment, bdistance, x, y);
152 cross[n_cross].segment[0] = asegment;
153 cross[n_cross].distance[0] = adistance;
154 cross[n_cross].segment[1] = bsegment;
155 cross[n_cross].distance[1] = bdistance;
156 cross[n_cross].x =
x;
157 cross[n_cross].y = y;
161 static int cmp_cross(
const void *pa,
const void *pb)
163 CROSS *p1 = (CROSS *) pa;
164 CROSS *p2 = (CROSS *) pb;
166 if (p1->segment[current] < p2->segment[current])
168 if (p1->segment[current] > p2->segment[current])
171 if (p1->distance[current] < p2->distance[current])
173 if (p1->distance[current] > p2->distance[current])
178 static double dist2(
double x1,
double y1,
double x2,
double y2)
184 return (dx * dx + dy * dy);
189 static int ident(
double x1,
double y1,
double x2,
double y2,
double thresh)
195 if ((dx * dx + dy * dy) <= thresh * thresh)
203 static struct line_pnts *APnts, *BPnts, *ABPnts[2], *IPnts;
207 static int snap_cross(
int asegment,
double *adistance,
int bsegment,
208 double *bdistance,
double *xc,
double *yc)
212 double dist, curdist, dthresh;
216 curdist = dist2(*xc, *yc, APnts->
x[seg], APnts->
y[seg]);
220 *adistance = curdist;
223 dist = dist2(*xc, *yc, APnts->
x[seg + 1], APnts->
y[seg + 1]);
224 if (dist < curdist) {
226 x = APnts->
x[seg + 1];
227 y = APnts->
y[seg + 1];
232 dist = dist2(*xc, *yc, BPnts->
x[seg], BPnts->
y[seg]);
235 if (dist < curdist) {
241 dist = dist2(*xc, *yc, BPnts->
x[seg + 1], BPnts->
y[seg + 1]);
242 if (dist < curdist) {
244 x = BPnts->
x[seg + 1];
245 y = BPnts->
y[seg + 1];
255 dthresh = d_ulp(x, y);
256 if (curdist < dthresh * dthresh) {
262 *adistance = dist2(*xc, *yc, APnts->
x[seg], APnts->
y[seg]);
264 *bdistance = dist2(*xc, *yc, BPnts->
x[seg], BPnts->
y[seg]);
273 static int cross_seg(
int i,
int j,
int b)
275 double x1, y1, z1, x2, y2, z2;
276 double y1min, y1max, y2min, y2max;
281 y1max = APnts->
y[i + 1];
282 if (APnts->
y[i] > APnts->
y[i + 1]) {
283 y1min = APnts->
y[i + 1];
288 y2max = BPnts->
y[j + 1];
289 if (BPnts->
y[j] > BPnts->
y[j + 1]) {
290 y2min = BPnts->
y[j + 1];
294 if (y1min > y2max || y1max < y2min)
300 APnts->
x[i + 1], APnts->
y[i + 1],
302 BPnts->
x[j], BPnts->
y[j],
304 BPnts->
x[j + 1], BPnts->
y[j + 1],
306 &x1, &y1, &z1, &x2, &y2, &z2, 0);
311 BPnts->
x[j + 1], BPnts->
y[j + 1],
313 APnts->
x[i], APnts->
y[i],
315 APnts->
x[i + 1], APnts->
y[i + 1],
317 &x1, &y1, &z1, &x2, &y2, &z2, 0);
322 G_debug(2,
" -> %d x %d: intersection type = %d", i, j, ret);
324 G_debug(3,
" in %f, %f ", x1, y1);
326 snap_cross(i, &adist, j, &bdist, &x1, &y1);
327 add_cross(i, adist, j, bdist, x1, y1);
329 add_cross(j, bdist, i, adist, x1, y1);
331 else if (ret == 2 || ret == 3 || ret == 4 || ret == 5) {
336 G_debug(3,
" in %f, %f; %f, %f", x1, y1, x2, y2);
337 snap_cross(i, &adist, j, &bdist, &x1, &y1);
338 add_cross(i, adist, j, bdist, x1, y1);
340 add_cross(j, bdist, i, adist, x1, y1);
341 snap_cross(i, &adist, j, &bdist, &x2, &y2);
342 add_cross(i, adist, j, bdist, x2, y2);
344 add_cross(j, bdist, i, adist, x2, y2);
355 #define GET_PARENT(p, c) ((p) = (int) (((c) - 2) / 3 + 1)) 356 #define GET_CHILD(c, p) ((c) = (int) (((p) * 3) - 1)) 375 static int cmp_q_x(
struct qitem *a,
struct qitem *b)
377 double x1, y1, z1, x2, y2, z2;
379 x1 = ABPnts[a->l]->
x[a->p];
380 y1 = ABPnts[a->l]->
y[a->p];
381 z1 = ABPnts[a->l]->
z[a->p];
383 x2 = ABPnts[b->l]->
x[b->p];
384 y2 = ABPnts[b->l]->
y[b->p];
385 z2 = ABPnts[b->l]->
z[b->p];
415 static int sift_up(
struct boq *q,
int start)
417 register int parent, child;
428 if (cmp_q_x(&a, b)) {
430 q->i[child] = q->i[parent];
446 static int boq_add(
struct boq *q,
struct qitem *i)
448 if (q->count + 2 >= q->alloc) {
449 q->alloc = q->count + 100;
450 q->i =
G_realloc(q->i, q->alloc *
sizeof(
struct qitem));
452 q->i[q->count + 1] = *i;
453 sift_up(q, q->count + 1);
461 static int boq_drop(
struct boq *q,
struct qitem *qi)
463 register int child, childr, parent;
482 while (
GET_CHILD(child, parent) <= q->count) {
485 for (childr = child + 1; childr <= q->count && childr < i; childr++) {
494 q->i[parent] = q->i[child];
499 if (parent < q->
count) {
500 q->i[parent] = q->i[q->count];
514 static int cmp_t_y(
const void *aa,
const void *bb)
516 double x1, y1, z1, x2, y2, z2;
517 struct qitem *a = (
struct qitem *) aa;
518 struct qitem *b = (
struct qitem *) bb;
520 x1 = ABPnts[a->l]->
x[a->p];
521 y1 = ABPnts[a->l]->
y[a->p];
522 z1 = ABPnts[a->l]->
z[a->p];
524 x2 = ABPnts[b->l]->
x[b->p];
525 y2 = ABPnts[b->l]->
y[b->p];
526 z2 = ABPnts[b->l]->
z[b->p];
556 double x1, y1, z1, x2, y2, z2;
564 for (i = 0; i < Pnts->
n_points - 1; i++) {
573 if (x1 == x2 && y1 == y2 && (!with_z || z1 == z2))
600 box.
W -= d_ulp(box.
W, box.
W);
601 box.
S -= d_ulp(box.
S, box.
S);
602 box.
B -= d_ulp(box.
B, box.
B);
603 box.
E += d_ulp(box.
E, box.
E);
604 box.
N += d_ulp(box.
N, box.
N);
605 box.
T += d_ulp(box.
T, box.
T);
692 int *nalines,
int *nblines,
int with_z)
694 int i, j, k,
l, nl, last_seg, seg, last;
696 double dist, last_x, last_y, last_z;
699 int seg1, seg2, vert1, vert2;
702 struct qitem qi, *found;
707 if (debug_level == -1) {
711 debug_level = atoi(dstr);
802 if (abbox.
N > ABox.
N)
804 if (abbox.
S < ABox.
S)
806 if (abbox.
E > ABox.
E)
808 if (abbox.
W < ABox.
W)
812 if (abbox.
T > BBox.
T)
814 if (abbox.
B < BBox.
B)
819 abbox.
N += d_ulp(abbox.
N, abbox.
N);
820 abbox.
S -= d_ulp(abbox.
S, abbox.
S);
821 abbox.
E += d_ulp(abbox.
E, abbox.
E);
822 abbox.
W -= d_ulp(abbox.
W, abbox.
W);
824 abbox.
T += d_ulp(abbox.
T, abbox.
T);
825 abbox.
B -= d_ulp(abbox.
B, abbox.
B);
829 G_fatal_error(
"Intersection with points is not yet supported");
836 bo_queue.i =
G_malloc(bo_queue.alloc *
sizeof(
struct qitem));
839 boq_load(&bo_queue, APnts, &abbox, 0, with_z);
843 boq_load(&bo_queue, BPnts, &abbox, 1, with_z);
854 while (boq_drop(&bo_queue, &qi)) {
862 cross_seg(qi.s, found->s, 0);
872 cross_seg(found->s, qi.s, 1);
903 G_debug(2,
"n_cross = %d", n_cross);
913 for (l = 1; l < nl; l++) {
914 for (i = 0; i < n_cross; i++)
921 G_debug(2,
"Clean and create array for line A");
929 G_debug(2,
"Clean and create array for line B");
938 qsort((
void *)cross,
sizeof(
char) * n_cross,
sizeof(CROSS),
943 if (debug_level > 2) {
944 for (i = 0; i < n_cross; i++) {
946 " cross = %d seg1/dist1 = %d/%f seg2/dist2 = %d/%f x = %f y = %f",
947 i, cross[i].segment[current],
948 sqrt(cross[i].distance[current]),
949 cross[i].segment[second], sqrt(cross[i].distance[second]),
950 cross[i].x, cross[i].y);
955 for (i = 0; i < n_cross; i++) {
956 if (use_cross[i] == 1) {
960 if ((cross[i].segment[current] == 0 &&
961 cross[i].x == Points1->
x[0] &&
962 cross[i].y == Points1->
y[0]) ||
963 (cross[i].segment[current] == j - 1 &&
964 cross[i].x == Points1->
x[j] &&
965 cross[i].y == Points1->
y[j])) {
967 G_debug(3,
"cross %d deleted (first/last point)", i);
983 for (i = 0; i < n_cross; i++) {
984 if (use_cross[i] == 0)
986 G_debug(3,
" is %d between colinear?", i);
988 seg1 = cross[i].segment[current];
989 seg2 = cross[i].segment[second];
992 if (cross[i].x == Points1->
x[seg1] &&
993 cross[i].y == Points1->
y[seg1]) {
996 else if (cross[i].x == Points1->
x[seg1 + 1] &&
997 cross[i].y == Points1->
y[seg1 + 1]) {
1001 G_debug(3,
" -> is not vertex on 1. line");
1008 if (cross[i].x == Points2->
x[seg2] &&
1009 cross[i].y == Points2->
y[seg2]) {
1012 else if (cross[i].x == Points2->
x[seg2 + 1] &&
1013 cross[i].y == Points2->
y[seg2 + 1]) {
1017 G_debug(3,
" -> is not vertex on 2. line");
1020 G_debug(3,
" seg1/vert1 = %d/%d seg2/ver2 = %d/%d", seg1,
1021 vert1, seg2, vert2);
1024 if (vert2 == 0 || vert2 == Points2->
n_points - 1) {
1025 G_debug(3,
" -> vertex 2 (%d) is first/last", vert2);
1030 if (!((Points1->
x[vert1 - 1] == Points2->
x[vert2 - 1] &&
1031 Points1->
y[vert1 - 1] == Points2->
y[vert2 - 1] &&
1032 Points1->
x[vert1 + 1] == Points2->
x[vert2 + 1] &&
1033 Points1->
y[vert1 + 1] == Points2->
y[vert2 + 1]) ||
1034 (Points1->
x[vert1 - 1] == Points2->
x[vert2 + 1] &&
1035 Points1->
y[vert1 - 1] == Points2->
y[vert2 + 1] &&
1036 Points1->
x[vert1 + 1] == Points2->
x[vert2 - 1] &&
1037 Points1->
y[vert1 + 1] == Points2->
y[vert2 - 1])
1040 G_debug(3,
" -> previous/next are not identical");
1046 G_debug(3,
" -> collinear -> remove");
1065 for (i = 0; i < n_cross; i++) {
1066 if (use_cross[i] == 0)
1072 seg = cross[i].segment[current];
1074 G_debug(3,
" duplicate ?: cross = %d seg = %d dist = %f", i,
1075 cross[i].segment[current], cross[i].distance[current]);
1076 if ((cross[i].segment[current] == cross[last].segment[current] &&
1077 cross[i].distance[current] == cross[last].distance[current])
1078 || (cross[i].segment[current] ==
1079 cross[last].segment[current] + 1 &&
1080 cross[i].distance[current] == 0 &&
1081 cross[i].x == cross[last].x &&
1082 cross[i].y == cross[last].y)) {
1083 G_debug(3,
" cross %d identical to last -> removed", i);
1094 G_debug(3,
" alive crosses:");
1095 for (i = 0; i < n_cross; i++) {
1096 if (use_cross[i] == 1) {
1103 if (n_alive_cross > 0) {
1105 use_cross[n_cross] = 1;
1107 cross[n_cross].x = Points->
x[j];
1108 cross[n_cross].y = Points->
y[j];
1109 cross[n_cross].segment[current] = Points->
n_points - 2;
1112 last_x = Points->
x[0];
1113 last_y = Points->
y[0];
1114 last_z = Points->
z[0];
1117 for (i = 0; i <= n_cross; i++) {
1118 seg = cross[i].segment[current];
1119 G_debug(2,
"%d seg = %d dist = %f", i, seg,
1120 cross[i].distance[current]);
1121 if (use_cross[i] == 0) {
1122 G_debug(3,
" removed -> next");
1130 G_debug(2,
" append last vert: %f %f", last_x, last_y);
1133 for (j = last_seg + 1; j <= seg; j++) {
1134 G_debug(2,
" segment j = %d", j);
1136 if ((j == last_seg + 1) && Points->
x[j] == last_x &&
1137 Points->
y[j] == last_y) {
1138 G_debug(2,
" -> skip (identical to last break)");
1143 G_debug(2,
" append first of seg: %f %f", Points->
x[j],
1148 last_x = cross[i].x;
1149 last_y = cross[i].y;
1152 if (Points->
z[last_seg] == Points->
z[last_seg + 1]) {
1153 last_z = Points->
z[last_seg + 1];
1155 else if (last_x == Points->
x[last_seg] &&
1156 last_y == Points->
y[last_seg]) {
1157 last_z = Points->
z[last_seg];
1159 else if (last_x == Points->
x[last_seg + 1] &&
1160 last_y == Points->
y[last_seg + 1]) {
1161 last_z = Points->
z[last_seg + 1];
1164 dist = dist2(Points->
x[last_seg],
1165 Points->
x[last_seg + 1],
1166 Points->
y[last_seg],
1167 Points->
y[last_seg + 1]);
1169 last_z = (Points->
z[last_seg] * sqrt(cross[i].distance[current]) +
1170 Points->
z[last_seg + 1] *
1171 (sqrt(dist) - sqrt(cross[i].distance[current]))) /
1178 G_debug(2,
" append cross / last point: %f %f", cross[i].x,
1183 G_debug(2,
" line is degenerate -> skipped");
1207 static int find_cross(
int i,
int j,
int b)
1209 double x1, y1, z1, x2, y2, z2;
1210 double y1min, y1max, y2min, y2max;
1213 y1min = APnts->
y[i];
1214 y1max = APnts->
y[i + 1];
1215 if (APnts->
y[i] > APnts->
y[i + 1]) {
1216 y1min = APnts->
y[i + 1];
1217 y1max = APnts->
y[i];
1220 y2min = BPnts->
y[j];
1221 y2max = BPnts->
y[j + 1];
1222 if (BPnts->
y[j] > BPnts->
y[j + 1]) {
1223 y2min = BPnts->
y[j + 1];
1224 y2max = BPnts->
y[j];
1227 if (y1min > y2max || y1max < y2min)
1233 APnts->
x[i + 1], APnts->
y[i + 1],
1235 BPnts->
x[j], BPnts->
y[j],
1237 BPnts->
x[j + 1], BPnts->
y[j + 1],
1239 &x1, &y1, &z1, &x2, &y2, &z2, 0);
1244 BPnts->
x[j + 1], BPnts->
y[j + 1],
1246 APnts->
x[i], APnts->
y[i],
1248 APnts->
x[i + 1], APnts->
y[i + 1],
1250 &x1, &y1, &z1, &x2, &y2, &z2, 0);
1263 G_warning(
_(
"Error while adding point to array. Out of memory"));
1269 G_warning(
_(
"Error while adding point to array. Out of memory"));
1271 G_warning(
_(
"Error while adding point to array. Out of memory"));
1280 struct line_pnts *BPoints,
int with_z,
int all)
1284 struct boq bo_queue;
1285 struct qitem qi, *found;
1286 struct RB_TREE *bo_ta, *bo_tb;
1289 double xa1, ya1, xa2, ya2, xb1, yb1, xb2, yb2, xi, yi;
1305 if (APoints->
x[0] == BPoints->
x[0] && APoints->
y[0] == BPoints->
y[0]) {
1309 APoints->
y[0], APoints->
z[0]))
1310 G_warning(
_(
"Error while adding point to array. Out of memory"));
1314 if (APoints->
z[0] == BPoints->
z[0]) {
1317 APoints->
y[0], APoints->
z[0]))
1318 G_warning(
_(
"Error while adding point to array. Out of memory"));
1335 if (dist <= d_ulp(APoints->
x[0], APoints->
y[0])) {
1339 G_warning(
_(
"Error while adding point to array. Out of memory"));
1352 if (dist <= d_ulp(BPoints->
x[0], BPoints->
y[0])) {
1356 G_warning(
_(
"Error while adding point to array. Out of memory"));
1378 if (abbox.
N > ABox.
N)
1380 if (abbox.
S < ABox.
S)
1382 if (abbox.
E > ABox.
E)
1384 if (abbox.
W < ABox.
W)
1387 abbox.
N += d_ulp(abbox.
N, abbox.
N);
1388 abbox.
S -= d_ulp(abbox.
S, abbox.
S);
1389 abbox.
E += d_ulp(abbox.
E, abbox.
E);
1390 abbox.
W -= d_ulp(abbox.
W, abbox.
W);
1393 if (abbox.
T > ABox.
T)
1395 if (abbox.
B < ABox.
B)
1398 abbox.
T += d_ulp(abbox.
T, abbox.
T);
1399 abbox.
B -= d_ulp(abbox.
B, abbox.
B);
1405 bo_queue.i =
G_malloc(bo_queue.alloc *
sizeof(
struct qitem));
1408 if (!boq_load(&bo_queue, APnts, &abbox, 0, with_z)) {
1414 if (!boq_load(&bo_queue, BPnts, &abbox, 1, with_z)) {
1433 while (boq_drop(&bo_queue, &qi)) {
1441 ret = find_cross(qi.s, found->s, 0);
1448 else if (intersect != 1) {
1452 if (xi == xa1 && yi == ya1) {
1453 if ((xi == xb1 && yi == yb1) ||
1454 (xi == xb2 && yi == yb2)) {
1458 else if (xi == xa2 && yi == ya2) {
1459 if ((xi == xb1 && yi == yb1) ||
1460 (xi == xb2 && yi == yb2)) {
1467 if (intersect == 1) {
1479 ret = find_cross(found->s, qi.s, 1);
1486 else if (intersect != 1) {
1490 if (xi == xa1 && yi == ya1) {
1491 if ((xi == xb1 && yi == yb1) ||
1492 (xi == xb2 && yi == yb2)) {
1496 else if (xi == xa2 && yi == ya2) {
1497 if ((xi == xb1 && yi == yb1) ||
1498 (xi == xb2 && yi == yb2)) {
1505 if (intersect == 1) {
1532 if (!all && intersect == 1) {
int line_check_intersection2(struct line_pnts *APoints, struct line_pnts *BPoints, int with_z, int all)
void void void void G_fatal_error(const char *,...) __attribute__((format(printf
int dig_line_box(const struct line_pnts *, struct bound_box *)
int n_points
Number of points.
int rbtree_init_trav(struct RB_TRAV *, struct RB_TREE *)
int Vect_line_get_intersections2(struct line_pnts *APoints, struct line_pnts *BPoints, struct line_pnts *IPoints, int with_z)
Get 2 lines intersection points.
void G_free(void *)
Free allocated memory.
int Vect_line_distance(const struct line_pnts *, double, double, double, int, double *, double *, double *, double *, double *, double *)
Calculate distance of point to line.
struct RB_TREE * rbtree_create(rb_compare_fn *, size_t)
int Vect_segment_intersection(double, double, double, double, double, double, double, double, double, double, double, double, double *, double *, double *, double *, double *, double *, int)
Check for intersect of 2 line segments.
double * x
Array of X coordinates.
Feature geometry info - coordinates.
int rbtree_insert(struct RB_TREE *, void *)
struct line_pnts * Vect_new_line_struct(void)
Creates and initializes a line_pnts structure.
int Vect_append_point(struct line_pnts *, double, double, double)
Appends one point to the end of a line.
void * rbtree_traverse(struct RB_TRAV *)
#define PORT_DOUBLE_MAX
Limits for portable types.
double * y
Array of Y coordinates.
int Vect_line_intersection2(struct line_pnts *APoints, struct line_pnts *BPoints, struct bound_box *pABox, struct bound_box *pBBox, struct line_pnts ***ALines, struct line_pnts ***BLines, int *nalines, int *nblines, int with_z)
Intersect 2 lines.
void G_warning(const char *,...) __attribute__((format(printf
double * z
Array of Z coordinates.
const char * G_getenv_nofatal(const char *)
Get environment variable.
int dig_line_degenerate(const struct line_pnts *)
int Vect_line_check_intersection2(struct line_pnts *APoints, struct line_pnts *BPoints, int with_z)
Check if 2 lines intersect.
int Vect_box_overlap(const struct bound_box *, const struct bound_box *)
Tests for overlap of two boxes.
void rbtree_destroy(struct RB_TREE *)
void Vect_destroy_line_struct(struct line_pnts *)
Frees all memory associated with a line_pnts structure, including the structure itself.
int G_debug(int, const char *,...) __attribute__((format(printf
void Vect_reset_line(struct line_pnts *)
Reset line.
int rbtree_remove(struct RB_TREE *, const void *)