---

geom_tools.h

Go to the documentation of this file.

#ifndef __GEOM_TOOLS__
#define __GEOM_TOOLS__

#include <cmath>

#include "geom.h"

namespace Geom_tools{

  using namespace Geometry;
  
  template<typename Real>
  Real min_distance_2_lines(const Vec3<Real>& base1,
                            const Vec3<Real>& direction1,
                            const Vec3<Real>& base2,
                            const Vec3<Real>& direction2,
                            Vec3<Real>* const cross_point = NULL);

  
  

  template<typename Real>
  inline void plane_equation(const Vec3<Real>& base,
                             const Vec3<Real>& normal,
                             Hvec3<Real>* const plane);


  template<typename Real>
  inline void plane_equation(const Vec3<Real>& base,
                             const Vec3<Real>& dir1,
                             const Vec3<Real>& dir2,
                             Hvec3<Real>* const plane);


  template<typename Real>
  inline bool on_same_side_of_plane(const Hvec3<Real>& plane,
                                    const Vec3<Real>& p1,
                                    const Vec3<Real>& p2);

  
  template<typename Real>
  void plane_line_intersection(const Vec3<Real>& base,
                               const Vec3<Real>& direction,
                               const Hvec3<Real>& plane,
                               Vec3<Real>* const cross);
  

  
  template<typename Real>
  bool nearest_sphere_line_intersection(const Vec3<Real>& base,
                                        const Vec3<Real>& direction,
                                        const Vec3<Real>& center,
                                        const Real        radius,
                                        Vec3<Real>* const cross);

  

   template<typename Real>
  bool oriented_sphere_line_intersection(const Vec3<Real>& base,
                                         const Vec3<Real>& direction,
                                         const Vec3<Real>& center,
                                         const Real        radius,
                                         Vec3<Real>* const cross);


  
  template<typename Real> bool
  oriented_ellipsoid_line_intersection(const Vec3<Real>&            base,
                                       const Vec3<Real>&            direction,
                                       const Vec3<Real>&            center,
                                       const Square_matrix<3,Real>& matrix,
                                       Vec3<Real>* const            cross);


  
  template<typename Real>
  void define_centered_ellipsoid(const Vec3<Real>&            x_axis,
                                 const Vec3<Real>&            y_axis,
                                 const Vec3<Real>&            z_axis,
                                 const Real                   x_radius,
                                 const Real                   y_radius,
                                 const Real                   z_radius,
                                 Square_matrix<3,Real>* const result);

  
  template<typename Real>
  bool point_inside_ellipsoid(const Vec3<Real>&            point,
                              const Vec3<Real>&            center,
                              const Square_matrix<3,Real>& matrix);
  


  
/*
  
  #############################################
  #############################################
  #############################################
  ######                                 ######
  ######   I M P L E M E N T A T I O N   ######
  ######                                 ######
  #############################################
  #############################################
  #############################################
  
*/






  
  template<typename Real>
  Real min_distance_2_lines(const Vec3<Real>& base1,
                            const Vec3<Real>& direction1,
                            const Vec3<Real>& base2,
                            const Vec3<Real>& direction2,
                            Vec3<Real>* const cross_point){

    typedef Real             real_type;
    typedef Vec3<real_type>  vector_type;
    typedef Vec3<real_type>  point_type;
    typedef Hvec3<real_type> plane_type;
    
    const vector_type normal = (direction1^direction2).normalize();
    const real_type dist = std::abs((base1-base2)*normal);
                                     
    if (cross_point == NULL){
      return dist;
    }


    // Compute nearest point to line1 on line2.
    
    plane_type plane1;
    const vector_type normal1 = (direction1^normal).normalize();
    plane_equation(base1,normal1,&plane1);
    
    point_type cross1;
    plane_line_intersection(base2,direction2,plane1,&cross1);


    // Compute nearest point to line2 on line1.

    plane_type plane2;
    const vector_type normal2 = (direction2^normal).normalize();
    plane_equation(base2,normal2,&plane2);
    
    point_type cross2;
    plane_line_intersection(base1,direction1,plane2,&cross2);


    // Return center point.
    
    *cross_point = 0.5 * (cross1 + cross2);

    return dist;
  }





  
  template<typename Real>
  void plane_equation(const Vec3<Real>& base,
                      const Vec3<Real>& normal,
                      Hvec3<Real>* const plane){

    *plane = Hvec3<Real>(normal,-base*normal);
  }

  
  template<typename Real>
  void plane_equation(const Vec3<Real>& base,
                      const Vec3<Real>& dir1,
                      const Vec3<Real>& dir2,
                      Hvec3<Real>* const plane){

    plane_equation(base,dir1^dir2,plane);
  }


  /* Epsilon to robustify the choice */
  template<typename Real>
  inline bool on_same_side_of_plane(const Hvec3<Real>& plane,
                                    const Vec3<Real>& p1,
                                    const Vec3<Real>& p2){
    
    return ((plane*Hvec3<Real>(p1,1)) * (plane*Hvec3<Real>(p2,1)) >= 0);
  }

  
  template<typename Real>
  void plane_line_intersection(const Vec3<Real>&  base,
                               const Vec3<Real>&  direction,
                               const Hvec3<Real>& plane,
                               Vec3<Real>* const  cross){

    typedef Real real_type;

    const Hvec3<Real> base_proxy(base,1);
    const Hvec3<Real> direction_proxy(direction,0);

    const real_type base_value = base_proxy*plane;
    const real_type direction_value = direction_proxy*plane;

    *cross = base - direction * (base_value / direction_value);
  }




  template<typename Real>
  bool nearest_sphere_line_intersection(const Vec3<Real>& base,
                                        const Vec3<Real>& direction,
                                        const Vec3<Real>& center,
                                        const Real        radius,
                                        Vec3<Real>* const cross){

    typedef Real            real_type;
    typedef Vec3<real_type> vector_type;
    typedef Vec3<real_type> point_type;

    const vector_type center2base = base - center;

    const real_type c2b_d = center2base * direction;
    const real_type sq_d = direction.square_norm();

    const real_type delta =
      c2b_d*c2b_d - sq_d*(center2base.square_norm() - radius*radius);

    // No intersection
    if (delta<0){
      return false;
    }

    const real_type t = (-c2b_d + ((c2b_d<0)?-1:1) * std::sqrt(delta)) / sq_d;

    *cross = base + t*direction;
    return true;
  }



  template<typename Real>
  bool oriented_sphere_line_intersection(const Vec3<Real>& base,
                                         const Vec3<Real>& direction,
                                         const Vec3<Real>& center,
                                         const Real        radius,
                                         Vec3<Real>* const cross){

    typedef Real            real_type;
    typedef Vec3<real_type> vector_type;
    typedef Vec3<real_type> point_type;

    const vector_type center2base = base - center;

    const real_type c2b_d = center2base * direction;
    const real_type sq_d = direction.square_norm();

    const real_type delta =
      c2b_d*c2b_d - sq_d*(center2base.square_norm() - radius*radius);

    // No intersection
    if (delta<0){
      return false;
    }

    const real_type sqrt_delta = std::sqrt(delta);
    
    const real_type t_minus = (-c2b_d - sqrt_delta) / sq_d;
    const real_type t_plus  = (-c2b_d + sqrt_delta) / sq_d;

    const real_type t = (t_minus*t_plus<0) ? t_plus : t_minus;

    *cross = base + t*direction;
    return true;
  }


  template<typename Real> bool
  oriented_ellipsoid_line_intersection(const Vec3<Real>&            base,
                                       const Vec3<Real>&            direction,
                                       const Vec3<Real>&            center,
                                       const Square_matrix<3,Real>& matrix,
                                       Vec3<Real>* const            cross){

    typedef Real            real_type;
    typedef Vec3<real_type> vector_type;
    typedef Vec3<real_type> point_type;

    const vector_type center2base = base - center;
    const vector_type matrix_dir  = matrix * direction;
    const vector_type matrix_c2b  = matrix * center2base;

    const real_type a = direction * matrix_dir;
    const real_type b = direction * matrix_c2b;
    const real_type c = center2base * matrix_c2b - static_cast<real_type>(1);

    const real_type delta = b*b - a*c;

    if (delta<0){
      return false;
    }
    else{
      
      const real_type sqrt_delta = std::sqrt(delta);

      const real_type t_minus = (-b - sqrt_delta) / a;
      const real_type t_plus  = (-b + sqrt_delta) / a;

      const real_type t = (t_minus*t_plus<0) ? t_plus : t_minus;

      *cross = base + t*direction;
      return true;
    }
  }


  template<typename Real>
  void define_centered_ellipsoid(const Vec3<Real>&            x_axis,
                                 const Vec3<Real>&            y_axis,
                                 const Vec3<Real>&            z_axis,
                                 const Real                   x_radius,
                                 const Real                   y_radius,
                                 const Real                   z_radius,
                                 Square_matrix<3,Real>* const result){

    typedef Square_matrix<3,Real> matrix_type;
    
    matrix_type change; // = transpose of (x,y,z)
    change(0,0) = x_axis.x();
    change(0,1) = x_axis.y();
    change(0,2) = x_axis.z();

    change(1,0) = y_axis.x();
    change(1,1) = y_axis.y();
    change(1,2) = y_axis.z();

    change(2,0) = z_axis.x();
    change(2,1) = z_axis.y();
    change(2,2) = z_axis.z();

    matrix_type diag;
    diag(0,0) = 1/(x_radius*x_radius);
    diag(1,1) = 1/(y_radius*y_radius);
    diag(2,2) = 1/(z_radius*z_radius);

    *result = change.transpose() * diag * change;
  }

  
  template<typename Real>
  bool point_inside_ellipsoid(const Vec3<Real>&            point,
                              const Vec3<Real>&            center,
                              const Square_matrix<3,Real>& matrix){

    typedef Real            real_type;
    typedef Vec3<real_type> vector_type;

    const vector_type v = point - center;
    
    return (v*(matrix*v) < 1);
  }


} // END OF namespace Geom_tools

#endif

---