cuik-kdtree.c

Go to the documentation of this file.

#include "cuik-kdtree.h"

#include "definitions.h"
#include "vector.h"
#include "random.h"
#include "assert.h"

#define TRUE_MEDIAN 0

TKDtree *BuildKDTree(Trectangle *ambient,unsigned int m,double r,
                     unsigned int n,double **points,unsigned int *ids);

void SearchKDtree(double dr2,unsigned int *topology,double *p,double *d2,
                  unsigned int *id,TKDtree *kdt);

void SearchInBallKDtree(double dr2,unsigned int *topology,double *p,double r2,
                        unsigned int *n,unsigned int *m,
                        unsigned int **id,TKDtree *kdt);


/************************************************************************/
/************************************************************************/
/************************************************************************/
TKDtree *BuildKDTree(Trectangle *ambient,unsigned int m,double r,
                     unsigned int n,double **points,unsigned int *ids)
{
  TKDtree *kdt;

  NEW(kdt,1,TKDtree);

  kdt->nd=GetRectangleDim(ambient);
  kdt->n=n;
  kdt->m=m;
  kdt->r=r;
  CopyRectangle(&(kdt->ambient),ambient);

  if (n<m)
    {
      unsigned int i;

      kdt->isLeaf=TRUE;

      kdt->height=1;

      NEW(kdt->p,kdt->m+1,double *);  /* +1 is useful when adding points */
      NEW(kdt->id,kdt->m+1,unsigned int);

      if (n==0)
        kdt->volume=0.0;
      else
        {
          for(i=0;i<n;i++)
            {
              kdt->p[i]=points[i];
              kdt->id[i]=ids[i];
              if (i==0)
                InitRectangleFromPoint(kdt->nd,points[i],&(kdt->boundingBox));
              else
                ExpandRectangle(points[i],&(kdt->boundingBox));
            }
          kdt->volume=EnlargeRectangleWithLimits(r,
                                                 &(kdt->ambient),
                                                 &(kdt->boundingBox),
                                                 &(kdt->samplingArea));
        }
      kdt->nextLeaf=NULL;

      #ifndef NDEBUG
        /* Reset the internal node info */
        kdt->splitDim=NO_UINT;
        kdt->splitValue=INF;
        kdt->left=NULL;
        kdt->right=NULL;
        kdt->leftLeaf=NULL;
        kdt->rightLeaf=NULL;
      #endif
    }
  else
    {
      signed int i,j;
      double *ps;
      unsigned int is;
      TKDtree *kdtLeft,*kdtRight;
      double u,l;

      kdt->isLeaf=FALSE;

      /* determine the split dimension and split point */
      kdt->splitDim=GetRectangleSplitDim(&(kdt->ambient));

      #if (TRUE_MEDIAN)
      {
        unsigned int med,right,left;
        double val;

        /* We adapt the quickselect algorithm to find the median in O(n) */
        med=(unsigned int)n/2; //position of the median
        left=0;
        right=n;
        i=0; // do not start the loop if r==l==med
        while (i!=med)
          {
            //we use the leftmost value as a pivot 
            //(can be any index betwee 'left' and 'right')
            val=points[left][kdt->splitDim]; 
            
            /*
            // this is only necessary if we pick a pivot value 
            // different from the left-most one
            if (pivot!=left)
              {
                //Save the pivot value in the leftmost position
                SWAP(points[left],points[i],ps);
                SWAP(ids[left],ids[i],is);
              }
            */

            // for the rest of values, find the first value
            // larger than the pivot value
            i=left+1;
            while ((i<right)&&(points[i][kdt->splitDim]<val))
              i++;
            // classify the values after 'i'
            j=i+1;
            while (j<right)
              {
                if (points[j][kdt->splitDim]<val)
                  {
                    SWAP(points[i],points[j],ps);
                    SWAP(ids[i],ids[j],is); 
                    i++;
                  }
                j++;
              }
            if (left!=i-1)
              {
                // insert the pivot just before the 1st value larger than it
                SWAP(points[left],points[i-1],ps);
                SWAP(ids[left],ids[i-1],is); 
              }
            if (i<med)
              left=i;
            else
              {
                if (i>med)
                  right=i;
              }
          }
        kdt->splitValue=points[i][kdt->splitDim];
      }
      #else
        //approximate the median by the mean
        kdt->splitValue=0.0;
        for(i=0;i<n;i++)
          kdt->splitValue+=points[i][kdt->splitDim];
        kdt->splitValue/=(double)n;


        //split the points in two sub-sets according to the split point
        i=0; 
        j=n-1;
        do {
          while ((i<n)&&(points[i][kdt->splitDim]<kdt->splitValue))
            i++;
          while ((j>=0)&&(points[j][kdt->splitDim]>=kdt->splitValue))
            j--;
          if ((i<n)&&(j>=0)&&(i<j)&&
              (points[i][kdt->splitDim]>=kdt->splitValue)&&
              (points[j][kdt->splitDim]<kdt->splitValue))
            {
              SWAP(points[i],points[j],ps);
              SWAP(ids[i],ids[j],is); 
            }
        } while (i<j);      
      #endif

      /* at this point we have the split value and the vector of points is
         sorted with 'i' elements with values below the split value and
         n-i elements with values above it. */

      /* Recursively build kd-trees */

      GetRectangleLimits(kdt->splitDim,&l,&u,&(kdt->ambient));

      SetRectangleUpperLimit(kdt->splitDim,kdt->splitValue,&(kdt->ambient));
      kdtLeft=BuildKDTree(&(kdt->ambient),m,r,i,points,ids);

      SetRectangleLimits(kdt->splitDim,kdt->splitValue,u,&(kdt->ambient));
      kdtRight=BuildKDTree(&(kdt->ambient),m,r,n-i,&(points[i]),&(ids[i]));

      SetRectangleLowerLimit(kdt->splitDim,l,&(kdt->ambient));
      
      /* Assemble the sub-trees */ 
      kdt->left=(void *)kdtLeft;
      kdt->right=(void *)kdtRight;

      kdt->leftLeaf=(kdtLeft->isLeaf?(void*)kdtLeft:kdtLeft->leftLeaf);
      kdt->rightLeaf=(kdtRight->isLeaf?(void*)kdtRight:kdtRight->rightLeaf);

      /* Update the smapling area */
      kdt->volume=kdtLeft->volume+kdtRight->volume;
      
      /* Update the height */
      kdt->height=1+(kdtLeft->height>kdtRight->height?
                     kdtLeft->height:
                     kdtRight->height);

      /* Chain the sub-tree leaves */
      if (kdtLeft->isLeaf)
        {
          if (kdtRight->isLeaf)
            kdtLeft->nextLeaf=(void *)kdtRight;
          else
            kdtLeft->nextLeaf=kdtRight->leftLeaf;
        }
      else
        {
          if (kdtRight->isLeaf)
            KDTPTR(kdtLeft->rightLeaf)->nextLeaf=(void *)kdtRight;
          else
            KDTPTR(kdtLeft->rightLeaf)->nextLeaf=kdtRight->leftLeaf;
        }

      #ifndef NDEBUG
        /* Reset the leaf info */
        kdt->nextLeaf=NULL;
        kdt->p=NULL;
        kdt->id=NULL;
      #endif
    }

  return(kdt);
}

void SearchKDtree(double dr2,unsigned int *topology,double *p,double *d2,
                  unsigned int *id,TKDtree *kdt)
{
  if (kdt->isLeaf)
    {
      if (SquaredDistanceToRectangle(*d2,p,topology,&(kdt->boundingBox))<*d2)
        {
          unsigned int i;
          double dp;
      
          for(i=0;i<kdt->n;i++)
            {
              dp=VectorSquaredDistanceTopologyMin(*d2,kdt->nd,topology,p,kdt->p[i]);
              if (dp<*d2)
                {
                  *d2=dp;
                  *id=kdt->id[i];
                }
            }
        }
    }
  else
    {
      TKDtree *t1,*t2;
      double dt0,dt1,dt2,d,v;

      v=p[kdt->splitDim];
      if (v<=kdt->splitValue)
        {
          /* The nearest point is most likely in the left branch */
          t1=KDTPTR(kdt->left);
          t2=KDTPTR(kdt->right);
        }
      else
        {
          /* The nearest point is most likely in the left branch */
          t1=KDTPTR(kdt->right);
          t2=KDTPTR(kdt->left);
        }

      dt0=dr2-SquaredDistanceToRectangleDimension(kdt->splitDim,v,topology,
                                                  &(kdt->ambient));

      dt1=SquaredDistanceToRectangleDimension(kdt->splitDim,v,topology,
                                              &(t1->ambient));
      d=dt0+dt1;
      if (d<*d2)
        SearchKDtree(d,topology,p,d2,id,t1);

      dt2=SquaredDistanceToRectangleDimension(kdt->splitDim,v,topology,
                                              &(t2->ambient));
      d=dt0+dt2;
      if (d<*d2)
        SearchKDtree(d,topology,p,d2,id,t2);
    }
}

void SearchInBallKDtree(double dr2,unsigned int *topology,double *p,double r2,
                        unsigned int *n,unsigned int *m,
                        unsigned int **id,TKDtree *kdt)
{
  if (kdt->isLeaf)
    {
      if (SquaredDistanceToRectangle(r2,p,topology,&(kdt->boundingBox))<r2)
        {
          unsigned int i;
          
          for(i=0;i<kdt->n;i++)
            {
              if (VectorSquaredDistanceTopologyMin(r2,kdt->nd,topology,p,kdt->p[i])<r2)
                {
                  if (*n==*m)
                    MEM_DUP(*id,*m,unsigned int);
                  (*id)[*n]=kdt->id[i];
                  (*n)++;
                }
            }
        }
    }
  else
    {
      TKDtree *t;
      double d,dt,v;

      v=p[kdt->splitDim];
      dt=dr2-SquaredDistanceToRectangleDimension(kdt->splitDim,v,topology,
                                                 &(kdt->ambient));
       
      t=KDTPTR(kdt->left);
      d=dt+SquaredDistanceToRectangleDimension(kdt->splitDim,v,topology,
                                               &(t->ambient));
      if (d<r2)
        SearchInBallKDtree(d,topology,p,r2,n,m,id,t);
       
      t=KDTPTR(kdt->right);
      d=dt+SquaredDistanceToRectangleDimension(kdt->splitDim,v,topology,
                                               &(t->ambient));
      if (d<r2)
        SearchInBallKDtree(d,topology,p,r2,n,m,id,t);
    }
}

/************************************************************************/
/************************************************************************/
/************************************************************************/

TKDtree *InitKDTree(Trectangle *ambient,unsigned int m,double r,
                    unsigned int n,double **points,unsigned int *ids)
{
  assert(m>=2);
  assert(r>0);

  return(BuildKDTree(ambient,m,r,n,points,ids));
}

void AddPoint2KDtree(unsigned int *topology,double *point,unsigned int id,TKDtree **kdt)
{
  TKDtree *t;

  t=*kdt;

  if (t->isLeaf)
    {
      t->p[t->n]=point;
      t->id[t->n]=id;
      t->n++;
      if (t->n>t->m)
        {
          TKDtree *nt;

          nt=BuildKDTree(&(t->ambient),t->m,t->r,t->n,t->p,t->id);
          DeleteKDtree(t);
          *kdt=nt;
        }
      else
        {
          if (t->n==1)
            InitRectangleFromPoint(t->nd,point,&(t->boundingBox));
          else
            ExpandRectangle(point,&(t->boundingBox));
          t->volume=EnlargeRectangleWithLimits(t->r,
                                               &(t->ambient),
                                               &(t->boundingBox),
                                               &(t->samplingArea));
        }
    }
  else
    {
      TKDtree *kdtLeft,*kdtRight;

      if (point[t->splitDim]<=t->splitValue)
        AddPoint2KDtree(topology,point,id,(TKDtree **)&(t->left));
      else
        AddPoint2KDtree(topology,point,id,(TKDtree **)&(t->right));

      t->n++;

      kdtLeft=KDTPTR(t->left);
      kdtRight=KDTPTR(t->right);

      /* One of the sub-trees changed -> update the summary at this level */
      
      /* The chain of leaves must be updated even if we re-balance.
         Before re-balancing, we need a correct chain of leaves. */

      t->leftLeaf=(kdtLeft->isLeaf?(void*)kdtLeft:kdtLeft->leftLeaf);
      t->rightLeaf=(kdtRight->isLeaf?(void*)kdtRight:kdtRight->rightLeaf);

      /* Chain the sub-tree leaves */
      if (kdtLeft->isLeaf)
        {
          if (kdtRight->isLeaf)
            kdtLeft->nextLeaf=(void *)kdtRight;
          else
            kdtLeft->nextLeaf=kdtRight->leftLeaf;
        }
      else
        {
          if (kdtRight->isLeaf)
            KDTPTR(kdtLeft->rightLeaf)->nextLeaf=(void *)kdtRight;
          else
            KDTPTR(kdtLeft->rightLeaf)->nextLeaf=kdtRight->leftLeaf;
        }

      /* If necessary, rebalance the tree -> re-build the tree */
      if ((KDTPTR(t->left)->height>KDTPTR(t->right)->height*2)||
          (KDTPTR(t->right)->height>KDTPTR(t->left)->height*2))
        {
          double **p;
          unsigned int *id;
          TKDtree *l,*nt;
          unsigned int i,j;

          /* Collect all the points in the leaves of the tree to rebuild */
          NEW(p,t->n,double *);
          NEW(id,t->n,unsigned int);
          l=KDTPTR(t->leftLeaf);
          i=0;
          while(i<t->n)
            {
              for(j=0;j<l->n;j++)
                {
                  p[i]=l->p[j];
                  id[i]=l->id[j];
                  i++;
                }
              l=KDTPTR(l->nextLeaf);
            }
          
          /* Re-build the tree */
          nt=BuildKDTree(&(t->ambient),t->m,t->r,t->n,p,id);
          DeleteKDtree(t);
          *kdt=nt;

          free(p);
          free(id);
        }
      else
        {
          /* Some "summary" information is only valid if we do not re-balance */
          t->volume=kdtLeft->volume+kdtRight->volume;     
          t->height=1+(kdtLeft->height>kdtRight->height?
                       kdtLeft->height:kdtRight->height);
        }
    }
}

unsigned int NearestNeighbour(unsigned int *topology,double *point,TKDtree *kdt)
{
  unsigned int id;
  double d;

  assert(kdt->n>0);

  id=NO_UINT;
  d=INF;
  SearchKDtree(0.0,topology,point,&d,&id,kdt);

  assert(id!=NO_UINT);

  return(id);
}

void NeighboursInBall(unsigned int *topology,double *point,double r,
                      unsigned int *n,unsigned int **ids,TKDtree *kdt)
{
  unsigned int m;

  assert(kdt->n>0);

  m=100;
  NEW(*ids,m,unsigned int);
  *n=0;
 
  SearchInBallKDtree(0.0,topology,point,r*r,n,&m,ids,kdt);

  if (*n==0)
    free(*ids);
}

void SampleInKDtree(double *point,TKDtree *kdt)
{
  if (kdt->isLeaf)
    RandomPointInRectangle(point,&(kdt->samplingArea));
  else
    {
      double r;

      r=randomDouble()*kdt->volume;
      
      if (r<KDTPTR(kdt->left)->volume)
        SampleInKDtree(point,KDTPTR(kdt->left));
      else
        SampleInKDtree(point,KDTPTR(kdt->right));
    }
}

void DeleteKDtree(TKDtree *kdt)
{
  if (kdt->isLeaf)
    {
      free(kdt->p);
      free(kdt->id);
      DeleteRectangle(&(kdt->boundingBox));
      DeleteRectangle(&(kdt->samplingArea));
    }
  else
    {
      DeleteKDtree(KDTPTR(kdt->left));
      DeleteKDtree(KDTPTR(kdt->right));
    }
  DeleteRectangle(&(kdt->ambient));
  free(kdt);
}