613 lines
19 KiB
C++
613 lines
19 KiB
C++
// Copyright (c) 2002,2011 Utrecht University (The Netherlands).
|
|
// All rights reserved.
|
|
//
|
|
// This file is part of CGAL (www.cgal.org).
|
|
//
|
|
// $URL: https://github.com/CGAL/cgal/blob/v5.1/Spatial_searching/include/CGAL/Incremental_neighbor_search.h $
|
|
// $Id: Incremental_neighbor_search.h dfe272f 2020-05-18T12:24:05+02:00 Laurent Rineau
|
|
// SPDX-License-Identifier: GPL-3.0-or-later OR LicenseRef-Commercial
|
|
//
|
|
//
|
|
// 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/disable_warnings.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 <iterator> // for std::distance
|
|
|
|
namespace CGAL {
|
|
|
|
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_false, Tag_false> >
|
|
class Incremental_neighbor_search {
|
|
|
|
public:
|
|
|
|
typedef Distance_ Distance;
|
|
typedef Tree_ Tree;
|
|
typedef typename SearchTraits::Point_d Point_d;
|
|
typedef typename SearchTraits::FT FT;
|
|
typedef typename SearchTraits::Dimension Dimension;
|
|
typedef typename Tree::Point_d_iterator Point_d_iterator;
|
|
typedef typename Tree::Node_const_handle Node_const_handle;
|
|
typedef typename Tree::Splitter Splitter;
|
|
typedef Kd_tree_rectangle<FT,Dimension> Node_box;
|
|
typedef typename Distance::Query_item Query_item;
|
|
|
|
class Cell {
|
|
|
|
private:
|
|
|
|
Node_box* the_box;
|
|
Node_const_handle the_node;
|
|
|
|
public:
|
|
|
|
// constructor
|
|
Cell (Node_box* Nb, Node_const_handle N)
|
|
:the_box(Nb), the_node(N)
|
|
{}
|
|
|
|
Node_box*
|
|
box()
|
|
{
|
|
return the_box;
|
|
}
|
|
|
|
Node_const_handle
|
|
node()
|
|
{
|
|
return the_node;
|
|
}
|
|
};
|
|
|
|
|
|
|
|
typedef std::pair<Point_d,FT> Point_with_transformed_distance;
|
|
typedef std::pair<Cell*,FT> Cell_with_distance;
|
|
|
|
|
|
typedef std::vector<Cell_with_distance*> Cell_with_distance_vector;
|
|
typedef std::vector<Point_with_transformed_distance*> Point_with_distance_vector;
|
|
typedef std::vector<FT> Distance_vector;
|
|
|
|
//data members
|
|
const Tree& m_tree;
|
|
Query_item m_query;
|
|
Distance m_dist;
|
|
FT m_Eps;
|
|
bool m_search_nearest;
|
|
|
|
public:
|
|
|
|
class iterator;
|
|
typedef iterator const_iterator;
|
|
|
|
// constructor
|
|
Incremental_neighbor_search(const Tree& tree, const Query_item& q,
|
|
FT Eps=FT(0.0), bool search_nearest=true,
|
|
const Distance& tr=Distance()):
|
|
m_tree(tree),m_query(q),m_dist(tr),m_Eps(Eps),m_search_nearest(search_nearest)
|
|
{}
|
|
|
|
iterator
|
|
begin() const
|
|
{
|
|
return iterator(m_tree,m_query,m_dist,m_Eps,m_search_nearest);
|
|
}
|
|
|
|
iterator
|
|
end() const
|
|
{
|
|
return iterator();
|
|
}
|
|
|
|
std::ostream&
|
|
statistics(std::ostream& s)
|
|
{
|
|
begin()->statistics(s);
|
|
return s;
|
|
}
|
|
|
|
|
|
template<class T>
|
|
struct Object_wrapper
|
|
{
|
|
T object;
|
|
Object_wrapper(const T& t):object(t){}
|
|
const T& operator* () const { return object; }
|
|
const T* operator-> () const { return &object; }
|
|
};
|
|
|
|
|
|
class iterator {
|
|
|
|
public:
|
|
|
|
typedef std::input_iterator_tag iterator_category;
|
|
typedef Point_with_transformed_distance value_type;
|
|
typedef const Point_with_transformed_distance* pointer;
|
|
typedef const Point_with_transformed_distance& reference;
|
|
typedef std::size_t size_type;
|
|
typedef std::ptrdiff_t difference_type;
|
|
typedef int distance_type;
|
|
|
|
|
|
class Iterator_implementation;
|
|
Iterator_implementation *ptr;
|
|
|
|
|
|
// default constructor
|
|
iterator()
|
|
: ptr(0)
|
|
{}
|
|
|
|
int
|
|
the_number_of_items_visited()
|
|
{
|
|
return ptr->number_of_items_visited;
|
|
}
|
|
|
|
// constructor
|
|
iterator(const Tree& tree, const Query_item& q, const Distance& tr, FT eps,
|
|
bool search_nearest)
|
|
: ptr(new Iterator_implementation(tree, q, tr, eps, search_nearest))
|
|
{}
|
|
|
|
// copy constructor
|
|
iterator(const iterator& Iter)
|
|
: ptr(Iter.ptr)
|
|
{
|
|
if (ptr != 0) ptr->reference_count++;
|
|
}
|
|
|
|
iterator& operator=(const iterator& Iter)
|
|
{
|
|
if (ptr!=Iter.ptr){
|
|
if (ptr != 0 && --(ptr->reference_count)==0) {
|
|
delete ptr;
|
|
}
|
|
ptr = Iter.ptr;
|
|
if (ptr != 0) ptr->reference_count++;
|
|
}
|
|
return *this;
|
|
}
|
|
|
|
const Point_with_transformed_distance&
|
|
operator* () const
|
|
{
|
|
return *(*ptr);
|
|
}
|
|
|
|
// -> operator
|
|
const Point_with_transformed_distance*
|
|
operator-> () const
|
|
{
|
|
return &*(*ptr);
|
|
}
|
|
|
|
// prefix operator
|
|
iterator& operator++()
|
|
{
|
|
++(*ptr);
|
|
return *this;
|
|
}
|
|
|
|
// postfix operator
|
|
Object_wrapper<Point_with_transformed_distance>
|
|
operator++(int)
|
|
{
|
|
return (*ptr)++;
|
|
}
|
|
|
|
bool
|
|
operator==(const iterator& It) const
|
|
{
|
|
if ( ((ptr == 0) ||
|
|
ptr->Item_PriorityQueue.empty()) &&
|
|
((It.ptr == 0) ||
|
|
It.ptr->Item_PriorityQueue.empty())
|
|
)
|
|
return true;
|
|
// else
|
|
return (ptr == It.ptr);
|
|
}
|
|
|
|
bool
|
|
operator!=(const iterator& It) const
|
|
{
|
|
return !(*this == It);
|
|
}
|
|
|
|
std::ostream&
|
|
statistics (std::ostream& s)
|
|
{
|
|
ptr->statistics(s);
|
|
return s;
|
|
}
|
|
|
|
~iterator()
|
|
{
|
|
if (ptr != 0) {
|
|
ptr->reference_count--;
|
|
if (ptr->reference_count==0) {
|
|
delete ptr;
|
|
ptr = 0;
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
class Iterator_implementation {
|
|
|
|
private:
|
|
|
|
FT multiplication_factor;
|
|
|
|
Query_item query_point;
|
|
|
|
FT distance_to_root;
|
|
|
|
bool search_nearest_neighbour;
|
|
|
|
FT rd;
|
|
|
|
internal::Distance_helper<Distance, SearchTraits> m_distance_helper;
|
|
int m_dim;
|
|
Tree const& m_tree;
|
|
|
|
class Priority_higher {
|
|
|
|
public:
|
|
|
|
bool search_nearest;
|
|
|
|
Priority_higher(bool search_the_nearest_neighbour)
|
|
: search_nearest(search_the_nearest_neighbour)
|
|
{}
|
|
|
|
//highest priority is smallest distance
|
|
bool operator() (Cell_with_distance* n1, Cell_with_distance* n2) const
|
|
{
|
|
return (search_nearest)? (n1->second > n2->second) : (n2->second > n1->second);
|
|
}
|
|
};
|
|
|
|
|
|
class Distance_smaller {
|
|
|
|
public:
|
|
|
|
bool search_nearest;
|
|
|
|
Distance_smaller(bool search_the_nearest_neighbour)
|
|
:search_nearest(search_the_nearest_neighbour)
|
|
{}
|
|
|
|
//highest priority is smallest distance
|
|
bool
|
|
operator() (Point_with_transformed_distance* p1, Point_with_transformed_distance* p2) const
|
|
{
|
|
return (search_nearest) ? (p1->second > p2->second) : (p2->second > p1->second);
|
|
}
|
|
};
|
|
|
|
|
|
std::priority_queue<Cell_with_distance*, Cell_with_distance_vector,
|
|
Priority_higher> PriorityQueue;
|
|
public:
|
|
std::priority_queue<Point_with_transformed_distance*, Point_with_distance_vector,
|
|
Distance_smaller> Item_PriorityQueue;
|
|
|
|
|
|
Distance distance;
|
|
|
|
public:
|
|
|
|
int reference_count;
|
|
|
|
int number_of_internal_nodes_visited;
|
|
int number_of_leaf_nodes_visited;
|
|
int number_of_items_visited;
|
|
int number_of_neighbours_computed;
|
|
|
|
// 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),
|
|
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),
|
|
number_of_leaf_nodes_visited(0), number_of_items_visited(0),
|
|
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()));
|
|
|
|
if (search_nearest) distance_to_root=
|
|
distance.min_distance_to_rectangle(q,*bounding_box);
|
|
else distance_to_root=
|
|
distance.max_distance_to_rectangle(q,*bounding_box);
|
|
|
|
|
|
|
|
Cell *Root_Cell = new Cell(bounding_box,tree.root());
|
|
Cell_with_distance *The_Root =
|
|
new Cell_with_distance(Root_Cell,distance_to_root);
|
|
|
|
PriorityQueue.push(The_Root);
|
|
|
|
// rd is the distance of the top of the priority queue to q
|
|
rd=The_Root->second;
|
|
Compute_the_next_nearest_neighbour();
|
|
}
|
|
|
|
// * operator
|
|
const Point_with_transformed_distance&
|
|
operator* () const
|
|
{
|
|
return *(Item_PriorityQueue.top());
|
|
}
|
|
|
|
// prefix operator
|
|
Iterator_implementation&
|
|
operator++()
|
|
{
|
|
Delete_the_current_item_top();
|
|
Compute_the_next_nearest_neighbour();
|
|
return *this;
|
|
}
|
|
|
|
// postfix operator
|
|
Object_wrapper<Point_with_transformed_distance>
|
|
operator++(int)
|
|
{
|
|
Object_wrapper<Point_with_transformed_distance> result( *(Item_PriorityQueue.top()) );
|
|
++*this;
|
|
return result;
|
|
}
|
|
|
|
// Print statistics of the general priority search process.
|
|
std::ostream&
|
|
statistics (std::ostream& s) const
|
|
{
|
|
s << "General priority search statistics:" << std::endl;
|
|
s << "Number of internal nodes visited:" <<
|
|
number_of_internal_nodes_visited << std::endl;
|
|
s << "Number of leaf nodes visited:" <<
|
|
number_of_leaf_nodes_visited << std::endl;
|
|
s << "Number of points visited:" <<
|
|
number_of_items_visited << std::endl;
|
|
s << "Number of neighbours computed:" <<
|
|
number_of_neighbours_computed << std::endl;
|
|
return s;
|
|
}
|
|
|
|
//destructor
|
|
~Iterator_implementation()
|
|
{
|
|
while (! PriorityQueue.empty()) {
|
|
Cell_with_distance* The_top=PriorityQueue.top();
|
|
PriorityQueue.pop();
|
|
delete The_top->first->box();
|
|
delete The_top->first;
|
|
delete The_top;
|
|
}
|
|
while (! Item_PriorityQueue.empty()) {
|
|
Point_with_transformed_distance* The_top=Item_PriorityQueue.top();
|
|
Item_PriorityQueue.pop();
|
|
delete The_top;
|
|
}
|
|
}
|
|
|
|
private:
|
|
|
|
void
|
|
Delete_the_current_item_top()
|
|
{
|
|
Point_with_transformed_distance* The_item_top=Item_PriorityQueue.top();
|
|
Item_PriorityQueue.pop();
|
|
delete The_item_top;
|
|
}
|
|
|
|
|
|
// With cache
|
|
bool search_in_leaf(typename Tree::Leaf_node_const_handle node, Tag_true, 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
|
|
bool search_in_leaf(typename Tree::Leaf_node_const_handle node, Tag_false, bool search_furthest)
|
|
{
|
|
typename Tree::iterator it_node_point = node->begin(), it_node_point_end = node->end();
|
|
|
|
for (; it_node_point != it_node_point_end; ++it_node_point)
|
|
{
|
|
number_of_items_visited++;
|
|
FT distance_to_query_point =
|
|
distance.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()
|
|
{
|
|
// compute the next item
|
|
bool next_neighbour_found=false;
|
|
if (!(Item_PriorityQueue.empty())) {
|
|
if (search_nearest_neighbour)
|
|
next_neighbour_found =
|
|
(multiplication_factor*rd > Item_PriorityQueue.top()->second);
|
|
else
|
|
next_neighbour_found =
|
|
(rd < multiplication_factor*Item_PriorityQueue.top()->second);
|
|
}
|
|
while ((!next_neighbour_found) && (!PriorityQueue.empty())) {
|
|
|
|
Cell_with_distance* The_node_top = PriorityQueue.top();
|
|
Node_const_handle N = The_node_top->first->node();
|
|
Node_box* B = The_node_top->first->box();
|
|
PriorityQueue.pop();
|
|
delete The_node_top->first;
|
|
delete The_node_top;
|
|
|
|
while (!(N->is_leaf())) {
|
|
typename Tree::Internal_node_const_handle node =
|
|
static_cast<typename Tree::Internal_node_const_handle>(N);
|
|
number_of_internal_nodes_visited++;
|
|
int new_cut_dim = node->cutting_dimension();
|
|
FT new_cut_val = node->cutting_value();
|
|
|
|
Node_box* lower_box = new Node_box(*B);
|
|
Node_box* upper_box = new Node_box(*B);
|
|
lower_box->split(*upper_box,new_cut_dim, new_cut_val);
|
|
delete B;
|
|
if (search_nearest_neighbour) {
|
|
FT distance_to_box_lower =
|
|
distance.min_distance_to_rectangle(query_point, *lower_box);
|
|
FT distance_to_box_upper =
|
|
distance.min_distance_to_rectangle(query_point, *upper_box);
|
|
if (distance_to_box_lower <= distance_to_box_upper) {
|
|
|
|
Cell* C_upper = new Cell(upper_box, node->upper());
|
|
Cell_with_distance *Upper_Child =
|
|
new Cell_with_distance(C_upper,distance_to_box_upper);
|
|
PriorityQueue.push(Upper_Child);
|
|
N=node->lower();
|
|
B=lower_box;
|
|
} else {
|
|
Cell* C_lower = new Cell(lower_box, node->lower());
|
|
Cell_with_distance *Lower_Child =
|
|
new Cell_with_distance(C_lower,distance_to_box_lower);
|
|
PriorityQueue.push(Lower_Child);
|
|
N=node->upper();
|
|
B=upper_box;
|
|
}
|
|
}
|
|
else { // search furthest
|
|
FT distance_to_box_lower =
|
|
distance.max_distance_to_rectangle(query_point, *lower_box);
|
|
FT distance_to_box_upper =
|
|
distance.max_distance_to_rectangle(query_point, *upper_box);
|
|
if (distance_to_box_lower >= distance_to_box_upper) {
|
|
Cell* C_upper = new Cell(upper_box, node->upper());
|
|
Cell_with_distance *Upper_Child =
|
|
new Cell_with_distance(C_upper,distance_to_box_upper);
|
|
PriorityQueue.push(Upper_Child);
|
|
N=node->lower();
|
|
B=lower_box;
|
|
}
|
|
else {
|
|
Cell* C_lower = new Cell(lower_box, node->lower());
|
|
Cell_with_distance *Lower_Child =
|
|
new Cell_with_distance(C_lower,distance_to_box_lower);
|
|
PriorityQueue.push(Lower_Child);
|
|
N=node->upper();
|
|
B=upper_box;
|
|
}
|
|
}
|
|
}
|
|
delete B;
|
|
|
|
// 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) {
|
|
typename internal::Has_points_cache<Tree, internal::has_Enable_points_cache<Tree>::type::value>::type dummy;
|
|
next_neighbour_found = search_in_leaf(node, dummy, !search_nearest_neighbour);
|
|
}
|
|
} // next_neighbour_found or priority queue is empty
|
|
// in the latter case also the item priority queue is empty
|
|
|
|
}
|
|
}; // class Iterator_implementation
|
|
}; // class iterator
|
|
}; // class
|
|
|
|
} // namespace CGAL
|
|
|
|
#include <CGAL/enable_warnings.h>
|
|
|
|
#endif // CGAL_INCREMENTAL_NEIGHBOR_SEARCH_H
|