source: GTP/trunk/Lib/Vis/Preprocessing/src/havran/ktball.cpp @ 2592

// ===================================================================
// $Id: $
//
// ktball.cpp
//     Implementation of interface functions for CKTB trees
//
// REPLACEMENT_STRING
//
// Copyright by Vlastimil Havran, 2007 - email to "vhavran AT seznam.cz"
// Initial coding by Vlastimil Havran, February 2007

// GOLEM library
#include "ktb.h"
#include "ktbai.h"
#include "ktball.h"
#include "ktbtrav.h"
#include "Environment.h"
#include "raypack.h"
#include "Mesh.h"

// standard headers
#include <fstream>
#include <iomanip>
#include <stack>

namespace GtpVisibilityPreprocessor {

// Static data structures fo CKTBTraversal class
GALIGN16 CKTBTraversal::SStackElem
CKTBTraversal::stack[CKTBTraversal::MAX_TRAVERSAL_HEIGHT];

CKTBTraversal::SStackElem2
CKTBTraversal::stack2[CKTBTraversal::MAX_TRAVERSAL_HEIGHT];

CKTBTraversal::SStackElem3
CKTBTraversal::stack3[CKTBTraversal::MAX_TRAVERSAL_HEIGHT];

CKTBTraversal::SStackElem4
CKTBTraversal::stack4[CKTBTraversal::MAX_TRAVERSAL_HEIGHT];

  
// ------------------------------------------------------------------
// CKTB class
// 
// Binary Space Partitioning Tree, that handles bounded objects only
// during intersection computation other way

// default constructor
CKTB::CKTB():
  builtUp(false),
  buildClass(NULL), traversalClass(NULL), sizeBuffer(0),buffer(0),
  indexRead(0), indexWrite(0)
{

}

// only for creating building class of CKTB tree
CKTBAllocManPredecessor*
CKTB::AllocBuildClass()
{
  CKTBAllocManPredecessor *bc = 0;
  bool useArray = true;
  
  // this should be already initialised, but we want to be sure
  if (useArray) {
    // the implementation based on arrays
    bc = new CKTBABuildUp(); // file ktbai.cpp
  }
  else {
    // the implementation based on lists
    // bc = new CKTBBuildUp(); // file ktbi.cpp
  }
    
  if (bc == NULL) {
    cerr << "The building class for KTB cannot be allocated"<< endl;
    exit(3);
  }

  return bc;
}

void
CKTB::AllocTraversalClass()
{
  if (traversalClass != NULL)
    delete traversalClass;

  traversalClass = NULL;

  // if we use min boxes idea in the tree

  Environment::GetSingleton()->GetBoolValue("BSP.minBoxes.use",
                                            makeMinBoxes);
  Environment::GetSingleton()->GetBoolValue("BSP.minBoxes.tight",
                                            makeTightMinBoxes);
  // the number of cells along ray path
  const int size1D = 150;

  // cerr<<"th1"<<endl;
  // $$JB tmp
  size2D = 1;
  // cerr<<"size2d="<<size2D<<endl;
  // initial index
  index2D = 0;
  AllocBuffer(size2D, size1D);
  //  cerr<<"th2"<<endl;

#if 0  
  // -------------------------------------------------------------------
  if (makeMinBoxes) {
    // This class uses information about the box
    // inside from which the ray starts from
    traversalClass = new CKTBMinBoxesTraversal(size1D);
  }
  else {
    // normal traversal always from root
    traversalClass = new CKTBTraversal; 
  }
#else
  // cerr<<"th3"<<endl;

  traversalClass = new CKTBTraversal; 
#endif

  assert(traversalClass);

  return;
}

// default destructor
CKTB::~CKTB()
{
  // Print the statistics
  PrintStats();
  
  Remove();
}

void
CKTB::Remove()
{
  if (buildClass) {
    // DEBUG << "Deleting build class from CKTB" << endl;
    buildClass->Remove();
    delete buildClass;
    buildClass = NULL;
  }

  if (traversalClass) {
    delete traversalClass;
    traversalClass = NULL;
  }

  return;
}

void
CKTB::ProvideID(ostream& app)
{
  app << "#CASDS = CKTB - KD-tree for only bounded objects\n";
  //app << (int)GetID() << "\n";
  
  //app << "#ActMaxDepth (Dreach)\tActual maximal depth of the KD-tree"
  //    << " reached\n";
  //app << world->_statistics->actMaxDepth << "\n";

  ProvidePars(app);
}

void
CKTB::ProvidePars(ostream& app)
{
  app << "#NumberOfObjects\t\tNumber of objects inside kd-tree\n";
  app << countObjects << "\n";
  
  app << "#BBox\t\t\tSize of axis-aligned box of the whole scene\n"
      << bbox.Max().x - bbox.Min().x << ","
      << bbox.Max().y - bbox.Min().y << ","
      << bbox.Max().z - bbox.Min().z << "\n";

#if 0  
  bool tempvar;
  Environment::GetSingleton()->GetBoolValue("BSP.pruning", tempvar);
  if (tempvar) {
    app << "#BSP.pruning \t\tPruning of CKTB disabled/enabled\n";
    app << tempvar << "\n";
  }
  app << "#BSP.constrMethod \tBuilding up method for KD-tree\n";
#endif  

  if (buildClass)
    buildClass->ProvideID(app);

  return;
}

void
CKTB::PrintStats()
{
  if (traversalClass)
    traversalClass->PrintStatsTR(cout);
}

// builds up CKTB tree 
void
CKTB::BuildUp(const ObjectContainer &alist)
{
  cerr<<"KTB buildup\n";
  Remove();

  // Create the list of objects to be processed.
  ObjectContainer *objlist = new ObjectContainer;

  // The minimum size
  bbox.Initialize();
  
  // Copy only finite (not invisible and bounded) objects from
  // the original list to 'objlist'
   
  // copy the pointers to objects to the array
  int i = 0;
  AxisAlignedBox3 objbox;  
  bool hasUnboundeds = false;
  countObjects = 0;
  for (ObjectContainer::iterator it = ((ObjectContainer*)&alist)->begin();
       it != alist.end(); it++, i++) {
    Intersectable* obj = *it;
    objbox = (*it)->GetBox();
    // invisible objects are not added - lights etc.
    bbox.Include(objbox);
    // This is a regular object, add it to the list
    objlist->push_back(obj);
  } // for
    
  // prepare building up for CKTB tree
  //  BuildingUp(*objlist);
  BuildingUp(*objlist, NULL, NULL, true);

  cerr<<"KTB buildup3\n";

  delete objlist;

  return;
}

bool
CKTB::CheckBoxes(ObjectContainer &objlist)
{
  // the minimum size of the box at any dimension
  const float epsilon = Limits::Small * 5.0f;

  for (ObjectContainer::iterator oi = objlist.begin();
       oi != objlist.end(); oi++) {
    AxisAlignedBox3 box = (*oi)->GetBox();
    Vector3 diff = box.Diagonal();
    for (int i = 0; i < 3; i++) {
      float d = fabs(diff[i]);
#if 0
      if (d < epsilon) {
        CObject3D &obj = **(oi);
        cerr << "CKTB::CheckBoxes - the scene incorrectly prepared, "
              << " the size of the box = " << d << " in dimension "
              << i << " is" << " below threshold = " << epsilon << endl;
        cerr << "object = " << (void*)(*oi) << " uniqueID = " <<
          (*oi)->GetUniqueID() << " description = " << endl;
        obj.Describe(cout, 1);
        cerr << "----------" << endl;
        abort();
        return true; // error
      }
#else
      if (d < epsilon) {
        int size = objlist.size();
        *oi = objlist[size-1];
        objlist.resize(size-1);
        oi--; // check this object as well
      }      
#endif
    } // for i    
  } // for all objecs

  return false;
}

// builds up CKTB tree without unbounded objects. The 'boxToInclude
// is a box that can be optionally given and the kd-tree constructed
// must contain such a box. The 'boxToBoundObjects' when given, is
// a box to which the objects boxes are bounded. This is used for special
// cases such a multilevel kd-trees.
void
CKTB::BuildingUp(ObjectContainer& objlist,
                 const AxisAlignedBox3 *boxToInclude,
                 const AxisAlignedBox3 *boxToBoundObjects,
                 bool verbose)
{
  // the number of objects for statistics purposes - it can be different
  // from objlist->size() since objlist need not be initialized at all!
  countObjects = objlist.size();

  // compute the estimate for the kd-tree constructed this way
  _expectedIntersectionCost = log((float)countObjects) / log(2.0f) + 2.0;
  
#ifdef _DEBUG
  // check the flatness of the boxes for all objects
  if (CheckBoxes(objlist)) {
    cerr << "Problem with the size of the box for some object" << endl;
    return;
  }
#endif // _DEBUG

  // cerr<<"hh2"<<endl;
  // possibly remove the old building class
  // and allocate the class for CKTB building up
  if (buildClass)
    delete buildClass;  
  buildClass = AllocBuildClass();

  // cerr<<"hh3"<<endl;

  // and for traversal
  AllocTraversalClass();
  // cerr<<"hh4"<<endl;

  if (boxToBoundObjects) {
    // maximum box of the scene is given in advance
    bbox = Intersect(bbox, *boxToBoundObjects);
  }

  // we can additionally include the user box 
  if (boxToInclude)
    bbox.Include(*boxToInclude);  

  // cerr<<"hh5"<<endl;

  // build up the CKTB tree and make setup for traversal
  SKTBNodeT *rootNode =
    buildClass->BuildUp(objlist, bbox, verbose);
  // cerr<<"hh6"<<endl;

  traversalClass->Setup(bbox, rootNode);
  // cerr<<"hh7"<<endl;

#ifdef _DEBUG
  //traversalClass->AssignIDs();
#endif
  builtUp = true;

#if 0  
  // if to clean up the whole structure
  bool pruning;
  Environment::GetSingleton()->GetBoolValue("BSP.pruning", pruning);
  if (pruning)
    // only the objects whose surfaces intersects with the subdivision
    PruneLeaves(); // cells are not pruned from the CKTB tree
#endif
}

// functions that allows correctly traverse recursively allowing to have
// the same traversal stack
void
CKTB::TraversalReset()
{
  assert(traversalClass);
  traversalClass->ResetTraversalDepth();
}

// Allocate 2D buffer of pointers to the interior nodes
void
CKTB::AllocBuffer(int size2D, int size1Dv)
{
  int i, j;
  
  if (buffer) {
    for (i=0; i < sizeBuffer; i++) {
      delete [] buffer[i];
    } // for
    delete [] buffer;
    buffer = 0;
  }

  sizeBuffer = size2D;
  this->size1D = size1Dv;
  buffer = new SKTBNodeT ** [sizeBuffer];
  assert(buffer);
  for (i=0; i < sizeBuffer; i++) {
    buffer[i] = new SKTBNodeT* [size1D];
    assert(buffer[i]);
    // reset the array content
    for (j = 0; j < size1D; j++) {
      buffer[i][j] = 0;
    }
  } // for

  return;
}

int
CKTB::FindNearestI(const SimpleRay &ray)
{
  return traversalClass->FindNearestI(ray);      
}

int
CKTB::FindNearestI(const SimpleRay &ray, Vector3 &boxmin, Vector3 &boxmax)
{
  const AxisAlignedBox3 localbox(boxmin, boxmax);
  return traversalClass->FindNearestI(ray, localbox);      
}

  
#ifdef _USE_HAVRAN_SSE  

#ifdef __SSE__
// --------------------------------------------------------------
// Ray packets
void
CKTB::FindNearestI(RayPacket2x2 &raypack)
{
  if (!makeMinBoxes)
    raypack.SetOrigin(0); // min boxes are not used, we cannot use them here !
  traversalClass->FindNearestI(raypack);
}

void
CKTB::FindNearestI(RayPacket2x2 &raypack, Vector3 &boxmin, Vector3 &boxmax)
{
  if (!makeMinBoxes)
    raypack.SetOrigin(0); // min boxes are not used, we cannot use them here !
  traversalClass->FindNearestI(raypack, boxmin, boxmax);
}

#endif // __SSE__

#endif // _USE_HAVRAN_SSE  

  
void
CKTB::DeleteBuildClass()
{
  if (buildClass)
    delete buildClass;
  buildClass = NULL;
}

void
CKTB::GatherStats(bool itself)
{
  // DEBUG << "CKTB::GatherStats called" << endl;
  struct EDir {
    SKTBNodeT *node;
    int depth;
    AxisAlignedBox3 bbox;
  };
  EDir stack[CKTBTraversal::MAX_TRAVERSAL_HEIGHT];
  int stackTop = 0; // pointer to the stack
  stack[stackTop].node = 0;
  stack[stackTop].depth = 0;
  stack[stackTop].bbox = bbox;
  int tmpdir;
#ifdef __TAGGED_KDR
  int taggedLists = 0;
#endif // __TAGGED_KDR
  
  /*
  if (!BuiltUp) {
    cerr << "Internal error: CASDS not built up. Giving up.\n" << flush;
    abort();
  }
  */
  
  // average depth for all leaves
  long avgLeafDepth = 0;
  // for full leaves
  long avgFullLeafDepth = 0;
  // for empty leaves
  long avgEmptyLeafDepth = 0;

  // Surface areas  (divided by the area of the box)
  // Surface area of leaves (empty + full)
  double saLeaves = 0.f;
  // Surface area of interior nodes
  double saInteriorNodes = 0.f;
  // Sum of surface area of (full) leaves multiplied by
  // the number of objects in leaves
  double sa_cntLeaves = 0.f;
  // number of boxes enclosing tightly the objects
  long int boxesMin = 0;
  long int boxesMinX = 0;
  long int boxesMinY = 0;
  long int boxesMinZ = 0;
  // the sum of depth of tight boxes
  long int boxesMinDepth = 0;

  // start from the root node
  SKTBNodeT *curr = buildClass->GetRootNode();
  // copy the box over the scene objects
  AxisAlignedBox3 wbox = this->bbox;
  int currDepth = 0;

  if (buildClass->IsLeaf_(curr)) {
    this->numElemCells++;
    if (buildClass->IsEmptyLeaf_(curr)) {
      this->numEmptyElemCells++;
      AxisAlignedBox3 bwbox = this->bbox;
      // Surface area for empty leaf
      this->emptyVolume += bwbox.GetVolume();
      saLeaves += bwbox.SurfaceArea();
    }
    else { // CKTB tree is single non-empty leaf
      AxisAlignedBox3 bwbox = this->bbox;
      this->nonEmptyVolume += bwbox.GetVolume();
      ObjectContainer *olist = curr->objlist;
      saLeaves += bwbox.SurfaceArea();
      if (!olist) {
        this->numSolidsInElemCells += olist->size();
        this->actMaxListLen = olist->size();
        // surface area for full leaf
        sa_cntLeaves = bwbox.SurfaceArea() * (float)(olist->size());
      }
    }
  }
  else { // CKTB tree is not a single leaf
    // now we access the interior node

    // surface area for interior node
    AxisAlignedBox3 bwbox = this->bbox;
    // surface area for interior node
    saInteriorNodes += bwbox.SurfaceArea();
    this->numHierCells++;
        
    // traverse through whole CKTB tree
    do {
      CKTBAxes::Axes nodeType = buildClass->GetNodeType(curr);
      if (nodeType == CKTBAxes::EE_Link) {
        // just relink
        curr = buildClass->GetLinkNode(curr);
        // cout << "Link node was accessed" << endl;
        continue;       
      }
      
      if ( (nodeType == CKTBAxes::EE_X_axis)||
           (nodeType == CKTBAxes::EE_Y_axis)||
           (nodeType == CKTBAxes::EE_Z_axis)||
           (nodeType == CKTBAxes::EE_X_axisBox)||
           (nodeType == CKTBAxes::EE_Y_axisBox)||
           (nodeType == CKTBAxes::EE_Z_axisBox)
         ) {

        // push onto the stack
        AxisAlignedBox3 rightBox = bwbox;
        float value = curr->splitPlane;
        switch(nodeType) {
          case CKTBAxes:: EE_X_axisBox: {
            rightBox.SetMin(0, value);
            bwbox.SetMax(0, value);
            boxesMin++;
            boxesMinX++;
            boxesMinDepth += currDepth;
            break;        
          }
          case CKTBAxes:: EE_X_axis: {
            rightBox.SetMin(0, value);
            bwbox.SetMax(0, value);
            break;        
          }
          case CKTBAxes:: EE_Y_axisBox: {
            rightBox.SetMin(0, value);
            bwbox.SetMax(0, value);
            boxesMin++;
            boxesMinY++;
            boxesMinDepth += currDepth;
            break;        
          }
          case CKTBAxes:: EE_Y_axis: {
            rightBox.SetMin(1, value);
            bwbox.SetMax(1, value);
            break;        
          }
          case CKTBAxes:: EE_Z_axisBox: {
            rightBox.SetMin(0, value);
            bwbox.SetMax(0, value);
            boxesMin++;
            boxesMinZ++;
            boxesMinDepth += currDepth;
            break;        
          }
          case CKTBAxes:: EE_Z_axis: {
            rightBox.SetMin(2, value);
            bwbox.SetMax(2, value);
            break;        
          }
        } // switch
      
        stack[stackTop].node = buildClass->GetRight(curr);
        stack[stackTop].depth = currDepth+1;
        stack[stackTop].bbox = rightBox;
        stackTop++;
        // where to go
        if ( (nodeType == CKTBAxes:: EE_X_axisBox)||
             (nodeType == CKTBAxes:: EE_Y_axisBox)||
             (nodeType == CKTBAxes:: EE_Z_axisBox) )
          curr = buildClass->GetLeftMinBox(curr);
        else
          curr = buildClass->GetLeft(curr);
        // the box was already set      
        currDepth++;
      
        if (this->actMaxDepth < currDepth)
          this->actMaxDepth = currDepth;

        // surface area for interior node
        saInteriorNodes += bwbox.SurfaceArea(); 
        this->numHierCells++;

        continue;
      } // interior nodes

      assert(nodeType == CKTBAxes::EE_Leaf);
      
      while ( (nodeType == CKTBAxes::EE_Leaf) && currDepth) {
        this->numElemCells++;
        avgLeafDepth += currDepth;

        ObjectContainer *olist = buildClass->GetObjList(curr);
        
        // test to empty leaf
        if (!olist) {
          this->numEmptyElemCells++;
          avgEmptyLeafDepth += currDepth;
          this->emptyVolume += bwbox.GetVolume();
          // Surface area for empty leaf
          saLeaves += bwbox.SurfaceArea();
        }
        else {
          // full leaf 
          this->nonEmptyVolume += bwbox.GetVolume();
          avgFullLeafDepth += currDepth;
          int length = olist->size();
          this->numSolidsInElemCells += length;
          // surface area for full leaf
          saLeaves += bwbox.SurfaceArea();
          sa_cntLeaves += bwbox.SurfaceArea() * (float)(olist->size());
          if (this->actMaxListLen < length)
            this->actMaxListLen = length;
        }
        
        // traverse up the tree
        stackTop--;
        if (stackTop < 0)
          goto FINISH;
        // pop the node from the stack
        curr = stack[stackTop].node;
        currDepth = stack[stackTop].depth;

        nodeType = buildClass->GetNodeType(curr);
        
      } // while is a leaf
    }
    while (stackTop >= 0);
  } // either single leaf or tree

FINISH:

  cout << "BBox " << this->bbox << "\n";
  
#ifdef __TAGGED_KDR
  DEBUGW << "Tagged lists = " << taggedLists << "\n";
#endif // __TAGGED_KDR

  cout << "#CntLeaves = \n" << this->numElemCells << "\n";
  cout << "#AverageLeafDepth = \n"
       << (float)avgLeafDepth/(float)this->numElemCells << "\n";
  cout << "#CntFullLeaves = \n" << (this->numElemCells-this->numEmptyElemCells)
        << "\n";
  cout << "#AverageFullLeafDepth = \n"
        << (float)avgFullLeafDepth/
            (float)(this->numElemCells - this->numEmptyElemCells) << "\n";
  cout << "#AverageEmptyLeafDepth = \n";
  if (this->numEmptyElemCells)
    cout << (float)avgEmptyLeafDepth / (float)this->numEmptyElemCells<<"\n";
  else
    cout << "undefined\n";

  // Normalized the surface area
  this->sumSurfaceAreaLeaves = saLeaves / wbox.SurfaceArea();
  this->sumSurfaceAreaMULcntLeaves = sa_cntLeaves / wbox.SurfaceArea();
  this->sumSurfaceAreaInteriorNodes = saInteriorNodes / wbox.SurfaceArea();

  cout << "#SumSurfaceLeafArea = \n" << saLeaves << "\n";
  cout << "#SumSurfaceLeafAreaMulCNT = \n" << sa_cntLeaves << "\n";
  cout << "#NormSurfaceLeafArea = \n" << this->sumSurfaceAreaLeaves << "\n";
  cout << "#NormSurfaceLeafAreaMulCNT = \n" << this->sumSurfaceAreaMULcntLeaves << "\n";
  cout << "#SurfaceAreaInteriorNodes = \n" << this->sumSurfaceAreaInteriorNodes<<"\n";  
  cout << "#MinBoxesCount = \n" << boxesMin << "\n";
  cout << "#MinBoxesXCount = \n" << boxesMinX << "\n";
  cout << "#MinBoxesYCount = \n" << boxesMinY << "\n";
  cout << "#MinBoxesZCount = \n" << boxesMinZ << "\n";
  if (boxesMin)
    cout << "#AverageMinBoxesDepth = \n"
          << (float)boxesMinDepth/(float)boxesMin << "\n";

  long int memUsed = 0;
#ifdef _KTB8Bytes
  memUsed += this->numHierCells*8;
  memUsed += (this->numElemCells-this->numEmptyElemCells)*8;
#ifdef _SHORT_FORM_MINBOX
  memUsed += boxesMin * (sizeof(SBBox) + 2*8);
#else
  memUsed += boxesMin * (5*8);
#endif
#else // _KTB8Bytes
  memUsed += this->numHierCells*12;
  memUsed += (this->numElemCells-this->numEmptyElemCells)*12;
#ifdef _SHORT_FORM_MINBOX
  memUsed += boxesMin * (sizeof(SBBox) + 2*12);
#else
  memUsed += boxesMin * (4*12);
#endif
#endif // _KTB8Bytes

  // The references to objects
  memUsed += this->numSolidsInElemCells * sizeof(Intersectable*);
  memUsed += (this->numElemCells-this->numEmptyElemCells) *
    sizeof(ObjectContainer);
  cout << "#MemUsedKBB = \n" << memUsed << "\n";
    
  return;
}


void CKTB::ExportBinLeaf(OUT_STREAM &stream, SKTBNodeT *leaf)
{
#define TYPE_LEAF -3
  int type = TYPE_LEAF;
  if (leaf->objlist == 0) {
    // empty leaf
    int size = 0;
    stream.write(reinterpret_cast<char *>(&type), sizeof(int));
    stream.write(reinterpret_cast<char *>(&size), sizeof(int));    
    return;
  }
  
  ObjectContainer::const_iterator it, it_end = leaf->objlist->end();  
  int size = (int)leaf->objlist->size();
  
  stream.write(reinterpret_cast<char *>(&type), sizeof(int));
  stream.write(reinterpret_cast<char *>(&size), sizeof(int));
  
  for (it = leaf->objlist->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));
  }
}


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

  
SKTBNodeT*
CKTB::ImportBinLeaf(IN_STREAM &stream, 
                    SKTBNodeT **nodeToLink,
                    const ObjectContainer &objects)
{
  int leafId = TYPE_LEAF;
  int objId = leafId;
  int size;
  
  stream.read(reinterpret_cast<char *>(&size), sizeof(int));
  if (size == 0) {
    // alloc empty leaf -- ?????????? I am not sure if this works !!!
    // since for 
    SKTBNodeT* leaf = buildClass->AllocLeaf(0);
    *nodeToLink = buildClass->nodeToLink;
    return leaf;
  }
  
  MeshInstance dummyInst(NULL);
  SKTBNodeT* leaf = buildClass->AllocLeaf(size);
  *nodeToLink = buildClass->nodeToLink;

  ObjectContainer *newobjlist = new ObjectContainer;
  
  // 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))
      newobjlist->push_back(*oit);
    else
      Debug << "error: object with id " << objId << " does not exist" << endl;
  } // for

  buildClass->SetFullLeaf(leaf, newobjlist);
  
  return leaf;
}


void CKTB::ExportBinInterior(OUT_STREAM &stream, SKTBNodeT *interior)
{
#define TYPE_INTERIOR -2
  int interiorid = TYPE_INTERIOR;
  stream.write(reinterpret_cast<char *>(&interiorid), sizeof(int));

  int axis = (int)(buildClass->GetNodeType(interior));
  float pos = buildClass->GetSplitValue(interior);

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


SKTBNodeT *
CKTB::ImportBinInterior(IN_STREAM  &stream, SKTBNodeT **nodeToLink)
{
  int axis;
  float pos;

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

  SKTBNodeT *interiorNode =
    buildClass->AllocInteriorNode(axis, pos, 0, 0);
  *nodeToLink = buildClass->nodeToLink;
        
  return interiorNode;
}


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

  int startMark = 0xf0f0;
  stream.write(reinterpret_cast<char *>(&startMark), sizeof(int));  
  
  // export binary version of mesh
  std::stack<SKTBNodeT *> tStack;
  tStack.push(buildClass->GetRootNode());
  int cntSaves = 0, cntSavesLeaf = 0, cntSavesIN = 0;

  while(!tStack.empty())
  {
    SKTBNodeT *node = tStack.top();
    tStack.pop();    

    int nodeType = buildClass->GetNodeType(node);
    
    if (nodeType == CKTBAxes::EE_Link)
    {
      tStack.push(buildClass->GetLinkNode(node));
    }
    else
    if (nodeType == CKTBAxes::EE_Leaf)
    {
      //Debug << "l";
      ExportBinLeaf(stream, node);
      cntSavesLeaf++; cntSaves++;      
    }
    else
    { // interior node
      //Debug << "i";
      ExportBinInterior(stream, node);
      cntSavesIN++; cntSaves++;      

      tStack.push(buildClass->GetRight(node));
      if (nodeType >= CKTBAxes::EE_X_axisBox)
        tStack.push(buildClass->GetLeftLong(node));
      else
        tStack.push(buildClass->GetLeft(node));
    }
  } // while

  int endMark = 0xf0ff;
  stream.write(reinterpret_cast<char *>(&endMark), sizeof(int));  

  cout << "Writes " << cntSaves << " cntLeafs "
       << cntSavesLeaf << " cntIN " << cntSavesIN << endl;
  
  stream.close();
  return true; // saved
}


SKTBNodeT *
CKTB::ImportNextNode(IN_STREAM &stream, 
                     SKTBNodeT **nodeToLink,
                     const ObjectContainer &objects)
{
  int nodeType;
  stream.read(reinterpret_cast<char *>(&nodeType), sizeof(int));

  if (nodeType == TYPE_LEAF)
    return ImportBinLeaf(stream, nodeToLink, objects);

  if (nodeType == TYPE_INTERIOR)
    return ImportBinInterior(stream, nodeToLink);

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


// creates the ASDS according to the file description
bool
CKTB::ImportBinTree(const string &filename, ObjectContainer &objects)
{
  // open the stream in text mode
  IN_STREAM stream(filename.c_str(), IN_BIN_MODE);

  if (!stream.is_open()) {
    cerr << "Kd-tree description file (.kbt) cannot be opened for reading\n";
    return true; // error
  }

  int mark;
  stream.read(reinterpret_cast<char *>(&mark), sizeof(int));
  if (mark != 0xf0f0) {
    cout << "Something wrong with the tree - heading" << endl;
    abort();
  }
  
  // sort objects by their id to allow list creation in kd-tree leaves
  //    if (!is_sorted(objects.begin(), objects.end(), ilt))
  sort(objects.begin(), objects.end(), ilt);
  
  // remove all the data in the current data structure
  CKTBAllocManPredecessor *bc = GetBuildClass();
  if (!bc)
    bc = buildClass = AllocBuildClass();
  else
    bc->Remove();

  // How many items can be allocated at once
  int maxItemsAtOnce = 1;
  if (makeMinBoxes) {
#ifdef _KTB8Bytes    
    // We need to allocate for boxes the memory in a row
    maxItemsAtOnce = 5; // 8x5=40 = 16+24;
#else
    maxItemsAtOnce = 4; // 12x4=48 = 24+24;
#endif // _KTB8Bytes
  }
  
  bc->InitAux(0, CKTBNodeAbstract::MAX_HEIGHT - 1, maxItemsAtOnce);  

  // Compute the box from all objects
  bbox.Initialize();
  for (ObjectContainer::iterator sc = objects.begin();
       sc != objects.end(); sc++) {
    AxisAlignedBox3 abox = (*sc)->GetBox();
    bbox.Include(abox);
  } // for
   
  // initialize the root node
  SKTBNodeT *node;

  bc->root = ImportNextNode(stream, &node, objects);
  assert(bc->root == node);

  std::stack<STraversalData> tStack;
  tStack.push(STraversalData(bc->root, 1, 0));
  tStack.push(STraversalData(bc->root, 0, 0));
  int depth, lastDepth;
  
  //int cntReads = 1, cntReadsLeaf = 0, cntReadsIN = 1;
  while(!tStack.empty())
  {    
    STraversalData tData = tStack.top();
    tStack.pop();
    
    node = tData.mNode;
    lastDepth = depth = tData.depth;
    //int nodeType = bc->GetNodeType(node);
    //cout << "nodeType = " << nodeType << " adr = " << (void*)node << endl;

    SKTBNodeT *childNodeLink = 0;
    SKTBNodeT *childNode = ImportNextNode(stream, &childNodeLink, objects);
    //    cout << "childNode = " << (void*)childNode
    // << " linkNode = " << (void*)childNodeLink << endl;

    if (tData.dir)
      bc->SetInteriorNodeRight(node, childNodeLink);
    else
      bc->SetInteriorNodeLeft(node, childNodeLink);
    
    //cntReads++;
    if (!bc->IsLeaf_(childNode))
    {
      //cntReadsIN++;
      tStack.push(STraversalData(childNode, 1, depth+1));
      tStack.push(STraversalData(childNode, 0, depth+1));
    }
    //else {
    //  cntReadsLeaf++;
    //  //cout << "leaf" << endl;
    // }
  } // while

  //cout << "Last depth = " << lastDepth << endl;
  //cout << "Reads " << cntReads << " cntLeafs "
  //   << cntReadsLeaf << " cntIN " << cntReadsIN << endl;
  
  stream.read(reinterpret_cast<char *>(&mark), sizeof(int));
  if (mark != 0xf0ff) {
    cout << "Something wrong with the kd-tree in the file"
         << endl;
    abort();
  }
  
  if (!traversalClass)
    AllocTraversalClass();
    
  // Make setup for traversal
  traversalClass->Setup(bbox, bc->GetRootNode());
  // Handles unbounded objects in traversal class
  // traversalClass->Setup2(0);
  
  stream.close();

  // now the tree is surely well constructed
  builtUp = true;  

  return false; // OK
}

void
CKTB::GatherPostStats()
{

}

void*
CKTB::Locate(const Vector3 &loc)
{
  return (void*)(traversalClass->Locate(loc));
}


} // namespace