diff --git a/Spatial_searching/include/CGAL/Euclidean_distance.h b/Spatial_searching/include/CGAL/Euclidean_distance.h index 00e0ad8cfb7..0b362b3def1 100644 --- a/Spatial_searching/include/CGAL/Euclidean_distance.h +++ b/Spatial_searching/include/CGAL/Euclidean_distance.h @@ -57,60 +57,144 @@ namespace CGAL { // default constructor Euclidean_distance(const SearchTraits& traits_=SearchTraits()):traits(traits_) {} - - inline FT transformed_distance(const Query_item& q, const Point_d& p) const { - return transformed_distance(q,p, D()); + // During the computation, if the partially-computed distance `pcd` gets greater or equal + // to `stop_if_geq_to_this`, the computation is stopped and `pcd` is returned + inline FT transformed_distance(const Query_item& q, const Point_d& p, + FT stop_if_geq_to_this = std::numeric_limits::max()) const + { + typename SearchTraits::Construct_cartesian_const_iterator_d construct_it = traits.construct_cartesian_const_iterator_d_object(); + typename SearchTraits::Cartesian_const_iterator_d p_begin = construct_it(p), p_end = construct_it(p, 0); + return transformed_distance(q, p_begin, p_end, stop_if_geq_to_this); } - //Dynamic version for runtime dimension - inline FT transformed_distance(const Query_item& q, const Point_d& p, Dynamic_dimension_tag) const { - FT distance = FT(0); - typename SearchTraits::Construct_cartesian_const_iterator_d construct_it=traits.construct_cartesian_const_iterator_d_object(); - typename SearchTraits::Cartesian_const_iterator_d qit = construct_it(q), - qe = construct_it(q,1), pit = construct_it(p); - for(; qit != qe; qit++, pit++){ - distance += ((*qit)-(*pit))*((*qit)-(*pit)); - } - return distance; + // During the computation, if the partially-computed distance `pcd` gets greater or equal + // to `stop_if_geq_to_this`, the computation is stopped and `pcd` is returned + template + inline FT transformed_distance(const Query_item& q, + Coord_iterator it_coord_begin, Coord_iterator it_coord_end, + FT stop_if_geq_to_this = std::numeric_limits::max()) const + { + return transformed_distance(q, it_coord_begin, it_coord_end, stop_if_geq_to_this, D()); } - //Generic version for DIM > 3 - template < int DIM > - inline FT transformed_distance(const Query_item& q, const Point_d& p, Dimension_tag) const { - FT distance = FT(0); - typename SearchTraits::Construct_cartesian_const_iterator_d construct_it=traits.construct_cartesian_const_iterator_d_object(); - typename SearchTraits::Cartesian_const_iterator_d qit = construct_it(q), - qe = construct_it(q,1), pit = construct_it(p); - for(; qit != qe; qit++, pit++){ - distance += ((*qit)-(*pit))*((*qit)-(*pit)); + // Dynamic version for runtime dimension, taking iterators on coordinates as parameters + // During the computation, if the partially-computed distance `pcd` gets greater or equal + // to `stop_if_geq_to_this`, the computation is stopped and `pcd` is returned + template + inline FT transformed_distance(const Query_item& q, + Coord_iterator it_coord_begin, Coord_iterator it_coord_end, + FT stop_if_geq_to_this, Dynamic_dimension_tag) const + { + FT distance = FT(0); + typename SearchTraits::Construct_cartesian_const_iterator_d construct_it = traits.construct_cartesian_const_iterator_d_object(); + typename SearchTraits::Cartesian_const_iterator_d qit = construct_it(q), + qe = construct_it(q, 1); + if (qe - qit >= 4) + { + // Every 4 coordinates, the current partially-computed distance + // is compared to stop_if_geq_to_this + // Note: the concept SearchTraits specifies that Cartesian_const_iterator_d + // must be a random-access iterator + typename SearchTraits::Cartesian_const_iterator_d qe_minus_3 = qe - 3; + for (; qit < qe_minus_3; ++qit, ++it_coord_begin) { + FT diff = (*qit) - (*it_coord_begin); + distance += diff*diff; + ++qit; ++it_coord_begin; + diff = (*qit) - (*it_coord_begin); + distance += diff*diff; + ++qit; ++it_coord_begin; + diff = (*qit) - (*it_coord_begin); + distance += diff*diff; + + if (distance >= stop_if_geq_to_this) + return distance; + + ++qit; ++it_coord_begin; + diff = (*qit) - (*it_coord_begin); + distance += diff*diff; } - return distance; + } + for (; qit != qe; ++qit, ++it_coord_begin) + { + FT diff = (*qit) - (*it_coord_begin); + distance += diff*diff; + } + return distance; + } + + // Generic version for DIM > 3 taking iterators on coordinates as parameters + // During the computation, if the partially-computed distance `pcd` gets greater or equal + // to `stop_if_geq_to_this`, the computation is stopped and `pcd` is returned + template + inline FT transformed_distance(const Query_item& q, + Coord_iterator it_coord_begin, Coord_iterator it_coord_end, + FT stop_if_geq_to_this, Dimension_tag) const + { + FT distance = FT(0); + typename SearchTraits::Construct_cartesian_const_iterator_d construct_it = traits.construct_cartesian_const_iterator_d_object(); + typename SearchTraits::Cartesian_const_iterator_d qit = construct_it(q), + qe = construct_it(q, 1); + if (qe - qit >= 4) + { + // Every 4 coordinates, the current partially-computed distance + // is compared to stop_if_geq_to_this + // Note: the concept SearchTraits specifies that Cartesian_const_iterator_d + // must be a random-access iterator + typename SearchTraits::Cartesian_const_iterator_d qe_minus_3 = qe - 3; + for (; qit < qe_minus_3; ++qit, ++it_coord_begin) { + FT diff = (*qit) - (*it_coord_begin); + distance += diff*diff; + ++qit; ++it_coord_begin; + diff = (*qit) - (*it_coord_begin); + distance += diff*diff; + ++qit; ++it_coord_begin; + diff = (*qit) - (*it_coord_begin); + distance += diff*diff; + + if (distance >= stop_if_geq_to_this) + return distance; + + ++qit; ++it_coord_begin; + diff = (*qit) - (*it_coord_begin); + distance += diff*diff; + } + } + for (; qit != qe; ++qit, ++it_coord_begin) + { + FT diff = (*qit) - (*it_coord_begin); + distance += diff*diff; + } + return distance; } //DIM = 2 loop unrolled - inline FT transformed_distance(const Query_item& q, const Point_d& p, Dimension_tag<2> ) const { - typename SearchTraits::Construct_cartesian_const_iterator_d construct_it=traits.construct_cartesian_const_iterator_d_object(); - typename SearchTraits::Cartesian_const_iterator_d qit = construct_it(q),pit = construct_it(p); - FT distance = square(*qit - *pit); - qit++;pit++; - distance += square(*qit - *pit); - return distance; + template + inline FT transformed_distance(const Query_item& q, + Coord_iterator it_coord_begin, Coord_iterator /*unused*/, + FT /*unused*/, Dimension_tag<2>) const { + typename SearchTraits::Construct_cartesian_const_iterator_d construct_it = traits.construct_cartesian_const_iterator_d_object(); + typename SearchTraits::Cartesian_const_iterator_d qit = construct_it(q); + FT distance = square(*qit - *it_coord_begin); + qit++; it_coord_begin++; + distance += square(*qit - *it_coord_begin); + return distance; } //DIM = 3 loop unrolled - inline FT transformed_distance(const Query_item& q, const Point_d& p, Dimension_tag<3> ) const { - typename SearchTraits::Construct_cartesian_const_iterator_d construct_it=traits.construct_cartesian_const_iterator_d_object(); - typename SearchTraits::Cartesian_const_iterator_d qit = construct_it(q),pit = construct_it(p); - FT distance = square(*qit - *pit); - qit++;pit++; - distance += square(*qit - *pit); - qit++;pit++; - distance += square(*qit - *pit); - return distance; + template + inline FT transformed_distance(const Query_item& q, + Coord_iterator it_coord_begin, Coord_iterator /*unused*/, + FT /*unused*/, Dimension_tag<3>) const { + typename SearchTraits::Construct_cartesian_const_iterator_d construct_it = traits.construct_cartesian_const_iterator_d_object(); + typename SearchTraits::Cartesian_const_iterator_d qit = construct_it(q); + FT distance = square(*qit - *it_coord_begin); + qit++; it_coord_begin++; + distance += square(*qit - *it_coord_begin); + qit++; it_coord_begin++; + distance += square(*qit - *it_coord_begin); + return distance; } - - inline FT min_distance_to_rectangle(const Query_item& q, const Kd_tree_rectangle& r) const {