songsenand
/
cgal
mirror of https://github.com/CGAL/cgal
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity
cgal/kdtree/include/CGAL/kdtree_d.h

1064 lines
26 KiB
C++
Raw Blame History

// Copyright (c) 1997 Tel-Aviv University (Israel).
// All rights reserved.
//
// This file is part of CGAL (www.cgal.org); you may redistribute it under
// the terms of the Q Public License version 1.0.
// See the file LICENSE.QPL distributed with CGAL.
//
// Licensees holding a valid commercial license may use this file in
// accordance with the commercial license agreement provided with the software.
//
// This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
// WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
//
// $URL$
// $Id$
//
//
// Author(s) : Sariel Har-Peled (sariel@math.tau.ac.il)
// Eyal Flato (flato@math.tau.ac.il)
#ifndef CGAL_KDTREE_D_H
#define CGAL_KDTREE_D_H
#include <cstdlib>
#include <cassert>
#include <cstring>
#include <list>
using std::list; // to avoid compiler crash on MSVC++
CGAL_BEGIN_NAMESPACE
/*=======================================================================
* Kdtree_interface -
* This is the default interface of point. It assume that PT (the
* point type have the following properties:
* default constructor
* int dimension()
* const coord_type & operator[]( int ) const
* ... operator=( const Pt & ) - copy operator
\*=======================================================================*/
template <class PT>
class Kdtree_interface
{
public:
typedef PT Point;
static int dimension( const PT & pnt )
{
// return pnt.dimensions();
return pnt.dimension();
}
static int compare( int d, const PT & a, const PT & b )
{
if ( a[ d ] < b[ d ] )
return -1;
if ( a[ d ] > b[ d ] )
return 1;
return 0;
}
static void copy_coord( int d, PT & a, const PT & b )
{
a[ d ] = b[ d ];
}
};
template <class PT>
class Kdtree_interface_2d
{
public:
typedef PT Point;
static int dimension( const PT & pnt )
{
// return pnt.dimensions();
return pnt.dimension();
}
static int compare( int d, const PT & a, const PT & b )
{
if ( a[ d ] < b[ d ] )
return -1;
if ( a[ d ] > b[ d ] )
return 1;
return 0;
}
static void copy_coord( int d, PT & a, const PT & b )
{
if ( d == 0 )
a = PT( b[ 0 ], a[1] );
else
if ( d == 1 )
a = PT( a[ 0 ], b[1] );
else {
assert( 0 );
}
}
};
template <class PT>
class Kdtree_interface_3d
{
public:
typedef PT Point;
static int dimension( const PT & pnt )
{
// return pnt.dimensions();
return pnt.dimension();
}
static int compare( int d, const PT & a, const PT & b )
{
if ( a[ d ] < b[ d ] )
return -1;
if ( a[ d ] > b[ d ] )
return 1;
return 0;
}
static void copy_coord( int d, PT & a, const PT & b )
{
if ( d == 0 )
a = PT( b[ 0 ], a[1], a[ 2 ] );
else
if ( d == 1 )
a = PT( a[ 0 ], b[1], a[ 2 ] );
else
if ( d == 2 )
a = PT( a[ 0 ], a[1], b[ 2 ] );
else {
assert( 0 );
}
}
};
/*=========================================================================
* kdtree_d -
* A the kdtree class.
*
* Remark: The kd-trees allocates all the memory it needs in advance,
* This results in a rather efficient memory management, and a fast
* iplementation.
\*=========================================================================*/
template <class Traits>
class Kdtree_d
{
class Plane;
public:
typedef typename Traits::Point Point;
typedef list<Point> List_points;
//-------------------------------------------------------------------------
// Extended_Point_d -
// A class for representing an extended d-dimal point: with
// ability to support +/- infinity. Templated by the kd-treee interface
// type.
//-------------------------------------------------------------------------
class ExtPoint
{
private:
class coordinate_type
{
public:
const Point * p_pnt;
int type;
//signed char type;
};
coordinate_type * p_arr;
int dim;
Point def_pnt;
void init( int _dim )
{
assert( _dim > 0 );
dim = _dim;
p_arr = (coordinate_type *)std::malloc( sizeof( coordinate_type )
* dim );
//printf( "p_arr(new): %p\n", (void *)p_arr );
assert( p_arr != NULL );
CGAL_CLIB_STD::memset( p_arr, 0, sizeof( coordinate_type ) * dim );
}
public:
enum { MINUS_INFINITY = -1, FINITE = 0, PLUS_INFINITY = 1 };
ExtPoint( int _type, int _dim )
{
assert( _type == MINUS_INFINITY || _type == PLUS_INFINITY );
init( _dim );
for ( int ind = 0; ind < dim; ind++ )
p_arr[ ind ].type = _type;
}
ExtPoint()
{
p_arr = NULL;
dim = -1;
}
ExtPoint( const ExtPoint & p )
{
init( p.dim );
def_pnt = p.def_pnt;
for ( int ind = 0; ind < dim; ind++ ) {
p_arr[ ind ] = p.p_arr[ ind ];
if ( p.p_arr[ ind ].p_pnt == &p.def_pnt )
p_arr[ ind ].p_pnt = &def_pnt;
}
}
ExtPoint & operator=( const ExtPoint & p )
{
term();
init( p.dim );
def_pnt = p.def_pnt;
for ( int ind = 0; ind < dim; ind++ ) {
p_arr[ ind ] = p.p_arr[ ind ];
if ( p.p_arr[ ind ].p_pnt == &p.def_pnt )
p_arr[ ind ].p_pnt = &def_pnt;
}
return *this;
}
ExtPoint( const Point & point, int dim )
{
init( dim );
def_pnt = point;
for ( int ind = 0; ind < dim; ind++ ) {
p_arr[ ind ].p_pnt = &def_pnt;
p_arr[ ind ].type = FINITE;
}
}
void term()
{
if ( p_arr != NULL ) {
//printf( "a: %p\n", p_arr );
std::free( p_arr );
//printf( "_a\n" );
p_arr = NULL;
}
dim = 0;
}
~ExtPoint()
{
term();
}
void set_coord( int k, Point & point )
{
assert( 0 <= k && k < dim );
p_arr[ k ].type = FINITE;
//p_arr[ k ].p_pnt = &point;
Traits::copy_coord( k, def_pnt, point );
p_arr[ k ].p_pnt = &def_pnt;
}
void set_coord( int k, const ExtPoint & point )
{
assert( 0 <= k && k < dim );
assert( 0 <= k && k < point.dim );
p_arr[ k ] = point.p_arr[ k ];
if ( p_arr[ k ].type == FINITE ) {
Traits::copy_coord( k, def_pnt, *(p_arr[ k ].p_pnt) );
p_arr[ k ].p_pnt = &def_pnt;
}
}
int compare( int k, const ExtPoint & point ) const
{
assert( 0 <= k && k < dim );
// the following does not compile on msvc++...
// coordinate_type & a( p_arr[ k ] ),
// & b( point.p_arr[ k ] );
coordinate_type & a = p_arr[ k ];
coordinate_type & b = point.p_arr[ k ];
if ( a.type != FINITE ) {
if ( b.type != FINITE ) {
return a.type - b.type;
} else {
return a.type;
}
} else {
if ( b.type != FINITE )
return -b.type;
else
return Traits::compare( k, *a.p_pnt, *b.p_pnt );
}
}
int compare_vector( const ExtPoint & point ) const
{
int ind, res;
for ( ind = 0; ind < dim; ind++ ) {
res = compare( ind, point );
if ( res != 0 )
return res;
}
return 0;
}
int compare( int k, const Point & point ) const
{
assert( 0 <= k && k < dim );
// coordinate_type & a( p_arr[ k ] );
coordinate_type & a = p_arr[ k ];
if ( a.type != FINITE )
return a.type;
else
return Traits::compare( k, *a.p_pnt, point );
}
int dimension() const
{
return dim;
}
int get_coord_status( int d ) const
{
assert( 0 <= d && d < dim );
return p_arr[ d ].type;
}
const Point * get_coord_point( int d ) const
{
assert( 0 <= d && d < dim );
return p_arr[ d ].p_pnt;
}
};
// Box - represents an axis parallel box.
class Box
{
public:
int dim;
ExtPoint left, right;
private:
friend class Plane;
ExtPoint & get_vertex( bool f_left )
{
return f_left? left : right;
}
public:
// constructors
//CHECK
Box()
{
}
Box( const Box & box )
{
dim = box.dim;
left = box.left;
right = box.right;
}
Box( const Point &l, const Point &r, int _dim )
{
dim = _dim;
left = ExtPoint( l, dim );
right = ExtPoint( r, dim );
}
Box( int _dim ) : left( ExtPoint::MINUS_INFINITY, _dim ),
right( ExtPoint::PLUS_INFINITY, _dim )
{
dim = _dim;
}
// data access
void set_left( Point &l)
{
left = ExtPoint( l, dim );
};
void set_right( Point &r)
{
right = ExtPoint( r, dim );
}
const ExtPoint &get_left() const
{
return left;
}
const ExtPoint &get_right() const
{
return right;
}
void set_coord_left( int k, Point & p )
{
left.set_coord( k, p );
}
void set_coord_right( int k, Point & p )
{
right.set_coord( k, p );
}
// operations
bool is_in( const Box &o) const
// checks if o is completely inside <this>
{
int dim = left.dimension();
for (int i = 0; i < dim; i++)
{
if ( (left.compare(i, o.get_left()) > 0 )
|| (right.compare(i, o.get_right()) < 0 ) )
return false;
}
return true;
}
bool is_in( const Point & o ) const
// checks if o is completely inside <this>
{
int dim = left.dimension();
for (int i = 0; i < dim; i++)
{
if ( (left.compare( i, o ) > 0 ) ||
(right.compare( i, o ) <= 0 ) )
return false;
}
return true;
}
bool is_coord_in_range( int k, const Point & o ) const
// checks if o is completely inside <this>
{
return ( ! ( (left.compare( k, o ) > 0 )
|| (right.compare( k, o ) <= 0 ) ) );
}
bool is_intersect( const Box &o) const
// checks if there is an intersection between o and this
{
int dim = left.dimension();
for (int i = 0; i < dim; i++)
{
if ( (left.compare(i, o.get_right()) >= 0) ||
(right.compare(i, o.get_left()) <= 0) )
return false;
}
return true;
}
// checks if there is an intersection between o and this box
// only in a specific coordinate...
bool is_intersect_in_dim( int d, const Box & o ) const
{
return (! ( (left.compare( d, o.get_right() ) >= 0 )
|| (right.compare( d, o.get_left() ) <= 0) ));
}
// checks if there is an intersection between o and this box
// only in a specific coordinate...
bool is_intersect_in_dim_closed( int d, const Box & o ) const
{
return (! ( (left.compare( d, o.get_right() ) > 0 )
|| (right.compare( d, o.get_left() ) < 0) ));
}
bool intersect(Box &o)
// intersects this with o. the intersection will be in this
// returns false if intersection is empty
{
int dim = left.dimension();
for (int i = 0; i < dim; i++)
{
// left is the maximal of the lefts
if (left.compare(i, o.get_left()) == -1)
left.set_coord(i, o.get_left());
// right is the minimal of the rights
if (right.compare(i, o.get_right()) == 1)
right.set_coord(i, o.get_right());
}
return !(is_empty());
}
bool is_empty() const
// return true if this is not an interval (left[k] > right[k])
{
int dim = left.dimension();
for (int i = 0; i < dim; i++)
{
if (left.compare(i, right) == 1)
return true;
}
return false;
}
bool is_empty_open() const
// return true if this is not an interval (left[k] > right[k])
{
int dim = left.dimension();
for (int i = 0; i < dim; i++)
{
if ( left.compare(i, right) >= 0 )
return true;
}
return false;
}
int comp( const Box & o ) const
{
int res;
res = left.compare_vector( o.left );
if ( res != 0 )
return res;
return right.compare_vector( o.right );
}
// destructor - needed in ...recursive ... DVP
~Box()
{
left.term();
right.term();
dim = 0;
}
};
private:
class Plane
{
private:
int coord;
Point * normal;
bool f_plus; // orientation of half space
// is (0, 0, ... , +inifinity, 0, ..., 0) inside plane
public:
Plane()
{
normal = NULL;
coord = 0;
}
Plane( int k, Point & p )
{
normal = &p;
coord = k;
}
Plane( const Plane & p )
{
coord = p.coord;
normal = p.normal;
}
void dump( void )
{
std::cout << "(" << coord << ": " << *normal << ")";
}
bool is_in( const Point & p ) const
{
int cmp;
cmp = Traits::compare( coord, p, *normal );
if ( ! f_plus )
cmp = -cmp;
return cmp >= 0;
}
void set_plane(int k, Point &p)
{
coord = k;
normal = &p;
//normal->copy( coord, p );
}
void split( Box & region, bool f_neg )
{
ExtPoint * p_p = &(region.get_vertex( ! f_neg ));
if ( f_neg ) {
if ( p_p->compare( coord, *normal ) > 0 )
p_p->set_coord( coord, *normal );
} else
if ( p_p->compare( coord, *normal ) < 0 )
p_p->set_coord( coord, *normal );
}
void orient_half_space( bool f_neg_side )
{
f_plus = ! f_neg_side;
}
int get_coord() const
{
return coord;
}
};
private:
class Node
{
public:
Plane plane;
Point * pnt;
Node * left, * right;
enum { LEFT, RIGHT };
const Plane & get_hs( int side ) const
{
((Plane *)&plane)->orient_half_space( side == LEFT );
return plane;
}
bool is_points_in_hs( const Plane & pl ) const
{
if ( is_point() )
return pl.is_in( *pnt );
if ( left != NULL && ( ! left->is_points_in_hs( pl ) ) )
return false;
if ( right != NULL && ( ! right->is_points_in_hs( pl ) ) )
return false;
return true;
}
bool is_valid() const
{
if ( is_point() )
return true;
if ( left != NULL )
if ( ! left->is_points_in_hs( get_hs( LEFT ) ) )
return false;
if ( right != NULL )
if ( ! right->is_points_in_hs( get_hs( RIGHT ) ) )
return false;
return true;
}
void dump( int depth )
{
int ind;
for ( ind = 0; ind < depth; ind++ )
std::cout << " ";
if ( is_point() ) {
std::cout << *pnt << "\n";
return;
}
plane.dump();
std::cout << "\n";
left->dump( depth + 1 );
for ( ind = 0; ind < depth; ind++ )
std::cout << " ";
std::cout << "!!!!!!!!!!!!\n";
right->dump( depth + 1 );
}
bool is_point() const
{
return ((left == NULL) && (right == NULL));
}
typedef std::back_insert_iterator<List_points> back_iter;
Node() : plane()
{
left = right = NULL;
}
void copy_subtree_points( back_iter & result,
const Box & rect )
{
if ( is_point() ) {
if ( rect.is_in( *pnt ) )
(*result++) = *pnt;
return;
}
if ( left != NULL )
left->copy_subtree_points( result, rect );
if ( right != NULL )
right->copy_subtree_points( result, rect );
}
static void search_recursive( back_iter & result,
Node * node,
const Box & rect,
Box & _region,
Plane & plane,
bool f_split_plus )
{
//printf( "search_recusrive\n" );
Box * p_r = new Box( _region );
//printf( "z" );
//fflush( stdout );
plane.split( *p_r, f_split_plus );
//printf( "c" );
//fflush( stdout );
assert( node != NULL );
//printf( "b" );
//fflush( stdout );
if ( rect.is_in( *p_r ) )
{
//printf( "5" );
//fflush( stdout );
node->copy_subtree_points( result, rect );
//printf( "\tsearch_recursive done...\n" );
//printf( "6" );
//fflush( stdout );
delete p_r;
return;
}
//printf( "v" );
//fflush( stdout );
if ( rect.is_intersect_in_dim_closed( plane.get_coord(), *p_r ) )
node->search( result, rect, *p_r );
//printf( "x" );
//fflush( stdout );
delete p_r;
}
void search( std::back_insert_iterator<List_points> result,
const Box &rect, Box &region )
{
if (is_point()) {
if ( rect.is_in( *pnt ) )
(*result++) = *pnt;
return;
}
//this is not a point so it is a hypeplane
if ( left != NULL )
search_recursive( result, left, rect,
region, plane, true );
if ( right != NULL )
search_recursive( result, right, rect,
region, plane, false );
}
};
// SunPro requires this :
friend class Box;
friend class Node;
typedef Point * Point_ptr;
int size;
Point * p_arr_pt;
Node *root;
int dim;
Node * p_node_arr;
int node_count;
Node * malloc_node( void )
{
Node * p_ret;
p_ret = &(p_node_arr[ node_count ]);
node_count++;
assert( node_count <= ( 2 * size ));
*p_ret = Node();
return p_ret;
}
static int comp( const Point & a, const Point & b, int dim )
{
return Traits::compare( dim, a, b );
}
static int partition( Point_ptr * arr, int left, int right,
Point * p_pivot, int dim )
{
int i, j;
Point_ptr tmp;
if ( left >= right )
return left;
i = left;
j = right;
while ( i < j ) {
if ( comp( *(arr[ i ]), *(arr[ j ]), dim ) > 0 ) {
tmp = arr[ i ];
arr[ i ] = arr[ j ];
arr[ j ] = tmp;
}
if ( comp( *(arr[ i ]), *p_pivot, dim ) < 0 ) {
i++;
} else
if ( comp( *p_pivot, *(arr[ j ]), dim ) <= 0 )
j--;
}
return (i > left)? i - 1 : left;
}
/* split the array into two sub-arrays, such that all the elements
* from left to pos_mid are smaller than the elements from pos+1 to
* right.
*/
static void split_arr( Point_ptr * arr,
int left,
int right,
int pos_mid,
int dim )
{
int pos;
if ( left >= right )
return;
pos = partition( arr, left, right, arr[ (left + right ) / 2 ],
dim );
if ( pos == pos_mid )
return;
if ( pos < pos_mid )
split_arr( arr, pos+1, right, pos_mid, dim );
else
split_arr( arr, left, pos, pos_mid, dim );
}
static Point_ptr get_max_element( Point_ptr * arr,
int left, int right, int d )
{
int max_pos = left;
Point mx = *(arr[ max_pos ]);
for ( int ind = left + 1; ind <= right; ind++ )
if ( comp( mx, *(arr[ ind ]), d ) < 0 ) {
mx = *(arr[ ind ]);
max_pos = ind;
}
return arr[ max_pos ];
}
Node *build_r( Point_ptr * arr, int left, int right,
int d )
{
int num, pos, next_d;
Node * n;
num = right - left + 1;
if ( num < 1)
return NULL;
// if the list contains only one point,
// construct a leaf for this node
if ( num == 1) {
//n = new node;
n = malloc_node();
n->pnt = arr[ left ];
return n;
}
// else divide space into two regions in
// dim-dim and cotinue recursively
pos = (left + right) / 2;
split_arr( arr, left, right, pos, d );
Point * p_median = get_max_element( arr, left, pos, d );
// create division plane;
Plane plane( d, *p_median );
//n = new node;
// assert( n != NULL );
n = malloc_node();
n->plane = plane;
next_d = d + 1;
if ( next_d >= dim )
next_d = 0;
// build left sub-tree
n->left = build_r( arr, left, pos, next_d );
// build right sub-tree
n->right = build_r( arr, pos + 1, right, next_d );
return n;
}
public:
typedef list<Point> list_points;
Kdtree_d(int k = 2)
{
dim = k;
root = NULL;
p_arr_pt = NULL;
p_node_arr = NULL;
}
~Kdtree_d()
{
delete_all();
}
bool is_valid( bool verbose = false, int level = 0 ) const
{
(void)verbose;
(void)level;
if ( root == NULL )
return true;
return root->is_valid();
}
void dump()
{
root->dump( 0);
}
void delete_all()
{
root = NULL;
if ( p_arr_pt != NULL )
delete[] p_arr_pt;
p_arr_pt = NULL;
if ( p_node_arr != NULL )
delete[] p_node_arr;
p_node_arr = NULL;
}
void search( std::back_insert_iterator<list_points> result,
Box & rect )
{
if (root == NULL)
return; // it is an empty tree - nothing to search in
Box region = Box( dim );
root->search( result, rect, region );
}
void build(list<Point> &l)
{
int i;
Point_ptr * p_arr;
size = l.size();
p_arr_pt = new Point[ size ];
assert( p_arr_pt != NULL );
p_arr = new Point_ptr[ size ];
assert( p_arr != NULL );
p_node_arr = new Node[ 2 * size ];
assert( p_node_arr != NULL );
node_count = 0;
/* fill the array */
i = 0;
for ( typename std::list<Point>::iterator j = l.begin();
j != l.end();
j++ )
{
p_arr_pt[ i ] = (*j);
p_arr[ i ] = p_arr_pt + i;
i++;
}
// recursively build the tree from the sorted list
// starting to divide it in coordinate 0
root = build_r( p_arr, 0, size - 1, 0 );
//printf( "b\n" );
delete[] p_arr;
}
};
CGAL_END_NAMESPACE
#endif /* CGAL_KDTREE_D_H */
Reference in New Issue View Git Blame Copy Permalink