analyticalGeometry.h

Go to the documentation of this file.

// -*- Mode: C++; tab-width: 2; -*-
// vi: set ts=2:
//
// $Id: analyticalGeometry.h,v 1.63 2004/02/23 17:26:02 anhi Exp $
//

#ifndef BALL_MATHS_ANALYTICALGEOMETRY_H
#define BALL_MATHS_ANALYTICALGEOMETRY_H

#ifndef BALL_COMMON_EXCEPTION_H
# include <BALL/COMMON/exception.h>
#endif

#ifndef BALL_MATHS_ANGLE_H
# include <BALL/MATHS/angle.h>
#endif

#ifndef BALL_MATHS_CIRCLE3_H
# include <BALL/MATHS/circle3.h>
#endif

#ifndef BALL_MATHS_LINE3_H
# include <BALL/MATHS/line3.h>
#endif

#ifndef BALL_MATHS_PLANE3_H
# include <BALL/MATHS/plane3.h>
#endif

#ifndef BALL_MATHS_SPHERE3_H
# include <BALL/MATHS/sphere3.h>
#endif

#ifndef BALL_MATHS_VECTOR3_H
# include <BALL/MATHS/vector3.h>
#endif

#define BALL_MATRIX_CELL(m, dim, row, col)   *((m) + (row) * (dim) + (col))
#define BALL_CELL(x, y)                      *((m) + (y) * (dim) + (x))

namespace BALL 
{


  template <typename T>
  BALL_INLINE
  T getDeterminant_(const T* m, Size dim)
  {
    T determinant = 0;
    Index dim1 = dim - 1;

    if (dim > 1)
    {
      T* submatrix = new T[dim1 * dim1];

      for (Index i = 0; i < (Index)dim; ++i) 
      {
        for (Index j = 0; j < dim1; ++j) 
        {
          for (Index k = 0; k < dim1; ++k) 
          {
            *(submatrix + j * dim1 + k) = *(m + (j + 1) * dim + (k < i ? k : k + 1));
          }
        }
        determinant += *(m + i) * (i / 2.0 == i / 2 ? 1 : -1) * getDeterminant_(submatrix, dim1);
      }

      delete [] submatrix;
    } 
    else 
    {
      determinant = *m;
    }

    return determinant;
  }

  template <typename T>
  T getDeterminant(const T* m, Size dim)
  {
    if (dim == 2)
    {
      return (BALL_CELL(0,0) * BALL_CELL(1,1) - BALL_CELL(0,1) * BALL_CELL(1,0));
    } 
    else if (dim == 3)
    {
      return (  BALL_CELL(0,0) * BALL_CELL(1,1) * BALL_CELL(2,2) 
              + BALL_CELL(0,1) * BALL_CELL(1,2) * BALL_CELL(2,0) 
              + BALL_CELL(0,2) * BALL_CELL(1,0) * BALL_CELL(2,1) 
              - BALL_CELL(0,2) * BALL_CELL(1,1) * BALL_CELL(2,0) 
              - BALL_CELL(0,0) * BALL_CELL(1,2) * BALL_CELL(2,1) 
              - BALL_CELL(0,1) * BALL_CELL(1,0) * BALL_CELL(2,2)); 
    } 
    else 
    {
      return getDeterminant_(m, dim);
    }
  }

  template <typename T>
  BALL_INLINE 
  T getDeterminant2(const T* m)
  {
    Size dim = 2;
    return (BALL_CELL(0,0) * BALL_CELL(1,1) - BALL_CELL(0,1) * BALL_CELL(1,0));
  }

  template <typename T>
  BALL_INLINE 
  T getDeterminant2(const T& m00, const T& m01, const T& m10, const T& m11)
  {
    return (m00 * m11 - m01 * m10);
  }

  template <typename T>
  BALL_INLINE 
  T getDeterminant3(const T *m)
  {
    Size dim = 3;
    return (  BALL_CELL(0,0) * BALL_CELL(1,1) * BALL_CELL(2,2) 
            + BALL_CELL(0,1) * BALL_CELL(1,2) * BALL_CELL(2,0) 
            + BALL_CELL(0,2) * BALL_CELL(1,0) * BALL_CELL(2,1) 
            - BALL_CELL(0,2) * BALL_CELL(1,1) * BALL_CELL(2,0) 
            - BALL_CELL(0,0) * BALL_CELL(1,2) * BALL_CELL(2,1) 
            - BALL_CELL(0,1) * BALL_CELL(1,0) * BALL_CELL(2,2)); 
  }

  template <typename T>
  BALL_INLINE T 
  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)
  {
    return (  m00 * m11 * m22 + m01 * m12 * m20 + m02 * m10 * m21 - m02 * m11 * m20 - m00 * m12 * m21 - m01 * m10 * m22); 
  }

  template <typename T>
  bool SolveSystem(const T* m, T* x, const Size dim)
  {
    T pivot;
    Index i, j, k, p;
    // the column dimension of the matrix
    const Size col_dim = dim + 1;
    T* matrix = new T[dim * (dim + 1)];
    const T* source = m;
    T* target = (T*)matrix;
    T* end = (T*)&BALL_MATRIX_CELL(matrix, col_dim, dim - 1, dim);

    while (target <= end)
    {
      *target++ = *source++;
    }

    for (i = 0; i < (Index)dim; ++i)
    {
      pivot = BALL_MATRIX_CELL(matrix, col_dim, i, i);
      p = i;
      for (j = i + 1; j < (Index)dim; ++j)
      {
        if (Maths::isLess(pivot, BALL_MATRIX_CELL(matrix, col_dim, j, i)))
        {
          pivot = BALL_MATRIX_CELL(matrix, col_dim, j, i);
          p = j;
        }
      }

      if (p != i)
      {
        T tmp;

        for (k = i; k < (Index)dim + 1; ++k)
        {
          tmp = BALL_MATRIX_CELL(matrix, dim, i, k);
          BALL_MATRIX_CELL(matrix, col_dim, i, k) = BALL_MATRIX_CELL(matrix, col_dim, p, k);
          BALL_MATRIX_CELL(matrix, col_dim, p, k) = tmp;
        }
      }
      else if (Maths::isZero(pivot) || Maths::isNan(pivot))
      { 
        // invariant: matrix m is singular
        delete [] matrix;
        
        return false;
      }

      for (j = dim; j >= i; --j) 
      {
        BALL_MATRIX_CELL(matrix, col_dim, i, j) /= pivot;
      }

      for (j = i + 1; j < (Index)dim; ++j)
      {
        pivot = BALL_MATRIX_CELL(matrix, col_dim, j, i);

        for (k = dim; k>= i; --k) 
        {
          BALL_MATRIX_CELL(matrix, col_dim, j, k) -= pivot * BALL_MATRIX_CELL(matrix, col_dim, i, k);
        }
      }
    }

    x[dim - 1] = BALL_MATRIX_CELL(matrix, col_dim, dim - 1, dim);

    for (i = dim - 2; i >= 0; --i)
    {
      x[i] = BALL_MATRIX_CELL(matrix, col_dim, i, dim);

      for (j = i + 1; j < (Index)dim; ++j) 
      {
        x[i] -= BALL_MATRIX_CELL(matrix, col_dim, i, j) * x[j]; 
      }
    }

    delete [] matrix;
    
    return true;
  }

#undef BALL_CELL
#undef BALL_MATRIX_CELL

  template <typename T>
  BALL_INLINE 
  bool SolveSystem2(const T& a1, const T& b1, const T& c1, const T& a2, const T& b2, const T& c2, T& x1, T& x2)
  {
    T quot = (a1 * b2 - a2 * b1);

    if (Maths::isZero(quot))
    {
      return false;
    }

    x1 = (c1 * b2 - c2 * b1) / quot;
    x2 = (a1 * c2 - a2 * c1) / quot;

    return true;
  }

  template <typename T>
  short SolveQuadraticEquation(const T& a, const T& b, const T &c, T &x1, T &x2)
  {
    if (a == 0)
    {
      if (b == 0)
      {
        return 0;
      }
      x1 = x2 = c / b;
      return 1;
    }
    T discriminant = b * b - 4 * a * c;

    if (Maths::isLess(discriminant, 0))
    {
      return 0;
    }

    T sqrt_discriminant = sqrt(discriminant);
    if (Maths::isZero(sqrt_discriminant))
    {
      x1 = x2 = -b / (2 * a);

      return 1;
    } 
    else 
    {
      x1 = (-b + sqrt_discriminant) / (2 * a);
      x2 = (-b - sqrt_discriminant) / (2 * a);

      return 2;
    }
  }

  template <typename T>
  BALL_INLINE 
  TVector3<T> GetPartition(const TVector3<T>& a, const TVector3<T>& b)
  {
    return TVector3<T>((b.x + a.x) / 2, (b.y + a.y) / 2, (b.z + a.z) / 2);
  }

  template <typename T>
  BALL_INLINE 
  TVector3<T> GetPartition(const TVector3<T>& a, const TVector3<T>& b, const T& r, const T& s)
    throw(Exception::DivisionByZero)
  {
    T sum = r + s;
    if (sum == (T)0)
    {
      throw Exception::DivisionByZero(__FILE__, __LINE__);
    }   
    return TVector3<T>
      ((s * a.x + r * b.x) / sum,
       (s * a.y + r * b.y) / sum,
       (s * a.z + r * b.z) / sum);
  }

  template <typename T>
  BALL_INLINE 
  T GetDistance(const TVector3<T>& a, const TVector3<T>& b)
  {
    T dx = a.x - b.x;
    T dy = a.y - b.y;
    T dz = a.z - b.z;
    
    return sqrt(dx * dx + dy * dy + dz * dz); 
  }

  template <typename T>
  BALL_INLINE 
  T GetDistance(const TLine3<T>& line, const TVector3<T>& point)
    throw(Exception::DivisionByZero)
  {
    if (line.d.getLength() == (T)0)
    {
      throw Exception::DivisionByZero(__FILE__, __LINE__);
    }   
    return ((line.d % (point - line.p)).getLength() / line.d.getLength());
  }

  template <typename T>
  BALL_INLINE 
  T GetDistance(const TVector3<T>& point, const TLine3<T>& line)
    throw(Exception::DivisionByZero)
  {
    return GetDistance(line, point);
  }

  template <typename T>
  T GetDistance(const TLine3<T>& a, const TLine3<T>& b)
    throw(Exception::DivisionByZero)
  {
    T cross_product_length = (a.d % b.d).getLength();
    
    if (Maths::isZero(cross_product_length))
    { // invariant: parallel lines
      if (a.d.getLength() == (T)0)
      {
        throw Exception::DivisionByZero(__FILE__, __LINE__);
      }         
      return ((a.d % (b.p - a.p)).getLength() / a.d.getLength());
    } 
    else 
    {
      T spat_product = TVector3<T>::getTripleProduct(a.d, b.d, b.p - a.p);

      if (Maths::isNotZero(spat_product))
      { // invariant: windschiefe lines
        
        return (Maths::abs(spat_product) / cross_product_length);
      } 
      else 
      { 
        // invariant: intersecting lines
        return 0;
      }
    }
  }

  template <typename T>
  BALL_INLINE 
  T GetDistance(const TVector3<T>& point, const TPlane3<T>& plane)
    throw(Exception::DivisionByZero)
  {
    T length = plane.n.getLength();

    if (length == (T)0)
    {
      throw Exception::DivisionByZero(__FILE__, __LINE__);
    }
    return (Maths::abs(plane.n * (point - plane.p)) / length);
  }

  template <typename T>
  BALL_INLINE 
  T GetDistance(const TPlane3<T>& plane, const TVector3<T>& point)
    throw(Exception::DivisionByZero)
  {
    return GetDistance(point, plane);
  }

  template <typename T>
  BALL_INLINE 
  T GetDistance(const TLine3<T>& line, const TPlane3<T>& plane)
    throw(Exception::DivisionByZero)
  {
    T length = plane.n.getLength();
    if (length == (T)0)
    {
      throw Exception::DivisionByZero(__FILE__, __LINE__);
    }
    return (Maths::abs(plane.n * (line.p - plane.p)) / length);
  }

  template <typename T>
  BALL_INLINE 
  T GetDistance(const TPlane3<T>& plane, const TLine3<T>& line)
    throw(Exception::DivisionByZero)
  {
    return GetDistance(line, plane);
  }

  template <typename T>
  BALL_INLINE 
  T GetDistance(const TPlane3<T>& a, const TPlane3<T>& b)
    throw(Exception::DivisionByZero)
  {
    T length = a.n.getLength();
    if (length == (T)0)
    {
      throw Exception::DivisionByZero(__FILE__, __LINE__);
    }   
    return (Maths::abs(a.n * (a.p - b.p)) / length);
  }

  template <typename T>
  BALL_INLINE 
  bool GetAngle(const TVector3<T>& a, const TVector3<T>& b, TAngle<T> &intersection_angle)
  {
    T length_product = a.getSquareLength() *  b.getSquareLength();
    if(Maths::isZero(length_product))
    {
      return false;
    }
    intersection_angle = a.getAngle(b);
    return true;
  }

  template <typename T>
  BALL_INLINE 
  bool GetAngle(const TLine3<T>& a, const TLine3<T>& b, TAngle<T>& intersection_angle)
  {
    T length_product = a.d.getSquareLength() *  b.d.getSquareLength();

    if(Maths::isZero(length_product))
    {
      return false;
    }
    intersection_angle = acos(Maths::abs(a.d * b.d) / sqrt(length_product));
    return true;
  }

  template <typename T>
  BALL_INLINE 
  bool GetAngle(const TPlane3<T>& plane, const TVector3<T>& vector, TAngle<T>& intersection_angle)
  {
    T length_product = plane.n.getSquareLength() * vector.getSquareLength();
    
    if (Maths::isZero(length_product))
    {
      return false;
    } 
    else 
    {
      intersection_angle = asin(Maths::abs(plane.n * vector) / sqrt(length_product));
      return true;
    }
  }

  template <typename T>
  BALL_INLINE 
  bool GetAngle(const TVector3<T>& vector ,const TPlane3<T>& plane, TAngle<T> &intersection_angle)
  {
    return GetAngle(plane, vector, intersection_angle);
  }

  template <typename T>
  BALL_INLINE 
  bool GetAngle(const TPlane3<T>& plane,const TLine3<T>& line, TAngle<T>& intersection_angle)
  {
    T length_product = plane.n.getSquareLength() * line.d.getSquareLength();
    
    if (Maths::isZero(length_product))
    {
      return false;
    } 

    intersection_angle = asin(Maths::abs(plane.n * line.d) / sqrt(length_product));
    return true;
  }

  template <typename T>
  BALL_INLINE 
  bool GetAngle(const TLine3<T>& line, const TPlane3<T>& plane, TAngle<T>& intersection_angle)
  {
    return GetAngle(plane, line, intersection_angle);
  }


  template <typename T>
  BALL_INLINE 
  bool GetAngle(const TPlane3<T>& a, const TPlane3<T>& b, TAngle<T>& intersection_angle)
  {
    T length_product = a.n.getSquareLength() * b.n.getSquareLength();

    if(Maths::isZero(length_product))
    {
      return false;
    }

    intersection_angle = acos(Maths::abs(a.n * b.n) / sqrt(length_product));
    return true;
  }

  template <typename T>
  bool GetIntersection(const TLine3<T>& a, const TLine3<T>& b, TVector3<T>& point)
  {
    T c1, c2;
    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)))
    {
      point.set(a.p.x + a.d.x * c1, a.p.y + a.d.y * c1, a.p.z + a.d.z * c1);
      return true;
    } 

    return false;
  }

  template <typename T>
  BALL_INLINE 
  bool GetIntersection(const TPlane3<T>& plane, const TLine3<T>& line, TVector3<T>& intersection_point)
  {
    T dot_product = plane.n * line.d;
    if (Maths::isZero(dot_product))
    {
      return false;
    } 
    intersection_point.set(line.p + (plane.n * (plane.p - line.p)) * line.d / dot_product);
    return true;
  }

  template <typename T>
  BALL_INLINE 
  bool GetIntersection(const TLine3<T>& line, const TPlane3<T>& plane, TVector3<T>& intersection_point)
  {
    return GetIntersection(plane, line, intersection_point);
  }

  template <typename T>
  bool GetIntersection(const TPlane3<T>& plane1, const TPlane3<T>& plane2, TLine3<T>& line)
  {
    T u = plane1.p*plane1.n;
    T v = plane2.p*plane2.n;
    T det = plane1.n.x*plane2.n.y-plane1.n.y*plane2.n.x;
    if (Maths::isZero(det))
    {
      det = plane1.n.x*plane2.n.z-plane1.n.z*plane2.n.x;
      if (Maths::isZero(det))
      {
        det = plane1.n.y*plane2.n.z-plane1.n.z*plane2.n.y;
        if (Maths::isZero(det))
        {
          return false;
        }
        else
        {
          T a = plane2.n.z/det;
          T b = -plane1.n.z/det;
          T c = -plane2.n.y/det;
          T d = plane1.n.y/det;
          line.p.x = 0;
          line.p.y = a*u+b*v;
          line.p.z = c*u+d*v;
          line.d.x = -1;
          line.d.y = a*plane1.n.x+b*plane2.n.x;
          line.d.z = c*plane1.n.x+d*plane2.n.x;
        }
      }
      else
      {
        T a = plane2.n.z/det;
        T b = -plane1.n.z/det;
        T c = -plane2.n.x/det;
        T d = plane1.n.x/det;
        line.p.x = a*u+b*v;
        line.p.y = 0;
        line.p.z = c*u+d*v;
        line.d.x = a*plane1.n.y+b*plane2.n.y;
        line.d.y = -1;
        line.d.z = c*plane1.n.y+d*plane2.n.y;
      }
    }
    else
    {
      T a = plane2.n.y/det;
      T b = -plane1.n.y/det;
      T c = -plane2.n.x/det;
      T d = plane1.n.x/det;
      line.p.x = a*u+b*v;
      line.p.y = c*u+d*v;
      line.p.z = 0;
      line.d.x = a*plane1.n.z+b*plane2.n.z;
      line.d.y = c*plane1.n.z+d*plane2.n.z;
      line.d.z = -1;
    }
    return true;
  }

  template <typename T>
  bool GetIntersection(const TSphere3<T>& sphere, const TLine3<T>& line, TVector3<T>& intersection_point1, TVector3<T>& intersection_point2)
  {
    T x1, x2;
    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);

    if (number_of_solutions == 0)
    {
      return false;
    }

    intersection_point1 = line.p + x1 * line.d;
    intersection_point2 = line.p + x2 * line.d;

    return true;
  }

  template <typename T>
  BALL_INLINE 
  bool GetIntersection(const TLine3<T>& line, const TSphere3<T>& sphere, TVector3<T>& intersection_point1, TVector3<T>& intersection_point2)
  {
    return GetIntersection(sphere, line, intersection_point1, intersection_point2);
  }

  template <typename T>
  bool GetIntersection(const TSphere3<T>& sphere, const TPlane3<T>& plane, TCircle3<T>& intersection_circle)
  {
    T distance = GetDistance(sphere.p, plane);

    if (Maths::isGreater(distance, sphere.radius))
    {
      return false;
    }

    TVector3<T> Vector3(plane.n);
    Vector3.normalize();

    if (Maths::isEqual(distance, sphere.radius))
    {
      intersection_circle.set(sphere.p + sphere.radius * Vector3, plane.n, 0);
    } 
    else 
    {
      intersection_circle.set
        (sphere.p + distance * Vector3, plane.n,
         sqrt(sphere.radius * sphere.radius - distance * distance));
    }

    return true;
  }

  template <typename T>
  BALL_INLINE bool
  GetIntersection(const TPlane3<T>& plane, const TSphere3<T>& sphere, TCircle3<T>& intersection_circle)
  {
    return GetIntersection(sphere, plane, intersection_circle);
  }
   
  template <typename T>
  bool GetIntersection(const TSphere3<T>& a, const TSphere3<T>& b, TCircle3<T>& intersection_circle)
  {
    TVector3<T> norm = b.p - a.p;
    T square_dist = norm * norm;
    if (Maths::isZero(square_dist))
    {
      return false;
    }
    T dist = sqrt(square_dist);
    if (Maths::isLess(a.radius + b.radius, dist))
    {
      return false;
    }
    if (Maths::isGreaterOrEqual(Maths::abs(a.radius - b.radius), dist))
    {
      return false;
    }

    T radius1_square = a.radius * a.radius;
    T radius2_square = b.radius * b.radius;
    T u = radius1_square - radius2_square + square_dist;
    T length = u / (2 * square_dist);
    T square_radius = radius1_square - u * length / 2;
    if (square_radius < 0)
    {
      return false;
    }

    intersection_circle.p = a.p + (norm * length);
    intersection_circle.radius = sqrt(square_radius);
    intersection_circle.n = norm / dist;

    return true;
  }

  template <class T>
  bool GetIntersection(const TSphere3<T>& s1, const TSphere3<T>& s2, const TSphere3<T>& s3, TVector3<T>& p1, TVector3<T>& p2, bool test = true)
  {
    T r1_square = s1.radius*s1.radius;
    T r2_square = s2.radius*s2.radius;
    T r3_square = s3.radius*s3.radius;
    T p1_square_length = s1.p*s1.p;
    T p2_square_length = s2.p*s2.p;
    T p3_square_length = s3.p*s3.p;
    T u = (r2_square-r1_square-p2_square_length+p1_square_length)/2;
    T v = (r3_square-r1_square-p3_square_length+p1_square_length)/2;
    TPlane3<T> plane1;
    TPlane3<T> plane2;
    try
    {
      plane1 = TPlane3<T>(s2.p.x-s1.p.x,s2.p.y-s1.p.y,s2.p.z-s1.p.z,u);
      plane2 = TPlane3<T>(s3.p.x-s1.p.x,s3.p.y-s1.p.y,s3.p.z-s1.p.z,v);
    }
    catch (Exception::DivisionByZero&)
    {
      return false;
    }
    TLine3<T> line;
    if (GetIntersection(plane1,plane2,line))
    {
      TVector3<T> diff(s1.p-line.p);
      T x1, x2;
      if (SolveQuadraticEquation(line.d*line.d, -diff*line.d*2, diff*diff-r1_square, x1,x2) > 0)
      {
        p1 = line.p+x1*line.d;
        p2 = line.p+x2*line.d;
        if (test)
        {
          TVector3<T> test = s1.p-p1;
          if (Maths::isNotEqual(test*test,r1_square))
          {
            return false;
          }
          test = s1.p-p2;
          if (Maths::isNotEqual(test*test,r1_square))
          {
            return false;
          }
          test = s2.p-p1;
          if (Maths::isNotEqual(test*test,r2_square))
          {
            return false;
          }
          test = s2.p-p2;
          if (Maths::isNotEqual(test*test,r2_square))
          {
            return false;
          }
          test = s3.p-p1;
          if (Maths::isNotEqual(test*test,r3_square))
          {
            return false;
          }
          test = s3.p-p2;
          if (Maths::isNotEqual(test*test,r3_square))
          {
            return false;
          }
        }
        return true;
      }
    }
    return false;
  }


  template <typename T>
  BALL_INLINE 
  bool isCollinear(const TVector3<T>& a, const TVector3<T>& b)
  {
    return (a % b).isZero();
  }

  template <typename T>
  BALL_INLINE 
  bool isComplanar(const TVector3<T>& a, const TVector3<T>& b, const TVector3<T>& c)
  {
    return Maths::isZero(TVector3<T>::getTripleProduct(a, b, c));
  }

  template <typename T>
  BALL_INLINE 
  bool isComplanar(const TVector3<T>& a, const TVector3<T>& b, const TVector3<T>& c, const TVector3<T>& d)
  {
    return isComplanar(a - b, a - c, a - d);
  }

  template <typename T>
  BALL_INLINE 
  bool isOrthogonal(const TVector3<T>& a, const TVector3<T>& b)
  {
    return Maths::isZero(a * b);
  }

  template <typename T>
  BALL_INLINE 
  bool isOrthogonal(const TVector3<T>& vector, const TLine3<T>& line)
  {
    return Maths::isZero(vector * line.d);
  }

  template <typename T>
  BALL_INLINE 
  bool isOrthogonal(const TLine3<T>& line, const TVector3<T>& vector)
  {
    return isOrthogonal(vector, line);
  }

  template <typename T>
  BALL_INLINE 
  bool isOrthogonal(const TLine3<T>& a, const TLine3<T>& b)
  {
    return Maths::isZero(a.d * b.d);
  }

  template <typename T>
  BALL_INLINE 
  bool isOrthogonal(const TVector3<T>& vector, const TPlane3<T>& plane)
  {
    return isCollinear(vector, plane.n);
  }

  template <typename T>
  BALL_INLINE 
  bool isOrthogonal(const TPlane3<T>& plane, const TVector3<T>& vector)
  {
    return isOrthogonal(vector, plane);
  }

  template <typename T>
  BALL_INLINE 
  bool isOrthogonal(const TPlane3<T>& a, const TPlane3<T>& b)
  {
    return Maths::isZero(a.n * b.n);
  }

  template <typename T>
  BALL_INLINE 
  bool isIntersecting(const TVector3<T>& point, const TLine3<T>& line)
  {
    return Maths::isZero(GetDistance(point, line));
  }

  template <typename T>
  BALL_INLINE 
  bool isIntersecting(const TLine3<T>& line, const TVector3<T>& point)
  {
    return isIntersecting(point, line);
  }

  template <typename T>
  BALL_INLINE 
  bool isIntersecting(const TLine3<T>& a, const TLine3<T>& b)
  {
    return Maths::isZero(GetDistance(a, b));
  }

  template <typename T>
  BALL_INLINE 
  bool isIntersecting(const TVector3<T>& point, const TPlane3<T>& plane)
  {
    return Maths::isZero(GetDistance(point, plane));
  }

  template <typename T>
  BALL_INLINE 
  bool isIntersecting(const TPlane3<T>& plane, const TVector3<T>& point)
  {
    return isIntersecting(point, plane);
  }

  template <typename T>
  BALL_INLINE 
  bool isIntersecting(const TLine3<T>& line, const TPlane3<T>& plane)
  {
    return Maths::isZero(GetDistance(line, plane));
  }

  template <typename T>
  BALL_INLINE 
  bool isIntersecting(const TPlane3<T>& plane, const TLine3<T>& line)
  {
    return isIntersecting(line, plane);
  }

  template <typename T>
  BALL_INLINE 
  bool isIntersecting(const TPlane3<T>& a, const TPlane3<T>& b)
  {
    return Maths::isZero(GetDistance(a, b));
  }

  template <typename T>
  BALL_INLINE 
  bool isParallel(const TLine3<T>& line, const TPlane3<T>& plane)
  {
    return isOrthogonal(line.d, plane.n);
  }

  template <typename T>
  BALL_INLINE 
  bool isParallel(const TPlane3<T>& plane, const TLine3<T>& line)
  {
    return isParallel(line, plane);
  }

  template <typename T>
  BALL_INLINE 
  bool isParallel(const TPlane3<T>& a, const TPlane3<T>& b)
  {
    return isCollinear(a.n, b.n);
  }

  template <typename T>
  TAngle<T> getOrientedAngle
    (const T& ax, const T& ay, const T& az,
     const T& bx, const T& by, const T& bz,
     const T& nx, const T& ny, const T& nz)
    throw(Exception::DivisionByZero)
  {
    // Calculate the length of the two normals
    T bl = (T) sqrt((double)ax * ax + ay * ay + az * az);
    T el = (T) sqrt((double)bx * bx + by * by + bz * bz);
    T bel = (T) (ax * bx + ay * by + az * bz);

    // if one or both planes are degenerated
    if (bl * el == 0)
    {
      throw Exception::DivisionByZero(__FILE__, __LINE__);
    }
    bel /= (bl * el);
    if (bel > 1.0)
    {
      bel = 1;
    }
    else if (bel < -1.0)
    {
      bel = -1;
    }

    T acosbel = (T) acos(bel);  // >= 0

    if (( nx * (az * by - ay * bz)
         + ny * (ax * bz - az * bx)
         + nz * (ay * bx - ax * by)) > 0)
    {
      acosbel = Constants::PI+Constants::PI-acosbel;
    }

    return TAngle<T>(acosbel);
  }

  template <typename T>
  BALL_INLINE 
  TAngle<T>getOrientedAngle(const TVector3<T>& a, const TVector3<T>& b, const TVector3<T>& normal)
    throw(Exception::DivisionByZero)
  {
    return getOrientedAngle(a.x, a.y, a.z, b.x, b.y, b.z, normal.x, normal.y, normal.z);
  }
 
  template <typename T>
  TAngle<T> getTorsionAngle
    (const T& ax, const T& ay, const T& az,
     const T& bx, const T& by, const T& bz,
     const T& cx, const T& cy, const T& cz, 
     const T& dx, const T& dy, const T& dz)
    throw(Exception::DivisionByZero)
  {
    T abx = ax - bx;
    T aby = ay - by;
    T abz = az - bz;

    T cbx = cx - bx;
    T cby = cy - by;
    T cbz = cz - bz;

    T cdx = cx - dx;
    T cdy = cy - dy;
    T cdz = cz - dz;

    // Calculate the normals to the two planes n1 and n2
    // this is given as the cross products:
    //     AB x BC
    //    --------- = n1
    //    |AB x BC|
    //
    //     BC x CD
    //    --------- = n2
    //    |BC x CD|

    // Normal to plane 1 
    T ndax = aby * cbz - abz * cby; 
    T nday = abz * cbx - abx * cbz;
    T ndaz = abx * cby - aby * cbx;

    // Normal to plane 2 
    T neax = cbz * cdy - cby * cdz; 
    T neay = cbx * cdz - cbz * cdx;
    T neaz = cby * cdx - cbx * cdy;

    // Calculate the length of the two normals 
    T bl = (T) sqrt((double)ndax * ndax + nday * nday + ndaz * ndaz);
    T el = (T) sqrt((double)neax * neax + neay * neay + neaz * neaz);
    T bel = (T) (ndax * neax + nday * neay + ndaz * neaz);
    
    // if one or both planes are degenerated
    if (bl * el == 0)
    {
      throw Exception::DivisionByZero(__FILE__, __LINE__);
    }
    bel /= (bl * el);
    if (bel > 1.0) 
    {
      bel = 1;
    } 
    else if (bel < -1.0) 
    {
      bel = -1;
    }

    T acosbel = (T) acos(bel);

    if ((cbx * (ndaz * neay - nday * neaz) 
         + cby * (ndax * neaz - ndaz * neax) 
         + cbz * (nday * neax - ndax * neay))
        < 0)
    {
      acosbel = -acosbel;
    }
    
    acosbel = (acosbel > 0.0) 
      ? Constants::PI - acosbel 
      : -(Constants::PI + acosbel);
    
    return TAngle<T>(acosbel);
  }
} // namespace BALL


#endif // BALL_MATHS_ANALYTICALGEOMETRY_H