kdtree.C

/*
 * This file is part of the "Archon" framework.
 * (http://files3d.sourceforge.net)
 *
 * Copyright © 2002 by Kristian Spangsege and Brian Kristiansen.
 *
 * Permission to use, copy, modify, and distribute this software and
 * its documentation under the terms of the GNU General Public License is
 * hereby granted. No representations are made about the suitability of
 * this software for any purpose. It is provided "as is" without express
 * or implied warranty. See the GNU General Public License
 * (http://www.gnu.org/copyleft/gpl.html) for more details.
 *
 * The characters in this file are ISO8859-1 encoded.
 *
 * The documentation in this file is in "Doxygen" style
 * (http://www.doxygen.org).
 */

#include <archon/util/kdtree.H>

namespace Archon
{
  namespace Utilities
  {
    void KDTree::buildSubKDTree(int start, int end, int axis)
    {
      sort(elements.begin()+start, elements.begin()+end+1, sort_func(axis));

      int midpoint = (end-start)/2 + start;
        
      if(start<midpoint-1) // Build left tree
        buildSubKDTree(start, midpoint-1, (axis+1)%3); 
      if(end>midpoint+1) // Build right tree
        buildSubKDTree(midpoint+1, end,(axis+1)%3);
    }


    void KDTree::findSubNearest(const Math::Vector3 &position, unsigned count, int start, int end, int axis)
    {
      // Calculate current point
      int midpoint = (end-start)/2 + start;

      // Add this Photon to the priority que if close enough
      Math::Vector3 m = elements[midpoint]->getPosition();
      const double dist = Math::dist(position, m);

      if(dist < radius)
      {
        QElement *q;
        if(nearest_queue.size() >= count)
        {
          q = nearest_queue.top();
          if(dist < q->dist)
          {
            // Replace top Photon
            nearest_queue.pop();
            q->element = elements[midpoint];
            q->dist = dist; 
            nearest_queue.push(q);
            radius = nearest_queue.top()->dist;
          }
        }
        else
        {
          q = new QElement();
          q->element = elements[midpoint];
          q->dist = dist; 

          nearest_queue.push(q);
          if(nearest_queue.size() == count) radius = nearest_queue.top()->dist;
        }
      }

      if(m[axis] <= position[axis])
      {
        if(m[axis]-radius <= position[axis]) // Look in right subtree
          if(end>=midpoint+1)
            findSubNearest(position, count, midpoint+1, end, (axis+1)%3);

        if(m[axis]+radius >= position[axis]) // Look in left subtree
          if(start<=midpoint-1)
            findSubNearest(position, count, start, midpoint-1, (axis+1)%3);
      }
      else
      {
        if(elements[midpoint]->getPosition()[axis]+radius >= position[axis]) // Look in left subtree
          if(start<=midpoint-1)
            findSubNearest(position, count, start, midpoint-1, (axis+1)%3);

        if(elements[midpoint]->getPosition()[axis]-radius <= position[axis]) // Look in right subtree
          if(end>=midpoint+1)
            findSubNearest(position, count, midpoint+1, end, (axis+1)%3);
      }
    }


    void KDTree::findNearest(const Math::Vector3 &position,
                             unsigned count, double max_radius, double &_radius,
                             std::vector<const Element *> &_elements)
    {
      radius = max_radius;
      if(!elements.empty()) findSubNearest(position, count, 0, elements.size()-1, 0);
      while(!nearest_queue.empty())
      {
        QElement *q = nearest_queue.top();
        _elements.push_back(q->element);
        nearest_queue.pop();
        delete q;
      }
      _radius = radius;
    }
  }
}

---