-
-
Save petrasvestartas/dad92732d4bbf1efd49378b5fcbec3cf to your computer and use it in GitHub Desktop.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| #pragma once | |
| //PlanePlanePlane | |
| //PlanePlane | |
| //LinePlane | |
| namespace CGAL_IntersectionUtil { | |
| inline int Solve3x3( | |
| const double(&row0)[3], const double(&row1)[3], const double(&row2)[3], | |
| double d0, double d1, double d2, | |
| double* x_addr, double* y_addr, double* z_addr, | |
| double* pivot_ratio) | |
| { | |
| /* Solve a 3x3 linear system using Gauss-Jordan elimination | |
| * with full pivoting. | |
| */ | |
| int i, j; | |
| double* p0; | |
| double* p1; | |
| double* p2; | |
| double x, y, workarray[12], maxpiv, minpiv; | |
| const int sizeof_row = 3 * sizeof(row0[0]); | |
| *pivot_ratio = *x_addr = *y_addr = *z_addr = 0.0; | |
| x = fabs(row0[0]); i = j = 0; | |
| y = fabs(row0[1]); if (y > x) { x = y; j = 1; } | |
| y = fabs(row0[2]); if (y > x) { x = y; j = 2; } | |
| y = fabs(row1[0]); if (y > x) { x = y; i = 1; j = 0; } | |
| y = fabs(row1[1]); if (y > x) { x = y; i = 1; j = 1; } | |
| y = fabs(row1[2]); if (y > x) { x = y; i = 1; j = 2; } | |
| y = fabs(row2[0]); if (y > x) { x = y; i = 2; j = 0; } | |
| y = fabs(row2[1]); if (y > x) { x = y; i = 2; j = 1; } | |
| y = fabs(row2[2]); if (y > x) { x = y; i = 2; j = 2; } | |
| if (x == 0.0) | |
| return 0; | |
| maxpiv = minpiv = fabs(x); | |
| p0 = workarray; | |
| switch (i) { | |
| case 1: /* swap rows 0 and 1 */ | |
| memcpy(p0, row1, sizeof_row); p0[3] = d1; p0 += 4; | |
| memcpy(p0, row0, sizeof_row); p0[3] = d0; p0 += 4; | |
| memcpy(p0, row2, sizeof_row); p0[3] = d2; | |
| break; | |
| case 2: /* swap rows 0 and 2 */ | |
| memcpy(p0, row2, sizeof_row); p0[3] = d2; p0 += 4; | |
| memcpy(p0, row1, sizeof_row); p0[3] = d1; p0 += 4; | |
| memcpy(p0, row0, sizeof_row); p0[3] = d0; | |
| break; | |
| default: | |
| memcpy(p0, row0, sizeof_row); p0[3] = d0; p0 += 4; | |
| memcpy(p0, row1, sizeof_row); p0[3] = d1; p0 += 4; | |
| memcpy(p0, row2, sizeof_row); p0[3] = d2; | |
| break; | |
| } | |
| switch (j) { | |
| case 1: /* swap columns 0 and 1 */ | |
| p0 = x_addr; x_addr = y_addr; y_addr = p0; | |
| p0 = &workarray[0]; | |
| x = p0[0]; p0[0] = p0[1]; p0[1] = x; p0 += 4; | |
| x = p0[0]; p0[0] = p0[1]; p0[1] = x; p0 += 4; | |
| x = p0[0]; p0[0] = p0[1]; p0[1] = x; | |
| break; | |
| case 2: /* swap columns 0 and 2 */ | |
| p0 = x_addr; x_addr = z_addr; z_addr = p0; | |
| p0 = &workarray[0]; | |
| x = p0[0]; p0[0] = p0[2]; p0[2] = x; p0 += 4; | |
| x = p0[0]; p0[0] = p0[2]; p0[2] = x; p0 += 4; | |
| x = p0[0]; p0[0] = p0[2]; p0[2] = x; | |
| break; | |
| } | |
| x = 1.0 / workarray[0]; | |
| /* debugger set workarray[0] = 1 */ | |
| p0 = p1 = workarray + 1; | |
| *p1++ *= x; *p1++ *= x; *p1++ *= x; | |
| x = -(*p1++); | |
| /* debugger set workarray[4] = 0 */ | |
| if (x == 0.0) | |
| p1 += 3; | |
| else | |
| { | |
| *p1++ += x * (*p0++); *p1++ += x * (*p0++); *p1++ += x * (*p0); p0 -= 2; | |
| } | |
| x = -(*p1++); | |
| /* debugger set workarray[8] = 0 */ | |
| if (x != 0.0) | |
| { | |
| *p1++ += x * (*p0++); *p1++ += x * (*p0++); *p1++ += x * (*p0); p0 -= 2; | |
| } | |
| x = fabs(workarray[5]); i = j = 0; | |
| y = fabs(workarray[6]); if (y > x) { x = y; j = 1; } | |
| y = fabs(workarray[9]); if (y > x) { x = y; i = 1; j = 0; } | |
| y = fabs(workarray[10]); if (y > x) { x = y; i = j = 1; } | |
| if (x == 0.0) | |
| return 1; // rank = 1; | |
| y = fabs(x); | |
| if (y > maxpiv) maxpiv = y; else if (y < minpiv) minpiv = y; | |
| if (j) { | |
| /* swap columns 1 and 2 */ | |
| p0 = workarray + 1; | |
| p1 = p0 + 1; | |
| x = *p0; *p0 = *p1; *p1 = x; p0 += 4; p1 += 4; | |
| x = *p0; *p0 = *p1; *p1 = x; p0 += 4; p1 += 4; | |
| x = *p0; *p0 = *p1; *p1 = x; p0 += 4; p1 += 4; | |
| p0 = y_addr; y_addr = z_addr; z_addr = p0; | |
| } | |
| if (i) { | |
| /* pivot is in row 2 */ | |
| p0 = workarray + 1; | |
| p1 = p0 + 8; | |
| p2 = p0 + 4; | |
| } | |
| else { | |
| /* pivot is in row 1 */ | |
| p0 = workarray + 1; | |
| p1 = p0 + 4; | |
| p2 = p0 + 8; | |
| } | |
| /* debugger set workarray[5+4*i] = 1 */ | |
| x = 1.0 / (*p1++); *p1++ *= x; *p1 *= x; p1--; | |
| x = -(*p0++); | |
| /* debugger set p0[-1] = 0 */ | |
| if (x != 0.0) { *p0++ += x * (*p1++); *p0 += x * (*p1); p0--; p1--; } | |
| x = -(*p2++); | |
| /* debugger set p2[-1] = 0 */ | |
| if (x != 0.0) { *p2++ += x * (*p1++); *p2 += x * (*p1); p2--; p1--; } | |
| x = *p2++; | |
| if (x == 0.0) | |
| return 2; // rank = 2; | |
| y = fabs(x); | |
| if (y > maxpiv) maxpiv = y; else if (y < minpiv) minpiv = y; | |
| /* debugger set p2[-1] = 1 */ | |
| *p2 /= x; | |
| x = -(*p1++); if (x != 0.0) *p1 += x * (*p2); | |
| /* debugger set p1[-1] = 0 */ | |
| x = -(*p0++); if (x != 0.0) *p0 += x * (*p2); | |
| /* debugger set p0[-1] = 0 */ | |
| *x_addr = workarray[3]; | |
| if (i) { | |
| *y_addr = workarray[11]; | |
| *z_addr = workarray[7]; | |
| } | |
| else { | |
| *y_addr = workarray[7]; | |
| *z_addr = workarray[11]; | |
| } | |
| *pivot_ratio = minpiv / maxpiv; | |
| return 3; | |
| } | |
| inline bool Intersect(IK::Plane_3& R, IK::Plane_3& S, IK::Plane_3& T, | |
| IK::Point_3& P) | |
| { | |
| double pr = 0.0; | |
| double x, y, z; | |
| const double R_[3] = { R.a() ,R.b() ,R.c() }; | |
| const double S_[3] = { S.a() ,S.b() ,S.c() }; | |
| const double T_[3] = { T.a() ,T.b() ,T.c() }; | |
| const int rank = Solve3x3( | |
| //&R.plane_equation.x, &S.plane_equation.x, &T.plane_equation.x, | |
| R_, S_, T_, | |
| -R.d(), -S.d(), -T.d(), | |
| &x, &y, &z, &pr); | |
| P = IK::Point_3(x, y, z); | |
| return (rank == 3) ? true : false; | |
| } | |
| inline bool Intersect( IK::Plane_3& R, IK::Plane_3& S, IK::Line_3& L) | |
| { | |
| IK::Vector_3 d = CGAL::cross_product(S.orthogonal_vector(), R.orthogonal_vector()); | |
| IK::Point_3 p = CGAL::midpoint(R.point(), S.point()); | |
| IK::Plane_3 T(p, d); | |
| IK::Point_3 l0; | |
| bool rc = Intersect(R, S, T, l0); | |
| L = IK::Line_3(l0, l0 + d); | |
| return rc; | |
| } | |
| inline double ValueAt(IK::Plane_3& Pl, IK::Point_3& P) | |
| { | |
| return (Pl.a() * P.hx() + Pl.b() * P.hy() + Pl.c() * P.hz() + Pl.d()); | |
| } | |
| inline bool Intersect( | |
| IK::Segment_3& L, | |
| IK::Plane_3& Pl, | |
| IK::Point_3& P | |
| //double* line_parameter | |
| ) | |
| { | |
| bool rc = false; | |
| double a, b, d, fd, t; | |
| auto pt0 = L[0]; | |
| auto pt1 = L[1]; | |
| a = ValueAt(Pl, pt0); | |
| b = ValueAt(Pl, pt1); | |
| d = a - b; | |
| if (d == 0.0) | |
| { | |
| if (fabs(a) < fabs(b)) | |
| t = 0.0; | |
| else if (fabs(b) < fabs(a)) | |
| t = 1.0; | |
| else | |
| t = 0.5; | |
| } | |
| else | |
| { | |
| d = 1.0 / d; | |
| fd = fabs(d); | |
| if (fd > 1.0 && (fabs(a) >= ON_DBL_MAX / fd || fabs(b) >= ON_DBL_MAX / fd)) | |
| { | |
| // double overflow - line may be (nearly) parallel to plane | |
| t = 0.5; | |
| } | |
| else | |
| { | |
| t = a / (a - b); // = a*d; a/(a-b) has more precision than a*d | |
| rc = true; | |
| } | |
| } | |
| //if (line_parameter) | |
| // *line_parameter = t; | |
| //s[0].z() | |
| // 26 Feb 2003 Dale Lear | |
| // Changed | |
| // return (1-t)*from + t*to; | |
| // to the following so that axis aligned lines will | |
| // return exact answers for large values of t. | |
| // See RR 9683. | |
| const double s = 1.0 - t; | |
| P = IK::Point_3( | |
| (L[0].x() == L[1].x()) ? L[0].x() : s * L[0].x() + t * L[1].x(), | |
| (L[0].y() == L[1].y()) ? L[0].y() : s * L[0].y() + t * L[1].y(), | |
| (L[0].z() == L[1].z()) ? L[0].z() : s * L[0].z() + t * L[1].z() | |
| ); | |
| return rc; | |
| } | |
| } |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment