Optimize (cache+interruptible) non-orthogonal nearest/furthest neighbor search

Spatial_searching/include/CGAL/Incremental_neighbor_search.h

@@ -17,20 +17,23 @@
 // 
 //
 // Author(s)     : Hans Tangelder (<hanst@cs.uu.nl>)
+//                 Clement Jamin (clement.jamin.pro@gmail.com)
 
 #ifndef CGAL_INCREMENTAL_NEIGHBOR_SEARCH_H
 #define CGAL_INCREMENTAL_NEIGHBOR_SEARCH_H
 
 #include <CGAL/license/Spatial_searching.h>
 
+#include <CGAL/Kd_tree_node.h>
+#include <CGAL/Kd_tree_rectangle.h>
+#include <CGAL/Euclidean_distance.h>
+#include <CGAL/internal/Search_helpers.h>
 
 #include <cstring>
 #include <list>
 #include <queue>
 #include <memory>
-#include <CGAL/Kd_tree_node.h>
+#include <iterator> // for std::distance
-#include <CGAL/Kd_tree_rectangle.h>
-#include <CGAL/Euclidean_distance.h>
 
 namespace CGAL {
 
@@ -95,7 +98,7 @@ namespace CGAL {
     Query_item m_query;
     Distance m_dist;
     FT m_Eps; 
-    bool m_search_nearest;    
+    bool m_search_nearest;
 
   public:
 
@@ -271,6 +274,9 @@ namespace CGAL {
 
 	FT rd;
 
+        internal::Distance_helper<Distance, SearchTraits> m_distance_helper;
+        int m_dim;
+        Tree const& m_tree;
 
 	class Priority_higher {
 	
@@ -330,7 +336,9 @@ namespace CGAL {
 	// constructor
 	Iterator_implementation(const Tree& tree, const Query_item& q,const Distance& tr,
 				FT Eps, bool search_nearest)
-	  : query_point(q), search_nearest_neighbour(search_nearest), 
+	  : query_point(q), search_nearest_neighbour(search_nearest),
+          m_distance_helper(tr, tree.traits()),
+          m_tree(tree),
 	  PriorityQueue(Priority_higher(search_nearest)),
 	  Item_PriorityQueue(Distance_smaller(search_nearest)),
 	  distance(tr), reference_count(1), number_of_internal_nodes_visited(0), 
@@ -338,7 +346,11 @@ namespace CGAL {
 	  number_of_neighbours_computed(0)
 	{
           if (tree.empty()) return;
-            
+
+          typename SearchTraits::Construct_cartesian_const_iterator_d construct_it =
+            m_tree.traits().construct_cartesian_const_iterator_d_object();
+          m_dim = static_cast<int>(std::distance(construct_it(q), construct_it(q, 0)));
+
 	  multiplication_factor= distance.transformed_distance(FT(1)+Eps);
 
 	  Node_box *bounding_box = new Node_box((tree.bounding_box()));
@@ -429,6 +441,86 @@ namespace CGAL {
 	  delete The_item_top;
 	}
 
+
+        template<bool use_cache>
+        bool search_in_leaf(typename Tree::Leaf_node_const_handle node, bool search_furthest);
+
+        // With cache
+        template<>
+        bool search_in_leaf<true>(typename Tree::Leaf_node_const_handle node, bool search_furthest)
+        {
+          typename Tree::iterator it_node_point = node->begin(), it_node_point_end = node->end();
+          typename std::vector<FT>::const_iterator cache_point_begin = m_tree.cache_begin() + m_dim*(it_node_point - m_tree.begin());
+
+          for (; it_node_point != node->end(); ++it_node_point)
+          {
+            number_of_items_visited++;
+            FT distance_to_query_point =
+              m_distance_helper.transformed_distance_from_coordinates(
+                query_point, *it_node_point, cache_point_begin, cache_point_begin + m_dim);
+
+            Point_with_transformed_distance *NN_Candidate =
+              new Point_with_transformed_distance(*it_node_point, distance_to_query_point);
+            Item_PriorityQueue.push(NN_Candidate);
+
+            cache_point_begin += m_dim;
+          }
+          // old top of PriorityQueue has been processed,
+          // hence update rd
+
+          bool next_neighbour_found;
+          if (!(PriorityQueue.empty()))
+          {
+            rd = PriorityQueue.top()->second;
+            next_neighbour_found = (search_furthest ?
+              multiplication_factor*rd < Item_PriorityQueue.top()->second
+              : multiplication_factor*rd > Item_PriorityQueue.top()->second);
+          }
+          else // priority queue empty => last neighbour found
+          {
+            next_neighbour_found = true;
+          }
+
+          number_of_neighbours_computed++;
+          return next_neighbour_found;
+        }
+
+        // Without cache
+        template<>
+        bool search_in_leaf<false>(typename Tree::Leaf_node_const_handle node, bool search_furthest)
+        {
+          typename Tree::iterator it_node_point = node->begin(), it_node_point_end = node->end();
+
+          for (; it_node_point != node->end(); ++it_node_point)
+          {
+            number_of_items_visited++;
+            FT distance_to_query_point =
+              orthogonal_distance_instance.transformed_distance(query_point, *it_node_point);
+
+            Point_with_transformed_distance *NN_Candidate =
+              new Point_with_transformed_distance(*it_node_point, distance_to_query_point);
+            Item_PriorityQueue.push(NN_Candidate);
+          }
+          // old top of PriorityQueue has been processed,
+          // hence update rd
+
+          bool next_neighbour_found;
+          if (!(PriorityQueue.empty()))
+          {
+            rd = PriorityQueue.top()->second;
+            next_neighbour_found = (search_furthest ?
+              multiplication_factor*rd < Item_PriorityQueue.top()->second
+              : multiplication_factor*rd > Item_PriorityQueue.top()->second);
+          }
+          else // priority queue empty => last neighbour found
+          {
+            next_neighbour_found = true;
+          }
+
+          number_of_neighbours_computed++;
+          return next_neighbour_found;
+        }
+
 	void 
 	Compute_the_next_nearest_neighbour() 
 	{
@@ -508,36 +600,14 @@ namespace CGAL {
 	      }
 	    }
 	    delete B;
-             typename Tree::Leaf_node_const_handle node =
+
+            // N is a leaf
+            typename Tree::Leaf_node_const_handle node =
               static_cast<typename Tree::Leaf_node_const_handle>(N);
 	    number_of_leaf_nodes_visited++;
 	    if (node->size() > 0) {
-	      for (typename Tree::iterator it = node->begin(); it != node->end(); it++) {
+              next_neighbour_found =
-		number_of_items_visited++;
+                search_in_leaf<internal::Has_points_cache<Tree, internal::has_Enable_points_cache<Tree>::type::value>::value>(node, !search_nearest_neighbour);
-		FT distance_to_query_point=
-		  distance.transformed_distance(query_point,*it);
-		Point_with_transformed_distance *NN_Candidate=
-		  new Point_with_transformed_distance(*it,distance_to_query_point);
-		Item_PriorityQueue.push(NN_Candidate);
-	      }
-	      // old top of PriorityQueue has been processed,
-	      // hence update rd
-	      if (!PriorityQueue.empty()) {
-		rd = PriorityQueue.top()->second;
-		if (search_nearest_neighbour)
-		  next_neighbour_found =
-		    (multiplication_factor*rd > 
-		     Item_PriorityQueue.top()->second);
-		else
-		  next_neighbour_found =
-		    (multiplication_factor*rd < 
-		     Item_PriorityQueue.top()->second);
-	      }
-	      else // priority queue empty => last neighbour found
-		{
-		  next_neighbour_found=true;
-		};
-	      number_of_neighbours_computed++;
 	    }
 	  }   // next_neighbour_found or priority queue is empty
 	  // in the latter case also the item priority queue is empty

Spatial_searching/include/CGAL/K_neighbor_search.h

@@ -16,18 +16,19 @@
 // $Id$
 // 
 //
-// Author(s)     : Hans Tangelder (<hanst@cs.uu.nl>)
+// Author(s)     : Hans Tangelder (<hanst@cs.uu.nl>),
+//                 Clement Jamin (clement.jamin.pro@gmail.com)
 
 #ifndef  CGAL_K_NEIGHBOR_SEARCH_H
 #define  CGAL_K_NEIGHBOR_SEARCH_H
 
 #include <CGAL/license/Spatial_searching.h>
 
-
 #include <CGAL/internal/K_neighbor_search.h>
 #include <CGAL/internal/Get_dimension_tag.h>
+#include <CGAL/internal/Search_helpers.h>
 
-
+#include <iterator> // for std::distance
 
 
 namespace CGAL {
@@ -38,8 +39,10 @@ template <class SearchTraits,
           class Distance= typename internal::Spatial_searching_default_distance<SearchTraits>::type,
           class Splitter= Sliding_midpoint<SearchTraits> ,
           class Tree= Kd_tree<SearchTraits, Splitter, Tag_true> >
-class K_neighbor_search: public internal::K_neighbor_search<SearchTraits,Distance,Splitter,Tree> {
+class K_neighbor_search: public internal::K_neighbor_search<SearchTraits,Distance,Splitter,Tree>
+{
   typedef  internal::K_neighbor_search<SearchTraits,Distance,Splitter,Tree> Base;
+  typedef typename Tree::Point_d Point;
   
 public:
   typedef typename Base::FT FT;  
@@ -47,9 +50,17 @@ public:
 
   K_neighbor_search(const Tree& tree, const typename Base::Query_item& q,  
     unsigned int k=1, FT Eps=FT(0.0), bool Search_nearest=true, const Distance& d=Distance(),bool sorted=true)
-    : Base(q,k,Eps,Search_nearest,d) 
+  : Base(q,k,Eps,Search_nearest,d),
+    m_distance_helper(this->distance_instance, tree.traits()),
+    m_tree(tree)
   {
     if (tree.empty()) return;
+
+    typename SearchTraits::Construct_cartesian_const_iterator_d construct_it = 
+      m_tree.traits().construct_cartesian_const_iterator_d_object();
+    m_dim = static_cast<int>(std::distance(
+      construct_it(this->query_object), construct_it(this->query_object, 0)));
+
     compute_neighbors_general(tree.root(),tree.bounding_box());
     if (sorted) this->queue.sort();    
   };
@@ -58,6 +69,10 @@ private:
   typedef typename Base::Node_const_handle Node_const_handle; 
   using Base::branch;
 
+  internal::Distance_helper<Distance, SearchTraits> m_distance_helper;
+  int m_dim;
+  Tree const& m_tree;
+
   void 
   compute_neighbors_general(typename Base::Node_const_handle N, const Kd_tree_rectangle<FT,D>& r) 
   {
@@ -122,21 +137,136 @@ private:
 
     }
     else
+    {
+      // n is a leaf
+      typename Tree::Leaf_node_const_handle node =
+        static_cast<typename Tree::Leaf_node_const_handle>(N);
+      this->number_of_leaf_nodes_visited++;
+      if (node->size() > 0)
       {
-        // n is a leaf
+        if (this->search_nearest)
-         typename Tree::Leaf_node_const_handle node =
+          search_nearest_in_leaf<internal::Has_points_cache<Tree, internal::has_Enable_points_cache<Tree>::type::value>::value>(node);
-          static_cast<typename Tree::Leaf_node_const_handle>(N);
+        else
-        this->number_of_leaf_nodes_visited++;
+          search_furthest_in_leaf<internal::Has_points_cache<Tree, internal::has_Enable_points_cache<Tree>::type::value>::value>(node);
-        if (node->size() > 0)
-          for (typename Tree::iterator it = node->begin(); it != node->end(); it++) {
-            this->number_of_items_visited++;
-            FT distance_to_query_object =
-              this->distance_instance.transformed_distance(this->query_object,*it);
-            this->queue.insert(std::make_pair(&(*it),distance_to_query_object));
-          }
       }
+    }
   }
-  
+
+  template<bool use_cache>
+  void search_nearest_in_leaf(typename Tree::Leaf_node_const_handle node);
+
+  // With cache
+  template<>
+  void search_nearest_in_leaf<true>(typename Tree::Leaf_node_const_handle node)
+  {
+    typename Tree::iterator it_node_point = node->begin(), it_node_point_end = node->end();
+    typename std::vector<FT>::const_iterator cache_point_begin = m_tree.cache_begin() + m_dim*(it_node_point - m_tree.begin());
+    // As long as the queue is not full, the node is just inserted
+    for (; !this->queue.full() && it_node_point != it_node_point_end; ++it_node_point)
+    {
+      this->number_of_items_visited++;
+
+      FT distance_to_query_object =
+        m_distance_helper.transformed_distance_from_coordinates(
+          this->query_object, *it_node_point, cache_point_begin, cache_point_begin + m_dim);
+      this->queue.insert(std::make_pair(&(*it_node_point), distance_to_query_object));
+
+      cache_point_begin += m_dim;
+    }
+    // Now that the queue is full, we can gain time by keeping track
+    // of the current worst distance to interrupt the distance computation earlier
+    FT worst_dist = this->queue.top().second;
+    for (; it_node_point != it_node_point_end; ++it_node_point)
+    {
+      this->number_of_items_visited++;
+
+      FT distance_to_query_object =
+        m_distance_helper.interruptible_transformed_distance(
+          this->query_object, *it_node_point, cache_point_begin, cache_point_begin + m_dim, worst_dist);
+
+      if (distance_to_query_object < worst_dist)
+      {
+        this->queue.insert(std::make_pair(&(*it_node_point), distance_to_query_object));
+        worst_dist = this->queue.top().second;
+      }
+
+      cache_point_begin += m_dim;
+    }
+  }
+
+  // Without cache
+  template<>
+  void search_nearest_in_leaf<false>(typename Tree::Leaf_node_const_handle node)
+  {
+    typename Tree::iterator it_node_point = node->begin(), it_node_point_end = node->end();
+    // As long as the queue is not full, the node is just inserted
+    for (; !this->queue.full() && it_node_point != it_node_point_end; ++it_node_point)
+    {
+      this->number_of_items_visited++;
+
+      FT distance_to_query_object =
+        this->distance_instance.transformed_distance(this->query_object, *it_node_point);
+      this->queue.insert(std::make_pair(&(*it_node_point), distance_to_query_object));
+    }
+    // Now that the queue is full, we can gain time by keeping track
+    // of the current worst distance to interrupt the distance computation earlier
+    FT worst_dist = this->queue.top().second;
+    for (; it_node_point != it_node_point_end; ++it_node_point)
+    {
+      this->number_of_items_visited++;
+
+      FT distance_to_query_object =
+        m_distance_helper.interruptible_transformed_distance(
+          this->query_object, *it_node_point, worst_dist);
+
+      if (distance_to_query_object < worst_dist)
+      {
+        this->queue.insert(std::make_pair(&(*it_node_point), distance_to_query_object));
+        worst_dist = this->queue.top().second;
+      }
+    }
+  }
+
+
+  template<bool use_cache>
+  void search_furthest_in_leaf(typename Tree::Leaf_node_const_handle node);
+
+  // With cache
+  template<>
+  void search_furthest_in_leaf<true>(typename Tree::Leaf_node_const_handle node)
+  {
+    typename Tree::iterator it_node_point = node->begin(), it_node_point_end = node->end();
+    typename std::vector<FT>::const_iterator cache_point_begin = m_tree.cache_begin() + m_dim*(it_node_point - m_tree.begin());
+    // In furthest search mode, the interruptible distance cannot be used to optimize
+    for (; it_node_point != it_node_point_end; ++it_node_point)
+    {
+      this->number_of_items_visited++;
+
+      FT distance_to_query_object =
+        m_distance_helper.transformed_distance_from_coordinates(
+          this->query_object, *it_node_point, cache_point_begin, cache_point_begin + m_dim);
+      this->queue.insert(std::make_pair(&(*it_node_point), distance_to_query_object));
+
+      cache_point_begin += m_dim;
+    }
+  }
+
+  // Without cache
+  template<>
+  void search_furthest_in_leaf<false>(typename Tree::Leaf_node_const_handle node)
+  {
+    typename Tree::iterator it_node_point = node->begin(), it_node_point_end = node->end();
+    // In furthest search mode, the interruptible distance cannot be used to optimize
+    for (; it_node_point != it_node_point_end; ++it_node_point)
+    {
+      this->number_of_items_visited++;
+
+      FT distance_to_query_object =
+        this->distance_instance.transformed_distance(this->query_object, *it_node_point);
+      this->queue.insert(std::make_pair(&(*it_node_point), distance_to_query_object));
+    }
+  }
+
 }; // class 
 
 } // namespace CGAL