23 #define SWAP(a,b) {double t = a; a = b; b = t;} 24 #define D ((ax2-ax1)*(by1-by2) - (ay2-ay1)*(bx1-bx2)) 25 #define DA ((bx1-ax1)*(by1-by2) - (by1-ay1)*(bx1-bx2)) 26 #define DB ((ax2-ax1)*(by1-ay1) - (ay2-ay1)*(bx1-ax1)) 29 #ifdef ASDASDASFDSAFFDAS 30 mpf_t p11, p12, p21, p22, t1, t2;
31 mpf_t dd, dda, ddb, delta;
36 void initialize_mpf_vars()
38 mpf_set_default_prec(2048);
64 void det22(mpf_t rop,
double a11,
double b11,
double a12,
double b12,
65 double a21,
double b21,
double a22,
double b22)
80 mpf_mul(t1, p11, p22);
81 mpf_mul(t2, p12, p21);
87 void swap(
double *a,
double *
b)
97 int segment_intersection_2d_e(
double ax1,
double ay1,
double ax2,
double ay2,
98 double bx1,
double by1,
double bx2,
double by2,
99 double *x1,
double *y1,
double *x2,
double *y2)
103 double max_ax, min_ax, max_ay, min_ay;
105 double max_bx, min_bx, max_by, min_by;
107 int sgn_d, sgn_da, sgn_db;
111 int f11, f12, f21, f22;
118 initialize_mpf_vars();
121 G_debug(3,
"segment_intersection_2d_e()");
122 G_debug(4,
" ax1 = %.18f, ay1 = %.18f", ax1, ay1);
123 G_debug(4,
" ax2 = %.18f, ay2 = %.18f", ax2, ay2);
124 G_debug(4,
" bx1 = %.18f, by1 = %.18f", bx1, by1);
125 G_debug(4,
" bx2 = %.18f, by2 = %.18f", bx2, by2);
127 f11 = ((ax1 == bx1) && (ay1 == by1));
128 f12 = ((ax1 == bx2) && (ay1 == by2));
129 f21 = ((ax2 == bx1) && (ay2 == by1));
130 f22 = ((ax2 == bx2) && (ay2 == by2));
133 if ((f11 && f22) || (f12 && f21)) {
134 G_debug(3,
" identical segments");
143 G_debug(3,
" connected by endpoints");
149 G_debug(3,
" connected by endpoints");
155 if ((
MAX(ax1, ax2) <
MIN(bx1, bx2)) || (
MAX(bx1, bx2) <
MIN(ax1, ax2))) {
156 G_debug(3,
" no intersection (disjoint bounding boxes)");
159 if ((
MAX(ay1, ay2) <
MIN(by1, by2)) || (
MAX(by1, by2) <
MIN(ay1, ay2))) {
160 G_debug(3,
" no intersection (disjoint bounding boxes)");
164 det22(dd, ax2, ax1, bx1, bx2, ay2, ay1, by1, by2);
167 G_debug(3,
" general position");
169 det22(dda, bx1, ax1, bx1, bx2, by1, ay1, by1, by2);
170 sgn_da = mpf_sgn(dda);
181 if ((sgn_da < 0) || (mpf_cmp(dda, dd) > 0)) {
182 G_debug(3,
" no intersection");
186 det22(ddb, ax2, ax1, bx1, ax1, ay2, ay1, by1, ay1);
187 sgn_db = mpf_sgn(ddb);
188 if ((sgn_db < 0) || (mpf_cmp(ddb, dd) > 0)) {
189 G_debug(3,
" no intersection");
194 if ((sgn_da > 0) || (mpf_cmp(dda, dd) < 0)) {
195 G_debug(3,
" no intersection");
199 det22(ddb, ax2, ax1, bx1, ax1, ay2, ay1, by1, ay1);
200 sgn_db = mpf_sgn(ddb);
201 if ((sgn_db > 0) || (mpf_cmp(ddb, dd) < 0)) {
202 G_debug(3,
" no intersection");
210 mpf_set_d(delta, ax2 - ax1);
211 mpf_mul(t1, dda, delta);
213 *x1 = ax1 + mpf_get_d(t2);
215 mpf_set_d(delta, ay2 - ay1);
216 mpf_mul(t1, dda, delta);
218 *y1 = ay1 + mpf_get_d(t2);
220 G_debug(3,
" intersection %.16g, %.16g", *x1, *y1);
225 det22(dda, bx1, ax1, bx1, bx2, by1, ay1, by1, by2);
226 sgn_da = mpf_sgn(dda);
229 G_debug(3,
" parallel segments");
242 else if (ax1 == ax2) {
253 else if (bx1 == bx2) {
260 G_debug(3,
" collinear segments");
262 if ((bx2 < ax1) || (bx1 > ax2)) {
263 G_debug(3,
" no intersection");
271 if ((ax1 < bx1) && (ax2 > bx2)) {
289 if ((ax1 > bx1) && (ax2 < bx2)) {
307 G_debug(3,
" partial overlap");
308 if ((bx1 > ax1) && (bx1 < ax2)) {
323 if ((bx2 > ax1) && (bx2 < ax2)) {
340 G_warning((
"segment_intersection_2d() ERROR (should not be reached)"));
355 double ay2,
double bx1,
double by1,
356 double bx2,
double by2,
double *x1,
357 double *y1,
double *x2,
double *y2,
362 double d, d1, d2, ra, rb,
t;
367 G_debug(4,
"segment_intersection_2d()");
368 G_debug(4,
" ax1 = %.18f, ay1 = %.18f", ax1, ay1);
369 G_debug(4,
" ax2 = %.18f, ay2 = %.18f", ax2, ay2);
370 G_debug(4,
" bx1 = %.18f, by1 = %.18f", bx1, by1);
371 G_debug(4,
" bx2 = %.18f, by2 = %.18f", bx2, by2);
379 G_debug(2,
" -> identical segments");
390 else if (bx1 == ax1) {
393 else if (bx2 == ax2) {
396 else if (by1 == ay1) {
418 d = (ax2 - ax1) * (by1 - by2) - (ay2 - ay1) * (bx1 - bx2);
419 d1 = (bx1 - ax1) * (by1 - by2) - (by1 - ay1) * (bx1 - bx2);
420 d2 = (ax2 - ax1) * (by1 - ay1) - (ay2 - ay1) * (bx1 - ax1);
426 tola = tol /
MAX(fabs(ax2 - ax1), fabs(ay2 - ay1));
427 tolb = tol /
MAX(fabs(bx2 - bx1), fabs(by2 - by1));
428 G_debug(2,
" tol = %.18g", tol);
429 G_debug(2,
" tola = %.18g", tola);
430 G_debug(2,
" tolb = %.18g", tolb);
431 if (!
FZERO(d, tol)) {
435 G_debug(2,
" not parallel/collinear: ra = %.18g", ra);
438 if ((ra <= -tola) || (ra >= 1 + tola) || (rb <= -tolb) ||
440 G_debug(2,
" no intersection");
445 *x1 = ax1 + ra * (ax2 - ax1);
446 *y1 = ay1 + ra * (ay2 - ay1);
448 G_debug(2,
" intersection %.18f, %.18f", *x1, *y1);
453 G_debug(3,
" -> parallel/collinear");
473 G_debug(2,
" -> collinear vertical");
484 if (ay1 > by2 || ay2 < by1) {
485 G_debug(2,
" -> no intersection");
490 if (
FEQUAL(ay1, by2, tol)) {
493 G_debug(2,
" -> connected by end points");
496 if (
FEQUAL(ay2, by1, tol)) {
499 G_debug(2,
" -> connected by end points");
504 G_debug(3,
" -> vertical overlap");
506 if (ay1 <= by1 && ay2 >= by2) {
507 G_debug(2,
" -> a contains b");
518 if (ay1 >= by1 && ay2 <= by2) {
519 G_debug(2,
" -> b contains a");
531 G_debug(2,
" -> partial overlap");
532 if (by1 > ay1 && by1 < ay2) {
548 if (by2 > ay1 && by2 < ay2) {
565 G_warning((
"Vect_segment_intersection() ERROR (collinear vertical segments)"));
574 G_debug(2,
" -> collinear non vertical");
577 if ((bx1 > ax1 && bx2 > ax1 && bx1 > ax2 && bx2 > ax2) ||
578 (bx1 < ax1 && bx2 < ax1 && bx1 < ax2 && bx2 < ax2)) {
579 G_debug(2,
" -> no intersection");
584 G_debug(2,
" -> overlap/connected end points");
587 if ((ax1 == bx1 && ay1 == by1) || (ax1 == bx2 && ay1 == by2)) {
590 G_debug(2,
" -> connected by end points");
593 if ((ax2 == bx1 && ay2 == by1) || (ax2 == bx2 && ay2 == by2)) {
596 G_debug(2,
" -> connected by end points");
618 if (ax1 <= bx1 && ax2 >= bx2) {
619 G_debug(2,
" -> a contains b");
630 if (ax1 >= bx1 && ax2 <= bx2) {
631 G_debug(2,
" -> b contains a");
643 G_debug(2,
" -> partial overlap");
644 if (bx1 > ax1 && bx1 < ax2) {
659 if (bx2 > ax1 && bx2 < ax2) {
676 G_warning((
"segment_intersection_2d() ERROR (collinear non vertical segments)"));
687 double bx1,
double by1,
double bx2,
double by2,
688 double *x1,
double *y1,
double *x2,
double *y2)
690 const int DLEVEL = 4;
694 int f11, f12, f21, f22;
699 G_debug(DLEVEL,
"segment_intersection_2d()");
700 G_debug(4,
" ax1 = %.18f, ay1 = %.18f", ax1, ay1);
701 G_debug(4,
" ax2 = %.18f, ay2 = %.18f", ax2, ay2);
702 G_debug(4,
" bx1 = %.18f, by1 = %.18f", bx1, by1);
703 G_debug(4,
" bx2 = %.18f, by2 = %.18f", bx2, by2);
705 f11 = ((ax1 == bx1) && (ay1 == by1));
706 f12 = ((ax1 == bx2) && (ay1 == by2));
707 f21 = ((ax2 == bx1) && (ay2 == by1));
708 f22 = ((ax2 == bx2) && (ay2 == by2));
711 if ((f11 && f22) || (f12 && f21)) {
712 G_debug(DLEVEL,
" identical segments");
721 G_debug(DLEVEL,
" connected by endpoints");
727 G_debug(DLEVEL,
" connected by endpoints");
733 if ((
MAX(ax1, ax2) <
MIN(bx1, bx2)) || (
MAX(bx1, bx2) <
MIN(ax1, ax2))) {
734 G_debug(DLEVEL,
" no intersection (disjoint bounding boxes)");
737 if ((
MAX(ay1, ay2) <
MIN(by1, by2)) || (
MAX(by1, by2) <
MIN(ay1, ay2))) {
738 G_debug(DLEVEL,
" no intersection (disjoint bounding boxes)");
749 else if (ax1 == ax2) {
760 else if (bx1 == bx2) {
769 G_debug(DLEVEL,
" general position");
782 if ((da < 0) || (da > d)) {
783 G_debug(DLEVEL,
" no intersection");
788 if ((db < 0) || (db > d)) {
789 G_debug(DLEVEL,
" no intersection");
794 if ((da > 0) || (da < d)) {
795 G_debug(DLEVEL,
" no intersection");
800 if ((db > 0) || (db < d)) {
801 G_debug(DLEVEL,
" no intersection");
809 *x1 = ax1 + (ax2 - ax1) * da / d;
810 *y1 = ay1 + (ay2 - ay1) * da / d;
812 G_debug(DLEVEL,
" intersection %.16g, %.16g", *x1, *y1);
819 if ((da != 0) || (db != 0)) {
821 G_debug(DLEVEL,
" parallel segments");
827 G_debug(DLEVEL,
" collinear segments");
829 if ((bx2 < ax1) || (bx1 > ax2)) {
830 G_debug(DLEVEL,
" no intersection");
838 if ((ax1 < bx1) && (ax2 > bx2)) {
839 G_debug(DLEVEL,
" a contains b");
856 if ((ax1 > bx1) && (ax2 < bx2)) {
857 G_debug(DLEVEL,
" b contains a");
874 G_debug(DLEVEL,
" partial overlap");
875 if ((bx1 > ax1) && (bx1 < ax2)) {
890 if ((bx2 > ax1) && (bx2 < ax2)) {
907 G_warning((
"segment_intersection_2d() ERROR (should not be reached)"));
926 if (a == 0 || b == 0) {
934 return (bits >
N + abs(ea - eb));
936 return (e < ea -
N + bits);
940 int segment_intersection_2d_test(
double ax1,
double ay1,
double ax2,
941 double ay2,
double bx1,
double by1,
942 double bx2,
double by2,
double *x1,
943 double *y1,
double *x2,
double *y2)
947 double max_ax, min_ax, max_ay, min_ay;
949 double max_bx, min_bx, max_by, min_by;
951 int sgn_d, sgn_da, sgn_db;
955 int f11, f12, f21, f22;
961 double d, da, db, ra, rb;
964 initialize_mpf_vars();
967 G_debug(3,
"segment_intersection_2d_test()");
968 G_debug(3,
" ax1 = %.18e, ay1 = %.18e", ax1, ay1);
969 G_debug(3,
" ax2 = %.18e, ay2 = %.18e", ax2, ay2);
970 G_debug(3,
" bx1 = %.18e, by1 = %.18e", bx1, by1);
971 G_debug(3,
" bx2 = %.18e, by2 = %.18e", bx2, by2);
973 f11 = ((ax1 == bx1) && (ay1 == by1));
974 f12 = ((ax1 == bx2) && (ay1 == by2));
975 f21 = ((ax2 == bx1) && (ay2 == by1));
976 f22 = ((ax2 == bx2) && (ay2 == by2));
979 if ((f11 && f22) || (f12 && f21)) {
980 G_debug(4,
" identical segments");
989 G_debug(4,
" connected by endpoints");
995 G_debug(4,
" connected by endpoints");
1001 if ((
MAX(ax1, ax2) <
MIN(bx1, bx2)) || (
MAX(bx1, bx2) <
MIN(ax1, ax2))) {
1002 G_debug(4,
" no intersection (disjoint bounding boxes)");
1005 if ((
MAX(ay1, ay2) <
MIN(by1, by2)) || (
MAX(by1, by2) <
MIN(ay1, ay2))) {
1006 G_debug(4,
" no intersection (disjoint bounding boxes)");
1010 d = (ax2 - ax1) * (by1 - by2) - (ay2 - ay1) * (bx1 - bx2);
1011 da = (bx1 - ax1) * (by1 - by2) - (by1 - ay1) * (bx1 - bx2);
1012 db = (ax2 - ax1) * (by1 - ay1) - (ay2 - ay1) * (bx1 - ax1);
1014 det22(dd, ax2, ax1, bx1, bx2, ay2, ay1, by1, by2);
1015 sgn_d = mpf_sgn(dd);
1016 s = mpf_get_str(
NULL, &exp, 10, 40, dd);
1017 G_debug(3,
" dd = %sE%d", (s[0] == 0) ?
"0" : s, exp);
1021 G_debug(3,
" general position");
1023 det22(dda, bx1, ax1, bx1, bx2, by1, ay1, by1, by2);
1024 det22(ddb, ax2, ax1, bx1, ax1, ay2, ay1, by1, ay1);
1025 sgn_da = mpf_sgn(dda);
1026 sgn_db = mpf_sgn(ddb);
1030 mpf_div(rra, dda, dd);
1031 mpf_div(rrb, ddb, dd);
1033 s = mpf_get_str(
NULL, &exp, 10, 40, rra);
1034 G_debug(4,
" rra = %sE%d", (s[0] == 0) ?
"0" : s, exp);
1035 G_debug(4,
" ra = %.18E", ra);
1036 s = mpf_get_str(
NULL, &exp, 10, 40, rrb);
1037 G_debug(4,
" rrb = %sE%d", (s[0] == 0) ?
"0" : s, exp);
1038 G_debug(4,
" rb = %.18E", rb);
1041 if ((sgn_da < 0) || (mpf_cmp(dda, dd) > 0)) {
1042 G_debug(DLEVEL,
" no intersection");
1046 if ((sgn_db < 0) || (mpf_cmp(ddb, dd) > 0)) {
1047 G_debug(DLEVEL,
" no intersection");
1052 if ((sgn_da > 0) || (mpf_cmp(dda, dd) < 0)) {
1053 G_debug(DLEVEL,
" no intersection");
1057 if ((sgn_db > 0) || (mpf_cmp(ddb, dd) < 0)) {
1058 G_debug(DLEVEL,
" no intersection");
1063 mpf_set_d(delta, ax2 - ax1);
1064 mpf_mul(t1, dda, delta);
1065 mpf_div(t2, t1, dd);
1066 *x1 = ax1 + mpf_get_d(t2);
1068 mpf_set_d(delta, ay2 - ay1);
1069 mpf_mul(t1, dda, delta);
1070 mpf_div(t2, t1, dd);
1071 *y1 = ay1 + mpf_get_d(t2);
1073 G_debug(2,
" intersection at:");
1074 G_debug(2,
" xx = %.18e", *x1);
1075 G_debug(2,
" x = %.18e", ax1 + ra * (ax2 - ax1));
1076 G_debug(2,
" yy = %.18e", *y1);
1077 G_debug(2,
" y = %.18e", ay1 + ra * (ay2 - ay1));
1081 G_debug(3,
" parallel/collinear...");
#define FEQUAL(X, Y, TOL)
int segment_intersection_2d_tol(double ax1, double ay1, double ax2, double ay2, double bx1, double by1, double bx2, double by2, double *x1, double *y1, double *x2, double *y2, double tol)
int segment_intersection_2d(double ax1, double ay1, double ax2, double ay2, double bx1, double by1, double bx2, double by2, double *x1, double *y1, double *x2, double *y2)
void G_warning(const char *,...) __attribute__((format(printf
int almost_equal(double a, double b, int bits)
int G_debug(int, const char *,...) __attribute__((format(printf