geom_tools.h

mean-shift. pointer sample

/*! \file

\verbatim

Copyright (c) 2004, Sylvain Paris and Francois Sillion
All rights reserved.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are
met:

    * Redistributions of source code must retain the above copyright
    notice, this list of conditions and the following disclaimer.
    
    * Redistributions in binary form must reproduce the above
    copyright notice, this list of conditions and the following
    disclaimer in the documentation and/or other materials provided
    with the distribution.

    * Neither the name of ARTIS, GRAVIR-IMAG nor the names of its
    contributors may be used to endorse or promote products derived
    from this software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

\endverbatim


 *  This file contains code made by Sylvain Paris under supervision of
 * Fran鏾is Sillion for his PhD work with <a
 * href="http://www-artis.imag.fr">ARTIS project</a>. ARTIS is a
 * research project in the GRAVIR/IMAG laboratory, a joint unit of
 * CNRS, INPG, INRIA and UJF.
 *
 *  Use <a href="http://www.stack.nl/~dimitri/doxygen/">Doxygen</a>
 * with DISTRIBUTE_GROUP_DOC option to produce an nice html
 * documentation.
 *
 *  This file provides some basic geometric tools.
 */
#ifndef __GEOM_TOOLS__
#define __GEOM_TOOLS__

#include <cmath>
#include <limits>

#include "geom.h"
#include "math_tools.h"
// #include "msg_stream.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);
  

  /*! Returns M_gamma * M_beta * M_alpha. */
  template<typename Real>
  void rotation_matrix(const Real&                  alpha,
		       const Real&                  beta,
		       const Real&                  gamma,
		       Square_matrix<3,Real>* const matrix);
  

  
  template<typename Real>
  void nearest_point_inside_triangle(const Vec3<Real>& A,
				     const Vec3<Real>& B,
				     const Vec3<Real>& C,
				     Vec3<Real>* const p);


  
  template<typename Vector1,typename Vector2>
  bool inside_bounding_box(const Vector1& corner1,
			   const Vector1& corner2,
			   const Vector2& p);


  template<typename Iterator,typename Vector>
  void get_bounding_box(Iterator begin,
			Iterator end,
			Vector* const min_corner,
			Vector* const max_corner);
  
/*
  
  #############################################
  #############################################
  #############################################
  ######                                 ######
  ######   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>