BALL
1.4.1
|
00001 // -*- Mode: C++; tab-width: 2; -*- 00002 // vi: set ts=2: 00003 // 00004 00005 #ifndef BALL_MATHS_ANALYTICALGEOMETRY_H 00006 #define BALL_MATHS_ANALYTICALGEOMETRY_H 00007 00008 #ifndef BALL_COMMON_EXCEPTION_H 00009 # include <BALL/COMMON/exception.h> 00010 #endif 00011 00012 #ifndef BALL_MATHS_ANGLE_H 00013 # include <BALL/MATHS/angle.h> 00014 #endif 00015 00016 #ifndef BALL_MATHS_CIRCLE3_H 00017 # include <BALL/MATHS/circle3.h> 00018 #endif 00019 00020 #ifndef BALL_MATHS_LINE3_H 00021 # include <BALL/MATHS/line3.h> 00022 #endif 00023 00024 #ifndef BALL_MATHS_PLANE3_H 00025 # include <BALL/MATHS/plane3.h> 00026 #endif 00027 00028 #ifndef BALL_MATHS_SPHERE3_H 00029 # include <BALL/MATHS/sphere3.h> 00030 #endif 00031 00032 #ifndef BALL_MATHS_VECTOR3_H 00033 # include <BALL/MATHS/vector3.h> 00034 #endif 00035 00036 #define BALL_MATRIX_CELL(m, dim, row, col) *((m) + (row) * (dim) + (col)) 00037 #define BALL_CELL(x, y) *((m) + (y) * (dim) + (x)) 00038 00039 namespace BALL 00040 { 00041 00048 00055 template <typename T> 00056 BALL_INLINE 00057 T getDeterminant_(const T* m, Size dim) 00058 { 00059 T determinant = 0; 00060 Index dim1 = dim - 1; 00061 00062 if (dim > 1) 00063 { 00064 T* submatrix = new T[dim1 * dim1]; 00065 00066 for (Index i = 0; i < (Index)dim; ++i) 00067 { 00068 for (Index j = 0; j < dim1; ++j) 00069 { 00070 for (Index k = 0; k < dim1; ++k) 00071 { 00072 *(submatrix + j * dim1 + k) = *(m + (j + 1) * dim + (k < i ? k : k + 1)); 00073 } 00074 } 00075 determinant += *(m + i) * (i / 2.0 == i / 2 ? 1 : -1) * getDeterminant_(submatrix, dim1); 00076 } 00077 00078 delete [] submatrix; 00079 } 00080 else 00081 { 00082 determinant = *m; 00083 } 00084 00085 return determinant; 00086 } 00087 00092 template <typename T> 00093 T getDeterminant(const T* m, Size dim) 00094 { 00095 if (dim == 2) 00096 { 00097 return (BALL_CELL(0,0) * BALL_CELL(1,1) - BALL_CELL(0,1) * BALL_CELL(1,0)); 00098 } 00099 else if (dim == 3) 00100 { 00101 return ( BALL_CELL(0,0) * BALL_CELL(1,1) * BALL_CELL(2,2) 00102 + BALL_CELL(0,1) * BALL_CELL(1,2) * BALL_CELL(2,0) 00103 + BALL_CELL(0,2) * BALL_CELL(1,0) * BALL_CELL(2,1) 00104 - BALL_CELL(0,2) * BALL_CELL(1,1) * BALL_CELL(2,0) 00105 - BALL_CELL(0,0) * BALL_CELL(1,2) * BALL_CELL(2,1) 00106 - BALL_CELL(0,1) * BALL_CELL(1,0) * BALL_CELL(2,2)); 00107 } 00108 else 00109 { 00110 return getDeterminant_(m, dim); 00111 } 00112 } 00113 00117 template <typename T> 00118 BALL_INLINE 00119 T getDeterminant2(const T* m) 00120 { 00121 Size dim = 2; 00122 return (BALL_CELL(0,0) * BALL_CELL(1,1) - BALL_CELL(0,1) * BALL_CELL(1,0)); 00123 } 00124 00131 template <typename T> 00132 BALL_INLINE 00133 T getDeterminant2(const T& m00, const T& m01, const T& m10, const T& m11) 00134 { 00135 return (m00 * m11 - m01 * m10); 00136 } 00137 00141 template <typename T> 00142 BALL_INLINE 00143 T getDeterminant3(const T *m) 00144 { 00145 Size dim = 3; 00146 return ( BALL_CELL(0,0) * BALL_CELL(1,1) * BALL_CELL(2,2) 00147 + BALL_CELL(0,1) * BALL_CELL(1,2) * BALL_CELL(2,0) 00148 + BALL_CELL(0,2) * BALL_CELL(1,0) * BALL_CELL(2,1) 00149 - BALL_CELL(0,2) * BALL_CELL(1,1) * BALL_CELL(2,0) 00150 - BALL_CELL(0,0) * BALL_CELL(1,2) * BALL_CELL(2,1) 00151 - BALL_CELL(0,1) * BALL_CELL(1,0) * BALL_CELL(2,2)); 00152 } 00153 00157 template <typename T> 00158 BALL_INLINE T 00159 getDeterminant3(const T& m00, const T& m01, const T& m02, const T& m10, const T& m11, const T& m12, const T& m20, const T& m21, const T& m22) 00160 { 00161 return ( m00 * m11 * m22 + m01 * m12 * m20 + m02 * m10 * m21 - m02 * m11 * m20 - m00 * m12 * m21 - m01 * m10 * m22); 00162 } 00163 00190 template <typename T> 00191 bool SolveSystem(const T* m, T* x, const Size dim) 00192 { 00193 T pivot; 00194 Index i, j, k, p; 00195 // the column dimension of the matrix 00196 const Size col_dim = dim + 1; 00197 T* matrix = new T[dim * (dim + 1)]; 00198 const T* source = m; 00199 T* target = (T*)matrix; 00200 T* end = (T*)&BALL_MATRIX_CELL(matrix, col_dim, dim - 1, dim); 00201 00202 while (target <= end) 00203 { 00204 *target++ = *source++; 00205 } 00206 00207 for (i = 0; i < (Index)dim; ++i) 00208 { 00209 pivot = BALL_MATRIX_CELL(matrix, col_dim, i, i); 00210 p = i; 00211 for (j = i + 1; j < (Index)dim; ++j) 00212 { 00213 if (Maths::isLess(pivot, BALL_MATRIX_CELL(matrix, col_dim, j, i))) 00214 { 00215 pivot = BALL_MATRIX_CELL(matrix, col_dim, j, i); 00216 p = j; 00217 } 00218 } 00219 00220 if (p != i) 00221 { 00222 T tmp; 00223 00224 for (k = i; k < (Index)dim + 1; ++k) 00225 { 00226 tmp = BALL_MATRIX_CELL(matrix, dim, i, k); 00227 BALL_MATRIX_CELL(matrix, col_dim, i, k) = BALL_MATRIX_CELL(matrix, col_dim, p, k); 00228 BALL_MATRIX_CELL(matrix, col_dim, p, k) = tmp; 00229 } 00230 } 00231 else if (Maths::isZero(pivot) || Maths::isNan(pivot)) 00232 { 00233 // invariant: matrix m is singular 00234 delete [] matrix; 00235 00236 return false; 00237 } 00238 00239 for (j = dim; j >= i; --j) 00240 { 00241 BALL_MATRIX_CELL(matrix, col_dim, i, j) /= pivot; 00242 } 00243 00244 for (j = i + 1; j < (Index)dim; ++j) 00245 { 00246 pivot = BALL_MATRIX_CELL(matrix, col_dim, j, i); 00247 00248 for (k = dim; k>= i; --k) 00249 { 00250 BALL_MATRIX_CELL(matrix, col_dim, j, k) -= pivot * BALL_MATRIX_CELL(matrix, col_dim, i, k); 00251 } 00252 } 00253 } 00254 00255 x[dim - 1] = BALL_MATRIX_CELL(matrix, col_dim, dim - 1, dim); 00256 00257 for (i = dim - 2; i >= 0; --i) 00258 { 00259 x[i] = BALL_MATRIX_CELL(matrix, col_dim, i, dim); 00260 00261 for (j = i + 1; j < (Index)dim; ++j) 00262 { 00263 x[i] -= BALL_MATRIX_CELL(matrix, col_dim, i, j) * x[j]; 00264 } 00265 } 00266 00267 delete [] matrix; 00268 00269 return true; 00270 } 00271 00272 #undef BALL_CELL 00273 #undef BALL_MATRIX_CELL 00274 00283 template <typename T> 00284 BALL_INLINE 00285 bool SolveSystem2(const T& a1, const T& b1, const T& c1, const T& a2, const T& b2, const T& c2, T& x1, T& x2) 00286 { 00287 T quot = (a1 * b2 - a2 * b1); 00288 00289 if (Maths::isZero(quot)) 00290 { 00291 return false; 00292 } 00293 00294 x1 = (c1 * b2 - c2 * b1) / quot; 00295 x2 = (a1 * c2 - a2 * c1) / quot; 00296 00297 return true; 00298 } 00299 00309 template <typename T> 00310 short SolveQuadraticEquation(const T& a, const T& b, const T &c, T &x1, T &x2) 00311 { 00312 if (a == 0) 00313 { 00314 if (b == 0) 00315 { 00316 return 0; 00317 } 00318 x1 = x2 = c / b; 00319 return 1; 00320 } 00321 T discriminant = b * b - 4 * a * c; 00322 00323 if (Maths::isLess(discriminant, 0)) 00324 { 00325 return 0; 00326 } 00327 00328 T sqrt_discriminant = sqrt(discriminant); 00329 if (Maths::isZero(sqrt_discriminant)) 00330 { 00331 x1 = x2 = -b / (2 * a); 00332 00333 return 1; 00334 } 00335 else 00336 { 00337 x1 = (-b + sqrt_discriminant) / (2 * a); 00338 x2 = (-b - sqrt_discriminant) / (2 * a); 00339 00340 return 2; 00341 } 00342 } 00343 00349 template <typename T> 00350 BALL_INLINE 00351 TVector3<T> GetPartition(const TVector3<T>& a, const TVector3<T>& b) 00352 { 00353 return TVector3<T>((b.x + a.x) / 2, (b.y + a.y) / 2, (b.z + a.z) / 2); 00354 } 00355 00365 template <typename T> 00366 BALL_INLINE 00367 TVector3<T> GetPartition(const TVector3<T>& a, const TVector3<T>& b, const T& r, const T& s) 00368 { 00369 T sum = r + s; 00370 if (sum == (T)0) 00371 { 00372 throw Exception::DivisionByZero(__FILE__, __LINE__); 00373 } 00374 return TVector3<T> 00375 ((s * a.x + r * b.x) / sum, 00376 (s * a.y + r * b.y) / sum, 00377 (s * a.z + r * b.z) / sum); 00378 } 00379 00385 template <typename T> 00386 BALL_INLINE 00387 T GetDistance(const TVector3<T>& a, const TVector3<T>& b) 00388 { 00389 T dx = a.x - b.x; 00390 T dy = a.y - b.y; 00391 T dz = a.z - b.z; 00392 00393 return sqrt(dx * dx + dy * dy + dz * dz); 00394 } 00395 00402 template <typename T> 00403 BALL_INLINE 00404 T GetDistance(const TLine3<T>& line, const TVector3<T>& point) 00405 { 00406 if (line.d.getLength() == (T)0) 00407 { 00408 throw Exception::DivisionByZero(__FILE__, __LINE__); 00409 } 00410 return ((line.d % (point - line.p)).getLength() / line.d.getLength()); 00411 } 00412 00419 template <typename T> 00420 BALL_INLINE 00421 T GetDistance(const TVector3<T>& point, const TLine3<T>& line) 00422 { 00423 return GetDistance(line, point); 00424 } 00425 00432 template <typename T> 00433 T GetDistance(const TLine3<T>& a, const TLine3<T>& b) 00434 { 00435 T cross_product_length = (a.d % b.d).getLength(); 00436 00437 if (Maths::isZero(cross_product_length)) 00438 { // invariant: parallel lines 00439 if (a.d.getLength() == (T)0) 00440 { 00441 throw Exception::DivisionByZero(__FILE__, __LINE__); 00442 } 00443 return ((a.d % (b.p - a.p)).getLength() / a.d.getLength()); 00444 } 00445 else 00446 { 00447 T spat_product = TVector3<T>::getTripleProduct(a.d, b.d, b.p - a.p); 00448 00449 if (Maths::isNotZero(spat_product)) 00450 { // invariant: windschiefe lines 00451 00452 return (Maths::abs(spat_product) / cross_product_length); 00453 } 00454 else 00455 { 00456 // invariant: intersecting lines 00457 return 0; 00458 } 00459 } 00460 } 00461 00468 template <typename T> 00469 BALL_INLINE 00470 T GetDistance(const TVector3<T>& point, const TPlane3<T>& plane) 00471 { 00472 T length = plane.n.getLength(); 00473 00474 if (length == (T)0) 00475 { 00476 throw Exception::DivisionByZero(__FILE__, __LINE__); 00477 } 00478 return (Maths::abs(plane.n * (point - plane.p)) / length); 00479 } 00480 00487 template <typename T> 00488 BALL_INLINE 00489 T GetDistance(const TPlane3<T>& plane, const TVector3<T>& point) 00490 { 00491 return GetDistance(point, plane); 00492 } 00493 00500 template <typename T> 00501 BALL_INLINE 00502 T GetDistance(const TLine3<T>& line, const TPlane3<T>& plane) 00503 { 00504 T length = plane.n.getLength(); 00505 if (length == (T)0) 00506 { 00507 throw Exception::DivisionByZero(__FILE__, __LINE__); 00508 } 00509 return (Maths::abs(plane.n * (line.p - plane.p)) / length); 00510 } 00511 00518 template <typename T> 00519 BALL_INLINE 00520 T GetDistance(const TPlane3<T>& plane, const TLine3<T>& line) 00521 { 00522 return GetDistance(line, plane); 00523 } 00524 00531 template <typename T> 00532 BALL_INLINE 00533 T GetDistance(const TPlane3<T>& a, const TPlane3<T>& b) 00534 { 00535 T length = a.n.getLength(); 00536 if (length == (T)0) 00537 { 00538 throw Exception::DivisionByZero(__FILE__, __LINE__); 00539 } 00540 return (Maths::abs(a.n * (a.p - b.p)) / length); 00541 } 00542 00549 template <typename T> 00550 BALL_INLINE 00551 bool GetAngle(const TVector3<T>& a, const TVector3<T>& b, TAngle<T> &intersection_angle) 00552 { 00553 T length_product = a.getSquareLength() * b.getSquareLength(); 00554 if(Maths::isZero(length_product)) 00555 { 00556 return false; 00557 } 00558 intersection_angle = a.getAngle(b); 00559 return true; 00560 } 00561 00568 template <typename T> 00569 BALL_INLINE 00570 bool GetAngle(const TLine3<T>& a, const TLine3<T>& b, TAngle<T>& intersection_angle) 00571 { 00572 T length_product = a.d.getSquareLength() * b.d.getSquareLength(); 00573 00574 if(Maths::isZero(length_product)) 00575 { 00576 return false; 00577 } 00578 intersection_angle = acos(Maths::abs(a.d * b.d) / sqrt(length_product)); 00579 return true; 00580 } 00581 00588 template <typename T> 00589 BALL_INLINE 00590 bool GetAngle(const TPlane3<T>& plane, const TVector3<T>& vector, TAngle<T>& intersection_angle) 00591 { 00592 T length_product = plane.n.getSquareLength() * vector.getSquareLength(); 00593 00594 if (Maths::isZero(length_product)) 00595 { 00596 return false; 00597 } 00598 else 00599 { 00600 intersection_angle = asin(Maths::abs(plane.n * vector) / sqrt(length_product)); 00601 return true; 00602 } 00603 } 00604 00611 template <typename T> 00612 BALL_INLINE 00613 bool GetAngle(const TVector3<T>& vector ,const TPlane3<T>& plane, TAngle<T> &intersection_angle) 00614 { 00615 return GetAngle(plane, vector, intersection_angle); 00616 } 00617 00624 template <typename T> 00625 BALL_INLINE 00626 bool GetAngle(const TPlane3<T>& plane,const TLine3<T>& line, TAngle<T>& intersection_angle) 00627 { 00628 T length_product = plane.n.getSquareLength() * line.d.getSquareLength(); 00629 00630 if (Maths::isZero(length_product)) 00631 { 00632 return false; 00633 } 00634 00635 intersection_angle = asin(Maths::abs(plane.n * line.d) / sqrt(length_product)); 00636 return true; 00637 } 00638 00645 template <typename T> 00646 BALL_INLINE 00647 bool GetAngle(const TLine3<T>& line, const TPlane3<T>& plane, TAngle<T>& intersection_angle) 00648 { 00649 return GetAngle(plane, line, intersection_angle); 00650 } 00651 00652 00659 template <typename T> 00660 BALL_INLINE 00661 bool GetAngle(const TPlane3<T>& a, const TPlane3<T>& b, TAngle<T>& intersection_angle) 00662 { 00663 T length_product = a.n.getSquareLength() * b.n.getSquareLength(); 00664 00665 if(Maths::isZero(length_product)) 00666 { 00667 return false; 00668 } 00669 00670 intersection_angle = acos(Maths::abs(a.n * b.n) / sqrt(length_product)); 00671 return true; 00672 } 00673 00680 template <typename T> 00681 bool GetIntersection(const TLine3<T>& a, const TLine3<T>& b, TVector3<T>& point) 00682 { 00683 T c1, c2; 00684 if ((SolveSystem2(a.d.x, -b.d.x, b.p.x - a.p.x, a.d.y, -b.d.y, b.p.y - a.p.y, c1, c2) == true && Maths::isEqual(a.p.z + a.d.z * c1, b.p.z + b.d.z * c2)) || (SolveSystem2(a.d.x, -b.d.x, b.p.x - a.p.x, a.d.z, -b.d.z, b.p.z - a.p.z, c1, c2) == true && Maths::isEqual(a.p.y + a.d.y * c1, b.p.y + b.d.y * c2)) || (SolveSystem2(a.d.y, -b.d.y, b.p.y - a.p.y, a.d.z, -b.d.z, b.p.z - a.p.z, c1, c2) == true && Maths::isEqual(a.p.x + a.d.x * c1, b.p.x + b.d.x * c2))) 00685 { 00686 point.set(a.p.x + a.d.x * c1, a.p.y + a.d.y * c1, a.p.z + a.d.z * c1); 00687 return true; 00688 } 00689 00690 return false; 00691 } 00692 00699 template <typename T> 00700 BALL_INLINE 00701 bool GetIntersection(const TPlane3<T>& plane, const TLine3<T>& line, TVector3<T>& intersection_point) 00702 { 00703 T dot_product = plane.n * line.d; 00704 if (Maths::isZero(dot_product)) 00705 { 00706 return false; 00707 } 00708 intersection_point.set(line.p + (plane.n * (plane.p - line.p)) * line.d / dot_product); 00709 return true; 00710 } 00711 00718 template <typename T> 00719 BALL_INLINE 00720 bool GetIntersection(const TLine3<T>& line, const TPlane3<T>& plane, TVector3<T>& intersection_point) 00721 { 00722 return GetIntersection(plane, line, intersection_point); 00723 } 00724 00731 template <typename T> 00732 bool GetIntersection(const TPlane3<T>& plane1, const TPlane3<T>& plane2, TLine3<T>& line) 00733 { 00734 T u = plane1.p*plane1.n; 00735 T v = plane2.p*plane2.n; 00736 T det = plane1.n.x*plane2.n.y-plane1.n.y*plane2.n.x; 00737 if (Maths::isZero(det)) 00738 { 00739 det = plane1.n.x*plane2.n.z-plane1.n.z*plane2.n.x; 00740 if (Maths::isZero(det)) 00741 { 00742 det = plane1.n.y*plane2.n.z-plane1.n.z*plane2.n.y; 00743 if (Maths::isZero(det)) 00744 { 00745 return false; 00746 } 00747 else 00748 { 00749 T a = plane2.n.z/det; 00750 T b = -plane1.n.z/det; 00751 T c = -plane2.n.y/det; 00752 T d = plane1.n.y/det; 00753 line.p.x = 0; 00754 line.p.y = a*u+b*v; 00755 line.p.z = c*u+d*v; 00756 line.d.x = -1; 00757 line.d.y = a*plane1.n.x+b*plane2.n.x; 00758 line.d.z = c*plane1.n.x+d*plane2.n.x; 00759 } 00760 } 00761 else 00762 { 00763 T a = plane2.n.z/det; 00764 T b = -plane1.n.z/det; 00765 T c = -plane2.n.x/det; 00766 T d = plane1.n.x/det; 00767 line.p.x = a*u+b*v; 00768 line.p.y = 0; 00769 line.p.z = c*u+d*v; 00770 line.d.x = a*plane1.n.y+b*plane2.n.y; 00771 line.d.y = -1; 00772 line.d.z = c*plane1.n.y+d*plane2.n.y; 00773 } 00774 } 00775 else 00776 { 00777 T a = plane2.n.y/det; 00778 T b = -plane1.n.y/det; 00779 T c = -plane2.n.x/det; 00780 T d = plane1.n.x/det; 00781 line.p.x = a*u+b*v; 00782 line.p.y = c*u+d*v; 00783 line.p.z = 0; 00784 line.d.x = a*plane1.n.z+b*plane2.n.z; 00785 line.d.y = c*plane1.n.z+d*plane2.n.z; 00786 line.d.z = -1; 00787 } 00788 return true; 00789 } 00790 00798 template <typename T> 00799 bool GetIntersection(const TSphere3<T>& sphere, const TLine3<T>& line, TVector3<T>& intersection_point1, TVector3<T>& intersection_point2) 00800 { 00801 T x1, x2; 00802 short number_of_solutions = SolveQuadraticEquation (line.d * line.d, (line.p - sphere.p) * line.d * 2, (line.p - sphere.p) * (line.p - sphere.p) - sphere.radius * sphere.radius, x1, x2); 00803 00804 if (number_of_solutions == 0) 00805 { 00806 return false; 00807 } 00808 00809 intersection_point1 = line.p + x1 * line.d; 00810 intersection_point2 = line.p + x2 * line.d; 00811 00812 return true; 00813 } 00814 00822 template <typename T> 00823 BALL_INLINE 00824 bool GetIntersection(const TLine3<T>& line, const TSphere3<T>& sphere, TVector3<T>& intersection_point1, TVector3<T>& intersection_point2) 00825 { 00826 return GetIntersection(sphere, line, intersection_point1, intersection_point2); 00827 } 00828 00835 template <typename T> 00836 bool GetIntersection(const TSphere3<T>& sphere, const TPlane3<T>& plane, TCircle3<T>& intersection_circle) 00837 { 00838 T distance = GetDistance(sphere.p, plane); 00839 00840 if (Maths::isGreater(distance, sphere.radius)) 00841 { 00842 return false; 00843 } 00844 00845 TVector3<T> Vector3(plane.n); 00846 Vector3.normalize(); 00847 00848 if (Maths::isEqual(distance, sphere.radius)) 00849 { 00850 intersection_circle.set(sphere.p + sphere.radius * Vector3, plane.n, 0); 00851 } 00852 else 00853 { 00854 intersection_circle.set 00855 (sphere.p + distance * Vector3, plane.n, 00856 sqrt(sphere.radius * sphere.radius - distance * distance)); 00857 } 00858 00859 return true; 00860 } 00861 00868 template <typename T> 00869 BALL_INLINE bool 00870 GetIntersection(const TPlane3<T>& plane, const TSphere3<T>& sphere, TCircle3<T>& intersection_circle) 00871 { 00872 return GetIntersection(sphere, plane, intersection_circle); 00873 } 00874 00883 template <typename T> 00884 bool GetIntersection(const TSphere3<T>& a, const TSphere3<T>& b, TCircle3<T>& intersection_circle) 00885 { 00886 TVector3<T> norm = b.p - a.p; 00887 T square_dist = norm * norm; 00888 if (Maths::isZero(square_dist)) 00889 { 00890 return false; 00891 } 00892 T dist = sqrt(square_dist); 00893 if (Maths::isLess(a.radius + b.radius, dist)) 00894 { 00895 return false; 00896 } 00897 if (Maths::isGreaterOrEqual(Maths::abs(a.radius - b.radius), dist)) 00898 { 00899 return false; 00900 } 00901 00902 T radius1_square = a.radius * a.radius; 00903 T radius2_square = b.radius * b.radius; 00904 T u = radius1_square - radius2_square + square_dist; 00905 T length = u / (2 * square_dist); 00906 T square_radius = radius1_square - u * length / 2; 00907 if (square_radius < 0) 00908 { 00909 return false; 00910 } 00911 00912 intersection_circle.p = a.p + (norm * length); 00913 intersection_circle.radius = sqrt(square_radius); 00914 intersection_circle.n = norm / dist; 00915 00916 return true; 00917 } 00918 00928 template <class T> 00929 bool GetIntersection(const TSphere3<T>& s1, const TSphere3<T>& s2, const TSphere3<T>& s3, TVector3<T>& p1, TVector3<T>& p2, bool test = true) 00930 { 00931 T r1_square = s1.radius*s1.radius; 00932 T r2_square = s2.radius*s2.radius; 00933 T r3_square = s3.radius*s3.radius; 00934 T p1_square_length = s1.p*s1.p; 00935 T p2_square_length = s2.p*s2.p; 00936 T p3_square_length = s3.p*s3.p; 00937 T u = (r2_square-r1_square-p2_square_length+p1_square_length)/2; 00938 T v = (r3_square-r1_square-p3_square_length+p1_square_length)/2; 00939 TPlane3<T> plane1; 00940 TPlane3<T> plane2; 00941 try 00942 { 00943 plane1 = TPlane3<T>(s2.p.x-s1.p.x,s2.p.y-s1.p.y,s2.p.z-s1.p.z,u); 00944 plane2 = TPlane3<T>(s3.p.x-s1.p.x,s3.p.y-s1.p.y,s3.p.z-s1.p.z,v); 00945 } 00946 catch (Exception::DivisionByZero&) 00947 { 00948 return false; 00949 } 00950 TLine3<T> line; 00951 if (GetIntersection(plane1,plane2,line)) 00952 { 00953 TVector3<T> diff(s1.p-line.p); 00954 T x1, x2; 00955 if (SolveQuadraticEquation(line.d*line.d, -diff*line.d*2, diff*diff-r1_square, x1,x2) > 0) 00956 { 00957 p1 = line.p+x1*line.d; 00958 p2 = line.p+x2*line.d; 00959 if (test) 00960 { 00961 TVector3<T> test = s1.p-p1; 00962 if (Maths::isNotEqual(test*test,r1_square)) 00963 { 00964 return false; 00965 } 00966 test = s1.p-p2; 00967 if (Maths::isNotEqual(test*test,r1_square)) 00968 { 00969 return false; 00970 } 00971 test = s2.p-p1; 00972 if (Maths::isNotEqual(test*test,r2_square)) 00973 { 00974 return false; 00975 } 00976 test = s2.p-p2; 00977 if (Maths::isNotEqual(test*test,r2_square)) 00978 { 00979 return false; 00980 } 00981 test = s3.p-p1; 00982 if (Maths::isNotEqual(test*test,r3_square)) 00983 { 00984 return false; 00985 } 00986 test = s3.p-p2; 00987 if (Maths::isNotEqual(test*test,r3_square)) 00988 { 00989 return false; 00990 } 00991 } 00992 return true; 00993 } 00994 } 00995 return false; 00996 } 00997 00998 01004 template <typename T> 01005 BALL_INLINE 01006 bool isCollinear(const TVector3<T>& a, const TVector3<T>& b) 01007 { 01008 return (a % b).isZero(); 01009 } 01010 01017 template <typename T> 01018 BALL_INLINE 01019 bool isComplanar(const TVector3<T>& a, const TVector3<T>& b, const TVector3<T>& c) 01020 { 01021 return Maths::isZero(TVector3<T>::getTripleProduct(a, b, c)); 01022 } 01023 01031 template <typename T> 01032 BALL_INLINE 01033 bool isComplanar(const TVector3<T>& a, const TVector3<T>& b, const TVector3<T>& c, const TVector3<T>& d) 01034 { 01035 return isComplanar(a - b, a - c, a - d); 01036 } 01037 01043 template <typename T> 01044 BALL_INLINE 01045 bool isOrthogonal(const TVector3<T>& a, const TVector3<T>& b) 01046 { 01047 return Maths::isZero(a * b); 01048 } 01049 01055 template <typename T> 01056 BALL_INLINE 01057 bool isOrthogonal(const TVector3<T>& vector, const TLine3<T>& line) 01058 { 01059 return Maths::isZero(vector * line.d); 01060 } 01061 01067 template <typename T> 01068 BALL_INLINE 01069 bool isOrthogonal(const TLine3<T>& line, const TVector3<T>& vector) 01070 { 01071 return isOrthogonal(vector, line); 01072 } 01073 01079 template <typename T> 01080 BALL_INLINE 01081 bool isOrthogonal(const TLine3<T>& a, const TLine3<T>& b) 01082 { 01083 return Maths::isZero(a.d * b.d); 01084 } 01085 01091 template <typename T> 01092 BALL_INLINE 01093 bool isOrthogonal(const TVector3<T>& vector, const TPlane3<T>& plane) 01094 { 01095 return isCollinear(vector, plane.n); 01096 } 01097 01103 template <typename T> 01104 BALL_INLINE 01105 bool isOrthogonal(const TPlane3<T>& plane, const TVector3<T>& vector) 01106 { 01107 return isOrthogonal(vector, plane); 01108 } 01109 01115 template <typename T> 01116 BALL_INLINE 01117 bool isOrthogonal(const TPlane3<T>& a, const TPlane3<T>& b) 01118 { 01119 return Maths::isZero(a.n * b.n); 01120 } 01121 01127 template <typename T> 01128 BALL_INLINE 01129 bool isIntersecting(const TVector3<T>& point, const TLine3<T>& line) 01130 { 01131 return Maths::isZero(GetDistance(point, line)); 01132 } 01133 01139 template <typename T> 01140 BALL_INLINE 01141 bool isIntersecting(const TLine3<T>& line, const TVector3<T>& point) 01142 { 01143 return isIntersecting(point, line); 01144 } 01145 01151 template <typename T> 01152 BALL_INLINE 01153 bool isIntersecting(const TLine3<T>& a, const TLine3<T>& b) 01154 { 01155 return Maths::isZero(GetDistance(a, b)); 01156 } 01157 01163 template <typename T> 01164 BALL_INLINE 01165 bool isIntersecting(const TVector3<T>& point, const TPlane3<T>& plane) 01166 { 01167 return Maths::isZero(GetDistance(point, plane)); 01168 } 01169 01175 template <typename T> 01176 BALL_INLINE 01177 bool isIntersecting(const TPlane3<T>& plane, const TVector3<T>& point) 01178 { 01179 return isIntersecting(point, plane); 01180 } 01181 01187 template <typename T> 01188 BALL_INLINE 01189 bool isIntersecting(const TLine3<T>& line, const TPlane3<T>& plane) 01190 { 01191 return Maths::isZero(GetDistance(line, plane)); 01192 } 01193 01199 template <typename T> 01200 BALL_INLINE 01201 bool isIntersecting(const TPlane3<T>& plane, const TLine3<T>& line) 01202 { 01203 return isIntersecting(line, plane); 01204 } 01205 01211 template <typename T> 01212 BALL_INLINE 01213 bool isIntersecting(const TPlane3<T>& a, const TPlane3<T>& b) 01214 { 01215 return Maths::isZero(GetDistance(a, b)); 01216 } 01217 01223 template <typename T> 01224 BALL_INLINE 01225 bool isParallel(const TLine3<T>& line, const TPlane3<T>& plane) 01226 { 01227 return isOrthogonal(line.d, plane.n); 01228 } 01229 01235 template <typename T> 01236 BALL_INLINE 01237 bool isParallel(const TPlane3<T>& plane, const TLine3<T>& line) 01238 { 01239 return isParallel(line, plane); 01240 } 01241 01247 template <typename T> 01248 BALL_INLINE 01249 bool isParallel(const TPlane3<T>& a, const TPlane3<T>& b) 01250 { 01251 return isCollinear(a.n, b.n); 01252 } 01253 01257 template <typename T> 01258 TAngle<T> getOrientedAngle 01259 (const T& ax, const T& ay, const T& az, 01260 const T& bx, const T& by, const T& bz, 01261 const T& nx, const T& ny, const T& nz) 01262 { 01263 // Calculate the length of the two normals 01264 T bl = (T) sqrt((double)ax * ax + ay * ay + az * az); 01265 T el = (T) sqrt((double)bx * bx + by * by + bz * bz); 01266 T bel = (T) (ax * bx + ay * by + az * bz); 01267 01268 // if one or both planes are degenerated 01269 if (bl * el == 0) 01270 { 01271 throw Exception::DivisionByZero(__FILE__, __LINE__); 01272 } 01273 bel /= (bl * el); 01274 if (bel > 1.0) 01275 { 01276 bel = 1; 01277 } 01278 else if (bel < -1.0) 01279 { 01280 bel = -1; 01281 } 01282 01283 T acosbel = (T) acos(bel); // >= 0 01284 01285 if (( nx * (az * by - ay * bz) 01286 + ny * (ax * bz - az * bx) 01287 + nz * (ay * bx - ax * by)) > 0) 01288 { 01289 acosbel = Constants::PI+Constants::PI-acosbel; 01290 } 01291 01292 return TAngle<T>(acosbel); 01293 } 01294 01298 template <typename T> 01299 BALL_INLINE 01300 TAngle<T>getOrientedAngle(const TVector3<T>& a, const TVector3<T>& b, const TVector3<T>& normal) 01301 { 01302 return getOrientedAngle(a.x, a.y, a.z, b.x, b.y, b.z, normal.x, normal.y, normal.z); 01303 } 01304 01321 template <typename T> 01322 TAngle<T> getTorsionAngle 01323 (const T& ax, const T& ay, const T& az, 01324 const T& bx, const T& by, const T& bz, 01325 const T& cx, const T& cy, const T& cz, 01326 const T& dx, const T& dy, const T& dz) 01327 { 01328 T abx = ax - bx; 01329 T aby = ay - by; 01330 T abz = az - bz; 01331 01332 T cbx = cx - bx; 01333 T cby = cy - by; 01334 T cbz = cz - bz; 01335 01336 T cdx = cx - dx; 01337 T cdy = cy - dy; 01338 T cdz = cz - dz; 01339 01340 // Calculate the normals to the two planes n1 and n2 01341 // this is given as the cross products: 01342 // AB x BC 01343 // --------- = n1 01344 // |AB x BC| 01345 // 01346 // BC x CD 01347 // --------- = n2 01348 // |BC x CD| 01349 01350 // Normal to plane 1 01351 T ndax = aby * cbz - abz * cby; 01352 T nday = abz * cbx - abx * cbz; 01353 T ndaz = abx * cby - aby * cbx; 01354 01355 // Normal to plane 2 01356 T neax = cbz * cdy - cby * cdz; 01357 T neay = cbx * cdz - cbz * cdx; 01358 T neaz = cby * cdx - cbx * cdy; 01359 01360 // Calculate the length of the two normals 01361 T bl = (T) sqrt((double)ndax * ndax + nday * nday + ndaz * ndaz); 01362 T el = (T) sqrt((double)neax * neax + neay * neay + neaz * neaz); 01363 T bel = (T) (ndax * neax + nday * neay + ndaz * neaz); 01364 01365 // if one or both planes are degenerated 01366 if (bl * el == 0) 01367 { 01368 throw Exception::DivisionByZero(__FILE__, __LINE__); 01369 } 01370 bel /= (bl * el); 01371 if (bel > 1.0) 01372 { 01373 bel = 1; 01374 } 01375 else if (bel < -1.0) 01376 { 01377 bel = -1; 01378 } 01379 01380 T acosbel = (T) acos(bel); 01381 01382 if ((cbx * (ndaz * neay - nday * neaz) 01383 + cby * (ndax * neaz - ndaz * neax) 01384 + cbz * (nday * neax - ndax * neay)) 01385 < 0) 01386 { 01387 acosbel = -acosbel; 01388 } 01389 01390 acosbel = (acosbel > 0.0) 01391 ? Constants::PI - acosbel 01392 : -(Constants::PI + acosbel); 01393 01394 return TAngle<T>(acosbel); 01395 } 01397 } // namespace BALL 01398 01399 01400 #endif // BALL_MATHS_ANALYTICALGEOMETRY_H