source: GTP/trunk/Lib/Vis/Preprocessing/src/KdTree.cpp @ 2615

#include <stack>
#include <algorithm>
#include <queue>
#include "Environment.h"
#include "Mesh.h"
#include "KdTree.h"
#include "ViewCell.h"
#include "Beam.h"
#include "ViewCell.h"
#include "IntersectableWrapper.h"


using namespace std;


#define TYPE_INTERIOR -2
#define TYPE_LEAF -3


namespace GtpVisibilityPreprocessor {

int KdNode::sMailId = 1;
int KdNode::sReservedMailboxes = 1;


inline static bool ilt(Intersectable *obj1, Intersectable *obj2)
{
  return obj1->mId < obj2->mId;
}


KdNode::KdNode(KdInterior *parent):
mParent(parent), mMailbox(0), mIntersectable(NULL)
{
  if (parent)
    mDepth = parent->mDepth+1;
  else
    mDepth = 0;
}


KdInterior::~KdInterior()
{
  // recursivly destroy children
  DEL_PTR(mFront);
  DEL_PTR(mBack);
}


KdLeaf::~KdLeaf()
{
  DEL_PTR(mViewCell);  
}


KdTree::KdTree()
{ 
  mRoot = new KdLeaf(NULL, 0);
  Environment::GetSingleton()->GetIntValue("KdTree.Termination.maxNodes",
                                           mTermMaxNodes);
  Environment::GetSingleton()->GetIntValue("KdTree.Termination.maxDepth",
                                           mTermMaxDepth);
  Environment::GetSingleton()->GetIntValue("KdTree.Termination.minCost",
                                           mTermMinCost);
  Environment::GetSingleton()->GetFloatValue("KdTree.Termination.maxCostRatio",
                                             mMaxCostRatio);
  Environment::GetSingleton()->GetFloatValue("KdTree.Termination.ct_div_ci",
                                             mCt_div_ci);
  Environment::GetSingleton()->GetFloatValue("KdTree.splitBorder",
                                             mSplitBorder);
  Environment::GetSingleton()->GetFloatValue("KdTree.pvsArea",
                                             mKdPvsArea);

  Environment::GetSingleton()->GetBoolValue("KdTree.sahUseFaces",
                                            mSahUseFaces);

  char splitType[64];
  Environment::GetSingleton()->GetStringValue("KdTree.splitMethod", splitType);
  
  mSplitMethod = SPLIT_SPATIAL_MEDIAN;
  if (strcmp(splitType, "spatialMedian") == 0)
    mSplitMethod = SPLIT_SPATIAL_MEDIAN;
  else
    if (strcmp(splitType, "objectMedian") == 0)
      mSplitMethod = SPLIT_OBJECT_MEDIAN;
    else
      if (strcmp(splitType, "SAH") == 0)
        mSplitMethod = SPLIT_SAH;
      else {
        cerr<<"Wrong kd split type "<<splitType<<endl;
        exit(1);
      }
  splitCandidates = NULL;
}


KdTree::~KdTree()
{
    DEL_PTR(mRoot);
        CLEAR_CONTAINER(mKdIntersectables);
}


bool
KdTree::Construct()
{
  if (!splitCandidates)
    splitCandidates = new vector<SortableEntry *>;

  // first construct a leaf that will get subdivide
  KdLeaf *leaf = (KdLeaf *) mRoot;

  mStat.nodes = 1;

  mBox.Initialize();
  ObjectContainer::const_iterator mi;
  for ( mi = leaf->mObjects.begin();
        mi != leaf->mObjects.end();
        mi++) {
        //      cout<<(*mi)->GetBox()<<endl;
        mBox.Include((*mi)->GetBox());
  }

  cout <<"KdTree Root Box:"<<mBox<<endl;
  mRoot = Subdivide(TraversalData(leaf, mBox, 0));

  // remove the allocated array
  CLEAR_CONTAINER(*splitCandidates);
  delete splitCandidates;

  float area = GetPvsArea();
  
  SetPvsTerminationNodes(area);
  
  return true;
}

float
KdTree::GetPvsArea() const {
  return GetBox().SurfaceArea() * mKdPvsArea;
}

KdNode *
KdTree::Subdivide(const TraversalData &tdata)
{

  KdNode *result = NULL;

  priority_queue<TraversalData> tStack;
  //  stack<STraversalData> tStack;
  
  tStack.push(tdata);
  AxisAlignedBox3 backBox, frontBox;

  while (!tStack.empty()) {
        //      cout<<mStat.Nodes()<<" "<<mTermMaxNodes<<endl;
        if (mStat.Nodes() > mTermMaxNodes) {
          //    if ( GetMemUsage() > maxMemory ) {
      // count statistics on unprocessed leafs
      while (!tStack.empty()) {
                EvaluateLeafStats(tStack.top());
                tStack.pop();
      }
      break;
    }
          
        
    TraversalData data = tStack.top();
    tStack.pop();
    
    KdNode *node = SubdivideNode((KdLeaf *) data.mNode,
                                 data.mBox,
                                 backBox,
                                 frontBox
                                 );

    if (result == NULL)
      result = node;
    
    if (!node->IsLeaf()) {
      KdInterior *interior = (KdInterior *) node;
      // push the children on the stack
      tStack.push(TraversalData(interior->mBack, backBox, data.mDepth+1));
      tStack.push(TraversalData(interior->mFront, frontBox, data.mDepth+1));
      
    }
    else {
      EvaluateLeafStats(data);
    }
  }
  
  return result;

}


bool
KdTree::TerminationCriteriaMet(const KdLeaf *leaf)
{
  const bool criteriaMet =
    ((int)leaf->mObjects.size() <= mTermMinCost) ||
    (leaf->mDepth >= mTermMaxDepth);
  
  if (0 && criteriaMet)
    cerr<<"\n OBJECTS="<<(int)leaf->mObjects.size()<<endl;
  
  return criteriaMet;
}


int
KdTree::SelectPlane(KdLeaf *leaf,
                    const AxisAlignedBox3 &box,
                    float &position
                    )
{
  int axis = -1;
  
  switch (mSplitMethod)
    {
    case SPLIT_SPATIAL_MEDIAN: {
      axis = box.Size().DrivingAxis();
      position = (box.Min()[axis] + box.Max()[axis])*0.5f;
      break;
    }
    case SPLIT_SAH: {
      int objectsBack, objectsFront;
      float costRatio;
      bool mOnlyDrivingAxis = true;

      if (mOnlyDrivingAxis) {
        axis = box.Size().DrivingAxis();
        costRatio = BestCostRatio(leaf,
                                  box,
                                  axis,
                                  position,
                                  objectsBack,
                                  objectsFront);
      }
      else {
        // for all 3 axes
        costRatio = MAX_FLOAT;
        for (int i=0; i < 3; i++) {
          float p;
          float r = BestCostRatio(leaf,
                                  box,
                                  i,
                                  p,
                                  objectsBack,
                                  objectsFront);
          if (r < costRatio) {
            costRatio = r;
            axis = i;
            position = p;
          }
        }
      }
      
      if (costRatio > mMaxCostRatio) {
        //cout<<"Too big cost ratio "<<costRatio<<endl;
        axis = -1;
      }
      break;
    }
    
    }
  return axis;
}

KdNode*
KdTree::SubdivideNode(
                      KdLeaf *leaf,
                      const AxisAlignedBox3 &box,
                      AxisAlignedBox3 &backBBox,
                      AxisAlignedBox3 &frontBBox
                      )
{  
  if (TerminationCriteriaMet(leaf))
    return leaf;

  float position;
  
  // select subdivision axis
  int axis = SelectPlane( leaf, box, position );

  if (axis == -1) {
    return leaf;
  }
  
  mStat.nodes += 2;
  mStat.splits[axis]++;
  
  // add the new nodes to the tree
  KdInterior *node = new KdInterior(leaf->mParent);

  node->mAxis = axis;
  node->mPosition = position;
  node->mBox = box;
  
  backBBox = box;
  frontBBox = box;
  
  // first count ray sides
  int objectsBack = 0;
  int objectsFront = 0;
  
  backBBox.SetMax(axis, position);
  frontBBox.SetMin(axis, position);

  ObjectContainer::const_iterator mi;

  for ( mi = leaf->mObjects.begin();
        mi != leaf->mObjects.end();
        mi++) {
    // determine the side of this ray with respect to the plane
    AxisAlignedBox3 box = (*mi)->GetBox();
    if (box.Max(axis) > position ) 
      objectsFront++;
    
    if (box.Min(axis) < position )
      objectsBack++;
  }

  KdLeaf *back = new KdLeaf(node, objectsBack);
  KdLeaf *front = new KdLeaf(node, objectsFront);
 
  // replace a link from node's parent
  if (  leaf->mParent )
    leaf->mParent->ReplaceChildLink(leaf, node);

  // and setup child links
  node->SetupChildLinks(back, front);
  
  for (mi = leaf->mObjects.begin();
       mi != leaf->mObjects.end();
       mi++) {
    // determine the side of this ray with respect to the plane
    AxisAlignedBox3 box = (*mi)->GetBox();
        
        // matt: no more ref
        // for handling multiple objects: keep track of references
        //if (leaf->IsRoot()) 
        //      (*mi)->mReferences = 1; // initialise references at root
        
        // matt: no more ref
        //-- (*mi)->mReferences; // remove parent ref


    if (box.Max(axis) >= position )
    {
      front->mObjects.push_back(*mi);
      //++ (*mi)->mReferences;
    }
        
    if (box.Min(axis) < position )
    {
      back->mObjects.push_back(*mi);
      // matt: no more ref
      //  ++ (*mi)->mReferences;
    }

    mStat.objectRefs -= (int)leaf->mObjects.size();
    mStat.objectRefs += objectsBack + objectsFront;
  }

  // store objects referenced in more than one leaf
  // for easy access
  ProcessMultipleRefs(back);
  ProcessMultipleRefs(front);
  
  delete leaf;
  return node;
}


void KdTree::ProcessMultipleRefs(KdLeaf *leaf) const
{
  // find objects from multiple kd-leaves
  ObjectContainer::const_iterator oit, oit_end = leaf->mObjects.end();

  for (oit = leaf->mObjects.begin(); oit != oit_end; ++ oit)
  {
    Intersectable *object = *oit;
                
    // matt: no more ref
    /*
      if (object->mReferences > 1) {
        leaf->mMultipleObjects.push_back(object);
      }
    */
  }
}


void
KdTreeStatistics::Print(ostream &app) const
{
  app << "===== KdTree statistics ===============\n";

  app << "#N_NODES ( Number of nodes )\n" << nodes << "\n";

  app << "#N_LEAVES ( Number of leaves )\n" << Leaves() << "\n";

  app << "#N_SPLITS ( Number of splits in axes x y z dx dy dz)\n";
  for (int i=0; i<7; i++)
    app << splits[i] <<" ";
  app <<endl;

  app << "#N_RAYREFS ( Number of rayRefs )\n" <<
    rayRefs << "\n";

  app << "#N_RAYRAYREFS  ( Number of rayRefs / ray )\n" <<
    rayRefs/(double)rays << "\n";

  app << "#N_LEAFRAYREFS  ( Number of rayRefs / leaf )\n" <<
    rayRefs/(double)Leaves() << "\n";

  app << "#N_MAXOBJECTREFS  ( Max number of object refs / leaf )\n" <<
    maxObjectRefs << "\n";

  app << "#N_NONEMPTYRAYREFS  ( Number of rayRefs in nonEmpty leaves / non empty leaf )\n" <<
    rayRefsNonZeroQuery/(double)(Leaves() - zeroQueryNodes) << "\n";

  app << "#N_LEAFDOMAINREFS  ( Number of query domain Refs / leaf )\n" <<
    objectRefs/(double)Leaves() << "\n";

  //  app << setprecision(4);

  app << "#N_PEMPTYLEAVES  ( Percentage of leaves with zero query domains )\n"<<
    zeroQueryNodes*100/(double)Leaves()<<endl;

  app << "#N_PMAXDEPTHLEAVES ( Percentage of leaves at maxdepth )\n"<<
    maxDepthNodes*100/(double)Leaves()<<endl;

  app << "#N_PMINCOSTLEAVES  ( Percentage of leaves with minCost )\n"<<
    minCostNodes*100/(double)Leaves()<<endl;

  app << "#N_ADDED_RAYREFS  (Number of dynamically added ray references )\n"<<
    addedRayRefs<<endl;

  app << "#N_REMOVED_RAYREFS  (Number of dynamically removed ray references )\n"<<
    removedRayRefs<<endl;

  //  app << setprecision(4);

  //  app << "#N_CTIME  ( Construction time [s] )\n"
  //      << Time() << " \n";

  app << "===== END OF KdTree statistics ==========\n";

}



void
KdTree::EvaluateLeafStats(const TraversalData &data)
{

  // the node became a leaf -> evaluate stats for leafs
  KdLeaf *leaf = (KdLeaf *)data.mNode;

  if (data.mDepth > mTermMaxDepth)
    mStat.maxDepthNodes++;
  
  if ( (int)(leaf->mObjects.size()) < mTermMinCost)
    mStat.minCostNodes++;
  
  
  if ( (int)(leaf->mObjects.size()) > mStat.maxObjectRefs)
    mStat.maxObjectRefs = (int)leaf->mObjects.size();
  
}



void
KdTree::SortSubdivisionCandidates(
                                  KdLeaf *node,
                                  const int axis
                                  )
{
  CLEAR_CONTAINER(*splitCandidates);
  //splitCandidates->clear();
  
  int requestedSize = 2*(int)node->mObjects.size();
  
  // creates a sorted split candidates array
  if (splitCandidates->capacity() > 500000 &&
      requestedSize < (int)(splitCandidates->capacity()/10) ) {         
    delete splitCandidates;
    splitCandidates = new vector<SortableEntry *>;
  }
  
  splitCandidates->reserve(requestedSize);
  
  // insert all queries 
  for(ObjectContainer::const_iterator mi = node->mObjects.begin();
      mi != node->mObjects.end();
      mi++) 
  {
    AxisAlignedBox3 box = (*mi)->GetBox();

    splitCandidates->push_back(new SortableEntry(SortableEntry::BOX_MIN,
                                                 box.Min(axis),
                                                 *mi)
                               );
    
    splitCandidates->push_back(new SortableEntry(SortableEntry::BOX_MAX,
                                                 box.Max(axis),
                                                 *mi)
                               );
  }
  
  stable_sort(splitCandidates->begin(), splitCandidates->end(), iltS);
}


float
KdTree::BestCostRatio(
                      KdLeaf *node,
                      const AxisAlignedBox3 &box,
                      const int axis,
                      float &position,
                      int &objectsBack,
                      int &objectsFront
                      )
{

#define DEBUG_COST 0

#if DEBUG_COST
  static int nodeId = -1;
  char filename[256];
  
  static int lastAxis = 100;
  if (axis <= lastAxis)
        nodeId++;

  lastAxis = axis;
  
  sprintf(filename, "sah-cost%d-%d.log", nodeId, axis);
  ofstream costStream;
  
  if (nodeId < 100)
    costStream.open(filename);

#endif
  
  SortSubdivisionCandidates(node, axis);
  
  // go through the lists, count the number of objects left and right
  // and evaluate the following cost funcion:
  // C = ct_div_ci  + (ol + or)/queries

  float totalIntersections = 0.0f;
  vector<SortableEntry *>::const_iterator ci;

  for(ci = splitCandidates->begin();
      ci < splitCandidates->end();
      ci++) 
    if ((*ci)->type == SortableEntry::BOX_MIN) {
      totalIntersections += (*ci)->intersectable->IntersectionComplexity();
    }
        
  float intersectionsLeft = 0;
  float intersectionsRight = totalIntersections;
        
  int objectsLeft = 0, objectsRight = (int)node->mObjects.size();
  
  float minBox = box.Min(axis);
  float maxBox = box.Max(axis);
  float boxArea = box.SurfaceArea();
  
  float minBand = minBox + mSplitBorder*(maxBox - minBox);
  float maxBand = minBox + (1.0f - mSplitBorder)*(maxBox - minBox);
  
  float minSum = 1e20f;
  
  for(ci = splitCandidates->begin();
      ci < splitCandidates->end();
      ci++) {
    switch ((*ci)->type) {
    case SortableEntry::BOX_MIN:
      objectsLeft++;
      intersectionsLeft += (*ci)->intersectable->IntersectionComplexity();
      break;
    case SortableEntry::BOX_MAX:
      objectsRight--;
      intersectionsRight -= (*ci)->intersectable->IntersectionComplexity();
      break;
    } // switch

    if ((*ci)->value > minBand && (*ci)->value < maxBand) {
      AxisAlignedBox3 lbox = box;
      AxisAlignedBox3 rbox = box;
      lbox.SetMax(axis, (*ci)->value);
      rbox.SetMin(axis, (*ci)->value);

      float sum;
      if (mSahUseFaces)
        sum = intersectionsLeft*lbox.SurfaceArea() + intersectionsRight*rbox.SurfaceArea();
      else
        sum = objectsLeft*lbox.SurfaceArea() + objectsRight*rbox.SurfaceArea();
      
      //      cout<<"pos="<<(*ci).value<<"\t q=("<<ql<<","<<qr<<")\t r=("<<rl<<","<<rr<<")"<<endl;
      //      cout<<"cost= "<<sum<<endl;

#if DEBUG_COST
      if (nodeId < 100) {
        float oldCost = mSahUseFaces ? totalIntersections : node->mObjects.size();
        float newCost = mCt_div_ci + sum/boxArea;
        float ratio = newCost/oldCost;
        costStream<<(*ci)->value<<" "<<ratio<<endl;
      }
#endif
      
      if (sum < minSum) {
        minSum = sum;
        position = (*ci)->value;
        
        objectsBack = objectsLeft;
        objectsFront = objectsRight;
      }
    }
  } // for ci
  
  float oldCost = mSahUseFaces ? totalIntersections : node->mObjects.size();
  float newCost = mCt_div_ci + minSum/boxArea;
  float ratio = newCost/oldCost;
  
#if 0
  cout<<"===================="<<endl;
  cout<<"costRatio="<<ratio<<" pos="<<position<<" t="<<(position - minBox)/(maxBox - minBox)
      <<"\t o=("<<objectsBack<<","<<objectsFront<<")"<<endl;
#endif
  return ratio;
}

int
KdTree::CastRay(
                Ray &ray
                )
{
  
  int hits = 0;
  
  stack<RayTraversalData> tStack;
  
  float maxt = 1e6;
  float mint = 0;

  //  ray.ComputeInvertedDir();
  Intersectable::NewMail();

  if (!mBox.GetMinMaxT(ray, &mint, &maxt))
    return 0;

  if (mint < 0)
    mint = 0;

  
  maxt += Limits::Threshold;
  
  Vector3 entp = ray.Extrap(mint);
  Vector3 extp = ray.Extrap(maxt);
  
  KdNode *node = mRoot;
  KdNode *farChild;
  float position;
  int axis;

  
  while (1) {
    if (!node->IsLeaf()) {
      KdInterior *in = (KdInterior *) node;
      position = in->mPosition;
      axis = in->mAxis;

      if (entp[axis] <= position) {
        if (extp[axis] <= position) {
          node = in->mBack;
          // cases N1,N2,N3,P5,Z2,Z3
          continue;
        }
        else {
          // case N4
          node = in->mBack;
          farChild = in->mFront;
        }
      }
      else {
        if (position <= extp[axis]) {
          node = in->mFront;
          // cases P1,P2,P3,N5,Z1
          continue;
        }
        else {
          node = in->mFront;
          farChild = in->mBack;
          // case P4
        }
      }
      // $$ modification 3.5.2004 - hints from Kamil Ghais
      // case N4 or P4
      float tdist = (position - ray.GetLoc(axis)) / ray.GetDir(axis);
      tStack.push(RayTraversalData(farChild, extp, maxt));
      extp = ray.GetLoc() + ray.GetDir()*tdist;
      maxt = tdist;
    }
    else {
      // compute intersection with all objects in this leaf
      KdLeaf *leaf = (KdLeaf *) node;
      if (ray.mFlags & Ray::STORE_KDLEAVES)
        ray.kdLeaves.push_back(leaf);
          
      ObjectContainer::const_iterator mi;
      for ( mi = leaf->mObjects.begin();
            mi != leaf->mObjects.end();
            mi++) {
        Intersectable *object = *mi;
        if (!object->Mailed() ) {
          object->Mail();
          if (ray.mFlags & Ray::STORE_TESTED_OBJECTS)
                        ray.testedObjects.push_back(object);
                  
          static int oi=1;
          if (MeshDebug) 
            cout<<"Object "<<oi++;
          
          hits += object->CastRay(ray);
          
          if (MeshDebug) {
            if (!ray.intersections.empty()) 
              cout<<"nearest t="<<ray.intersections[0].mT<<endl;
            else
              cout<<"nearest t=-INF"<<endl;
          }       
        }
      }
          
      if (hits && ray.GetType() == Ray::LOCAL_RAY)
        if (ray.intersections[0].mT <= maxt)
          break;
      
      // get the next node from the stack
      if (tStack.empty())
        break;
      
      entp = extp;
      mint = maxt;
      if (ray.GetType() == Ray::LINE_SEGMENT && mint > 1.0f)
        break;
      
      RayTraversalData &s  = tStack.top();
      node = s.mNode;
      extp = s.mExitPoint;
      maxt = s.mMaxT;
      tStack.pop();
    }
  }
  return hits;
}

int KdTree::CastLineSegment(const Vector3 &origin,
                            const Vector3 &termination,
                            ViewCellContainer &viewcells)
{
  int hits = 0;

  float mint = 0.0f, maxt = 1.0f;
  const Vector3 dir = termination - origin;
  
  stack<RayTraversalData> tStack;
  
  Intersectable::NewMail();
  
  //maxt += Limits::Threshold;
  
  Vector3 entp = origin;
  Vector3 extp = termination;
  
  KdNode *node = mRoot;
  KdNode *farChild;
  
  float position;
  int axis;

  while (1)
  {
    if (!node->IsLeaf())
    {
      KdInterior *in = static_cast<KdInterior *>(node);
      position = in->mPosition;
      axis = in->mAxis;
      
      if (entp[axis] <= position)
      {
        if (extp[axis] <= position)
        {
          node = in->mBack;
          // cases N1,N2,N3,P5,Z2,Z3
          continue;
        } 
        else
        {
          // case N4
          node = in->mBack;
          farChild = in->mFront;
        }
      }
      else
      {
        if (position <= extp[axis])
        {
          node = in->mFront;
          // cases P1,P2,P3,N5,Z1
          continue;
        }
        else
        {
          node = in->mFront;
          farChild = in->mBack;
          // case P4
        }
      }
      
      // $$ modification 3.5.2004 - hints from Kamil Ghais
      // case N4 or P4
      float tdist = (position - origin[axis]) / dir[axis];
      //tStack.push(RayTraversalData(farChild, extp, maxt)); //TODO
      extp = origin + dir * tdist;
      maxt = tdist;
    }
    else
    {
      // compute intersection with all objects in this leaf
      KdLeaf *leaf = static_cast<KdLeaf *>(node);
      
      // add view cell to intersections
      ViewCell *vc = leaf->mViewCell;
      
      if (!vc->Mailed())
      {
        vc->Mail();
        viewcells.push_back(vc);
        ++ hits;
      }
      
      // get the next node from the stack
      if (tStack.empty())
        break;
      
      entp = extp;
      mint = maxt;
      
      RayTraversalData &s  = tStack.top();
      node = s.mNode;
      extp = s.mExitPoint;
      maxt = s.mMaxT;
      tStack.pop();
    }
  }
  return hits;
}

void
KdTree::CollectKdObjects(const AxisAlignedBox3 &box,
                         ObjectContainer &objects
                        )
{
  stack<KdNode *> nodeStack;
  
  nodeStack.push(mRoot);

  while (!nodeStack.empty()) {
    KdNode *node = nodeStack.top();
    nodeStack.pop();
    if (node->IsLeaf() || node->mPvsTermination == 1)  {
      Intersectable *object = GetOrCreateKdIntersectable(node);
      if (!node->Mailed()) {
        node->Mail();
        objects.push_back(object);
      }
    }
    else {
      KdInterior *interior = (KdInterior *)node;
          
      if ( box.Max()[interior->mAxis] > interior->mPosition )
        nodeStack.push(interior->mFront);
      
      if (box.Min()[interior->mAxis] < interior->mPosition)
        nodeStack.push(interior->mBack);
    }
  } // while
}

void
KdTree::CollectSmallKdObjects(const AxisAlignedBox3 &box,
                                                          ObjectContainer &objects,
                                                          const float maxAreaFrac
                                                          )
{
  stack<KdNode *> nodeStack;
  
  nodeStack.push(mRoot);
  float maxArea = GetPvsArea()*maxAreaFrac;
  
  while (!nodeStack.empty()) {
    KdNode *node = nodeStack.top();
    nodeStack.pop();
        if (!node->Mailed()) {
          node->Mail();
          if (node->IsLeaf() || GetSurfaceArea(node) <= maxArea)  {
                Intersectable *object = GetOrCreateKdIntersectable(node);
                if (!node->Mailed()) {
                  objects.push_back(object);
                }
          }
          else {
                KdInterior *interior = (KdInterior *)node;
                
                if ( box.Max()[interior->mAxis] > interior->mPosition )
                  nodeStack.push(interior->mFront);
                
                if (box.Min()[interior->mAxis] < interior->mPosition)
                  nodeStack.push(interior->mBack);
          }
        }
  } // while
}


void
KdTree::CollectObjects(const AxisAlignedBox3 &box,
                       ObjectContainer &objects)
{
  stack<KdNode *> nodeStack;

  nodeStack.push(mRoot);

  while (!nodeStack.empty()) {
    KdNode *node = nodeStack.top();
    nodeStack.pop();
    if (node->IsLeaf()) {
      KdLeaf *leaf = (KdLeaf *)node;
      for (int j=0; j < leaf->mObjects.size(); j++) {
        Intersectable *object = leaf->mObjects[j];
        if (!object->Mailed() && Overlap(box, object->GetBox())) {
          object->Mail();
          objects.push_back(object);
        }
      }
    } else {
      KdInterior *interior = (KdInterior *)node;

      if ( box.Max()[interior->mAxis] > interior->mPosition )
        nodeStack.push(interior->mFront);
      
      if (box.Min()[interior->mAxis] < interior->mPosition)
        nodeStack.push(interior->mBack);
    }
  }
}

void
KdTree::CollectObjects(KdNode *n, ObjectContainer &objects)
{
  stack<KdNode *> nodeStack;

  nodeStack.push(n);

  while (!nodeStack.empty()) {
    KdNode *node = nodeStack.top();
    nodeStack.pop();
    if (node->IsLeaf()) {
      KdLeaf *leaf = (KdLeaf *)node;
      for (int j=0; j < leaf->mObjects.size(); j++) {
        Intersectable *object = leaf->mObjects[j];
        if (!object->Mailed()) {
          object->Mail();
          objects.push_back(object);
        }
      }
    } else {
      KdInterior *interior = (KdInterior *)node;
      nodeStack.push(interior->mFront);
      nodeStack.push(interior->mBack);
    }
  }
}

// Find random neighbor which was not mailed
KdNode *
KdTree::FindRandomNeighbor(KdNode *n,
                           bool onlyUnmailed
                           )
{
  stack<KdNode *> nodeStack;
  
  nodeStack.push(mRoot);

  AxisAlignedBox3 box = GetBox(n);
  int mask = rand();

  while (!nodeStack.empty()) {
    KdNode *node = nodeStack.top();
    nodeStack.pop();
    if (node->IsLeaf()) {
      if ( node != n && (!onlyUnmailed || !node->Mailed()) )
        return node;
    } else {
      KdInterior *interior = (KdInterior *)node;
      if (interior->mPosition > box.Max(interior->mAxis))
        nodeStack.push(interior->mBack);
      else
        if (interior->mPosition < box.Min(interior->mAxis))
          nodeStack.push(interior->mFront);
        else {
          // random decision
          if (mask&1)
            nodeStack.push(interior->mBack);
          else
            nodeStack.push(interior->mFront);
          mask = mask>>1;
        }
    }
  }
  
  return NULL;
}

int
KdTree::FindNeighbors(KdNode *n,
                      vector<KdNode *> &neighbors,
                      bool onlyUnmailed
                      )
{
  stack<KdNode *> nodeStack;
  
  nodeStack.push(mRoot);

  AxisAlignedBox3 box = GetBox(n);

  while (!nodeStack.empty()) {
    KdNode *node = nodeStack.top();
    nodeStack.pop();
    if (node->IsLeaf()) {
      if ( node != n && (!onlyUnmailed || !node->Mailed()) ) 
        neighbors.push_back(node);
    }
    else {
      KdInterior *interior = (KdInterior *)node;
      if (interior->mPosition > box.Max(interior->mAxis))
        nodeStack.push(interior->mBack);
      else {
        if (interior->mPosition < box.Min(interior->mAxis))
          nodeStack.push(interior->mFront);
        else {
          // random decision
          nodeStack.push(interior->mBack);
          nodeStack.push(interior->mFront);
        }
      }
    }
  }
  
  return (int)neighbors.size();
}

// Find random neighbor which was not mailed
KdNode *
KdTree::GetRandomLeaf(const Plane3 &plane)
{
  stack<KdNode *> nodeStack;
  
  nodeStack.push(mRoot);
  
  int mask = rand();
  
  while (!nodeStack.empty()) {
    KdNode *node = nodeStack.top();
    nodeStack.pop();
    if (node->IsLeaf()) {
      return node;
    } else {
      KdInterior *interior = (KdInterior *)node;
      KdNode *next;
        if (GetBox(interior->mBack).Side(plane) < 0)
          next = interior->mFront;
        else
          if (GetBox(interior->mFront).Side(plane) < 0)
            next = interior->mBack;
          else {
            // random decision
            if (mask&1)
              next = interior->mBack;
            else
              next = interior->mFront;
            mask = mask>>1;
          }
        nodeStack.push(next);
    }
  }
  
  
  return NULL;
}

void
KdTree::CollectLeaves(vector<KdLeaf *> &leaves)
{
  stack<KdNode *> nodeStack;
  nodeStack.push(mRoot);

  while (!nodeStack.empty()) {
    KdNode *node = nodeStack.top();
    nodeStack.pop();
    if (node->IsLeaf()) {
      KdLeaf *leaf = (KdLeaf *)node;
      leaves.push_back(leaf);
    } else {
      KdInterior *interior = (KdInterior *)node;
      nodeStack.push(interior->mBack);
      nodeStack.push(interior->mFront);
    }
  }
}

void
KdTree::CreateAndCollectViewCells(ViewCellContainer &vc) const
{
  stack<KdNode *> nodeStack;
  nodeStack.push(mRoot);

  while (!nodeStack.empty()) {
    KdNode *node = nodeStack.top();
    nodeStack.pop();
    if (node->IsLeaf()) {
      KdLeaf *leaf = (KdLeaf *)node;
          // kdtree used as view cell container => create view cell
          KdViewCell *viewCell = new KdViewCell();
          leaf->mViewCell = viewCell;
          // push back pointer to this leaf
          viewCell->mLeaves[0] = leaf;
      vc.push_back(viewCell);
    } else {
      KdInterior *interior = (KdInterior *)node;
      nodeStack.push(interior->mBack);
      nodeStack.push(interior->mFront);
    }
  }
}


KdNode *
KdTree::GetRandomLeaf(const bool onlyUnmailed)
{
  stack<KdNode *> nodeStack;
  nodeStack.push(mRoot);
  
  int mask = rand();
        
  while (!nodeStack.empty()) {
    KdNode *node = nodeStack.top();
    nodeStack.pop();
    if (node->IsLeaf()) {
      if ( (!onlyUnmailed || !node->Mailed()) )
                                return node;
    } else {
      KdInterior *interior = (KdInterior *)node;
                        // random decision
                        if (mask&1)
                                nodeStack.push(interior->mBack);
                        else
                                nodeStack.push(interior->mFront);
                        mask = mask>>1;
                }
        }
  return NULL;
}


int KdTree::CastBeam(Beam &beam)
{
  stack<KdNode *> nodeStack;
  nodeStack.push(mRoot);
  
  while (!nodeStack.empty()) {
    KdNode *node = nodeStack.top();
    nodeStack.pop();
        
        int side = beam.ComputeIntersection(GetBox(node));
        switch (side) {
        case -1:
          beam.mKdNodes.push_back(node);
          break;
        case 0:
          if (node->IsLeaf())
                beam.mKdNodes.push_back(node);
          else {
                KdInterior *interior = (KdInterior *)node;
                KdNode *first = interior->mBack;
                KdNode *second = interior->mFront;
                
                if (interior->mAxis < 3) {
                  // spatial split -> decide on the order of the nodes
                  if (beam.mPlanes[0].mNormal[interior->mAxis] > 0)
                        swap(first, second);
                }

                nodeStack.push(first);
                nodeStack.push(second);
          }
          break;
          // default: cull
        }
  }

  if (beam.mFlags & Beam::STORE_OBJECTS)
  {
          vector<KdNode *>::const_iterator it, it_end = beam.mKdNodes.end();
        
          Intersectable::NewMail();
          for (it = beam.mKdNodes.begin(); it != it_end; ++ it)
          {
                  CollectObjects(*it, beam.mObjects);
          }
  }

  return (int)beam.mKdNodes.size();
}


void KdTree::ExportBinLeaf(OUT_STREAM &stream, KdLeaf *leaf)
{
  ObjectContainer::const_iterator it, it_end = leaf->mObjects.end();
  
  int type = TYPE_LEAF;
  int size = (int)leaf->mObjects.size();
  
  stream.write(reinterpret_cast<char *>(&type), sizeof(int));
  stream.write(reinterpret_cast<char *>(&size), sizeof(int));
  
  for (it = leaf->mObjects.begin(); it != it_end; ++ it)
    {   
      Intersectable *obj = *it;
      int id = obj->mId;                
      
      //stream.write(reinterpret_cast<char *>(&origin), sizeof(Vector3));
      stream.write(reinterpret_cast<char *>(&id), sizeof(int));
    }
}


KdLeaf *KdTree::ImportBinLeaf(IN_STREAM &stream, 
                              KdInterior *parent,
                              const ObjectContainer &objects)
{
  int leafId = TYPE_LEAF;
  int objId = leafId;
  int size;
  
  stream.read(reinterpret_cast<char *>(&size), sizeof(int));
  KdLeaf *leaf = new KdLeaf(parent, size);

  MeshInstance dummyInst(NULL);
        
  // read object ids
  // note: this can also be done geometrically
  for (int i = 0; i < size; ++ i)
    {   
      stream.read(reinterpret_cast<char *>(&objId), sizeof(int));
      dummyInst.SetId(objId);
      
      ObjectContainer::const_iterator oit =
        lower_bound(objects.begin(), objects.end(), (Intersectable *)&dummyInst, ilt);
      
      if ((oit != objects.end()) && ((*oit)->GetId() == objId))
        leaf->mObjects.push_back(*oit);
      else
        Debug << "error: object with id " << objId << " does not exist" << endl;
    }
  
  return leaf;
}


void KdTree::ExportBinInterior(OUT_STREAM &stream, KdInterior *interior)
{
        int interiorid = TYPE_INTERIOR;
        stream.write(reinterpret_cast<char *>(&interiorid), sizeof(int));

        int axis = interior->mAxis;
        float pos = interior->mPosition;

        stream.write(reinterpret_cast<char *>(&axis), sizeof(int));
        stream.write(reinterpret_cast<char *>(&pos), sizeof(float));
}


KdInterior *KdTree::ImportBinInterior(IN_STREAM  &stream, KdInterior *parent)
{
        KdInterior *interior = new KdInterior(parent);

        int axis;
        float pos;

        stream.read(reinterpret_cast<char *>(&axis), sizeof(int));
        stream.read(reinterpret_cast<char *>(&pos), sizeof(float));

        interior->mAxis = axis;
        interior->mPosition = pos;

        return interior;
}


bool KdTree::ExportBinTree(const string &filename)
{
        OUT_STREAM stream(filename.c_str(), OUT_BIN_MODE);
        
        if (!stream.is_open()) return false;

        // export binary version of mesh
        queue<KdNode *> tStack;
        tStack.push(mRoot);

        while(!tStack.empty())
        {
                KdNode *node = tStack.front();
                tStack.pop();
                
                if (node->IsLeaf())
                {
                        //Debug << "l";
                        ExportBinLeaf(stream, static_cast<KdLeaf *>(node));
                }
                else
                {
                        //Debug << "i";
                        KdInterior *interior = static_cast<KdInterior *>(node);

                        ExportBinInterior(stream, interior);
                        
                        tStack.push(interior->mFront);
                        tStack.push(interior->mBack);
                }
        }

        return true;
}


KdNode *KdTree::ImportNextNode(IN_STREAM &stream, 
                                                           KdInterior *parent,
                                                           const ObjectContainer &objects)
{
        int nodeType;
        stream.read(reinterpret_cast<char *>(&nodeType), sizeof(int));

        if (nodeType == TYPE_LEAF)
                return ImportBinLeaf(stream, static_cast<KdInterior *>(parent), objects);

        if (nodeType == TYPE_INTERIOR)
                return ImportBinInterior(stream, static_cast<KdInterior *>(parent));

        Debug << "error! loading failed!" << endl;
        return NULL;
}


bool KdTree::ImportBinTree(const string &filename, ObjectContainer &objects)
{
        // export binary version of mesh
        queue<TraversalData> tStack;
        IN_STREAM stream(filename.c_str(), IN_BIN_MODE);

        if (!stream.is_open()) return false;

        // sort objects by their id
        //      if (!is_sorted(objects.begin(), objects.end(), ilt))
        sort(objects.begin(), objects.end(), ilt);

        mBox.Initialize();
        ObjectContainer::const_iterator oit, oit_end = objects.end();

        ///////////////////////////
        //-- compute bounding box of object space

        for (oit = objects.begin(); oit != oit_end; ++ oit)
        {
                const AxisAlignedBox3 box = (*oit)->GetBox();
                mBox.Include(box);
        }

        // hack: we make a new root
        DEL_PTR(mRoot);

        mRoot = ImportNextNode(stream, NULL, objects);

        tStack.push(TraversalData(mRoot, mBox, 0));
        mStat.Reset();
        mStat.nodes = 1;

        while(!tStack.empty())
        {
                TraversalData tData = tStack.front();
                tStack.pop();

                KdNode *node = tData.mNode;

                if (!node->IsLeaf())
                {
                        mStat.nodes += 2;

                        //Debug << "i" ;
                        KdInterior *interior = static_cast<KdInterior *>(node);
                        interior->mBox = tData.mBox;

                        KdNode *front = ImportNextNode(stream, interior, objects);
                        KdNode *back = ImportNextNode(stream, interior, objects);

                        interior->SetupChildLinks(back, front);

                        ++ mStat.splits[interior->mAxis];

                        // compute new bounding box
                        AxisAlignedBox3 frontBox, backBox;

                        tData.mBox.Split(interior->mAxis, 
                                interior->mPosition, 
                                frontBox, 
                                backBox);

                        tStack.push(TraversalData(front, frontBox, tData.mDepth + 1));                  
                        tStack.push(TraversalData(back, backBox, tData.mDepth + 1));
                }
                else
                {
                        EvaluateLeafStats(tData);
                        //cout << "l";
                }
        }

        float area = GetBox().SurfaceArea()*mKdPvsArea;

        SetPvsTerminationNodes(area);

        Debug << mStat << endl;

        return true;
}


KdIntersectable *
KdTree::GetOrCreateKdIntersectable(KdNode *node)
{
        if (node == NULL)
                return NULL;

        if (node->mIntersectable == NULL) 
        {
                // not in map => create new entry
                const int id = (int)mKdIntersectables.size();
                KdIntersectable *kdObj = new KdIntersectable(node, GetBox(node));
                node->mIntersectable = kdObj;

                mKdIntersectables.push_back(kdObj);
                kdObj->SetId(id);
#ifdef USE_BIT_PVS
                // hack: for kd pvs the kd intersecables are the pvs objects
                ObjectPvsIterator::sObjects.push_back(kdObj);
#endif
        }

        return node->mIntersectable;
}


void
KdTree::SetPvsTerminationNodes(const float maxArea)
{
  stack<KdNode *> nodeStack;
  
  nodeStack.push(mRoot);

  float area = 0.0f;
  float radius = 0.0f;
  int nodes = 0;
  
  while (!nodeStack.empty()) {
    KdNode *node = nodeStack.top();
    nodeStack.pop();
        
    node->mPvsTermination = 0;
    if (node->IsLeaf() || (GetSurfaceArea(node) <= maxArea) ) {
      area += GetSurfaceArea(node);
      radius += GetBox(node).Radius();
      nodes++;
      node->mPvsTermination = 1;
      // create dummy kd intersectable
      Intersectable *object = GetOrCreateKdIntersectable(node);
    } else {
      KdInterior *interior = (KdInterior *)node;
      nodeStack.push(interior->mFront);
      nodeStack.push(interior->mBack);
    }
  }
  
  if (nodes) {
    area /= nodes;
    radius /= nodes;
    cout<<"Number of nodes for storing in the PVS = "<<nodes<<endl;
    cout<<"Average rel. node area = "<<area/GetSurfaceArea(mRoot)<<endl;
    cout<<"Average rel. node radius = "<<radius/GetBox(mRoot).Radius()<<endl;
    cout<<"Avg node radius = "<<radius<<endl;
  }
  
}


/*KdNode *
KdTree::GetPvsNode(const Triangle3 &tri) const
{
  KdNode *node = mRoot;
  
  while (node->mPvsTermination == 0 ) 
  {
          KdInterior *inter = (KdInterior *)node;
          if (point[inter->mAxis] < inter->mPosition)
                  node = inter->mBack;
          else
                  node = inter->mFront;
  }

  return node;
}*/



KdNode *
KdTree::GetPvsNode(const Vector3 &point) const
{
  KdNode *node = mRoot;
  
  while (node->mPvsTermination == 0 ) {
    KdInterior *inter = (KdInterior *)node;
    if (point[inter->mAxis] < inter->mPosition)
      node = inter->mBack;
    else
      node = inter->mFront;
  }
  
  return node;
}

KdNode *
KdTree::GetNode(const Vector3 &point,
                                const float maxArea) const
{
  KdNode *node = mRoot;
  
  while (!node->IsLeaf() && (GetSurfaceArea(node) > maxArea) ) {
    KdInterior *inter = (KdInterior *)node;
    if (point[inter->mAxis] < inter->mPosition)
      node = inter->mBack;
    else
      node = inter->mFront;
  }
  
  return node;
}


void KdTree::GetBoxIntersections(const AxisAlignedBox3 &box,
                                 vector<KdLeaf *> &leaves)
{
  stack<KdNode *> tStack;
  
  tStack.push(mRoot);
  
  while (!tStack.empty())
  {
          KdNode *node = tStack.top();
          tStack.pop();

          if (node->IsLeaf())
          {
                  leaves.push_back(static_cast<KdLeaf *>(node));
          }
          else // interior
          {
                  KdInterior *interior = static_cast<KdInterior *>(node);

                  if (box.Max(interior->mAxis) >= interior->mPosition)
                  {
                          tStack.push(interior->mFront);
                  }

                  if (box.Min(interior->mAxis) < interior->mPosition)
                  {
                          tStack.push(interior->mBack);
                  }
          }
  }
}



}