dernière version du kd_tree

git-svn-id: svn+ssh://scm.gforge.inria.fr/svnroot/gudhi/branches/bottleneck_integration@1785 636b058d-ea47-450e-bf9e-a15bfbe3eedb

Former-commit-id: a9c6dd3085c1a6a1f48fe624394f662b7f579781

2 files changed, 655 insertions, 571 deletions

diff --git a/src/Bottleneck_distance/include/gudhi/CGAL/Kd_tree.h b/src/Bottleneck_distance/include/gudhi/CGAL/Kd_tree.h
index 9db9b8b6..16043b76 100644
--- a/src/Bottleneck_distance/include/gudhi/CGAL/Kd_tree.h
+++ b/src/Bottleneck_distance/include/gudhi/CGAL/Kd_tree.h
@@ -47,465 +47,533 @@ template <class SearchTraits, class Splitter_=Sliding_midpoint<SearchTraits>, cl
 class Kd_tree {
 
 public:
-    typedef SearchTraits Traits;
-    typedef Splitter_ Splitter;
-    typedef typename SearchTraits::Point_d Point_d;
-    typedef typename Splitter::Container Point_container;
-
-    typedef typename SearchTraits::FT FT;
-    typedef Kd_tree_node<SearchTraits, Splitter, UseExtendedNode > Node;
-    typedef Kd_tree_leaf_node<SearchTraits, Splitter, UseExtendedNode > Leaf_node;
-    typedef Kd_tree_internal_node<SearchTraits, Splitter, UseExtendedNode > Internal_node;
-    typedef Kd_tree<SearchTraits, Splitter> Tree;
-    typedef Kd_tree<SearchTraits, Splitter,UseExtendedNode> Self;
-
-    typedef Node* Node_handle;
-    typedef const Node* Node_const_handle;
-    typedef Leaf_node* Leaf_node_handle;
-    typedef const Leaf_node* Leaf_node_const_handle;
-    typedef Internal_node* Internal_node_handle;
-    typedef const Internal_node* Internal_node_const_handle;
-    typedef typename std::vector<const Point_d*>::const_iterator Point_d_iterator;
-    typedef typename std::vector<const Point_d*>::const_iterator Point_d_const_iterator;
-    typedef typename Splitter::Separator Separator;
-    typedef typename std::vector<Point_d>::const_iterator iterator;
-    typedef typename std::vector<Point_d>::const_iterator const_iterator;
-
-    typedef typename std::vector<Point_d>::size_type size_type;
-
-    typedef typename internal::Get_dimension_tag<SearchTraits>::Dimension D;
+  typedef SearchTraits Traits;
+  typedef Splitter_ Splitter;
+  typedef typename SearchTraits::Point_d Point_d;
+  typedef typename Splitter::Container Point_container;
+
+  typedef typename SearchTraits::FT FT;
+  typedef Kd_tree_node<SearchTraits, Splitter, UseExtendedNode > Node;
+  typedef Kd_tree_leaf_node<SearchTraits, Splitter, UseExtendedNode > Leaf_node;
+  typedef Kd_tree_internal_node<SearchTraits, Splitter, UseExtendedNode > Internal_node;
+  typedef Kd_tree<SearchTraits, Splitter> Tree;
+  typedef Kd_tree<SearchTraits, Splitter,UseExtendedNode> Self;
+
+  typedef Node* Node_handle;
+  typedef const Node* Node_const_handle;
+  typedef Leaf_node* Leaf_node_handle;
+  typedef const Leaf_node* Leaf_node_const_handle;
+  typedef Internal_node* Internal_node_handle;
+  typedef const Internal_node* Internal_node_const_handle;
+  typedef typename std::vector<const Point_d*>::const_iterator Point_d_iterator;
+  typedef typename std::vector<const Point_d*>::const_iterator Point_d_const_iterator;
+  typedef typename Splitter::Separator Separator;
+  typedef typename std::vector<Point_d>::const_iterator iterator;
+  typedef typename std::vector<Point_d>::const_iterator const_iterator;
+
+  typedef typename std::vector<Point_d>::size_type size_type;
+
+  typedef typename internal::Get_dimension_tag<SearchTraits>::Dimension D;
 
 private:
-    SearchTraits traits_;
-    Splitter split;
+  SearchTraits traits_;
+  Splitter split;
 
 
-    // wokaround for https://svn.boost.org/trac/boost/ticket/9332
+  // wokaround for https://svn.boost.org/trac/boost/ticket/9332
 #if   (_MSC_VER == 1800) && (BOOST_VERSION == 105500)
-    std::deque<Internal_node> internal_nodes;
-    std::deque<Leaf_node> leaf_nodes;
+  std::deque<Internal_node> internal_nodes;
+  std::deque<Leaf_node> leaf_nodes;
 #else
-    boost::container::deque<Internal_node> internal_nodes;
-    boost::container::deque<Leaf_node> leaf_nodes;
+  boost::container::deque<Internal_node> internal_nodes;
+  boost::container::deque<Leaf_node> leaf_nodes;
 #endif
 
-    Node_handle tree_root;
+  Node_handle tree_root;
 
-    Kd_tree_rectangle<FT,D>* bbox;
-    std::vector<Point_d> pts;
+  Kd_tree_rectangle<FT,D>* bbox;
+  std::vector<Point_d> pts;
 
-    // Instead of storing the points in arrays in the Kd_tree_node
-    // we put all the data in a vector in the Kd_tree.
-    // and we only store an iterator range in the Kd_tree_node.
-    //
-    std::vector<const Point_d*> data;
+  // Instead of storing the points in arrays in the Kd_tree_node
+  // we put all the data in a vector in the Kd_tree.
+  // and we only store an iterator range in the Kd_tree_node.
+  //
+  std::vector<const Point_d*> data;
 
 
-#ifdef CGAL_HAS_THREADS
-    mutable CGAL_MUTEX building_mutex;//mutex used to protect const calls inducing build()
-#endif
-    bool built_;
-    bool removed_;
+  #ifdef CGAL_HAS_THREADS
+  mutable CGAL_MUTEX building_mutex;//mutex used to protect const calls inducing build()
+  #endif
+  bool built_;
+  bool removed_;
 
-    // protected copy constructor
-    Kd_tree(const Tree& tree)
-        : traits_(tree.traits_),built_(tree.built_)
-    {};
+  // protected copy constructor
+  Kd_tree(const Tree& tree)
+    : traits_(tree.traits_),built_(tree.built_)
+  {};
 
 
-    // Instead of the recursive construction of the tree in the class Kd_tree_node
-    // we do this in the tree class. The advantage is that we then can optimize
-    // the allocation of the nodes.
+  // Instead of the recursive construction of the tree in the class Kd_tree_node
+  // we do this in the tree class. The advantage is that we then can optimize
+  // the allocation of the nodes.
 
-    // The leaf node
-    Node_handle
-    create_leaf_node(Point_container& c)
-    {
-        Leaf_node node(true , static_cast<unsigned int>(c.size()));
-        std::ptrdiff_t tmp = c.begin() - data.begin();
-        node.data = pts.begin() + tmp;
+  // The leaf node
+  Node_handle
+  create_leaf_node(Point_container& c)
+  {
+    Leaf_node node(true , static_cast<unsigned int>(c.size()));
+    std::ptrdiff_t tmp = c.begin() - data.begin();
+    node.data = pts.begin() + tmp;
 
-        leaf_nodes.push_back(node);
-        Leaf_node_handle nh = &leaf_nodes.back();
+    leaf_nodes.push_back(node);
+    Leaf_node_handle nh = &leaf_nodes.back();
 
 
-        return nh;
-    }
+    return nh;
+  }
 
 
-    // The internal node
+  // The internal node
 
-    Node_handle
-    create_internal_node(Point_container& c, const Tag_true&)
-    {
-        return create_internal_node_use_extension(c);
-    }
+  Node_handle
+  create_internal_node(Point_container& c, const Tag_true&)
+  {
+    return create_internal_node_use_extension(c);
+  }
 
-    Node_handle
-    create_internal_node(Point_container& c, const Tag_false&)
-    {
-        return create_internal_node(c);
-    }
+  Node_handle
+  create_internal_node(Point_container& c, const Tag_false&)
+  {
+    return create_internal_node(c);
+  }
 
 
 
-    // TODO: Similiar to the leaf_init function above, a part of the code should be
-    //       moved to a the class Kd_tree_node.
-    //       It is not proper yet, but the goal was to see if there is
-    //       a potential performance gain through the Compact_container
-    Node_handle
-    create_internal_node_use_extension(Point_container& c)
-    {
-        Internal_node node(false);
-        internal_nodes.push_back(node);
-        Internal_node_handle nh = &internal_nodes.back();
+  // TODO: Similiar to the leaf_init function above, a part of the code should be
+  //       moved to a the class Kd_tree_node.
+  //       It is not proper yet, but the goal was to see if there is
+  //       a potential performance gain through the Compact_container
+  Node_handle
+  create_internal_node_use_extension(Point_container& c)
+  {
+    Internal_node node(false);
+    internal_nodes.push_back(node);
+    Internal_node_handle nh = &internal_nodes.back();
 
-        Separator sep;
-        Point_container c_low(c.dimension(),traits_);
-        split(sep, c, c_low);
-        nh->set_separator(sep);
+    Separator sep;
+    Point_container c_low(c.dimension(),traits_);
+    split(sep, c, c_low);
+    nh->set_separator(sep);
 
-        int cd  = nh->cutting_dimension();
-        if(!c_low.empty())
-            nh->low_val = c_low.tight_bounding_box().max_coord(cd);
-        else
-            nh->low_val = c_low.bounding_box().min_coord(cd);
-        if(!c.empty())
-            nh->high_val = c.tight_bounding_box().min_coord(cd);
-        else
-            nh->high_val = c.bounding_box().max_coord(cd);
+    int cd  = nh->cutting_dimension();
+    if(!c_low.empty()){
+      nh->lower_low_val = c_low.tight_bounding_box().min_coord(cd);
+      nh->lower_high_val = c_low.tight_bounding_box().max_coord(cd);
+    }
+    else{
+      nh->lower_low_val = nh->cutting_value();
+      nh->lower_high_val = nh->cutting_value();
+    }
+    if(!c.empty()){
+      nh->upper_low_val = c.tight_bounding_box().min_coord(cd);
+      nh->upper_high_val = c.tight_bounding_box().max_coord(cd);
+    }
+    else{
+      nh->upper_low_val = nh->cutting_value();
+      nh->upper_high_val = nh->cutting_value();
+    }
 
-        CGAL_assertion(nh->cutting_value() >= nh->low_val);
-        CGAL_assertion(nh->cutting_value() <= nh->high_val);
+    CGAL_assertion(nh->cutting_value() >= nh->lower_low_val);
+    CGAL_assertion(nh->cutting_value() <= nh->upper_high_val);
 
-        if (c_low.size() > split.bucket_size()){
-            nh->lower_ch = create_internal_node_use_extension(c_low);
-        }else{
-            nh->lower_ch = create_leaf_node(c_low);
-        }
-        if (c.size() > split.bucket_size()){
-            nh->upper_ch = create_internal_node_use_extension(c);
-        }else{
-            nh->upper_ch = create_leaf_node(c);
-        }
+    if (c_low.size() > split.bucket_size()){
+      nh->lower_ch = create_internal_node_use_extension(c_low);
+    }else{
+      nh->lower_ch = create_leaf_node(c_low);
+    }
+    if (c.size() > split.bucket_size()){
+      nh->upper_ch = create_internal_node_use_extension(c);
+    }else{
+      nh->upper_ch = create_leaf_node(c);
+    }
 
 
 
 
-        return nh;
-    }
+    return nh;
+  }
 
 
-    // Note also that I duplicated the code to get rid if the if's for
-    // the boolean use_extension which was constant over the construction
-    Node_handle
-    create_internal_node(Point_container& c)
-    {
-        Internal_node node(false);
-        internal_nodes.push_back(node);
-        Internal_node_handle nh = &internal_nodes.back();
-        Separator sep;
+  // Note also that I duplicated the code to get rid if the if's for
+  // the boolean use_extension which was constant over the construction
+  Node_handle
+  create_internal_node(Point_container& c)
+  {
+    Internal_node node(false);
+    internal_nodes.push_back(node);
+    Internal_node_handle nh = &internal_nodes.back();
+    Separator sep;
 
-        Point_container c_low(c.dimension(),traits_);
-        split(sep, c, c_low);
-        nh->set_separator(sep);
+    Point_container c_low(c.dimension(),traits_);
+    split(sep, c, c_low);
+    nh->set_separator(sep);
 
-        if (c_low.size() > split.bucket_size()){
-            nh->lower_ch = create_internal_node(c_low);
-        }else{
-            nh->lower_ch = create_leaf_node(c_low);
-        }
-        if (c.size() > split.bucket_size()){
-            nh->upper_ch = create_internal_node(c);
-        }else{
-            nh->upper_ch = create_leaf_node(c);
-        }
+    if (c_low.size() > split.bucket_size()){
+      nh->lower_ch = create_internal_node(c_low);
+    }else{
+      nh->lower_ch = create_leaf_node(c_low);
+    }
+    if (c.size() > split.bucket_size()){
+      nh->upper_ch = create_internal_node(c);
+    }else{
+      nh->upper_ch = create_leaf_node(c);
+    }
 
 
 
-        return nh;
-    }
+    return nh;
+  }
 
 
 
 public:
 
-    Kd_tree(Splitter s = Splitter(),const SearchTraits traits=SearchTraits())
-        : traits_(traits),split(s), built_(false), removed_(false)
-    {}
-
-    template <class InputIterator>
-    Kd_tree(InputIterator first, InputIterator beyond,
-            Splitter s = Splitter(),const SearchTraits traits=SearchTraits())
-        : traits_(traits),split(s), built_(false), removed_(false)
-    {
-        pts.insert(pts.end(), first, beyond);
+  Kd_tree(Splitter s = Splitter(),const SearchTraits traits=SearchTraits())
+    : traits_(traits),split(s), built_(false), removed_(false)
+  {}
+
+  template <class InputIterator>
+  Kd_tree(InputIterator first, InputIterator beyond,
+          Splitter s = Splitter(),const SearchTraits traits=SearchTraits())
+    : traits_(traits),split(s), built_(false), removed_(false)
+  {
+    pts.insert(pts.end(), first, beyond);
+  }
+
+  bool empty() const {
+    return pts.empty();
+  }
+
+  void
+  build()
+  {
+    // This function is not ready to be called when a tree already exists, one
+    // must call invalidate_built() first.
+    CGAL_assertion(!is_built());
+    CGAL_assertion(!removed_);
+    const Point_d& p = *pts.begin();
+    typename SearchTraits::Construct_cartesian_const_iterator_d ccci=traits_.construct_cartesian_const_iterator_d_object();
+    int dim = static_cast<int>(std::distance(ccci(p), ccci(p,0)));
+
+    data.reserve(pts.size());
+    for(unsigned int i = 0; i < pts.size(); i++){
+      data.push_back(&pts[i]);
     }
-
-    bool empty() const {
-        return pts.empty();
+    Point_container c(dim, data.begin(), data.end(),traits_);
+    bbox = new Kd_tree_rectangle<FT,D>(c.bounding_box());
+    if (c.size() <= split.bucket_size()){
+      tree_root = create_leaf_node(c);
+    }else {
+      tree_root = create_internal_node(c, UseExtendedNode());
     }
 
-    void
-    build()
-    {
-        const Point_d& p = *pts.begin();
-        typename SearchTraits::Construct_cartesian_const_iterator_d ccci=traits_.construct_cartesian_const_iterator_d_object();
-        int dim = static_cast<int>(std::distance(ccci(p), ccci(p,0)));
-
-        data.reserve(pts.size());
-        for(unsigned int i = 0; i < pts.size(); i++){
-            data.push_back(&pts[i]);
-        }
-        Point_container c(dim, data.begin(), data.end(),traits_);
-        bbox = new Kd_tree_rectangle<FT,D>(c.bounding_box());
-        if (c.size() <= split.bucket_size()){
-            tree_root = create_leaf_node(c);
-        }else {
-            tree_root = create_internal_node(c, UseExtendedNode());
-        }
-
-        //Reorder vector for spatial locality
-        std::vector<Point_d> ptstmp;
-        ptstmp.resize(pts.size());
-        for (std::size_t i = 0; i < pts.size(); ++i){
-            ptstmp[i] = *data[i];
-        }
-        for(std::size_t i = 0; i < leaf_nodes.size(); ++i){
-            std::ptrdiff_t tmp = leaf_nodes[i].begin() - pts.begin();
-            leaf_nodes[i].data = ptstmp.begin() + tmp;
-        }
-        pts.swap(ptstmp);
-
-        data.clear();
-
-        built_ = true;
+    //Reorder vector for spatial locality
+    std::vector<Point_d> ptstmp;
+    ptstmp.resize(pts.size());
+    for (std::size_t i = 0; i < pts.size(); ++i){
+      ptstmp[i] = *data[i];
     }
-
-private:
-    //any call to this function is for the moment not threadsafe
-    void const_build() const {
-#ifdef CGAL_HAS_THREADS
-        //this ensure that build() will be called once
-        CGAL_SCOPED_LOCK(building_mutex);
-        if(!is_built())
-#endif
-            const_cast<Self*>(this)->build(); //THIS IS NOT THREADSAFE
+    for(std::size_t i = 0; i < leaf_nodes.size(); ++i){
+      std::ptrdiff_t tmp = leaf_nodes[i].begin() - pts.begin();
+      leaf_nodes[i].data = ptstmp.begin() + tmp;
     }
-public:
+    pts.swap(ptstmp);
 
-    bool is_built() const
-    {
-        return built_;
-    }
+    data.clear();
 
-    void invalidate_built()
-    {
-        if(is_built()){
-            internal_nodes.clear();
-            leaf_nodes.clear();
-            data.clear();
-            delete bbox;
-            built_ = false;
-        }
-    }
+    built_ = true;
+  }
 
-    void clear()
-    {
-        invalidate_built();
-        pts.clear();
-        removed_ = false;
-    }
+private:
+  //any call to this function is for the moment not threadsafe
+  void const_build() const {
+    #ifdef CGAL_HAS_THREADS
+    //this ensure that build() will be called once
+    CGAL_SCOPED_LOCK(building_mutex);
+    if(!is_built())
+    #endif
+      const_cast<Self*>(this)->build(); //THIS IS NOT THREADSAFE
+  }
+public:
 
-    void
-    insert(const Point_d& p)
-    {
-        if (removed_) throw std::logic_error("Once you start removing points, you cannot insert anymore, you need to start again from scratch.");
-        invalidate_built();
-        pts.push_back(p);
+  bool is_built() const
+  {
+    return built_;
+  }
+
+  void invalidate_built()
+  {
+    if(removed_){
+      // Walk the tree to collect the remaining points.
+      // Writing directly to pts would likely work, but better be safe.
+      std::vector<Point_d> ptstmp;
+      //ptstmp.resize(root()->num_items());
+      root()->tree_items(std::back_inserter(ptstmp));
+      pts.swap(ptstmp);
+      removed_=false;
+      CGAL_assertion(is_built()); // the rest of the cleanup must happen
     }
-
-    template <class InputIterator>
-    void
-    insert(InputIterator first, InputIterator beyond)
-    {
-        if (removed_ && first != beyond) throw std::logic_error("Once you start removing points, you cannot insert anymore, you need to start again from scratch.");
-        invalidate_built();
-        pts.insert(pts.end(),first, beyond);
+    if(is_built()){
+      internal_nodes.clear();
+      leaf_nodes.clear();
+      data.clear();
+      delete bbox;
+      built_ = false;
     }
+  }
+
+  void clear()
+  {
+    invalidate_built();
+    pts.clear();
+    removed_ = false;
+  }
+
+  void
+  insert(const Point_d& p)
+  {
+    invalidate_built();
+    pts.push_back(p);
+  }
+
+  template <class InputIterator>
+  void
+  insert(InputIterator first, InputIterator beyond)
+  {
+    invalidate_built();
+    pts.insert(pts.end(),first, beyond);
+  }
 
-    void
-    remove(const Point_d& p)
-    {
-        // This does not actually remove points, and further insertions
-        // would make the points reappear, so we disallow it.
-        removed_ = true;
-        // Locate the point
-        Internal_node_handle grandparent = 0;
-        Internal_node_handle parent = 0;
-        bool islower = false, islower2;
-        Node_handle node = root(); // Calls build() if needed.
-        while (!node->is_leaf()) {
-            grandparent = parent; islower2 = islower;
-            parent = static_cast<Internal_node_handle>(node);
-            islower = traits().construct_cartesian_const_iterator_d_object()(p)[parent->cutting_dimension()] < parent->cutting_value();
-            if (islower) {
-                node = parent->lower();
-            } else {
-                node = parent->upper();
-            }
-        }
-        Leaf_node_handle lnode = static_cast<Leaf_node_handle>(node);
-        if (lnode->size() > 1) {
-            iterator pi = std::find(lnode->begin(), lnode->end(), p);
-            CGAL_assertion (pi != lnode->end());
-            iterator lasti = lnode->end() - 1;
-            if (pi != lasti) {
-                // Hack to get a non-const iterator
-                std::iter_swap(pts.begin()+(pi-pts.begin()), pts.begin()+(lasti-pts.begin()));
-            }
-            lnode->drop_last_point();
-        } else if (grandparent) {
-             CGAL_assertion (p == *lnode->begin());
-            Node_handle brother = islower ? parent->upper() : parent->lower();
-            if (islower2)
-                grandparent->set_lower(brother);
-            else
-                grandparent->set_upper(brother);
-        } else if (parent) {
-            tree_root = islower ? parent->upper() : parent->lower();
-        } else {
-            clear();
-        }
+private:
+  struct Equal_by_coordinates {
+    SearchTraits const* traits;
+    Point_d const* pp;
+    bool operator()(Point_d const&q) const {
+      typename SearchTraits::Construct_cartesian_const_iterator_d ccci=traits->construct_cartesian_const_iterator_d_object();
+      return std::equal(ccci(*pp), ccci(*pp,0), ccci(q));
     }
+  };
+  Equal_by_coordinates equal_by_coordinates(Point_d const&p){
+    Equal_by_coordinates ret = { &traits(), &p };
+    return ret;
+  }
 
-    //For efficiency; reserve the size of the points vectors in advance (if the number of points is already known).
-    void reserve(size_t size)
-    {
-        pts.reserve(size);
+public:
+  void
+  remove(const Point_d& p)
+  {
+    remove(p, equal_by_coordinates(p));
+  }
+
+  template<class Equal>
+  void
+  remove(const Point_d& p, Equal const& equal_to_p)
+  {
+#if 0
+    // This code could have quadratic runtime.
+    if (!is_built()) {
+      std::vector<Point_d>::iterator pi = std::find(pts.begin(), pts.end(), p);
+      // Precondition: the point must be there.
+      CGAL_assertion (pi != pts.end());
+      pts.erase(pi);
+      return;
     }
-
-    //Get the capacity of the underlying points vector.
-    size_t capacity()
-    {
-        return pts.capacity();
+#endif
+    // This does not actually remove points, and further insertions
+    // would make the points reappear, so we disallow it.
+    removed_ = true;
+
+    CGAL_assertion_code(bool success = )
+    remove_(p, 0, false, 0, false, root(), equal);
+    CGAL_assertion(success);
+  }
+private:
+  template<class Equal>
+  bool remove_(const Point_d& p,
+      Internal_node_handle grandparent, bool islower,
+      Internal_node_handle parent, bool islower2,
+      Node_handle node, Equal const& equal_to_p) {
+    // Recurse to locate the point
+    if (!node->is_leaf()) {
+      Internal_node_handle newparent = static_cast<Internal_node_handle>(node);
+      // FIXME: This should be if(x<y) remove low; else remove up;
+      if (traits().construct_cartesian_const_iterator_d_object()(p)[newparent->cutting_dimension()] <= newparent->cutting_value()) {
+        if (remove_(p, parent, islower2, newparent, true, newparent->lower(), equal_to_p))
+          return true;
+      }
+      //if (traits().construct_cartesian_const_iterator_d_object()(p)[newparent->cutting_dimension()] >= newparent->cutting_value())
+        return remove_(p, parent, islower2, newparent, false, newparent->upper(), equal_to_p);
+
+      CGAL_assertion(false); // Point was not found
     }
 
-
-    template <class OutputIterator, class FuzzyQueryItem>
-    OutputIterator
-    search(OutputIterator it, const FuzzyQueryItem& q) const
-    {
-        if(! pts.empty()){
-
-            if(! is_built()){
-                const_build();
-            }
-            Kd_tree_rectangle<FT,D> b(*bbox);
-            return tree_root->search(it,q,b);
-        }
-        return it;
+    // Actual removal
+    Leaf_node_handle lnode = static_cast<Leaf_node_handle>(node);
+    if (lnode->size() > 1) {
+      iterator pi = std::find_if(lnode->begin(), lnode->end(), equal_to_p);
+      // FIXME: we should ensure this never happens
+      if (pi == lnode->end()) return false;
+      iterator lasti = lnode->end() - 1;
+      if (pi != lasti) {
+        // Hack to get a non-const iterator
+        std::iter_swap(pts.begin()+(pi-pts.begin()), pts.begin()+(lasti-pts.begin()));
+      }
+      lnode->drop_last_point();
+    } else if (!equal_to_p(*lnode->begin())) {
+      // FIXME: we should ensure this never happens
+      return false;
+    } else if (grandparent) {
+      Node_handle brother = islower ? parent->upper() : parent->lower();
+      if (islower2)
+        grandparent->set_lower(brother);
+      else
+        grandparent->set_upper(brother);
+    } else if (parent) {
+      tree_root = islower ? parent->upper() : parent->lower();
+    } else {
+      clear();
     }
+    return true;
+  }
 
-
-    template <class FuzzyQueryItem>
-    boost::optional<Point_d>
-    search_any_point(const FuzzyQueryItem& q) const
-    {
-        if(! pts.empty()){
-
-            if(! is_built()){
-                const_build();
-            }
-            Kd_tree_rectangle<FT,D> b(*bbox);
-            return tree_root->search_any_point(q,b);
-        }
-        return boost::none;
+public:
+  //For efficiency; reserve the size of the points vectors in advance (if the number of points is already known).
+  void reserve(size_t size)
+  {
+    pts.reserve(size);
+  }
+
+  //Get the capacity of the underlying points vector.
+  size_t capacity()
+  {
+    return pts.capacity();
+  }
+
+
+  template <class OutputIterator, class FuzzyQueryItem>
+  OutputIterator
+  search(OutputIterator it, const FuzzyQueryItem& q) const
+  {
+    if(! pts.empty()){
+
+      if(! is_built()){
+        const_build();
+      }
+      Kd_tree_rectangle<FT,D> b(*bbox);
+      return tree_root->search(it,q,b);
     }
+    return it;
+  }
 
 
-    ~Kd_tree() {
-        if(is_built()){
-            delete bbox;
-        }
-    }
-
+  template <class FuzzyQueryItem>
+  boost::optional<Point_d>
+  search_any_point(const FuzzyQueryItem& q) const
+  {
+    if(! pts.empty()){
 
-    const SearchTraits&
-    traits() const
-    {
-        return traits_;
+      if(! is_built()){
+        const_build();
+      }
+      Kd_tree_rectangle<FT,D> b(*bbox);
+      return tree_root->search_any_point(q,b);
     }
+    return boost::none;
+  }
 
-    Node_const_handle
-    root() const
-    {
-        if(! is_built()){
-            const_build();
-        }
-        return tree_root;
-    }
 
-    Node_handle
-    root()
-    {
-        if(! is_built()){
-            build();
-        }
-        return tree_root;
+  ~Kd_tree() {
+    if(is_built()){
+      delete bbox;
     }
+  }
 
-    void
-    print() const
-    {
-        if(! is_built()){
-            const_build();
-        }
-        root()->print();
-    }
 
-    const Kd_tree_rectangle<FT,D>&
-    bounding_box() const
-    {
-        if(! is_built()){
-            const_build();
-        }
-        return *bbox;
-    }
+  const SearchTraits&
+  traits() const
+  {
+    return traits_;
+  }
 
-    const_iterator
-    begin() const
-    {
-        return pts.begin();
+  Node_const_handle
+  root() const
+  {
+    if(! is_built()){
+      const_build();
     }
-
-    const_iterator
-    end() const
-    {
-        return pts.end();
+    return tree_root;
+  }
+
+  Node_handle
+  root()
+  {
+    if(! is_built()){
+      build();
     }
-
-    size_type
-    size() const
-    {
-        return pts.size();
+    return tree_root;
+  }
+
+  void
+  print() const
+  {
+    if(! is_built()){
+      const_build();
     }
-
-    // Print statistics of the tree.
-    std::ostream&
-    statistics(std::ostream& s) const
-    {
-        if(! is_built()){
-            const_build();
-        }
-        s << "Tree statistics:" << std::endl;
-        s << "Number of items stored: "
-          << root()->num_items() << std::endl;
-        s << "Number of nodes: "
-          << root()->num_nodes() << std::endl;
-        s << " Tree depth: " << root()->depth() << std::endl;
-        return s;
+    root()->print();
+  }
+
+  const Kd_tree_rectangle<FT,D>&
+  bounding_box() const
+  {
+    if(! is_built()){
+      const_build();
+    }
+    return *bbox;
+  }
+
+  const_iterator
+  begin() const
+  {
+    return pts.begin();
+  }
+
+  const_iterator
+  end() const
+  {
+    return pts.end();
+  }
+
+  size_type
+  size() const
+  {
+    return pts.size();
+  }
+
+  // Print statistics of the tree.
+  std::ostream&
+  statistics(std::ostream& s) const
+  {
+    if(! is_built()){
+      const_build();
     }
+    s << "Tree statistics:" << std::endl;
+    s << "Number of items stored: "
+      << root()->num_items() << std::endl;
+    s << "Number of nodes: "
+      << root()->num_nodes() << std::endl;
+    s << " Tree depth: " << root()->depth() << std::endl;
+    return s;
+  }
 
 
 };
diff --git a/src/Bottleneck_distance/include/gudhi/CGAL/Kd_tree_node.h b/src/Bottleneck_distance/include/gudhi/CGAL/Kd_tree_node.h
index d6d01439..49b0c022 100644
--- a/src/Bottleneck_distance/include/gudhi/CGAL/Kd_tree_node.h
+++ b/src/Bottleneck_distance/include/gudhi/CGAL/Kd_tree_node.h
@@ -28,10 +28,10 @@
 
 namespace CGAL {
 
-  template <class SearchTraits, class Splitter, class UseExtendedNode> 
+  template <class SearchTraits, class Splitter, class UseExtendedNode>
   class Kd_tree;
 
-  template < class TreeTraits, class Splitter, class UseExtendedNode > 
+  template < class TreeTraits, class Splitter, class UseExtendedNode >
   class Kd_tree_node {
 
      friend class Kd_tree<TreeTraits,Splitter,UseExtendedNode>;
@@ -52,7 +52,7 @@ namespace CGAL {
 
     bool leaf;
 
-  public : 
+  public :
     Kd_tree_node(bool leaf_)
       :leaf(leaf_){}
 
@@ -60,18 +60,18 @@ namespace CGAL {
       return leaf;
     }
 
-    std::size_t 
+    std::size_t
     num_items() const
     {
       if (is_leaf()){
-        Leaf_node_const_handle node = 
+        Leaf_node_const_handle node =
           static_cast<Leaf_node_const_handle>(this);
         return node->size();
       }
       else {
-        Internal_node_const_handle node = 
+        Internal_node_const_handle node =
           static_cast<Internal_node_const_handle>(this);
-	return node->lower()->num_items() + node->upper()->num_items();
+        return node->lower()->num_items() + node->upper()->num_items();
       }
     }
 
@@ -80,48 +80,48 @@ namespace CGAL {
     {
       if (is_leaf()) return 1;
       else {
-        Internal_node_const_handle node = 
+        Internal_node_const_handle node =
           static_cast<Internal_node_const_handle>(this);
-	return node->lower()->num_nodes() + node->upper()->num_nodes();
+        return node->lower()->num_nodes() + node->upper()->num_nodes();
       }
     }
 
-    int 
+    int
     depth(const int current_max_depth) const
     {
       if (is_leaf()){
-	return current_max_depth;
+        return current_max_depth;
       }
       else {
-        Internal_node_const_handle node = 
+        Internal_node_const_handle node =
           static_cast<Internal_node_const_handle>(this);
-        return 
-	     (std::max)( node->lower()->depth(current_max_depth + 1),
-			 node->upper()->depth(current_max_depth + 1));
+        return
+             (std::max)( node->lower()->depth(current_max_depth + 1),
+                         node->upper()->depth(current_max_depth + 1));
       }
     }
 
-    int 
+    int
     depth() const
     {
-      return depth(1); 
+      return depth(1);
     }
 
     template <class OutputIterator>
-    OutputIterator 
+    OutputIterator
     tree_items(OutputIterator it) const {
       if (is_leaf()) {
-         Leaf_node_const_handle node = 
+         Leaf_node_const_handle node =
           static_cast<Leaf_node_const_handle>(this);
-	 if (node->size()>0) 
-	    for (iterator i=node->begin(); i != node->end(); i++) 
-	      {*it=*i; ++it;} 
-	}
+         if (node->size()>0)
+            for (iterator i=node->begin(); i != node->end(); i++)
+              {*it=*i; ++it;}
+        }
       else {
-         Internal_node_const_handle node = 
+         Internal_node_const_handle node =
           static_cast<Internal_node_const_handle>(this);
-	  it=node->lower()->tree_items(it);  
-	  it=node->upper()->tree_items(it); 
+          it=node->lower()->tree_items(it);
+          it=node->upper()->tree_items(it);
       }
       return it;
     }
@@ -133,14 +133,14 @@ namespace CGAL {
       if (is_leaf()) {
          Leaf_node_const_handle node =
           static_cast<Leaf_node_const_handle>(this);
-	 if (node->size()>0){
+         if (node->size()>0){
            return boost::make_optional(*(node->begin()));
          }
-	}
+        }
       else {
          Internal_node_const_handle node =
           static_cast<Internal_node_const_handle>(this);
-	  result = node->lower()->any_tree_item();
+          result = node->lower()->any_tree_item();
           if(! result){
             result = node->upper()->any_tree_item();
           }
@@ -149,75 +149,75 @@ namespace CGAL {
     }
 
 
-     void 
+     void
     indent(int d) const
     {
       for(int i = 0; i < d; i++){
-	std::cout << " ";
+        std::cout << " ";
       }
     }
 
 
-    void 
-    print(int d = 0) const 
+    void
+    print(int d = 0) const
     {
       if (is_leaf()) {
-        Leaf_node_const_handle node = 
+        Leaf_node_const_handle node =
           static_cast<Leaf_node_const_handle>(this);
-	indent(d);
-	std::cout << "leaf" << std::endl;
-	if (node->size()>0)
-	  for (iterator i=node->begin(); i != node->end(); i++)
-	  {indent(d);std::cout << *i << std::endl;}
+        indent(d);
+        std::cout << "leaf" << std::endl;
+        if (node->size()>0)
+          for (iterator i=node->begin(); i != node->end(); i++)
+          {indent(d);std::cout << *i << std::endl;}
       }
       else {
-        Internal_node_const_handle node = 
+        Internal_node_const_handle node =
           static_cast<Internal_node_const_handle>(this);
-	indent(d);
-	std::cout << "lower tree" << std::endl;
-	node->lower()->print(d+1);
-	indent(d);
-	std::cout << "separator: dim = " << node->cutting_dimension() << "  val = " << node->cutting_value() << std::endl;
-	indent(d);
-	std::cout << "upper tree" << std::endl;
-	node->upper()->print(d+1); 
+        indent(d);
+        std::cout << "lower tree" << std::endl;
+        node->lower()->print(d+1);
+        indent(d);
+        std::cout << "separator: dim = " << node->cutting_dimension() << "  val = " << node->cutting_value() << std::endl;
+        indent(d);
+        std::cout << "upper tree" << std::endl;
+        node->upper()->print(d+1);
       }
     }
 
 
     template <class OutputIterator, class FuzzyQueryItem>
-    OutputIterator 
+    OutputIterator
     search(OutputIterator it, const FuzzyQueryItem& q,
-	   Kd_tree_rectangle<FT,D>& b) const
+           Kd_tree_rectangle<FT,D>& b) const
     {
-      if (is_leaf()) { 
-        Leaf_node_const_handle node = 
+      if (is_leaf()) {
+        Leaf_node_const_handle node =
           static_cast<Leaf_node_const_handle>(this);
-	if (node->size()>0) 
-	  for (iterator i=node->begin(); i != node->end(); i++) 
-	    if (q.contains(*i)) 
-	      {*it++=*i;}
+        if (node->size()>0)
+          for (iterator i=node->begin(); i != node->end(); i++)
+            if (q.contains(*i))
+              {*it++=*i;}
       }
       else {
-         Internal_node_const_handle node = 
+         Internal_node_const_handle node =
           static_cast<Internal_node_const_handle>(this);
-	// after splitting b denotes the lower part of b
-	Kd_tree_rectangle<FT,D> b_upper(b);
-	b.split(b_upper, node->cutting_dimension(),
-		node->cutting_value());
-                             
-	if (q.outer_range_contains(b)) 	
-	  it=node->lower()->tree_items(it);
-	else
-	  if (q.inner_range_intersects(b)) 
-	    it=node->lower()->search(it,q,b);
-	if  (q.outer_range_contains(b_upper))     
-	  it=node->upper()->tree_items(it);
-	else
-	  if (q.inner_range_intersects(b_upper)) 
-	    it=node->upper()->search(it,q,b_upper);
+        // after splitting b denotes the lower part of b
+        Kd_tree_rectangle<FT,D> b_upper(b);
+        b.split(b_upper, node->cutting_dimension(),
+                node->cutting_value());
+
+        if (q.outer_range_contains(b))
+          it=node->lower()->tree_items(it);
+        else
+          if (q.inner_range_intersects(b))
+            it=node->lower()->search(it,q,b);
+        if  (q.outer_range_contains(b_upper))
+          it=node->upper()->tree_items(it);
+        else
+          if (q.inner_range_intersects(b_upper))
+            it=node->upper()->search(it,q,b_upper);
       };
-      return it;				
+      return it;
     }
 
 
@@ -227,99 +227,99 @@ namespace CGAL {
                      Kd_tree_rectangle<FT,D>& b) const
     {
       boost::optional<Point_d> result = boost::none;
-      if (is_leaf()) { 
-        Leaf_node_const_handle node = 
+      if (is_leaf()) {
+        Leaf_node_const_handle node =
           static_cast<Leaf_node_const_handle>(this);
-	if (node->size()>0) 
-	  for (iterator i=node->begin(); i != node->end(); i++) 
-	    if (q.contains(*i)) 
-	      { result = *i; break; }
+        if (node->size()>0)
+          for (iterator i=node->begin(); i != node->end(); i++)
+            if (q.contains(*i))
+              { result = *i; break; }
       }
       else {
-         Internal_node_const_handle node = 
+         Internal_node_const_handle node =
           static_cast<Internal_node_const_handle>(this);
-	// after splitting b denotes the lower part of b
-	Kd_tree_rectangle<FT,D> b_upper(b);
-	b.split(b_upper, node->cutting_dimension(),
-		node->cutting_value());
-                             
-	if (q.outer_range_contains(b)){ 	
+        // after splitting b denotes the lower part of b
+        Kd_tree_rectangle<FT,D> b_upper(b);
+        b.split(b_upper, node->cutting_dimension(),
+                node->cutting_value());
+
+        if (q.outer_range_contains(b)){
           result = node->lower()->any_tree_item();
-	}else{
-	  if (q.inner_range_intersects(b)){ 
-	    result = node->lower()->search_any_point(q,b);
+        }else{
+          if (q.inner_range_intersects(b)){
+            result = node->lower()->search_any_point(q,b);
           }
         }
         if(result){
           return result;
         }
-	if  (q.outer_range_contains(b_upper)){     
-	  result = node->upper()->any_tree_item();
-	}else{
-	  if (q.inner_range_intersects(b_upper)) 
-	    result = node->upper()->search_any_point(q,b_upper);
+        if  (q.outer_range_contains(b_upper)){
+          result = node->upper()->any_tree_item();
+        }else{
+          if (q.inner_range_intersects(b_upper))
+            result = node->upper()->search_any_point(q,b_upper);
         }
       }
-      return result;				
+      return result;
     }
 
   };
 
 
-  template < class TreeTraits, class Splitter, class UseExtendedNode > 
+  template < class TreeTraits, class Splitter, class UseExtendedNode >
   class Kd_tree_leaf_node : public Kd_tree_node< TreeTraits, Splitter, UseExtendedNode >{
 
     friend class Kd_tree<TreeTraits,Splitter,UseExtendedNode>;
-    
+
     typedef typename Kd_tree<TreeTraits,Splitter,UseExtendedNode>::iterator iterator;
     typedef Kd_tree_node< TreeTraits, Splitter, UseExtendedNode> Base;
     typedef typename TreeTraits::Point_d Point_d;
 
   private:
-    
+
     // private variables for leaf nodes
     boost::int32_t n; // denotes number of items in a leaf node
     iterator data; // iterator to data in leaf node
 
-                    
+
   public:
-		
+
     // default constructor
-    Kd_tree_leaf_node() 
+    Kd_tree_leaf_node()
     {}
 
-    Kd_tree_leaf_node(bool leaf_ ) 
+    Kd_tree_leaf_node(bool leaf_ )
       : Base(leaf_)
     {}
 
-    Kd_tree_leaf_node(bool leaf_,unsigned int n_ ) 
+    Kd_tree_leaf_node(bool leaf_,unsigned int n_ )
       : Base(leaf_), n(n_)
     {}
 
     // members for all nodes
-   
+
     // members for leaf nodes only
-    inline 
-    unsigned int 
-    size() const 
-    { 
+    inline
+    unsigned int
+    size() const
+    {
       return n;
     }
-  
-    inline 
+
+    inline
     iterator
-    begin() const  
+    begin() const
     {
       return data;
     }
 
-    inline 
-    iterator 
-    end() const 
+    inline
+    iterator
+    end() const
     {
       return data + n;
     }
- 
+
     inline
     void
     drop_last_point()
@@ -331,7 +331,7 @@ namespace CGAL {
 
 
 
-  template < class TreeTraits, class Splitter, class UseExtendedNode> 
+  template < class TreeTraits, class Splitter, class UseExtendedNode>
   class Kd_tree_internal_node : public Kd_tree_node< TreeTraits, Splitter, UseExtendedNode >{
 
     friend class Kd_tree<TreeTraits,Splitter,UseExtendedNode>;
@@ -344,7 +344,7 @@ namespace CGAL {
     typedef typename Kd_tree<TreeTraits,Splitter,UseExtendedNode>::Separator Separator;
 
   private:
-    
+
        // private variables for internal nodes
     boost::int32_t cut_dim;
     FT cut_val;
@@ -352,50 +352,53 @@ namespace CGAL {
 
 
     // private variables for extended internal nodes
-    FT low_val;
-    FT high_val;
-                
+    FT upper_low_val;
+    FT upper_high_val;
+    FT lower_low_val;
+    FT lower_high_val;
+
+
   public:
 
     // default constructor
-    Kd_tree_internal_node() 
+    Kd_tree_internal_node()
     {}
 
-    Kd_tree_internal_node(bool leaf_) 
+    Kd_tree_internal_node(bool leaf_)
       : Base(leaf_)
     {}
-    
-    
+
+
     // members for internal node and extended internal node
 
-    inline 
-    Node_const_handle 
-    lower() const 
+    inline
+    Node_const_handle
+    lower() const
     {
-      return lower_ch; 
+      return lower_ch;
     }
 
-    inline 
-    Node_const_handle 
-    upper() const 
+    inline
+    Node_const_handle
+    upper() const
     {
-      return upper_ch; 
+      return upper_ch;
     }
 
-    inline 
-    Node_handle 
+    inline
+    Node_handle
     lower()
     {
-      return lower_ch; 
+      return lower_ch;
     }
 
-    inline 
-    Node_handle 
+    inline
+    Node_handle
     upper()
     {
-      return upper_ch; 
+      return upper_ch;
     }
-  	
+
     inline
     void
     set_lower(Node_handle nh)
@@ -417,47 +420,60 @@ namespace CGAL {
       cut_dim = sep.cutting_dimension();
       cut_val = sep.cutting_value();
     }
-  	
-    inline 
-    FT 
-    cutting_value() const 
+
+    inline
+    FT
+    cutting_value() const
     {
       return cut_val;
     }
-  	
-    inline 
-    int 
-    cutting_dimension() const 
+
+    inline
+    int
+    cutting_dimension() const
     {
       return cut_dim;
     }
 
     // members for extended internal node only
-    inline 
+    inline
+    FT
+    upper_low_value() const
+    {
+      return upper_low_val;
+    }
+
+    inline
     FT
-    low_value() const 
-    { 
-      return low_val; 
+    upper_high_value() const
+    {
+      return upper_high_val;
     }
-    
-    inline 
+
+    inline
     FT
-    high_value() const 
+    lower_low_value() const
     {
-      return high_val; 
+      return lower_low_val;
     }
-       
 
-    /*Separator& 
-    separator() 
+    inline
+    FT
+    lower_high_value() const
+    {
+      return lower_high_val;
+    }
+
+    /*Separator&
+    separator()
     {
       return Separator(cutting_dimension,cutting_value);
     }*/
-	
+
 
   };//internal node
 
- template < class TreeTraits, class Splitter> 
+ template < class TreeTraits, class Splitter>
  class Kd_tree_internal_node<TreeTraits,Splitter,Tag_false> : public Kd_tree_node< TreeTraits, Splitter, Tag_false >{
 
     friend class Kd_tree<TreeTraits,Splitter,Tag_false>;
@@ -470,54 +486,54 @@ namespace CGAL {
     typedef typename Kd_tree<TreeTraits,Splitter,Tag_false>::Separator Separator;
 
   private:
-    
+
        // private variables for internal nodes
     boost::uint8_t cut_dim;
     FT cut_val;
 
     Node_handle lower_ch, upper_ch;
-                
+
   public:
 
     // default constructor
-    Kd_tree_internal_node() 
+    Kd_tree_internal_node()
     {}
 
-    Kd_tree_internal_node(bool leaf_) 
+    Kd_tree_internal_node(bool leaf_)
       : Base(leaf_)
     {}
-    
-    
+
+
     // members for internal node and extended internal node
 
-    inline 
-    Node_const_handle 
-    lower() const 
+    inline
+    Node_const_handle
+    lower() const
     {
-      return lower_ch; 
+      return lower_ch;
     }
 
-    inline 
-    Node_const_handle 
-    upper() const 
+    inline
+    Node_const_handle
+    upper() const
     {
-      return upper_ch; 
+      return upper_ch;
     }
 
-    inline 
-    Node_handle 
+    inline
+    Node_handle
     lower()
     {
-      return lower_ch; 
+      return lower_ch;
     }
 
-    inline 
-    Node_handle 
+    inline
+    Node_handle
     upper()
     {
-      return upper_ch; 
+      return upper_ch;
     }
-  	
+
     inline
     void
     set_lower(Node_handle nh)
@@ -540,27 +556,27 @@ namespace CGAL {
       cut_dim = sep.cutting_dimension();
       cut_val = sep.cutting_value();
     }
-  	
-    inline 
-    FT 
-    cutting_value() const 
+
+    inline
+    FT
+    cutting_value() const
     {
       return cut_val;
     }
-  	
-    inline 
-    int 
-    cutting_dimension() const 
+
+    inline
+    int
+    cutting_dimension() const
     {
       return cut_dim;
     }
 
-   /* Separator& 
-    separator() 
+   /* Separator&
+    separator()
     {
       return Separator(cutting_dimension,cutting_value);
     }*/
-	
+
 
   };//internal node