BALL: analyticalGeometry.h Source File

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