Index: /GTP/trunk/Lib/Vis/Preprocessing/src/havran/ktbs.cpp
===================================================================
--- /GTP/trunk/Lib/Vis/Preprocessing/src/havran/ktbs.cpp	(revision 2632)
+++ /GTP/trunk/Lib/Vis/Preprocessing/src/havran/ktbs.cpp	(revision 2632)
@@ -0,0 +1,1634 @@
+// ===================================================================
+// $Id: $
+//
+// ktbs.cpp
+//     The classes for building up CKTB tree by statistical optimization.
+//     This building up is based on sampling.
+//
+// REPLACEMENT_STRING
+//
+// Copyright by Vlastimil Havran, 2007 - email to "vhavran AT seznam.cz"
+// Initial coding by Vlasta Havran, January 2008.
+
+// standard headers
+#include <algorithm>
+#include <iostream>
+#include <string>
+
+// GOLEM headers
+#include "ktbs.h"
+#include "timer.h"
+#include "Environment.h"
+
+//#define _DEBUG
+
+#define AVOIDCHECKLIST
+
+#define _USE_OPTIMIZE_DIVIDE_AX
+
+// Note that the RADIX SORT does not work properly with -mtune=pentium3 for GCC.4.0 !!!!
+// Note that the RADIX SORT does not work properly with -mtune=pentium4 for GCC.3.4 !!!!
+// The reason is unknown, probably the bug in compiler
+
+namespace GtpVisibilityPreprocessor {
+
+// ------------------------------------------------
+// for debugging cost function in the scene
+//#define _DEBUG_COSTFUNCTION
+#ifdef _DEBUG_COSTFUNCTION
+
+const int indexToFindSS = 0;
+
+static CFileIO ffcost;
+static int     costEvalCnt;
+static int     costCntObjects;
+static int     costIndexDebug;
+static string  fnameCost;
+static float   maxCost;
+static float   minCost;
+static float   maxPos;
+static float   minPos;
+static float   minPosAxis;
+static float   maxPosAxis;
+static bool _alreadyDebugged = false;
+static bool _streamOpened = false;
+
+static void
+InitCostStream(int indexDebug, int cntObjects,
+	       float minx, float maxx)
+{
+  maxCost = -INFINITY;
+  minCost = INFINITY;
+  costEvalCnt = 0;
+  costIndexDebug = indexDebug;
+  costCntObjects = cntObjects;
+  minPosAxis = minx;
+  maxPosAxis = maxx;
+  
+  // and init the stream - rewrite
+  string name = "debugcostfile"
+  //Environment::GetSingleton()->GetBoolValue("OutputFileName", name);
+  char lns[100];
+  sprintf(lns, "%05d.res", indexDebug);
+  ChangeFilenameExtension(name, string(lns), fnameCost);
+
+  ffcost.SetFilename(fnameCost.c_str());
+  if (ffcost.OpenInMode(CFileIO::EE_Mode_w)) {
+    cerr << "Opening of stream failed" << endl;
+    abort();;
+  }
+  cout << "Saving debug to " << fnameCost << endl;
+  ffcost.WriteChars("\n");
+  sprintf(lns, "#CntObjects = %d\n", costCntObjects);
+  ffcost.WriteChars(lns);
+  sprintf(lns, "#IndexToDebug = %d\n", costIndexDebug);
+  ffcost.WriteChars(lns);
+  ffcost.WriteChars("#File ");
+  ffcost.WriteChars(fnameCost.c_str());
+  ffcost.WriteChars("\n");
+  ffcost.WriteChars("#==============================================\n");
+
+  cout << "#---------------------------------------" << endl;
+  _streamOpened = true;
+}
+
+static void
+CloseCostStream()
+{
+  if (_streamOpened) {    
+    char lns[255];
+    ffcost.WriteChars("#==============================================\n");
+    ffcost.WriteChars("#EndOfFile\n");
+    sprintf(lns, "#minCost = %8.6f minPos = %8.6f\n", minCost, minPos);
+    ffcost.WriteChars(lns);    
+    sprintf(lns, "#maxCost = %8.6f maxPos = %8.6f\n", maxCost, maxPos);
+    ffcost.WriteChars(lns);    
+    sprintf(lns, "#ratioMin/Max =                                %6.5f\n",
+	    minCost/maxCost);
+    ffcost.WriteChars(lns);    
+    sprintf(lns, "#CntEval = %d\n", costEvalCnt);
+    ffcost.WriteChars(lns);
+    sprintf(lns, "#CntObjects = %d\n", costCntObjects);
+    ffcost.WriteChars(lns);    
+    sprintf(lns, "#IndexToDebug = %d\n", costIndexDebug);
+    ffcost.WriteChars(lns);
+    sprintf(lns, "#MinPosAxis = %8.6f maxPosAxis = %8.6f\n", minPosAxis, maxPosAxis);
+    ffcost.WriteChars(lns);
+    ffcost.WriteChars("#File ");
+    ffcost.WriteChars(fnameCost.c_str());
+    ffcost.WriteChars("\n");
+    ffcost.Close();
+    //cout << "#=======================================" << endl;
+    _streamOpened = false;
+    _alreadyDebugged = true;
+    // Temporarily
+    exit(0);
+  }
+}
+
+static void
+ReportCostStream(float pos, float cost)
+{
+  // Here we normalize a position to the interval <0-1>
+  float posn = (pos - minPosAxis)/(maxPosAxis - minPosAxis);
+
+  if (_streamOpened) {
+    costEvalCnt++;
+    if (cost < minCost) {
+      minCost = cost;
+      minPos = pos;
+    }
+    if (cost > maxCost) {
+      maxCost = cost;
+      maxPos = pos;
+    }
+    char lns[255];
+    sprintf(lns, "%8.6f %8.6f\n", posn, cost);
+    //cout << lns << endl;
+    ffcost.WriteChars(lns);    
+  }
+}
+#endif // _DEBUG_COSTFUNCTION
+
+// -------------------------------------------------------
+// the next two axes for a given one
+const CKTBAxes::Axes
+CKTBSBuildUp::oaxes[3][2]=
+{{CKTBAxes::EE_Y_axis, CKTBAxes::EE_Z_axis},
+ {CKTBAxes::EE_Z_axis, CKTBAxes::EE_X_axis},
+ {CKTBAxes::EE_X_axis, CKTBAxes::EE_Y_axis}};
+  
+//----------------------------------------------------------------
+//---------- Implementation of CKTB tree with irregular -----------
+//---------- placed splitting planes -----------------------------
+//----------------------------------------------------------------
+
+// default constructor
+CKTBSBuildUp::CKTBSBuildUp()
+{
+  // verbose is set
+  verbose = true;
+
+  // Maximum depth of the tree is set and stack is allocated
+  maxTreeDepth = MAX_HEIGHT;
+  stackDepth = maxTreeDepth + 2;
+  stackID = new GALIGN16 SInputData[stackDepth];
+  assert(stackID);
+  stackIndex = 0;
+  return;
+}
+
+//  destructor
+CKTBSBuildUp::~CKTBSBuildUp()
+{
+  delete [] stackID;
+  stackID = 0;
+}
+
+void
+CKTBSBuildUp::ProvideID(ostream &app)
+{
+  bool tempvar;
+  
+  string termCrit;
+  Environment::GetSingleton()->GetStringValue("BSP.termCrit", termCrit);
+  app << "#BSP.termCrit\t\tTermination criteria to build up BSP tree\n";
+  app << termCrit << "\n";
+
+  maxCountTrials = 0;
+  if ( (!termCrit.compare("auto")) &&
+       (!termCrit.compare("auto2")) ) {
+    // everything except auto settings, when depth is determined 
+    // It should be reported for adhoc and possibly others
+    Environment::GetSingleton()->GetIntValue("BSP.maxDepthAllowed",
+					     maxDepthAllowed);
+    Environment::GetSingleton()->GetIntValue("BSP.maxListLength",
+					     maxListLength);
+    app << "#MaxDepth (Dmax)\tMaximal allowed depth of the BSP tree\n";
+    app << maxDepthAllowed << "\n";
+    app << "#MaxListLength (Noilf)\tMaximal number of solids in one cell\n";
+    app << maxListLength << "\n";
+    // maximum number of trials to further subdivide
+    maxCountTrials = (int)(1.0 + 0.2 * (float)maxDepthAllowed);
+    if (maxCountTrials < 3)
+      maxCountTrials = 3;
+  }
+  else {
+    if (!termCrit.compare("auto2") ) {
+      Environment::GetSingleton()->GetIntValue("BSP.maxListLength",
+					       maxListLength);
+      app << "#MaxDepth (Dmax)\tMaximal allowed depth of the BSP tree\n";
+    }
+  }
+  
+  float eCt, eCi, eCd;
+  Environment::GetSingleton()->GetFloatValue("BSP.decisionCost", eCd);
+  Environment::GetSingleton()->GetFloatValue("BSP.intersectionCost", eCi);
+  Environment::GetSingleton()->GetFloatValue("BSP.traversalCost", eCt);
+
+  // Ci should no be changed from 1.0, only Cd and Ct should be changed
+  app << "#SetBSP.(Cd, Ci, Ct) \tDecision, Intersection, ";
+  app << "and Traversal costs\n";
+  app << " ( " << eCd << ", " << eCi << " ," << eCt << " )\n";
+
+  return;
+}
+
+// Init required parameters
+void
+CKTBSBuildUp::InitReqPref(SReqPrefParams *pars)
+{
+  // nothing is obligatory as default
+  pars->reqPosition = Limits::Infinity;
+  pars->reqAxis = CKTBAxes::EE_Leaf;
+  pars->useReqAxis = false;
+  
+  // the values for automatic termination criteria, since it was
+  // not subdivided
+  pars->ratioLast = 1000.0;
+  pars->ratioLastButOne = 1000.0;
+  pars->failedSubDivCount = 0;
+
+  Environment::GetSingleton()->GetIntValue("BSP.axisSelectionAlg",
+					   _algorithmForAxisSelection);
+  if (_algorithmForAxisSelection != 0) {
+    // The axis as prefered one - testing all 3 axes. Start
+    // from the axis cutting the longest side of the box
+    pars->reqAxis = (CKTBAxes::Axes)(wBbox.Diagonal().DrivingAxis());
+  }
+
+  return;
+}
+
+// creates all the auxiliary structures for building up CKTB tree
+CKTBSBuildUp::SInputData*
+CKTBSBuildUp::Init(ObjectContainer *objlist, const AxisAlignedBox3 &box)
+{
+#ifdef  _RANDOMIZE_POSITION
+  //world->_environment.GetFloat("SetBSP.randomizePosition.Eps", randomEps);
+#endif // _RANDOMIZE_POSITION
+  
+#ifdef _KTB_CONSTR_STATS
+  _stats_interiorCount = 0;
+  _stats_bboxCount = 0;
+  _stats_leafNodeCount = 0;
+  _stats_emptyLeftNodeCount = 0;
+  // Aggregate statistics
+  _maxDepth = 0;  
+  _sumLeafDepth = 0;
+  _sumFullLeafDepth = 0;
+  _sumObjectRefCount = 0;
+  _maxObjectRefInLeaf = 0;
+  // surface areas
+  _sumSurfaceAreaLeaves = 0.f;
+  _sumSurfaceAreaMULcntLeaves = 0.f;
+  _sumSurfaceAreaInteriorNodes = 0.f;
+#endif
+
+  // If to print out the tree during contruction
+  Environment::GetSingleton()->GetBoolValue("BSP.printCuts", _printCuts);
+
+  // if to cut off empty space in postprocessing and how it is performed
+  Environment::GetSingleton()->GetIntValue("BSP.absMaxAllowedDepth",
+					   absMaxAllowedDepth);
+  Environment::GetSingleton()->GetIntValue("BSP.maxEmptyCutDepth",
+					   maxEmptyCutDepth);
+  Environment::GetSingleton()->GetIntValue("BSP.algAutoTermination",
+					   algorithmAutoTermination);
+  Environment::GetSingleton()->GetFloatValue("BSP.biasFreeCuts",
+					     biasFreeCuts);
+
+  // if to make tagging lists inside the tree
+  Environment::GetSingleton()->GetBoolValue("BSP.minBoxes.use",
+					    makeMinBoxes);
+  Environment::GetSingleton()->GetBoolValue("BSP.minBoxes.tight",
+					    makeTightMinBoxes);
+  Environment::GetSingleton()->GetIntValue("BSP.minBoxes.minObjects",
+					    minObjectsToCreateMinBox);
+  Environment::GetSingleton()->GetIntValue("BSP.minBoxes.minDepthDistance",
+					   minDepthDistanceBetweenMinBoxes);
+  Environment::GetSingleton()->GetFloatValue("BSP.minBoxes.minSA2ratio",
+					     minSA2ratioMinBoxes);
+
+  // 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
+    assert(minObjectsToCreateMinBox >= 1);
+    assert(minDepthDistanceBetweenMinBoxes >= 0);
+    assert(minDepthDistanceBetweenMinBoxes <= 50);
+  }
+
+  // the array of preferred parameters used as a stack
+  pars = new SReqPrefParams[MAX_HEIGHT];
+  assert(pars);
+  
+  // Initiate the allocator
+  InitAux(0, MAX_HEIGHT - 1, maxItemsAtOnce);
+  
+  // since six planes are enough to cull empty space from leaves
+  if ( (maxEmptyCutDepth < 0) ||
+       (maxEmptyCutDepth > 6) ) {
+    cerr << "BSP.maxEmptyCutDepth = " << maxEmptyCutDepth
+	  << " must be in range <0,6>\n";
+    abort();;
+  }
+
+  wBbox.Convert(box); // the box of the whole scene
+  boxSize = box.Diagonal(); // the size of the box along the axes
+  
+  wholeBoxArea = wBbox.SA2(); // the half of the surface area of the box
+  // the array of bounding boxes and duplications to the objects
+  int objlistcnt = objlist->size();
+
+  SInputData* data = AllocNewData();
+  // set the box
+  data->box = wBbox;
+  data->tightbox = wBbox;
+  data->objlist = new ObjectContainer(objlist->begin(), objlist->end());
+  data->count = objlistcnt;
+
+  // Initialization of the first value to insert the min boxes
+  data->lastMinBoxSA2 = box.SurfaceArea() * 10000.f;
+  data->lastDepthForMinBoxes = -20; // for root you always put the node
+  data->lastMinBoxNode = 0;
+  
+  // Init the parameters from the environment file
+  InitReqPref(&(data->pars));
+
+  // the number of buckets;
+  state.bucketN = 128;
+  state.bucketMin = new int[state.bucketN];
+  assert(state.bucketMin);
+  state.bucketMax = new int[state.bucketN];
+  assert(state.bucketMin);
+  
+  // Init the termination criteria
+  string termCrit;
+  Environment::GetSingleton()->GetStringValue("BSP.termCrit", termCrit);
+
+  maxCountTrials = 0;
+  if (!termCrit.compare("adhoc")) {
+    // termination criteria are the max depth of the hierarchy, number
+    // of primitives
+    data->modeSubDiv = EE_SUBDIVADHOC;
+    Environment::GetSingleton()->GetIntValue("BSP.maxListLength",
+					     maxListLength);
+    Environment::GetSingleton()->GetIntValue("BSP.maxDepthAllowed",
+					     maxDepthAllowed);
+  }
+  else {
+    if ( (!termCrit.compare("auto")) ||
+	 (!termCrit.compare("auto2")) ) {
+      // automatic termination criteria are used, everything is computed
+      // automatically from number of objects etc.
+      int estHeight = (int)(log((float)initcnt)/log((float)2.0) + 0.9);
+      // cout << "EstHeight=" << estHeight << endl;
+      maxDepthAllowed = (int)((float)estHeight * 1.15f + 1.6f);
+
+      // maximum number of trials to further subdivide
+      maxCountTrials = (int)(1.0 + 0.2 * (float)maxDepthAllowed);
+      if (maxCountTrials < 3)
+	maxCountTrials = 3;
+      
+      // for 'auto2' we specify the length of the object list by hand
+      if (!termCrit.compare("auto2"))
+	Environment::GetSingleton()->GetIntValue("BSP.maxListLength",
+						 maxListLength);
+      else
+	// for 'auto' we set the length of the object list that is supposed
+	// to be the best for ray shooting
+	maxListLength = 1;
+
+      if (verbose) {
+	cout << "TERMCRIT:maximum height of a leaf set to "
+	       << maxDepthAllowed << "\n";
+	cout << "TERMCRIT:maximum number of objects in leaf set to "
+	       << maxListLength << "\n" ;
+      }
+      // set the mode to be recognized
+      data->modeSubDiv = EE_SUBDIVAUTOMATIC;
+    }
+    else {
+      cerr << " unknown termination criteria for BSP tree\n";
+      cerr << "It was specified: " << termCrit << "\n";
+      cerr << "Allowed types = auto, auto2, adhoc" << endl;
+      exit(3);;
+    }
+  }
+
+  // for some evaluation it is necessary to determine the following costs
+  Environment::GetSingleton()->GetFloatValue("BSP.traversalCost", Ct);
+  Environment::GetSingleton()->GetFloatValue("BSP.intersectionCost", Ci);
+  if (verbose)
+    cout << "Ct=" << Ct << " Ci=" << Ci << "\n";
+  
+  if (cutEmptySpace) {
+    // correct the maximum abs depth, when late cutting is allowed
+    if (absMaxAllowedDepth < maxDepthAllowed)
+      absMaxAllowedDepth = maxDepthAllowed;
+    if (absMaxAllowedDepth > (maxDepthAllowed + maxEmptyCutDepth) )
+      absMaxAllowedDepth = maxDepthAllowed + maxEmptyCutDepth;
+    if (absMaxAllowedDepth >= MAX_HEIGHT) {
+      absMaxAllowedDepth = MAX_HEIGHT;
+      maxDepthAllowed = MAX_HEIGHT - maxEmptyCutDepth;
+    }
+    
+    if (verbose) {
+      cout << "Cutting off empty spaces ON\n";
+      cout << "BSP.absMaxAllowedDepth = " << absMaxAllowedDepth << "\n";
+      cout << "BSP.maxEmptyCutDepth = " << maxEmptyCutDepth << endl;
+    }
+  }
+
+  if (verbose)
+    cout << "MaxListLength = " << maxListLength << endl;  
+
+  return data;
+}
+
+// interface function for building up CKTB tree
+SKTBNodeT*
+CKTBSBuildUp::BuildUp(ObjectContainer &objlist,
+		      const AxisAlignedBox3 &box,
+		      bool verboseF)
+{
+  // check the number of objects
+  if (objlist.size() == 0)
+    return 0; // nothing
+
+  // ---------------------------------------------------
+  // Rewriting the triangles to the just wrappers to save
+  // the memory during building!!!!
+  bool useWrappers = false;
+  if (useWrappers) {
+    cout << "WARNING: using only wrappers, not objects to save the memory!"
+	 << endl;
+    cout << "Size of(AxisAlignedBox3Intersectable) = " << sizeof(AxisAlignedBox3Intersectable) << endl;
+    cout << "Size of(TriangleIntersectable) = " << sizeof(TriangleIntersectable) << endl;
+    for (ObjectContainer::iterator it = objlist.begin();
+	 it != objlist.end(); it++) {
+      // take the triangle
+      Intersectable *i = *it;
+      // store the properties to new variables
+      AxisAlignedBox3 b = i->GetBox();
+      int mId = i->mId;
+      // delete the triangle
+      delete i;
+      // create the wrapper with the same box as triangle
+      AxisAlignedBox3Intersectable *a = new AxisAlignedBox3Intersectable(b);
+      a->mId = mId;
+      // and put it back to the list of objects
+      *it = a;
+    } // for    
+    cout << "Rewriting to wrappers is finished!" << endl;
+  } // if
+  // ---------------------------------------------------
+
+  initcnt = objlist.size();
+  
+  // copy verbose to be used consistenly further on
+  this->verbose = verboseF;
+  
+  // the boundary entries
+  SInputData *d = Init(&objlist, box);
+
+  
+  if (verbose)
+    cout << "Building up KTB tree for " << objlist.size()
+		 << " objects by sampling." << endl << flush;
+  
+  // -----------------------------------------------
+  // Start to build the tree
+  if ( (cutEmptySpace) &&
+       (initcnt <= maxListLength)) {
+    // only cutting off empty space for initial leaf
+    d->modeSubDiv = EE_SUBDIVCUTEMPTY;
+    root = SubDiv(d);
+  }
+  else {
+    // normal subdivision scheme, creating whole tree
+    root = SubDiv(d);
+  }
+
+#ifdef _DEBUG
+  if (GetDepth() != 0) {
+    cerr << "Using depth value does not work correctly, depth = "
+	  << GetDepth() << endl;
+  }
+#endif  
+  assert(root);  
+
+  // the pointer to the root node
+  return GetRootNode();
+}
+
+int
+CKTBSBuildUp::UpdateEvaluation(float &eval, const float &newEval)
+{
+  if (newEval < eval) {
+    eval = newEval;
+    return 1; // updated
+  }
+  return 0; // not updated
+}
+
+// recursive function for subdividing the CKTB tree into two
+// halves .. creates one node of CKTB tree
+// list .. list of boundaries, count .. number of objects in current node
+// bb .. current bounding box of node, (pars,reqGlobAxis, modeSubDiv) .. cut pref.
+SKTBNodeT*
+CKTBSBuildUp::SubDiv(SInputData *d)
+{
+#if 0
+  static int index = 0;
+  cout << "SubDiv index = " << index << endl;
+  const int indexToFind = 13916;
+  if (index == indexToFind) {
+    cout << " IndexToFind = " << indexToFind << endl;
+    cout << " You should start debug" << endl;
+  }
+  index++;
+#endif
+
+  // This is the node to return from this function
+  SKTBNodeT *nodeToReturn = 0;
+
+  // if to make the interior node extended by the box here
+  makeMinBoxHere = false;
+
+  // -----------------------------------------------------
+  if (makeMinBoxes) {
+    if ( (d->count >= minObjectsToCreateMinBox) &&
+	 (GetDepth() - d->lastDepthForMinBoxes >= minDepthDistanceBetweenMinBoxes) )  {
+      makeMinBoxHere = true;
+      d->lastDepthForMinBoxes = GetDepth();
+      d->lastMinBoxSA2 = d->box.SA2();
+    }
+  }
+
+  if ( (makeMinBoxHere) && (makeTightMinBoxes) ) {
+    // In case we should make the box of the current
+    // interior node and this should be tight, we have
+    // to first compute its extent before splitting
+    SBBox tightbox;
+    GetTightBox(*d, tightbox);
+    // Here we decrease the box to the minimum size
+    d->box = tightbox;
+  }
+  
+  // ------------------------------------------------------------------
+  // if we are forced to make subdivision at given point
+  if (d->pars.reqPosition != Limits::Infinity) {
+    // this code preceeds switch(mode) since 2-level evaluation
+    d->position = d->pars.reqPosition;
+    d->axis = d->pars.reqAxis;
+    // next subdivison will not be forced
+    d->pars.reqPosition = Limits::Infinity; 
+    d->pars.reqAxis = CKTBAxes::EE_Leaf;
+    IncDepth();
+    SKTBNodeT* n = MakeOneCut(d);
+    if (!nodeToReturn)
+      nodeToReturn = n;
+    DecDepth();
+    return nodeToReturn;
+  }
+  
+  // -------------------------------------------------------------------------
+  // determine between subdivision modes - different termination criteria
+  switch (d->modeSubDiv) { // subdivision mode
+    case EE_SUBDIVCUTEMPTY: { // cutting off empty space
+      if ((GetDepth() >= absMaxAllowedDepth) ||
+	  (GetDepth() >= (startEmptyCutDepth + maxEmptyCutDepth)) ||
+	  (d->count == 0) ) {
+        SKTBNodeT* n = MakeLeaf(d);
+	if (!nodeToReturn)
+	  nodeToReturn = n;
+	// cout << "LEC\n";
+        return nodeToReturn;
+      }
+      // to require the axis has no meaning in cutting off 
+      d->pars.reqAxis = d->axis = CKTBAxes::EE_Leaf;
+      break;
+    }
+    // both adhoc, clustering and automatic termination criteria
+    case EE_SUBDIVADHOC:
+    case EE_SUBDIVAUTOMATIC: {
+      // automatic termination criteria are used in this phase as
+      // the absolute threshold similarly to SUBDIVADHOC mode
+      if ( (GetDepth() >= maxDepthAllowed) ||
+	   (d->count <= maxListLength)) {
+        SKTBNodeT* n = MakeLeaf(d);
+	if (!nodeToReturn)
+	  nodeToReturn = n;
+	// cout << "LLA\n";
+        return nodeToReturn;
+      }
+      break;
+    }
+    default: {
+      cerr << " unknown subdivision mode in ibsp.cpp\n";
+      abort();;
+    }
+  } // switch(modeSubDiv)
+  
+  // the axis where splitting should be done
+  CKTBAxes::Axes axis = CKTBAxes::EE_Leaf;
+  d->twoSplits = -1;
+  d->bestCost = WorstEvaluation() * 0.5f;
+  d->position = MAXFLOAT;
+  const bool secondPositionMax = true;
+
+#define CallGetSplitPlane GetSplitPlaneOpt
+  
+  // if the axis is required in pars structure for some reasons
+  if (d->pars.useReqAxis) {
+    axis = d->pars.reqAxis;
+    // The next subdivision is not prescribed
+    d->pars.useReqAxis = false;
+    switch(axis) {
+      case CKTBAxes::EE_X_axis : {
+	state.InitXaxis(d->count, d->box); // init
+	CallGetSplitPlane(d, 0); // evaluate
+	break;
+      }
+      case CKTBAxes::EE_Y_axis : {
+	state.InitYaxis(d->count, d->box); // init
+	CallGetSplitPlane(d, 1); // evaluate
+	break;
+      }
+      case CKTBAxes::EE_Z_axis : {
+	state.InitZaxis(d->count, d->box); // init
+	CallGetSplitPlane(d, 2); // evaluate
+	break;
+      }
+      default: {
+	cerr << "No other option is allowed here" << endl;
+	abort();;
+      }
+    }
+    // compute the cost, normalized by surface area
+    state.NormalizeCostBySA2();
+    d->bestCost /= state.areaWholeSA2;
+    if (UpdateEvaluation(d->bestCost, state.bestCost)) {
+      // Compute correct position to be used for splitting
+      d->position = state.bestPosition;
+      d->twoSplits = state.bestTwoSplits; // one or two splitting planes planed
+      if (d->twoSplits > 1) {
+	if (secondPositionMax)
+	  d->position2 = state.bestPosition2;
+	else
+	  d->position2 = state.box.Max(axis);
+      }
+    }
+  }
+  else {
+    int algorithmForAxisSel = _algorithmForAxisSelection;
+    if (d->modeSubDiv == EE_SUBDIVCUTEMPTY)
+      algorithmForAxisSel = 0;
+
+    // only a single axis will be tested
+    bool useSingleAxis = true;
+        
+    // the required axis is by global function .. e.g. cyclic change x, y, z
+    switch (algorithmForAxisSel) {
+      case 0: {
+	// all three axis will be used, starting from x, y, z
+	useSingleAxis = false;
+	break;
+      }
+      case 1: {
+	// cyclic order of axes, x, y, z, x, y....
+	axis = d->pars.reqAxis;
+	// compute the axis for the next subdivision step
+	d->pars.reqAxis = oaxes[axis][0];
+	break;
+      }
+      case 2 : {
+	// The next time will be determined the same way
+	axis = (CKTBAxes::Axes)(d->box.Diagonal().DrivingAxis());
+	break;
+      }
+      case 3 : {
+	float p = RandomValue(0.0f, 1.0f);
+	const float thresholdAxisP = 0.8f;
+	if (p < thresholdAxisP) {	  
+	  axis = (CKTBAxes::Axes)(d->box.Diagonal().DrivingAxis());
+	}
+	else {
+	  // Use the axis prescribed from previous step
+	  axis = d->pars.reqAxis;
+	}
+	// for the next time, different axis
+	d->pars.reqAxis = oaxes[axis][0];
+	break;
+      }
+      case 4 : {
+	float p = RandomValue(0.0f, 1.0f);
+	const float thresholdAxisP = 0.3f;
+	if (p < thresholdAxisP) {	  
+	  axis = (CKTBAxes::Axes)(d->box.Diagonal().DrivingAxis());
+	}
+	else {
+	  // Use the axis prescribed from previous step
+	  axis = d->pars.reqAxis;
+	}
+	// for the next time, different axis
+	d->pars.reqAxis = oaxes[axis][0];
+	break;
+      }
+      case 5 : {
+	float p = RandomValue(0.0f, 1.0f);
+	const float thresholdAxisP = 0.2f;
+	d->pars.reqAxis = (CKTBAxes::Axes)-1;
+	if (p < thresholdAxisP) {
+	  axis = (CKTBAxes::Axes)(d->box.Diagonal().DrivingAxis());
+	  // for the next time, different axis
+	}
+	else {
+	  // Use the axis for which cost model gets minimum,
+	  // compute it for all axes x, y, z
+	  useSingleAxis = false;
+	}
+	break;
+      }
+      default: {
+	cerr << "Selection algorithm = " << algorithmForAxisSel
+	      << " for axis is not implemented" << endl;
+	abort();;
+      }
+    } // switch
+
+    // How the algorithm is used
+    if (useSingleAxis) {
+      // -------------------------------------------
+      // Compute subdivision only for one axis
+      switch(axis) {
+        case CKTBAxes::EE_X_axis : {
+	  state.InitXaxis(d->count, d->box); // init
+	  CallGetSplitPlane(d, 0); // evaluate
+	  break;
+	}
+        case CKTBAxes::EE_Y_axis : {
+	  state.InitYaxis(d->count, d->box); // init
+	  CallGetSplitPlane(d, 1); // evaluate
+	  break;
+	}
+        case CKTBAxes::EE_Z_axis : {
+	  state.InitZaxis(d->count, d->box); // init
+	  CallGetSplitPlane(d, 2); // evaluate
+	  break;
+	}
+	//case CKTBAxes::EE_Leaf : {
+	//goto MAKE_LEAF;
+        //}
+        default: {
+	  cerr << "Selection algorithm = " << algorithmForAxisSel
+		<< " for axis = " << axis <<" is not implemented" << endl;
+	  abort();;
+	}
+      }
+      state.NormalizeCostBySA2();
+      d->bestCost /= state.areaWholeSA2;
+      if (UpdateEvaluation(d->bestCost, state.bestCost)) {
+	// Compute correct position to be used for splitting
+	d->position = state.bestPosition;
+	d->twoSplits = state.bestTwoSplits; // one or two splitting planes planed
+	if (d->twoSplits > 1) {
+	  if (secondPositionMax)
+	    d->position2 = state.bestPosition2;
+	  else
+	    d->position2 = state.box.Max(axis);
+	}
+      }
+    }
+    else {
+      // no axis is required, we can pickup any we like. We test all three axes
+      // and we pickup the minimum cost for all three axes.
+      state.InitXaxis(d->count, d->box); // init for x axis
+      CallGetSplitPlane(d, 0); // evaluate for x axis
+      if (UpdateEvaluation(d->bestCost, state.bestCost)) {      
+	axis = CKTBAxes::EE_X_axis; // the x-axis should be used
+	// Compute correct position to be used for splitting
+	d->bestCost = state.bestCost;
+	d->position = state.bestPosition;
+	d->twoSplits = state.bestTwoSplits; // one or two splitting planes planed
+	if (d->twoSplits > 1) {
+	  if (secondPositionMax)
+	    d->position2 = state.bestPosition2;
+	  else
+	    d->position2 = state.box.Max(axis);
+	}
+      }
+      // -----------------------
+      // now test for y axis
+      state.InitYaxis(d->count, d->box); // init for x axis
+      CallGetSplitPlane(d, 1); // evaluate for x axis
+      if (UpdateEvaluation(d->bestCost, state.bestCost)) {
+	axis = CKTBAxes::EE_Y_axis; // the y-axis should be used
+	// Compute correct position to be used for splitting
+	d->bestCost = state.bestCost;
+	d->position = state.bestPosition;
+	d->twoSplits = state.bestTwoSplits; // one or two splitting planes planed
+	if (d->twoSplits > 1) {
+	  if (secondPositionMax)
+	    d->position2 = state.bestPosition2;
+	  else
+	    d->position2 = state.box.Max(axis);
+	}
+      }
+      // -----------------------
+      // now test for z axis
+      state.InitZaxis(d->count, d->box); // init for x axis
+      CallGetSplitPlane(d, 2); // evaluate for x axis
+      if (UpdateEvaluation(d->bestCost, state.bestCost)) {
+	axis = CKTBAxes::EE_Z_axis; // the z-axis should be used
+	// Compute correct position to be used for splitting
+	d->bestCost = state.bestCost;
+	d->position = state.bestPosition;
+	d->twoSplits = state.bestTwoSplits; // one or two splitting planes planed
+	if (d->twoSplits > 1) {
+	  if (secondPositionMax)
+	    d->position2 = state.bestPosition2;
+	  else
+	    d->position2 = state.box.Max(axis);
+	}
+      }
+      // Now we have to renormalize the cost for purpose of termination the building
+      d->bestCost /= state.areaWholeSA2;
+    } // arbitrary order of axes
+  } // for which axis to compute surface area heuristics
+
+  // ----------------------------------------------------------
+  // when automatic termination criteria should be used
+  if (d->modeSubDiv == EE_SUBDIVAUTOMATIC) {
+    bool stopSubdivision = false;
+    float ratio;
+    // which algorithm to use for automatic termination criteria
+    switch (algorithmAutoTermination) {
+      // --------------------------------------------------
+      case 0: { // This is described in my thesis.
+	// This is the cost of unsubdivided node
+	float leafCost = (float)(d->count);
+	ratio = d->bestCost / leafCost;
+	const float minRatio = 0.75;
+	if (ratio > minRatio)
+	  stopSubdivision = true;
+	break;
+      }
+      // --------------------------------------------------
+      case 1: {
+	// This is the cost of unsubdivided node, with uniformly distributed
+	// objects and the spatial median subdivision, without any object straddling
+	// the splitting plane. In some sense ideal setting for uniform distribution.
+	Vector3 s = d->box.Diagonal();
+	// Taking the widest side of the object to be subdivided
+	int diagAxis = s.DrivingAxis();
+	// Half of the width
+	// ----------------------
+	float w2 = s[diagAxis] * 0.5;
+	float t = s[oaxes[diagAxis][0]];
+	float h = s[oaxes[diagAxis][1]];
+	float sah2 = w2*(t+h) + h*t; // half of the surface area of a child
+	// This is the cost for case when the node is subdivided in the half and
+	// the half of the object is on the left and half of the objects on the right
+	// This is not the best cost possible !!!
+	// = float subdividedCost = 2 * (sah2/state.areaWholeSA2 * count/2);
+	float subdividedCost = sah2/state.areaWholeSA2 * (float)(d->count);
+	ratio = d->bestCost / subdividedCost;
+ 	// This is the max ratio allowed for splitting node subdivision
+
+	// The computed ratio = 1.0 corresponds to 'ideal case', so the threshold
+	// must be higher than 1.0 !
+	const float minRatio = 1.1;
+	if (ratio > minRatio)
+	  stopSubdivision = true;
+	break;
+      }
+      default : {
+	cerr << "Uknown algorithm for automatic termination criteria = "
+	      << algorithmAutoTermination << endl;
+      }
+       
+    } // switch
+    
+    //cout << "R=" << ratio << " ";
+    if (stopSubdivision) {
+      // cout << "F" << pars.failedSubDivCount << " ";
+      // when small improvement by the subdivision is reached
+      d->pars.failedSubDivCount++;
+      if (d->pars.failedSubDivCount > maxCountTrials) {
+	// make leaf and finish with this KD-tree branch
+	// cout << "L, cnt=" << count << " ,d=" << GetDepth()()
+	//     << " ,c=" << bestQ << " ,rat=" << ratio << "\n";
+	// possibly cut empty space before leaf is created
+	if (cutEmptySpace) {
+	  // setting initial depth if empty space cutting
+	  startEmptyCutDepth = GetDepth();
+	  IncDepth();
+	  d->modeSubDiv = EE_SUBDIVCUTEMPTY;
+	  // create the leaf first, with late empty cutting
+	  SKTBNodeT *n = SubDiv(d);
+	  if (!nodeToReturn)
+	    nodeToReturn = n;
+	  DecDepth();
+	  return nodeToReturn; // and finish by constructing a leaf
+	}
+
+	// make leaf and finish
+	SKTBNodeT *n = MakeLeaf(d);
+	if (!nodeToReturn)
+	  nodeToReturn = n;
+	return nodeToReturn;
+      }
+    }
+
+    // Do not stop subdivision now
+    
+    // update the progress in the parameters
+    d->pars.ratioLastButOne = d->pars.ratioLast;
+    d->pars.ratioLast = ratio;
+
+    // assure that axis is defined, if finding the splitting plane has failed
+    if ( (axis == CKTBAxes::EE_Leaf) ||
+	 (d->position == MAXFLOAT) ) {
+      // compute the spatial median for arbitrary axis
+      axis = (CKTBAxes::Axes)int(RandomValue(0.f, 2.98f));
+      // set position based algorithm for the next cut and children
+      d->position = (d->box.Min(axis) + d->box.Max(axis)) * 0.5f;
+      d->twoSplits = 1;
+    }
+  } // subdivision automatic
+
+  // -----------------------------------------------
+  // We have evaluated surface area heuristics above
+  // if no winner for subdivision exists, make a leaf
+  if ( (axis == CKTBAxes::EE_Leaf) ||
+       (d->position == MAXFLOAT) )
+  {
+    //cerr << "This shall not happen" << endl;
+    SKTBNodeT *n = MakeLeaf(d);
+    if (!nodeToReturn)
+      nodeToReturn = n;
+    // cout << "LL\n";
+    return nodeToReturn;
+  }
+  
+  if (verbose) {
+#ifdef _DEBUG
+
+//#define _KTBPRINTCUTS
+#ifndef _KTBPRINTCUTS
+    if (GetDepth() == 0) 
+#else
+    if (_printCuts)
+#endif
+    {
+      cout << "position: " << d->position << ", axis: "
+	     << (int)axis << ", depth=" << GetDepth() << ", box:" << endl;
+      Describe(d->box, cout, 0);
+      cout << endl;
+    }
+#endif // _DEBUG
+  }
+
+#ifdef _DEBUG
+  if (d->twoSplits == -1) {
+    cerr << "Some problem in implementation" << endl;
+    abort();;
+  }
+#endif
+
+  // copy the parameters to use for splitting
+  d->axis = axis;
+
+  if (d->twoSplits == 1) {
+    // create interior node with two successors
+    // case (1)
+    // DEBUG << "case 1 \n";
+    // Make easy cut, using one position
+    IncDepth();
+    SKTBNodeT* n = MakeOneCut(d);
+    if (!nodeToReturn)
+      nodeToReturn = n;
+    DecDepth();
+    return nodeToReturn;
+  }
+
+#if 1
+  cerr << "Unexpected use in this implementation" << endl;
+  cerr << "ABORTING " << endl;
+  abort();;
+#endif
+  
+//  case       case
+//  (2)   or   (3) structure must be created
+//   O          O                #
+//  / \        / \               #
+// L  RI      LI  R              #
+//   /  \    / \                 #
+//   RL RR  LL LR                #
+
+#ifdef _DEBUG
+  if ( (d->box.Min()[axis] >= d->position) ||
+       (d->position >= d->position2) ||
+       (d->position2 >= d->box.Max()[axis]) ) {
+    cerr << " the case 2 or 3 is defined incorrectly\n";
+    abort();;
+  }
+#endif
+
+  // To be reworked !!!
+  abort();;
+  if (d->twoSplits == 2) {
+    // -----------------------------------------
+    // case (2)
+    // DEBUG << "case 2 \n";
+
+    // make L part, linked in DFS order
+    int cntLeft = 1;
+    int cntRight = 0;
+    IncDepth();
+    SKTBNodeT *n = MakeOneCut(d);
+    if (!nodeToReturn)
+      nodeToReturn = n;
+
+    // make RI part, linked to the left node explictly
+    SBBox rbb = d->box; // the bounding box
+    rbb.Reduce(axis, 1, d->position);
+    d->box = rbb;
+    d->pars.reqAxis = axis;
+    d->pars.reqPosition = d->position2;    
+    SKTBNodeT *n2 = SubDiv(d);
+    DecDepth();
+    //Link the node
+    SetInteriorNodeLinks(n, 0, n2);
+    return nodeToReturn;
+  }
+
+  // -----------------------------------------
+  // case (3) ????????????????? I am not sure if this is correct implementation !!! VH
+  // DEBUG << "case 3 \n";
+  
+  // make LI part
+  SBBox lbb = d->box; // the bounding box
+  lbb.Reduce(axis, 0, d->position2);
+  int cntLeft = 0;
+  int cntRight = 1;
+  d->box = lbb;
+  d->position = d->position2;
+  d->axis = axis;
+  // the next subdivision step for R part
+  d->pars.reqAxis = axis;
+  d->pars.reqPosition = d->position;
+  
+  IncDepth();
+  SKTBNodeT *nl = SubDiv(d);
+  if (!nodeToReturn)
+    nodeToReturn = nl;
+  
+  // make R part
+  SKTBNodeT *n = MakeOneCut(d);
+  DecDepth();
+  SetInteriorNodeLinks(nl, 0, n);
+  // return left node, linked in DFS order to the previous one
+  return nodeToReturn;
+}
+
+
+
+// makes cutting on the given position on given axis
+// at value position, bb is the bounding box of current node,
+// count is the number of objects in the node
+// flags LeftF and RightF declares if to continue down after the recursion
+SKTBNodeT *
+CKTBSBuildUp::MakeOneCut(SInputData *d)
+{
+#ifdef _DEBUG
+  assert(d->position >= d->box.Min(d->axis));
+  assert(d->position <= d->box.Max(d->axis));
+#endif
+  
+  SKTBNodeT *nodeToReturn = 0;
+
+#ifdef  _KTB_CONSTR_STATS
+  // surface area of the interior nodes to be subdivided
+  _sumSurfaceAreaInteriorNodes += d->box.SA2();
+#endif // _KTB_CONSTR_STATS
+  
+  //if (splitClip) {
+  //  // empty object list to store the pointers to the split objects
+  //  ObjectContainer objlist;
+  //  cerr << "Not yet handled" << endl;
+  //  abort();;
+  // }
+
+#if 0
+  // check if the lists are correctly sorted .. in debug
+  static int index = 0;
+#if 1
+  cout << "MakeOneCut index = " << index << " .. check axis = "
+	<< d->axis << " count = " << d->count
+	<< " depth = " << GetDepth() << endl;
+  const int indexToFind = 3743;
+  if (index == indexToFind) {
+    
+    cout << "Debug should start " << endl;
+    cout << "index = indexToFind" << endl;
+  }
+#endif
+  index++;  
+#endif
+    
+  // We have to allocate a new node for right child
+  SInputData* rightData = AllocNewData();
+  // Copy the basic data from parent
+  rightData->CopyBasicData(d);
+  // We compute the tight box after the splitting
+  rightData->tightbox.Initialize();
+  rightData->objlist = new ObjectContainer;
+  assert(rightData->objlist);
+
+  // we compute the tight box for the objects on the
+  // left of the splitting plane
+  d->tightbox.Initialize();
+  
+  // Simply go through the list of primitives and those that belong
+  // to the left keep on the left (d), those that belong to the right
+  // assign to the right list
+  int cntL = 0, cntR = 0;
+  {
+    ObjectContainer::iterator endit = d->objlist->end();
+    int axis = d->axis;
+    AxisAlignedBox3 obox;
+    for (ObjectContainer::iterator it = d->objlist->begin(); it != endit; it++) {
+      Intersectable *obj = *it;
+      obox = obj->GetBox();
+      if (obox.Max(axis) > d->position) {
+	rightData->objlist->push_back(obj);
+	cntR++;
+	// the number of objects on the right of the splitting plane
+	rightData->tightbox.Include(obox);
+      }
+      if (obox.Min(axis) >= d->position) {
+	// we simply shorten the list - this object belongs only to the right list
+	(*it) = *(endit-1);
+	it--;
+	endit--;
+      }
+      else {
+	// the number of objects on the right of the splitting plane
+	cntL++;
+	d->tightbox.Include(obox);
+      }
+    } // for
+  }
+
+  assert(cntL + cntR >= d->count);
+
+  // ---------------------------------------------
+  // initialize the left bounding box 'bb'
+  d->box.Reduce(d->axis, 0, d->position);
+  d->objlist->resize(cntL);
+  
+  // initialize the right bounding box 'rbb'
+  rightData->box.Reduce(d->axis, 1, d->position);
+
+  // Set correct number of objects
+  d->count = cntL;
+  rightData->count = cntR;
+
+  d->tightbox = Intersect(d->tightbox, d->box);
+  rightData->tightbox =
+    Intersect(rightData->tightbox, rightData->box);
+  
+  // Allocate the representation of the interior node
+  SKTBNodeT *node = 0;
+
+  if (makeMinBoxHere) {
+    // -------------------------------------------------------
+    // interior node with the box
+    node = AllocInteriorNodeWithBox(// interior node data
+				    d->axis, d->position, cntL, cntR, 
+				    // box data
+				    d->box, d->lastMinBoxNode,
+				    d->lastDepthForMinBoxes);
+
+    //cout << "depth = " << GetDepth() << " node = " << node
+    //	 << " lastMinBoxNode = " << d->lastMinBoxNode << endl;
+    
+    d->lastMinBoxNode = node; // we have to remember the last node
+    rightData->lastMinBoxNode = node;
+    makeMinBoxHere = false; // reset for the next time
+  }
+  else
+    // normal interior node (=without box)
+    node = AllocInteriorNode(d->axis, d->position, cntL, cntR);
+  
+#ifdef _DEBUG
+  if (!node) {
+    cerr << "Allocation of Interior Node failed.\n";
+    abort();;
+  }
+#endif // _DEBUG
+  // set the node to be returned
+  if (!nodeToReturn)
+    nodeToReturn = nodeToLink;
+  assert(node);
+  assert(nodeToReturn);
+  
+  // ---------------------------------------------
+  // initialize the left bounding box 'bb'
+  d->box.Reduce(d->axis, 0, d->position);
+
+  // initialize the right bounding box 'rbb'
+  rightData->box.Reduce(d->axis, 1, d->position);
+
+  // Set correct number of objects
+  d->count = cntL;
+  rightData->count = cntR;
+  
+  // The children nodes to be created
+  SKTBNodeT *nodeL=0, *nodeR=0;
+
+  // Recurse to the left child
+  if (d->makeSubdivisionLeft) {
+    // subdivide branches
+    ESubdivMode modeLeft = d->modeSubDiv ;
+    if ( (cutEmptySpace) &&
+	 (modeLeft != EE_SUBDIVCUTEMPTY) &&
+	 (d->count <= maxListLength) ) {
+      // setting initial depth if empty space cutting
+      startEmptyCutDepth = GetDepth();
+      d->modeSubDiv = EE_SUBDIVCUTEMPTY;
+    }
+    // DEBUG << "Left cnt= " << (cnt[0] >> 1) << " depth= "
+    //       << GetDepth() << "\n";
+    nodeL = SubDiv(d);
+  }
+  
+  // Recurse to the right child
+  if (rightData->makeSubdivisionRight) {
+    ESubdivMode modeRight = d->modeSubDiv ;
+    if ( (cutEmptySpace) &&
+	 (modeRight != EE_SUBDIVCUTEMPTY) &&
+	 (rightData->count <= maxListLength) ) {
+      // setting initial depth if empty space cutting
+      startEmptyCutDepth = GetDepth();
+      rightData->modeSubDiv = EE_SUBDIVCUTEMPTY;
+    }
+    // DEBUG << "Right cnt= " << (cnt[1] >> 1) << " depth= "
+    // << GetDepth() << "\n";
+    nodeR = SubDiv(rightData);
+
+  }
+
+  delete rightData->objlist;
+  rightData->objlist = 0;
+  delete d->objlist;
+  d->objlist = 0;
+  
+  // Free the data to be reused further on
+  FreeLastData();
+
+  // Set the node pointers to the children for created node
+  SetInteriorNodeLinks(node, nodeL, nodeR);
+
+  return nodeToReturn; // return the node 
+}
+
+// returns a box enclosing all the objects in the node
+void
+CKTBSBuildUp::GetTightBox(const SInputData &d, SBBox &tbox)
+{
+  tbox = d.tightbox;
+}
+
+// make the full leaf from current node
+SKTBNodeT*
+CKTBSBuildUp::MakeLeaf(SInputData *d)
+{
+#ifdef  _KTBPRINTCUTS
+  cout << " Creating leaf, depth = " << GetDepth()
+       << " cntObjs = " << count << endl;
+  //exit(3);
+#endif
+  
+#ifdef _KTB_CONSTR_STATS
+  // update the statistics
+  float sa2bb;
+  _sumSurfaceAreaLeaves += (sa2bb = d->box.SA2());
+  _sumSurfaceAreaMULcntLeaves += ((float)d->count * sa2bb);
+#endif // _KTB_CONSTR_STATS
+  
+  // ------------------------------------------------------
+  // The version with allocating empty leaves. This makes
+  // sense since in DFS order it works correctly
+  SKTBNodeT *node = AllocLeaf(d->count);
+
+  if (d->count > 0) {
+    // copy the list of objects
+    ObjectContainer *objlist = d->objlist;
+    assert(objlist->size() == d->count);
+    // Do not delete the object list, it is used as allocated above
+    SetFullLeaf(node, objlist);
+    // We delete the object list since this is used at the level of leaves
+    d->objlist = 0;
+  }
+
+  // We assume that the allocation of the single leaf cannot
+  // fail, since this one shall be the last in the sequence.
+  assert(node == nodeToLink);
+  
+  // Return the node to be used in the linking
+  return nodeToLink;
+}
+
+// --------------------------------------------------------------------
+// class CKTBSBuildUp::CTestAx
+
+// The initialization for the first axis to be tested, this time *****Z axis*******
+void
+CKTBSBuildUp::SSplitState::InitXaxis(int cnt, const SBBox &boxN)
+{
+  // initialize the variables, mainly for surface area estimates
+  cntAll = cnt;     // the number of all objects in the bounding box
+  cntLeft = 0;      // the count of bounding boxes on the left
+  cntRight = cnt;   // the count of bounding boxes on the right
+  thickness = 0;    // the count of bounding boxes straddling the splitting plane
+  axis = CKTBAxes::EE_X_axis; // the axis, where the splitting is proposed
+  box = boxN;        // the box, that is subdivided
+  //int topAxis = 1;        // axis that is considered in top .. depth
+  //int frontAxis = 2;      // axis that is considered in front .. height  
+  // the size of bounding box along the axis to be split
+  width = box.Max().x - box.Min().x;
+  // and along two other axes  
+  topw = box.Max().y - box.Min().y;
+  frontw = box.Max().z - box.Min().z;
+  // surface area of the splitting plane .. one face !!
+  areaSplitPlane = topw * frontw;
+  // the sum of length of the boxes not to be subdivided
+  areaSumLength = topw + frontw;
+  // The surface are of the whole box
+  areaWholeSA2 = width * areaSumLength + areaSplitPlane;
+  // initial evaluation .. the worst cost
+  bestCost = WorstEvaluation();
+  // the buckets are zeroed
+  InitBuckets();
+}
+
+// The initialization for the first axis to be tested, this time *****Y axis*******
+// This can be run independently.
+void
+CKTBSBuildUp::SSplitState::InitYaxis(int cnt, const SBBox &boxN)
+{
+  // initialize the variables, mainly for surface area estimates
+  cntAll = cnt;     // the number of all objects in the bounding box
+  cntLeft = 0;      // the count of bounding boxes on the left
+  cntRight = cnt;   // the count of bounding boxes on the right
+  thickness = 0;    // the count of bounding boxes straddling the splitting plane
+  axis = CKTBAxes::EE_Y_axis; // the axis, where the splitting is proposed
+  box = boxN;        // the box, that is subdivided
+  //int frontAxis = oaxes[axis][0]; // axis that is considered in front .. height - x-axis
+  //int topAxis = oaxes[axis][1]; // axis that is considered in top .. depth - y-axis
+  // the size along the second axis - height
+  frontw = box.Max().x - box.Min().x;
+  // the size of bounding box along the axis to be split - width
+  width = box.Max().y - box.Min().y;
+  // The size along third axis - depth
+  topw = box.Max().z - box.Min().z;
+  // surface area of the splitting plane .. one face !!
+  areaSplitPlane = topw * frontw;
+  // the sum of length of the boxes not to be subdivided
+  areaSumLength = topw + frontw;
+  areaWholeSA2 = width * areaSumLength + areaSplitPlane;
+  // initial evaluation .. the worst cost
+  bestCost = WorstEvaluation();
+  // the buckets are zeroed
+  InitBuckets();
+}
+
+// The initialization for the first axis to be tested, this time *****Z axis*******
+// This can be run independently.
+void
+CKTBSBuildUp::SSplitState::InitZaxis(int cnt, const SBBox &boxN)
+{
+  // initialize the variables, mainly for surface area estimates
+  cntAll = cnt;     // the number of all objects in the bounding box
+  cntLeft = 0;      // the count of bounding boxes on the left
+  cntRight = cnt;   // the count of bounding boxes on the right
+  thickness = 0;    // the count of bounding boxes straddling the splitting plane
+  axis = CKTBAxes::EE_Z_axis; // the axis, where the splitting is proposed
+  box = boxN;       // the box, that is subdivided
+  //int frontAxis = oaxes[axis][1]; // axis that is considered in front .. height - x axis
+  //int topAxis = oaxes[axis][0]; // axis that is considered in top .. depth - y axis
+  // The size along third axis - depth
+  topw = box.Max().x - box.Min().x;
+  // the size along the second axis - height
+  frontw = box.Max().y - box.Min().y;
+  // the size of bounding box along the axis to be split - width
+  width = box.Max().z - box.Min().z;
+  // surface area of the splitting plane .. one face !!
+  areaSplitPlane = topw * frontw;
+  // the sum of length of the boxes not to be subdivided
+  areaSumLength = topw + frontw;
+  areaWholeSA2 = width * areaSumLength + areaSplitPlane;
+  // initial evaluation .. the worst cost
+  bestCost = WorstEvaluation();
+  // the buckets are zeroed
+  InitBuckets();
+}
+
+// This is the computation of the cost using surface area heuristics
+// using LINEAR ESTIMATE
+void
+CKTBSBuildUp::EvaluateCost(SSplitState &state)
+{
+  // the surface area of the bounding box for the left child
+  assert(state.position > -1e-9);
+  float areaLeft = state.position * state.areaSumLength + state.areaSplitPlane;
+
+  // the surface area of the right bounding box for the right child
+  assert(state.width - state.position > -1e-9);
+  float areaRight = (state.width - state.position) * state.areaSumLength +
+                    state.areaSplitPlane;
+
+  // computation of the cost .. smaller is better
+  float cost = areaLeft * state.cntLeft + areaRight * state.cntRight;
+
+#ifdef _DEBUG_COSTFUNCTION
+  // Put there normalized position and cost also normalized somehow
+  ReportCostStream(state.position / state.width,
+		   cost / (state.areaWholeSA2 * state.cntAll));
+#endif
+  
+  // This normalization is not in fact necessary here
+  //cost //= state.areaWholeSA2;
+  if (cost < state.bestCost) {
+    state.bestCost = cost;
+    // The iterator pointing to the best boundary
+    state.bestPosition = state.position;
+    // The number of objects whose boundaries are duplicated
+    //state.bestThickness = state.thickness;
+  }
+  return;
+}
+
+// This is the computation of the cost using surface area heuristics
+void
+CKTBSBuildUp::EvaluateCostFreeCut(SSplitState &state)
+{
+  // This is assumed to work for free cut (no object intersected)
+  assert(state.thickness == 0);
+  assert((state.cntLeft == 0) || (state.cntRight == 0));
+  
+  // the surface area of the bounding box for the left child
+  assert(state.position > -1e-9);
+  float areaLeft = state.position * state.areaSumLength + state.areaSplitPlane;
+
+  // the surface area of the right bounding box for the right child
+  assert(state.width - state.position > -1e-9);
+  float areaRight = (state.width - state.position) * state.areaSumLength +
+                    state.areaSplitPlane;
+
+  // computation of the cost .. smaller is better
+  float cost = biasFreeCuts *(areaLeft * state.cntLeft + areaRight * state.cntRight);
+
+#ifdef _DEBUG_COSTFUNCTION
+  // Put there normalized position and cost also normalized somehow
+  ReportCostStream(state.position / state.width,
+		   cost / (state.areaWholeSA2 * state.cntAll));
+#endif
+  
+  // This normalization is not in fact necessary here
+  //cost //= state.areaWholeSA2;
+  if (cost < state.bestCost) {
+    state.bestCost = cost;
+    // The iterator pointing to the best boundary
+    state.bestPosition = state.position;
+    // The number of objects whose boundaries are duplicated
+    // state.bestThickness = 0;
+  }
+  return;
+}
+
+// ---------------------------------------------------------------------
+// For a given X-axis search for the best splitting plane position.
+// This is implemented by sampling
+void
+CKTBSBuildUp::GetSplitPlaneOpt(SInputData *d, int axisToTest)
+{
+  // some necessary space is required to cut off
+  const float MinPosition = d->tightbox.Min(axisToTest);
+  const float MaxPosition = d->tightbox.Max(axisToTest);
+  const float bMinPosition = d->box.Min(axisToTest);
+  state.bestTwoSplits = 1;
+
+  // This makes no sense to compute, if the width of the box
+  // is zero!
+  if (bMinPosition == d->box.Max(axisToTest)) {
+    state.bestPosition = bMinPosition;
+    return;
+  }
+
+  int bucketN = state.bucketN;
+
+  for (int i = 0; i < bucketN; i++) {
+    state.bucketMin[i] = 0;
+    state.bucketMax[i] = 0;
+  }
+  
+  float mult = (float)bucketN / (MaxPosition - MinPosition);
+
+  ObjectContainer::iterator endit = d->objlist->end();
+  int axis = axisToTest;
+  AxisAlignedBox3 obox;
+  for (ObjectContainer::iterator it = d->objlist->begin(); it != endit; it++) {
+    Intersectable *obj = *it;
+    obox = obj->GetBox();
+    float v = obox.Min(axis);
+    if (v < MinPosition)
+      state.bucketMin[0]++;
+    else
+      if (v > MaxPosition)
+	state.bucketMin[bucketN-1]++;
+      else {	
+	assert(v >= MinPosition);
+	assert(v <= MaxPosition);    
+	int bucketPos = (int)((v - MinPosition) * mult);
+	assert(bucketPos >= 0);
+	assert(bucketPos <= bucketN);
+	if (bucketPos == bucketN)
+	  bucketPos = bucketN-1;
+	state.bucketMin[bucketPos]++;
+      }
+
+    float v2 = obox.Max(axis);
+    if (v2 < MinPosition)
+      state.bucketMax[0]++;
+    else
+      if (v2 > MaxPosition)
+	state.bucketMax[bucketN-1]++;
+      else {	      
+	assert(v2 >= MinPosition);
+	assert(v2 <= MaxPosition);    
+	int bucketPos2 = (int)((v2 - MinPosition) * mult);
+	assert(bucketPos2 >= 0);
+	assert(bucketPos2 <= bucketN);
+	if (bucketPos2 == bucketN)
+	  bucketPos2 = bucketN-1;
+	state.bucketMax[bucketPos2]++;
+      }
+  } // for
+
+  // First evaluate the cost, when the splitting plane is on the left
+  state.position = MinPosition - bMinPosition;
+  assert(state.position >= 0.f);
+  assert(state.position <= state.width);
+  state.cntLeft = 0;
+  state.cntRight = d->count;
+  state.thickness = 0;
+  EvaluateCostFreeCut(state);
+  int cntLeft = 0;
+  int cntRight = d->count;
+  int thickness = 0;
+  // --------------------------------------
+  float imult = (MaxPosition - MinPosition) / (float)bucketN;
+  for (int i = 0; i < bucketN-1; i++) {
+    cntLeft += state.bucketMin[i];
+    cntRight -= state.bucketMax[i];
+    thickness = cntLeft + cntRight - d->count;
+    assert(thickness >= 0);
+    state.cntLeft = cntLeft;
+    state.cntRight = cntRight;
+    state.thickness = thickness;
+    state.position = MinPosition - bMinPosition + imult * ((float)(i+1));
+    assert(state.position >= 0);
+    assert(state.position <= state.width);
+
+    EvaluateCost(state);
+  } // for
+
+  // free cut
+  state.position = MaxPosition - bMinPosition;
+  assert(state.position >= 0);
+  assert(state.position <= state.width);
+  state.cntLeft = d->count;
+  state.cntRight = 0;
+  state.thickness = 0;
+  EvaluateCostFreeCut(state);
+
+  // we move back to the space
+  state.bestPosition += bMinPosition;
+  
+  // In this case the best result is kept in 'state' variable
+  return;
+} // CTestAx::GetSplitPlaneOpt (for X, Y, Z)
+
+}
+
Index: /GTP/trunk/Lib/Vis/Preprocessing/src/havran/ktbs.h
===================================================================
--- /GTP/trunk/Lib/Vis/Preprocessing/src/havran/ktbs.h	(revision 2632)
+++ /GTP/trunk/Lib/Vis/Preprocessing/src/havran/ktbs.h	(revision 2632)
@@ -0,0 +1,364 @@
+// ============================================================================
+// $Id: $
+//
+// ktbai.h
+//     classes for building up the different KD-trees
+//
+// Class: CKTBBuildUp, CKTBBuildUp_new
+//
+// REPLACEMENT_STRING
+//
+// Initial coding by Vlasta Havran, January 2008
+
+#ifndef __KTBS_H__
+#define __KTBS_H__
+
+// GOLEM headers
+#include "configh.h"
+#include "ktbconf.h"
+#include "ktb.h"
+#include "ktb8b.h"
+#include "ktbai.h"
+#include "Containers.h"
+#include "IntersectableWrapper.h"
+
+namespace GtpVisibilityPreprocessor {
+
+// forward declarations
+class SKTBNode;
+
+#ifndef _KTB8Bytes
+// Use 12 Bytes representation
+#define CKTBAllocManPredecessor CKTBAllocMan
+#undef  SKTBNodeT
+#define SKTBNodeT CKTBNodeAbstract::SKTBNode
+#else
+// Use 8 Bytes representation per node
+#define CKTBAllocManPredecessor CKTB8BAllocMan
+#undef  SKTBNodeT
+#define SKTBNodeT CKTB8BNodeAbstract::SKTBNode
+#endif
+
+#ifndef INFINITY
+#define INFINITY 10e10
+#endif
+
+// ---------------------------------------------------------------  
+// The base class for KD-tree with irregular change of axes, where
+// the splitting plane can be positioned.
+class CKTBSBuildUp:
+  public CKTBAllocManPredecessor
+{
+protected:
+  struct SSplitState
+  {
+    // counts
+    int   cntAll;       // the number of all objects in the bounding box
+    int   cntLeft;      // the count of bounding boxes on the left
+    int   cntRight;     // the count of bounding boxes on the right
+    int   thickness;    // the count of bounding boxes straddling the splitting plane
+
+    // The buckets
+    int   bucketN; // the number of buckets
+    int   *bucketMin; // the buckets for left (minimum) boundaries
+    int   *bucketMax; // the buckets for right (max) boundaries
+
+    CKTBAxes::Axes  axis;  // the axis, where the splitting is proposed
+    float sizeb[3];     // the size of the box for x, y, and z
+    SBBox box;          // the box, that is subdivided
+
+    // derived values from basic ones
+    float width;        // the size of bounding box along the axis
+    float frontw;       // the size of the bounding box in another axis (depth)
+    float topw;         // the size of the bounding box in next next axis (height)
+    float areaSplitPlane; // the area of the splitting plane
+    float areaSumLength; // the size of the bounding as sum of height and depth
+    float areaWholeSA2; // the half of the surface area of the whole box for this node
+
+    // The position for this splitting plane to be evaluated
+    float position; // the distance from the left boundary of the box for this node
+    // The position for the next position, makes sense only for free interval (thickness=0)
+    float position2; // the distance from the left boundary of the box for this node
+    
+    // The evaluation best cost until now
+    float bestCost;
+    // The position to be used
+    float bestPosition;
+    // The second best position - NOT USED AT THE MOMENT FOR SAMPLING !!!
+    float bestPosition2;
+    // Which mechanism to be used for splitting, either 0,1, or 2 splitting planes
+    int   bestTwoSplits;
+
+    // setting the evaluation for split cases that must not be done
+    float WorstEvaluation() const { return MAXFLOAT;}
+    
+    // The initialization for the first axis to be tested.
+    void InitXaxis(int cnt, const SBBox &box);
+    void InitYaxis(int cnt, const SBBox &box);
+    void InitZaxis(int cnt, const SBBox &box);
+
+    // Normalize the best cost by surface area of the box
+    void NormalizeCostBySA2() { bestCost /= areaWholeSA2;}
+
+    SSplitState():bucketN(0), bucketMin(0), bucketMax(0) { }
+    ~SSplitState() { delete []bucketMin; delete []bucketMax;}
+
+    void InitBuckets() {
+      assert(bucketMin);
+      assert(bucketMax);
+      for (int i = 0; i < bucketN; i++) {
+	bucketMin[i] = 0;
+	bucketMax[i] = 0;
+      } // for
+    }
+  };
+
+  // splitting state for current search
+  SSplitState state;
+  
+  // structure for prefered and required params for evaluation functions
+  // and the termination criteria
+  struct SReqPrefParams
+  {
+    //if any position on required axis is preferred for next subdivision step
+    float reqPosition; // then reqPosition>0
+    // if any axis is prefered for next step
+    bool  useReqAxis;
+    // the prescribed axis for the next subdivision
+    CKTBAxes::Axes reqAxis;
+
+    // -------------- AUTOMATIC TERMINATION CRITERIA ---------------------
+    // the ratio of improvement for the cost by subdivision and not-subdividing
+    // for the previous subdivision
+    float ratioLast;
+    // the ratio of improvement for the subdivision in the previous step
+    float ratioLastButOne;
+    // the number of subdivision from the root node, where the improvement
+    // in the cost failed
+    int   failedSubDivCount;
+    void Init() {
+      reqPosition = Limits::Infinity;
+      useReqAxis = false;
+      reqAxis = CKTBAxes::EE_Leaf;
+  
+      ratioLast = 1000.0;
+      ratioLastButOne = 1000.0;
+      failedSubDivCount = 0;
+    }
+  };
+
+  // initialize required and preferenced parameters before first subdivision
+  void InitReqPref(SReqPrefParams *pars);
+
+  // the array of preferred parameters used as a stack
+  SReqPrefParams *pars;
+  
+  // --------------------------------------------------------------------
+  bool verbose;
+
+  // ---------------------------------
+  // The selection of the axis
+  int _algorithmForAxisSelection;
+  
+  //  ---- termination criteria -----
+  int algorithmAutoTermination; // the algorithm for automatic termination criteria
+  int maxDepthAllowed; // maximal depth of CKTB tree
+  int maxListLength; // maximal list length of CKTB tree
+  int maxCountTrials; // maximum number of trials for automatic termination criteria
+  // the cutting off empty space in leaves
+  bool cutEmptySpace; // if to cut off empty space in leaves in postprocessing
+  int absMaxAllowedDepth; // maximal depth from the root - mut not be surpassed
+  // maximal depth allowed for cutting within the leaf .. cut off empty space
+  int maxEmptyCutDepth; // must be <0,1,2,3,4,5,6> since six planes are enough
+  // This is working variable, denoting the depth of the leaf to be created.
+  int startEmptyCutDepth;
+
+  // Biasing the empty cuts (no objects are split). The cost is multiplied
+  // by the coefficient which is assumed to be 0.8-0.9
+  float  biasFreeCuts;
+  
+  // flag if to put minimum enclosing boxes sparsely during the construction
+  bool    makeMinBoxes;
+  // if we make tight boxes if we put min box !
+  bool    makeTightMinBoxes;
+  // parameters to drive the minboxes construction
+  int     minObjectsToCreateMinBox, minDepthDistanceBetweenMinBoxes;
+  float   minSA2ratioMinBoxes;
+  // Make min box here
+  bool   makeMinBoxHere;
+
+  // two next axes are stored in oaxes for each axis
+  static const CKTBAxes::Axes oaxes[3][2];  
+
+  // for some functions it is necessary to have determined the following costs
+  float       Ct; // traversal cost - going in given direction != decision
+  float       Ci; // intersection cost - average intersection cost with object
+  
+  // ------ data to create the tree --------------------
+  int         initcnt;      // initial number of objects
+  SBBox       wBbox; // the box of the world in float values
+  Vector3   boxSize;   // the size of world bounding box in float
+  float       wholeBoxArea; // the surface area of the scene bounding box
+
+  // if to print out the tree during construction
+  bool _printCuts;
+
+  // ------------------------------------------------------
+  // A structure for a single step of subdivision
+  struct SInputData {
+    // the list of objects associated with the interior node
+    ObjectContainer *objlist;
+    
+    // the traversal bounding box of the scene (not necessarily tight)
+    SBBox box;
+    // the box, which is tight and it lies inside the traversal box
+    SBBox tightbox;
+    
+    // the number of objects in the node (= number_of_boundaries/2)
+    int count;
+
+    // ----------------------------
+    // The mode of subdivision
+    ESubdivMode modeSubDiv;
+
+    // Some prescribed parameters to be used
+    SReqPrefParams pars;
+
+    // ----------------------------------
+    // Axis to be used if prescribed
+    CKTBAxes::Axes axis;
+    // the position to be used for MakeOneCut
+    float position;
+    float position2;
+
+    // if 1 or 2 splits
+    int twoSplits;
+    // the best cost
+    float bestCost;
+
+    // if to make subdivision on the left node
+    int makeSubdivisionLeft;
+    // if to make subdivision on the right node
+    int makeSubdivisionRight;
+
+    // -----------------------------------
+    // When the min boxes was inserted as the first one
+    int lastDepthForMinBoxes;
+    // The surface area for the last minimum box inserted
+    float lastMinBoxSA2;
+    // The pointer to the last inserted minimum box
+    SKTBNodeT* lastMinBoxNode;
+  private:
+    void Init() {
+      objlist = 0;
+      box.Initialize();
+      tightbox.Initialize();
+      count = 0;
+      pars.Init();
+      makeSubdivisionLeft = makeSubdivisionRight = 1;
+      lastDepthForMinBoxes = 0;
+      lastMinBoxSA2 = INFINITY;
+      lastMinBoxNode = 0;
+    }
+    
+  public:
+
+    // -----------------------------------
+    // Implicit constructor
+    SInputData() {
+      Init();
+    }
+    ~SInputData() {
+      Free();
+    }
+
+    void Free() {
+      delete objlist;
+      objlist = 0;
+    }
+    
+    void CopyBasicData(SInputData *d) {
+      box = d->box;
+      count = 0;
+      modeSubDiv = d->modeSubDiv;
+      pars = d->pars;
+      axis = d->axis;
+      position = d->position;
+      // added 12/2007 VH
+      position2 = d->position2;
+      //
+      makeSubdivisionLeft = d->makeSubdivisionLeft;
+      makeSubdivisionRight = d->makeSubdivisionRight;      
+      // Intentionally, do not copy vectors of items
+      lastDepthForMinBoxes = d->lastDepthForMinBoxes;
+      lastMinBoxSA2 = d->lastMinBoxSA2;
+      lastMinBoxNode = d->lastMinBoxNode;
+    }
+  };
+
+  // Stack of data to be used
+  SInputData *stackID;
+  // current index
+  int         stackIndex;
+  // the maximum depth of tree
+  int         maxTreeDepth;
+  // the depth of the stack
+  int         stackDepth;
+  
+  SInputData* AllocNewData() {
+    int i = stackIndex;
+    stackIndex++;
+    assert(stackIndex < stackDepth);
+    return &(stackID[i]);
+  }
+  // Free the last data allocated
+  void FreeLastData() { stackIndex--; }
+
+  // returns a box enclosing all the objects in the node
+  void GetTightBox(const SInputData &i, SBBox &tbox);
+
+  // Compute if to subdivide and where
+  SKTBNodeT* SubDiv(SInputData *d);
+  // creates one cut inside CKTB tree
+  SKTBNodeT* MakeOneCut(SInputData *i);
+
+  SInputData* Init(ObjectContainer *objlist, const AxisAlignedBox3 &box);
+
+  // make the full leaf from current node
+  SKTBNodeT* MakeLeaf(SInputData *d);
+
+  // It goes through the splitting plane positions and computes the cost
+  void GetSplitPlaneOpt(SInputData *d, int axisToTest);
+
+  // Given the splitting plane position, it computes the cost
+  void EvaluateCost(SSplitState &state);
+  void EvaluateCostFreeCut(SSplitState &state);
+
+  // setting the evaluation for split cases that must not be done
+  float WorstEvaluation() const { return MAXFLOAT;}
+
+  // update the best value for evaluation
+  int UpdateEvaluation(float &eval, const float &newEval);
+
+public:
+  // default constructor
+  CKTBSBuildUp();
+
+  // default destructor
+  virtual ~CKTBSBuildUp();
+
+  // provide info about construction method
+  virtual void ProvideID(ostream &app);  
+  
+  // constructs the KD-tree for given objectlist and given bounding box
+  // returns NULL in case of failure, in case of success returns
+  // the pointer to the root node of constructed KD-tree.
+  virtual SKTBNodeT* BuildUp(ObjectContainer &objlist,
+			     const AxisAlignedBox3 &box,
+			     bool verbose = true);
+};
+
+}
+
+#endif // __KTBS_H__
+
