---

guma_rkdtree.cpp

Go to the documentation of this file.

/* LIBGUL - Geometry Utility Library
 * Copyright (C) 1998-1999 Norbert Irmer
 *
 * This library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Library General Public
 * License as published by the Free Software Foundation; either
 * version 2 of the License, or (at your option) any later version.
 *
 * This library is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * Library General Public License for more details.
 *
 * You should have received a copy of the GNU Library General Public
 * License along with this library; if not, write to the
 * Free Software Foundation, Inc., 59 Temple Place - Suite 330,
 * Boston, MA 02111-1307, USA.
 */
 
#include "stdafx.h"

#include "gul_types.h"
#include "guma_rkdtree.h"
#include "guma_random.h"
#include "guma_sorting.h"

#include <iostream>

namespace guma {

using gul::uint32;
using gul::uint64;
using gul::rtr;
using gul::Ptr;
using gul::List;
using gul::ListNode;
using gul::Map;
using gul::Swap;
using std::cout;

/*---------------------------------------------------------------------------
  Randomized kd-tree
*---------------------------------------------------------------------------*/

template< class U, class V >
void kdtnode<U,V>::height( int cur_height, int *max_height, 
                           int *avg_count, double *avg_sum )
{
  (*avg_count)++;
  (*avg_sum) += (double)cur_height;

  if( m_parent == 0 )
    cout << "warning: parent == 0 in node " << (void *)this
         << "\n";      

  if( !m_left && !m_right )
  {
    if( cur_height > *max_height )
      *max_height = cur_height;
  }
  else
  {
    if( m_left ) 
      m_left->height( cur_height+1, max_height, avg_count, avg_sum );
    if( m_right ) 
      m_right->height( cur_height+1, max_height, avg_count, avg_sum );
  }
} 

template< class U, class V >
void kdtnode<U,V>::dump(int dim,int space)
{
  int i;
  ListNode< kdrec<U,V> > *r;
  
  for( i = 0; i < space; i++ ) cout << " ";
  cout << "discriminant = " << m_disc << ", number of elements = " 
       << m_nelems << "\n";

  for( i = 0; i < space; i++ ) cout << " ";
  cout << "records: {";
  for( r = m_recs.First(); r; r = r->next )
  {
    cout << "(" << r->el[0];
    for( i = 1; i < dim; i++ )
      cout << ", " << r->el[i];
    cout << ") ";
  }
  cout << "}\n";

  for( i = 0; i < space; i++ ) cout << " ";
  cout << "left subtree: " << (void *)m_left << "\n";
  if( m_left )
  {
    m_left->dump(dim,space+4);
  }
  for( i = 0; i < space; i++ ) cout << " ";
  cout << "right subtree: " << (void *)m_right << "\n";
  if( m_right )
  {
    m_right->dump(dim,space+4);
  }
}

template< class U, class V >
GULAPI void kdnnrec<U,V>::dump( int dim )
{
  ListNode< kdrec<U,V> > *r;
  int i;
  
  cout << "dist = " << m_dist << ", { ";
  for( r = m_recs.First(); r; r = r->next )
  {
    cout << (void *)r->el.m_base << ":(" << r->el[0];
    for( i = 1; i < dim; i++ )
      cout << ", " << r->el[i];
    cout << ") ";
  }
  cout << "}\n";
}

template< class U, class V >
void rkdtree_base<U,V>::split_recs(  
      List< ListNode< kdrec<U,V> > >& recs, const V& key, int i, 
      List< ListNode< kdrec<U,V> > >& recs1, List< ListNode< kdrec<U,V> > >& recs2,
      List< ListNode< kdrec<U,V> > >& C ) 
{
  ListNode< kdrec<U,V> > *r;
  int sign;
  
  r = recs.First();
  while( r )
  {
    recs.Remove(r);

    sign = rtr<V>::cmp( r->el[i], key );
    if( sign < 0 )
    {
      recs1.Append(r);
    }
    else if( sign > 0 )
    {
      recs2.Append(r);
    }
    else
      C.Append(r);

    r = recs.First();
  }
}
template< class U, class V >
int kdtsearch<U,V>::compare_key_to_node( V *k, kdnnrec<U,V> *r )
{
  return rtr<V>::cmp( *k, r->m_dist ); 
} 

template< class U, class V >
V kdtsearch<U,V>::length( const Ptr<V>& P )
{
  V dist;
  int i;
  
  dist = (V)0;
  for( i = 0; i < m_dim; i++ )
    dist += P[i] * P[i];
  dist = rtr<V>::sqrt(dist);
  return dist;
}

template< class U, class V >
void kdtsearch<U,V>::scan_records( 
                       const U& P, const List< ListNode< kdrec<U,V> > >& recs )
{
  ListNode< kdrec<U,V> > *rec;
  V dist,d;
  int i,side;
  Map< kdnnrec<U,V> >::Node mnode,mnode1;
 
  for( rec = recs.First(); rec; rec = rec->next )
  {
    dist = (V)0;
    for( i = 0; i < m_dim; i++ )
    {
      d = P[i] - rec->el[i];
      dist += d * d;
    }
    dist = rtr<V>::sqrt(dist);

    if( m_nNeighbors == 0 )  // initialize
    {
      m_maxDist = dist;
      
      //
      // the following two lines are a replacement for
      //
      //   "mnode = m_neighbors.NewNode(nnrec(dist,rec->el));"
      //
      // which doesn't works with gcc 3.0 anymore (???):
      //
      mnode = m_neighbors.NewNode();
      mnode.key()->m_dist = dist;
      mnode.key()->m_recs.Append( new gul::ListNode< kdrec<U,V> >(rec->el) );

      m_neighbors.InsertNode( mnode, m_neighbors.root, 1 );

      m_nNeighbors++;

      continue;
    }
             
    if( (m_nNeighbors == m_maxNeighbors) && (dist > m_maxDist) ) // too far away
      continue;

    side = m_neighbors.SearchNode( &dist, compare_key_to_node, &mnode);
    if( side == 0 ) // there is already a node with the same distancce
    {
      mnode.key()->m_recs.Append( new ListNode< kdrec<U,V> >(rec->el) );
    }
    else // insert a new node
    {
      if( dist > m_maxDist )
        m_maxDist = dist;

      mnode1 = m_neighbors.NewNode();
      mnode1.key()->m_dist = dist;
      mnode1.key()->m_recs.Append( new gul::ListNode< kdrec<U,V> >(rec->el) );

      m_neighbors.InsertNode( mnode1, mnode, side );
      m_nNeighbors++;
    }

    if( m_nNeighbors > m_maxNeighbors ) // remove the last node
    {
      mnode = m_neighbors.Last();

      mnode1 = m_neighbors.Predecessor(mnode);
      m_maxDist = mnode1.key()->m_dist;

      m_neighbors.RemoveNode(mnode);
      m_neighbors.FreeNode(mnode);

      m_nNeighbors--;
    }
  }
};

template< class U, class V >
void kdtsearch<U,V>::process_node( 
                 const U& P, const kdtnode<U,V> *n, const Ptr<V>& D, V d, 
                 List< ListNode< kdtsnode<U,V> > >& stack )
{
  int i,j;
  V dist,d1,d2;
  Ptr<V> D1,D2;
  const kdtnode<U,V> *T1, *T2;
  ListNode< kdtsnode<U,V> > *sn;
  
  scan_records( P, n->m_recs );

  i = n->m_disc;
  dist = P[i] - n->key();
  
  if( dist < (V)0 )
  {
    T1 = n->m_left;
    if( T1 )
    {
      if( (D[i] > (V)0) || (D[i] < dist) )  // set D1[i] = 0
      {
        D1.reserve_pool(m_dim);
        for( j = 0; j < m_dim; j++ )
          D1[j] = D[j];
        D1[i] = (V)0;
        d1 = length( D1 );
      }
      else
      {
        D1 = D;  // distance to cell remains the same
        d1 = d;
      }
    }
    T2 = n->m_right;
  }
  else if( dist > 0 )
  {
    T1 = n->m_right;
    if( T1 )
    {
      if( (D[i] < (V)0) || (D[i] > dist) )  // set D1[i] = 0
      {
        D1.reserve_pool(m_dim);
        for( j = 0; j < m_dim; j++ )
          D1[j] = D[j];
        D1[i] = (V)0;
        d1 = length( D1 );
      }
      else
      {
        D1 = D;  // distance to cell remains the same
        d1 = d;
      }
    }
    T2 = n->m_left;
  }
  else  /* dist = 0 */
  {
    T1 = n->m_left;
    if( T1 )
    {
      if( D[i] )    // set D1[i] = 0
      {
        D1.reserve_pool(m_dim);
        for( j = 0; j < m_dim; j++ )
          D1[j] = D[j];
        D1[i] = (V)0;
        d1 = length( D1 );
      }
      else
      {
        D1 = D;  // distance to cell hasn't changed
        d1 = d;
      }
    }
    T2 = n->m_right;
  }

  if( T2 )
  {
    if( dist != D[i] )    // set D2[i] = dist
    {
      D2.reserve_pool(m_dim);
      for( j = 0; j < m_dim; j++ )
        D2[j] = D[j];
      D2[i] = dist;
      d2 = length( D2 );
    }
    else
    {
      D2 = D;  // distance to cell hasn't changed
      d2 = d;
    }
  }

  if( T1 && ((d1 <= m_maxDist) || (m_nNeighbors < m_maxNeighbors)) )
  {
    sn = new ListNode< kdtsnode<U,V> >( kdtsnode<U,V>(T1,D1,d1) );
    stack.Append(sn);
  }
  if( T2 && ((d2 <= m_maxDist) || (m_nNeighbors < m_maxNeighbors)) )
  {
    sn = new ListNode< kdtsnode<U,V> >( kdtsnode<U,V>(T2,D2,d2) );
    stack.Append(sn);
  }
}

template< class U, class V >
GULAPI int rkdtree_base<U,V>::nearest_neighbors( 
        const U& P, int m, Ptr< kdnnrec<U,V> >& M  )
{
  Ptr<V> D;
  V d;
  ListNode< kdtsnode<U,V> > *sn;
  List< ListNode< kdtsnode<U,V> > > S1,S2;
  List< ListNode< kdtsnode<U,V> > >* stack1 = &S1, *stack2 = &S2;
  const kdtnode<U,V> *curnod;
  int i, nEl;
  Ptr<kdnnrec<U,V> *> pm;
  
  if( (m <= 0) || (!m_root) ) return 0;
  
  kdtsearch<U,V> nns(m_dim,m);

  D.reserve_pool(m_dim);
  for( i = 0; i < m_dim; i++ )
    D[i] = (V)0;
  d = 0;
  
  sn = new ListNode< kdtsnode<U,V> >( kdtsnode<U,V>(m_root,D,d) );
  stack1->Append(sn);
  
  while( !stack1->IsEmpty() )
  {
    sn = stack1->First();
    while( sn )
    {
      curnod = sn->el.m_n;
      D = sn->el.m_D;
      d = sn->el.m_d;
      stack1->Remove( sn );
      delete sn;

      nns.process_node( P, curnod, D, d, *stack2 );

      sn = stack1->First();
    }

    Swap( stack1, stack2 );
  }

  nEl = nns.m_neighbors.ConvertToArray(&pm);
  for( i = 0; i < nEl; i++ )
    M[i] = *(pm[i]);

  return nEl;
}

template< class U, class V >
GULAPI void rkdtree_base<U,V>::dump_statistics()
{
  int max_height = 0;
  double avg_pathlen = 0;
  int avg_count = 0;

  if( m_root )
  {
    cout << "nelems (without collision records) = " << m_root->m_nelems << "\n";
    m_root->height( 1, &max_height, &avg_count, &avg_pathlen );
    cout << "max height = " << max_height << "\n";
    cout << "average pathlen = " << avg_pathlen/((double)avg_count) << "\n";
  }
  else
  {
    cout << "empty tree\n";
  }
}

template< class U, class V >
GULAPI void rkdtree_base<U,V>::dump_tree()
{
  if( m_root )
  {
    m_root->dump(m_dim,0);
  }
  else
  {
    cout << "empty tree\n";
  }
}



template< class U, class V, class Rand >
GULAPI kdtnode<U,V> *rkdtree<U,V,Rand>::insert( 
                            kdtnode<U,V> *T, const kdrec<U,V>& rec )
{
  int n,sign,k;
  double p;
  kdtnode<U,V> *L, *R, *root, *S, *Sold;
  
  if( !T )
  {
    k = (int)(Rand::Random()*(double)m_dim);
    return new kdtnode<U,V>(rec, k);
  }
  
  n = T->m_nelems;
  p = Rand::Random();

  if( p*((double)(n+1)) < (double)n ) // becomes not root
  {
    sign = rtr<V>::cmp( rec[T->m_disc], T->key() );

    if( sign < 0 )
    {
      if( !T->m_left )
      {
        k = (int)(Rand::Random()*(double)m_dim);
        T->set_left( new kdtnode<U,V>(rec, k) );
      }
      else
      {
        Sold = T->m_left;
        S = insert( T->m_left, rec );
        if( S != Sold ) 
          T->set_left(S);
      }
    }
    else if( sign > 0 )
    {
      if( !T->m_right )
      {
        k = (int)(Rand::Random()*(double)m_dim);
        T->set_right( new kdtnode<U,V>(rec, k) );
      }
      else
      {
        Sold = T->m_right;
        S = insert( T->m_right, rec );
        if( S != Sold ) 
          T->set_right(S);
      }
    }
    else   // collision
    {
      T->add_rec( rec );
    }

    root = T;
  }
  else
  {
    k = (int)(Rand::Random()*(double)m_dim);
    root = new kdtnode<U,V>(rec, k);

    split( T, rec, k, &L, &R, root->m_recs );
    root->set_childs(L, R);
  }

  return root;
}

template< class U, class V, class Rand >
GULAPI kdtnode<U,V> *rkdtree<U,V,Rand>::remove( 
                 kdtnode<U,V> *T, 
                 const kdrec<U,V>& rec,
                 gul::List< gul::ListNode< kdrec<U,V> > >& outrecs )
{
  int sign,i;
  kdtnode<U,V> *L,*Lold,*R,*Rold,*Told;
  gul::ListNode< kdrec<U,V> > *r,*r_next;
  gul::List< gul::ListNode< kdrec<U,V> > > dummyrecs;

  if( !T ) return 0;

  // search record which shall be removed
  sign = rtr<V>::cmp( rec[T->m_disc], T->key() );
    
  if( sign < 0 )
  {
    Lold = T->m_left;
    L = remove(T->m_left,rec,outrecs);
    if( L != Lold ) T->set_left(L);
  }
  else if( sign > 0 )
  {
    Rold = T->m_right;
    R = remove(T->m_right,rec,outrecs);
    if( R != Rold ) T->set_right(R);
  }
  else // found a node which is identical in its discriminant component
  {
    // remove records which are completely identical to 'rec'
    r = T->m_recs.First();
    while( r )
    {
      for( i = 0; i < m_dim; i++ )
      {
        sign = rtr<V>::cmp( r->el[i], rec[i] );
        if( sign != 0 ) break;
      }
      r_next = r->next;
      if( sign == 0 )  
      {
        T->m_recs.Remove(r);
        outrecs.Append(r);
      }
      r = r_next;
    }

    // remove node if necessary
    if( T->m_recs.nElems == 0 )
    {
      i = T->m_disc;
      Told = T;
      T->unlink(&L,&R,dummyrecs);
      T = join( L, R, i );
      delete Told; // deleting T now prevents Tnew==Told
    }
  }
  return T;
}

template< class U, class V, class Rand >
void rkdtree<U,V,Rand>::split( 
                   kdtnode<U,V> *T, const V& key, int i, 
                   kdtnode<U,V> **retL, kdtnode<U,V> **retR, 
                   List< ListNode< kdrec<U,V> > >& C ) 
{
  kdtnode<U,V> *L, *R, *TL, *TR, *L1, *L2, *R1, *R2, *LR, *RL;
  List< ListNode< kdrec<U,V> > > recs,recs1,recs2;
  int j, sign;
  
  // if T is empty return empty trees 
  if( !T )
  {
    *retL = 0;
    *retR = 0;
    return;
  }

  // note: the tree which shall be splitted shouldn't have a parent,
  // since the parent isn't updated in this function
  T->m_parent = 0;

  j = T->m_disc;
  
  // T has the same discriminant j
  if( j == i )
  {
    sign = rtr<V>::cmp( T->key(), key );
    
    // Y[j] < X[i]
    if( sign < 0 )
    {
      L = T;
      split( L->m_right, key, i, &LR, &R, C );
      L->set_right(LR);
    }
    // Y[j] > X[i]
    else if( sign > 0 )
    {
      R = T;
      split( R->m_left, key, i, &L, &RL, C );
      R->set_left(RL);
    }
    else
    {
      T->unlink(&L,&R,C);
      delete T;     
    }
  }
  // T has a different discriminant j
  else
  {
    T->unlink( &TL, &TR, recs );
    split_recs( recs, key, i, recs1, recs2, C );
    split( TL, key, i, &L1, &L2, C );
    split( TR, key, i, &R1, &R2, C );

    if( recs1.nElems )
    {
      L = new kdtnode<U,V>( recs1, j );
      L->set_childs( L1, R1 );
    }
    else
    {
      L = join( L1, R1, j );
    }
    
    if( recs2.nElems )
    {
      R = new kdtnode<U,V>( recs2, j );
      R->set_childs( L2, R2 );
    }
    else
    {
      R = join( L2, R2, j );
    }

    delete T;
  }

  *retL = L;
  *retR = R;
}

template< class U, class V, class Rand >
kdtnode<U,V> *rkdtree<U,V,Rand>::join( kdtnode<U,V> *A, kdtnode<U,V> *B, int i )
{
  kdtnode<U,V> *root, *A1, *A2, *B1, *B2;
  int n,m,j;
  double p;
  
  if( (!A) && (!B) ) return 0;  

  // note: the trees which shall be joined shouldn't have parents,
  // since the parents aren't updated in this function

  if( !A ) { B->m_parent = 0; return B; }
  if( !B ) { A->m_parent = 0; return A; }

  A->m_parent = 0;
  B->m_parent = 0;
  
  n = A->m_nelems;
  m = B->m_nelems;
  p = Rand::Random();

  if( p*((double)(n+m)) < (double)n ) // root of A becomes new root
  {
    j = A->m_disc;
    root = A;

    if( j == i )
    {
      A2 = join( A->m_right, B, i );
      root->set_right( A2 );
    } 
    else
    {
      split( B, A->key(), j, &B1, &B2, A->m_recs );
      A1 = join( A->m_left, B1, i );
      A2 = join( A->m_right, B2, i );
      A->set_childs( A1, A2 );
    }
  }
  else    // root of B becomes new root
  {
    j = B->m_disc;
    root = B;

    if( j == i )
    {
      B1 = join( A, B->m_left, i );
      root->set_left( B1 );
    } 
    else
    {
      split( A, B->key(), j, &A1, &A2, B->m_recs );
      B1 = join( A1, B->m_left, i );
      B2 = join( A2, B->m_right, i );
      B->set_childs( B1, B2 );
    }
  }

  return root;
}

// template instantiation

// 2d and 3d points
template class kdnnrec< gul::point<float>, float >;
template class rkdtree_base< gul::point<float>, float >;
template class rkdtree< gul::point<float>, float, random_des >;

template class kdnnrec< gul::point2<float>, float >;
template class rkdtree_base< gul::point2<float>, float >;
template class rkdtree< gul::point2<float>, float, random_des >;

// any number of dimensions
template class kdnnrec< gul::kdarray<float>, float >;
template class rkdtree_base< gul::kdarray<float>, float >;
template class rkdtree< gul::kdarray<float>, float, random_des >;

// records with tagged data
template class kdnnrec< gul::kdpoint< float,gul::point<float> >, float >;
template class rkdtree_base< gul::kdpoint< float,gul::point<float> >, float >;
template class rkdtree< gul::kdpoint< float,gul::point<float> >, float, 
                        random_des >;

template class kdnnrec< gul::kdpoint< float,gul::point2<float> >, float >;
template class rkdtree_base< gul::kdpoint< float,gul::point2<float> >, float >;
template class rkdtree< gul::kdpoint< float,gul::point2<float> >, float, 
                        random_des >;

// 2d and 3d points
template class kdnnrec< gul::point<double>, double >;
template class rkdtree_base< gul::point<double>, double >;
template class rkdtree< gul::point<double>, double, random_des >;

template class kdnnrec< gul::point2<double>, double >;
template class rkdtree_base< gul::point2<double>, double >;
template class rkdtree< gul::point2<double>, double, random_des >;

// any number of dimensions
template class kdnnrec< gul::kdarray<double>, double >;
template class rkdtree_base< gul::kdarray<double>, double >;
template class rkdtree< gul::kdarray<double>, double, random_des >;

// records with tagged data
template class kdnnrec< gul::kdpoint< double,gul::point<double> >,double >;
template class rkdtree_base< gul::kdpoint< double,gul::point<double> >,double >;
template class rkdtree< gul::kdpoint< double,gul::point<double> >, double, 
                        random_des >;

template class kdnnrec< gul::kdpoint< double,gul::point2<double> >,double >;
template class rkdtree_base< gul::kdpoint< double,gul::point2<double> >,double >;
template class rkdtree< gul::kdpoint< double,gul::point2<double> >, double, 
                        random_des >;

}

---