geom_tools.h

mean-shift. pointer sample

  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;
    }
  }


  /*! Assume that the base vectors are orthogonal and normalized. */
  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);
  }



  template<typename Real>
  void rotation_matrix(const Real&                  alpha,
		       const Real&                  beta,
		       const Real&                  gamma,
		       Square_matrix<3,Real>* const matrix){

    typedef Real                  real_type;
    typedef Square_matrix<3,Real> matrix_type;

    const real_type cos_alpha = cos(alpha);
    const real_type sin_alpha = sin(alpha);

    const real_type cos_beta = cos(beta);
    const real_type sin_beta = sin(beta);

    const real_type cos_gamma = cos(gamma);
    const real_type sin_gamma = sin(gamma);

    matrix_type m_alpha;
    m_alpha(0,0) = 1.0;
    m_alpha(1,1) = cos_alpha;
    m_alpha(1,2) = sin_alpha;
    m_alpha(2,1) = -sin_alpha;
    m_alpha(2,2) = cos_alpha;

    matrix_type m_beta;
    m_beta(0,0) = cos_beta;
    m_beta(0,2) = sin_beta;
    m_beta(1,1) = 1.0;
    m_beta(2,0) = -sin_beta;
    m_beta(2,2) = cos_beta;

    matrix_type m_gamma;
    m_gamma(0,0) = cos_gamma;
    m_gamma(0,1) = sin_gamma;
    m_gamma(1,0) = -sin_gamma;
    m_gamma(1,1) = cos_gamma;
    m_gamma(2,2) = 1.0;

    *matrix = m_gamma * m_beta * m_alpha;
  }



  
  template<typename Real>
  void nearest_point_inside_triangle(const Vec3<Real>& P,
				     const Vec3<Real>& A,
				     const Vec3<Real>& B,
				     const Vec3<Real>& C,
				     Vec3<Real>* const res){
    using namespace Math_tools;
    
    typedef Real       real_type;
    typedef Vec2<Real> real2_type;
    typedef Vec3<Real> real3_type;
    
    real3_type& Q = *res;

    const real3_type AB = B - A;
    const real3_type AC = C - A;
    const real3_type BC = C - B;
    const real3_type AP = P - A;
    const real3_type BP = P - B;

    const real3_type X = AB.unit();
    const real3_type Y = (AC - (AC * X) * X).unit();

    const real2_type a(0.0,0.0);
    const real2_type b(AB * X,AB * Y);
    const real2_type c(AC * X,AC * Y);
    const real2_type p(AP * X,AP * Y);

    real_type alpha,beta,gamma;
    barycentric_coordinates(a,b,c,p,&alpha,&beta,&gamma);
    
    if ((alpha >= 0) && (beta >= 0) && (gamma >= 0)){

      Q = alpha * A + beta * B + gamma * C;
      return;
    }

    const real3_type& ABu = X;
    const real3_type ACu  = AC.unit();
    const real3_type BCu  = (C - B).unit();

    const real_type n_P_AB = clamp(0.0,AB.norm(),AP * ABu);
    const real3_type P_AB  = n_P_AB * ABu + A;
    const real_type d_AB   = (P - P_AB).norm();
    
    const real_type n_P_AC = clamp(0.0,AC.norm(),AP * ACu);
    const real3_type P_AC  = n_P_AC * ACu + A;
    const real_type d_AC   = (P - P_AC).norm();
    
    const real_type n_P_BC = clamp(0.0,BC.norm(),BP * BCu);
    const real3_type P_BC  = n_P_BC * BCu + B;
    const real_type d_BC   = (P - P_BC).norm();

    if ((d_AB <= d_AC) && (d_AB <= d_BC)){
      Q = P_AB;
      return;
    }

    if (d_AC <= d_BC){
      Q = P_AC;
      return;
    }

    Q = P_BC;
  }



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

    typedef typename Vector2::value_type real_type;
    
    for(unsigned int n = 0;n < Vector2::dimension;++n){
      
      real_type m = corner1[n];
      real_type M = corner2[n];
      real_type x = p[n];

      if (m > M) swap(m,M);

      if ((x < m) || (x > M)){
	return false;
      }
    }

    return true;
  }
  
//   template<typename Vector1,typename Vector2,typename Vector3>
//   bool inside_bounding_box(const Vector1& corner1,
// 			   const Vector2& corner2,
// 			   const Vector3& p){

//     typedef typename Vector1::value_type real_type;
    
//     for(unsigned int n = 0;n < Vector1::dimension;++n){
      
//       real_type m = corner1[n];
//       real_type M = corner2[n];
//       real_type x = p[n];

//       if (m > M) swap(m,M);

//       if ((x < m) || (x > M)){
// 	return false;
//       }
//     }

//     return true;
//   }

  template<typename Iterator,typename Vector>
  void get_bounding_box(Iterator begin,
			Iterator end,
			Vector* const min_corner,
			Vector* const max_corner){

    if (begin == end) return;
    
    typedef typename Vector::value_type real_type;
    typedef unsigned int                size_type;

    const size_type N = Vector::dimension;

    Vector& m_corner = *min_corner;
    Vector& M_corner = *max_corner;
    
    m_corner = *begin;
    M_corner = *begin;
    ++begin;
    
    for(Iterator i = begin;i != end;++i){

      Vector& v = *i;
      
      for(size_type n = 0;n < N;++n){
	real_type& m         = m_corner[n];
	real_type& M         = M_corner[n];
	const real_type& v_n = v[n];

	if (m > v_n){
	  m = v_n;
	}
	else if (M < v_n){
	  M = v_n;
	}
      }
    }
  }
  

} // END OF namespace Geom_tools

#endif