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

// ===================================================================
// $Id: $
//
// ktbai.cpp
//     The classes for building up CKTB tree by statistical optimization.
//     The bounding boxes of the all objects are sorted along all
//     axes (x,y,z) in the array-based structure by radix sort algorithm.
//
// REPLACEMENT_STRING
//
// Copyright by Vlastimil Havran, 2007 - email to "vhavran AT seznam.cz"
// Initial coding by Vlasta Havran, 2005.

// standard headers
#include <algorithm>
#include <iostream>
#include <string>

// GOLEM headers
#include "ktbai.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 testing accuracy of setting position from the ideal position
//#define _RANDOMIZE_POSITION


// This is epsilon for randomization
static
float randomEps = 0.0f; // By max 5 percent = 0.05

static void
RandomizePosition(float &value, float vmin, float vmax)
{
  assert(vmax > vmin);
  assert( ((value >= vmin) && (value <= vmax) ) );

  // copy the value
  const float origvalue = value;
  float offset = 0.f;

#define _RND_OFFSET  
#ifdef _RND_OFFSET
  const int maxCount = 10;
  int count = 0;
  do {
    if (count > maxCount) {
      value = origvalue;
      break;
    }
    offset = -(vmax - vmin) * randomEps/2.0f;
    offset += randomEps * RandomValue(vmin,vmax);    
    value = origvalue + offset;
    count++;
  }
  while (! ((value > vmin) && (value < vmax)) );
#else
  offset = (vmax - vmin) * randomEps/2.0f;
  if (RandomValue(0.f, 1.f) < 0.5f)
    offset = -offset;  
  value = origvalue + offset;
  if ( (value < vmin) || (value > vmax) )
    value = origvalue - offset;
  //cout << "offset = " << 100.0f * offset / (vmax-vmin) << "\n";
#endif

  float epsShift = 1e-5;
  if ( (value <= (vmin + (vmax-vmin) * epsShift)) ||
       (value >= (vmax - (vmax-vmin) * epsShift)) ) {
    value = origvalue;
  }

  assert( ((value >= vmin) && (value <= vmax) ) );
  
  return;
}

// ------------------------------------------------
// 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

//----------------------------------------------------------------
//---------- Implementation of CKTB tree with irregular -----------
//---------- placed splitting planes -----------------------------
//----------------------------------------------------------------

// default constructor
CKTBABuildUp::CKTBABuildUp()
{
  // verbose is set
  verbose = true;

  // Maximum depth of the tree is set and stack is allocated
  maxTreeDepth = 50;
  stackDepth = 2*maxTreeDepth;
  stackID = new GALIGN16 SInputData[stackDepth];
  assert(stackID);
  stackIndex = 0;
  return;
}

//  destructor
CKTBABuildUp::~CKTBABuildUp()
{
  delete [] stackID;
  stackID = 0;
}

void
CKTBABuildUp::ProvideID(ostream &app)
{
  bool tempvar;

  Environment::GetSingleton()->GetBoolValue("BSP.splitclip", tempvar);
  if (tempvar)
    app << "#BSP.splitClip \t\tChange of bboxes(splitclip) "
        << "dis/en\n" << tempvar << "\n";
  Environment::GetSingleton()->GetBoolValue("BSP.emptyCut", tempvar);
  app << "#BSP.emptyCut\t\tLate cutting off empty space in leaves"
      << " dis/en\n" << tempvar << "\n";

  if (tempvar) {
    app << "#BSP.absMaxAllowedDepth\tMaximum abs depth for empty cutting\n";
    app << absMaxAllowedDepth << "\n";
    app << "#BSP.maxEmptyCutDepth\tMaximum allowed depth for late cutting"
        << "(0-6)\n";
    app << maxEmptyCutDepth << "\n";
  }
  
  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;
}

// -------------------------------------------------------
// the next two axes for a given one
const CKTBAxes::Axes
CKTBABuildUp::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}};

// -------------------------------------------------------
// compare function passed to quicksort
int
CKTBABuildUp::Compare(const SItem *p, const SItem *q)
{
  register float pv = p->pos;
  register float qv = q->pos;

  if (pv < qv)
    return 1;
  else
    if (pv > qv)
      return -1;
    else
      // the coordinates are equal
      if ( (p->IsRightBoundary()) &&
           (q->IsLeftBoundary()) )
        return 1; // right_boundary < left_boundary
      else
        if ( (p->IsLeftBoundary()) &&
             (q->IsRightBoundary()) )
          return -1; // left_boundary > right_boundary

  return 0; // coordinates are equal, the same value and type
}

// --------------------------------------------
// This is sorting using quicksort
void
CKTBABuildUp::SortOneAxis(SItemVec &itemvec, int cnt, int * const stack)
{
  // the pointer to the stack
  int stackUk = 0;
  // setting initial beg and end index for the whole array
  stack[++stackUk] = 0;
  stack[++stackUk] = cnt - 1;
  SItem auxSwap;
  int e, b, i, j;
  
  while ( stackUk !=0 ) {
    // retrieving indeces from the stack
    j = e = stack[stackUk--]; // begin and end in the array
    i = b = stack[stackUk--]; // auxiliary indeces

    // selecting pivot .. in the best case median
    SItem X = itemvec[(i+j) / 2];
    do {
      while (Compare(&(itemvec[i]), &X) > 0)
        i++; // find the array[i] > array[x]=pivot
      while (Compare(&(itemvec[j]), &X) < 0)
        j--; // find the array[j] < array[x]=pivot
      if (i < j) { // swap the elements
        auxSwap = itemvec[i];
        itemvec[i] = itemvec[j];
        itemvec[j] = auxSwap;
        i++; j--;
      }
      else
        if (i == j) {
          i++;
          j--;
        }
    }
    while (i <= j);
    // Now all the elements on the left are smaller than pivot and
    // all the right are larger than pivot
#if 0
#ifdef _DEBUG
    int t;
    for (t = b; t < j; t++) {
      if (X.pos < itemvec[t].pos) {
        cout << "Problem 1 for t = " << t << endl;
        cout << "X->pos = " << X.pos << "a[t]= " << itemvec[t].pos << endl;
      }
    }
    for (t = i; t < e; t++) {
      if (X.pos > itemvec[t].pos) {
        cout << "Problem 2 for t = " << t << endl;
        cout << "X->pos = " << X.pos << "a[t]= " << itemvec[t].pos << endl;
      }
    }
#endif    
#endif    
    if (b < j) { // go to the left
      stack[++stackUk] = b;
      stack[++stackUk] = j;
    }
    if (i < e) { // go to the right
      stack[++stackUk] = i;
      stack[++stackUk] = e;
    }
    // Just a check
    assert(stackUk <= cnt);
  } // while (stackUk)

  return;
}

void
CKTBABuildUp::CopyToAuxArray(const SItemVec &bounds, SItemVecRadix &aux)
{
  // assume that both arrays are of the same size
  assert(bounds.size() <= aux.capacity());

#if 1
  // source iterator
  SItemVec::const_iterator src = bounds.begin();
  for (SItemVecRadix::iterator it = aux.begin();
       it != aux.end();
       it++, src++)
  {
    *it = *src; // copy one elem
  }
#else
  // Attempt for vectorization
  int i;
  const int cnt = aux.size();
  for (i = 0; i < cnt; i++) {
    aux[i] = bounds[i];
  }
#endif
  
  return;
}

// -----------------------------------------------------------------
// The first pass of radix sort setting the links
void
CKTBABuildUp::RadixPassHoffset11(SItemVecRadix &bounds, int bit,
                                 SRadix *rb, float offset,
                                 SItemRadix **start)
{
  // preparing the hash table
  memset(rb, 0, sizeof(SRadix) * RXBUFS30);

  SItemRadix *p;
  SItemRadix *q;
  //SRadix *r;

  // linearly process the input array
  //for (SItemVecRadix::iterator it = bounds.begin(); it != bounds.end(); it++)
  const int cnt = bounds.size();
  for (int i = 0; i < cnt; i++)
  {
    // Take the element linearly from the array
    //p = &(*it);
    p = &(bounds[i]);

    // add offset to get positive value to be sorted
    p->pos += offset;
    assert(p->pos >= 0.f);
    
    // compute the index of the bucket
    unsigned int *val = (unsigned int*)&(p->pos);
    unsigned int bucket = (( (*val) >> bit) & (RXBUFS30 - 1));
    assert( bucket < RXBUFS30 );
    // compute the address of correct bucket
    //r = rb + bucket;
    if (!rb[bucket].beg) {
      rb[bucket].end = rb[bucket].beg = p;   // inserting into empty cell, mark begin
      p->next = 0; // new item is the end of chain
    }
    else {
      // if the end of bounding box
      if (p->IsRightBoundary()) {
        // right boundary - store item at the beginning of the group
        p->next = rb[bucket].beg;
        rb[bucket].beg = p;
      }
      else {
        // left boundary - store item at the end of the group
        p->next = 0; // it is the end elem
        rb[bucket].end->next = p;   // chain last item to new item
        rb[bucket].end = p;
      }
    }
  } // for all input data

  // append groups together
  // q used as start item
  SItemRadix **fset = &q;

  for(int j = 0; j < RXBUFS30; j++) {
    // only used elements
    if (rb[j].beg) {
      *fset = rb[j].beg;             // store pointer to the next group
      fset = &(rb[j].end->next); // prepare group's end for update
    }
  } // for all buckets

  *start = q;
  return;
}

// radix sort pass starting with 'bit' over 'axis', using buckets in 'rb'
void
CKTBABuildUp::RadixPass11(SItemRadix **start, int cnt, int bit, SRadix *rb)
{
  // preparing the hash table
  memset(rb, 0, sizeof(SRadix) * RXBUFS30);

  // list grouping
  SItemRadix *p = *start;
  SItemRadix *q;
  //SRadix *r;

  // process the items using links
  for (int i = 0; i < cnt; i++) {
    assert(p);
    unsigned int *val = (unsigned int*)&(p->pos);
    unsigned int bucket = (( (*val) >> bit) & (RXBUFS30 - 1));
    assert(bucket < RXBUFS30);
    //r = rb + bucket;
    if (!rb[bucket].beg)
      rb[bucket].beg = p;   // inserting into empty cell, mark begin
    else
      rb[bucket].end->next = p;   // chain last item to new item

    rb[bucket].end = p;  // update the end of group
    q = p->next; // save the next position
    p->next = 0; // new item is the end of chain
    p = q;
  } // for all items

  // append groups together
  //r = rb;
  // q used as start item
  SItemRadix **fset = &q;

  for(int j = 0; j < RXBUFS30; j++)
    // only used elements
    if (rb[j].beg) {
      *fset = rb[j].beg;       // store pointer to the next group
      fset = &(rb[j].end->next); // prepare group's end for update
    }

  *start = q;
  return;
}

// radix sort pass starting with 'bit' over 'axis', using buckets in 'rb'
void
CKTBABuildUp::RadixPassOffset10(SItemRadix **start, int cnt, int bit,
                                SRadix *rb, float offset)
{
  // preparing the hash table
  memset(rb, 0, sizeof(SRadix) * RXBUFS30_2);

  // list grouping
  SItemRadix *p = *start;
  SItemRadix *q;
  //SRadix *r;
  
  // process the items using links
  for (int i = 0; i < cnt; i++) {
    assert(p);
    unsigned int *val = (unsigned int*)&(p->pos);
    unsigned int bucket = (( (*val) >> bit) & (RXBUFS30_2 - 1));
    assert(bucket < RXBUFS30_2);
    //r = rb + bucket;
    if (!(rb[bucket].beg)) {
      rb[bucket].beg = p;   // inserting into empty cell, mark begin
    }
    else {
      rb[bucket].end->next = p;   // chain last item to new item
    }

    rb[bucket].end = p;  // update the end of group

    // Compute the correct value back, using the offset for the
    // first pass
    p->pos -= offset;

    // go to the next item
    q = p->next; // save the next position
    p->next = 0; // new item is the end of chain
    p = q; // take the next item
  } // for all items

  // append groups together and set bidirectional links
  //r = rb;
  SItemRadix *prev = 0;
  for (int j = 0; j < RXBUFS30_2; j++) {
    if (rb[j].beg) {
      if (prev) {
        // set forward link
        prev->next = rb[j].beg; 
      }
      prev = rb[j].end;
    }
  } // for i

  // Find the first item in the list
  //r = rb;
  for (int k = 0; k < RXBUFS30_2; k++) {
    if (rb[k].beg) {
      // find the beginning of the chain
      *start = rb[k].beg;
      break;
    }
  } // for
  
  return;
}

void
CKTBABuildUp::CopyFromAuxArray(SItemRadix *sorted, SItemVec &bounds)
{
  // source iterator
  SItemRadix *src = sorted;
  //for (SItemVec::iterator it = bounds.begin(); it != bounds.end(); it++)

  int i;
  const int cnt = bounds.size();
  for (i = 0; i < cnt; i++)
  {
    bounds[i] = *src; // copy one elem, forgetting the pointer 'next' 
    src = src->next;
  }

  return;
}


// ---------------------------------------------------------------
// sort the boundaries of objects' bounding boxes along all 3 axes
void
CKTBABuildUp::SortAxes(SInputData *data)
{
  // Note that cnt is the number of boundaries = odd number
  assert(data);
  assert(data->count);
  
  CTimer timer;
  timer.Start();

  // the number of boundaries
  int cnt = 2*data->count;

  if (!_useRadixSort) {
    cout << "Sorting by quicksort " << flush;    
#if 1
    // this is the worst case allocation !!!
    int *stackQuickSort = new GALIGN16 int [cnt+1];    
    if (stackQuickSort == NULL) {
      cerr << " ktbai.cpp: quicksort allocation failed\n";
      exit(3);;
    }
    cout << " Qsort, sizeof(SItem)=" << sizeof(SItem)
         << " size(vec[0])= " << sizeof(data->xvec[0]) << endl;
    assert(cnt == data->xvec->size());
    assert(cnt == data->yvec->size());
    assert(cnt == data->zvec->size());
    // Sort all three axis by Quicksort
    //cout << "Sort x" << endl;
    SortOneAxis(*(data->xvec), data->xvec->size(), stackQuickSort);
    //cout << "Check x" << endl;
    Check1List(data->xvec, 0, data->xvec->size()/2);
    //cout << "Sort y" << endl;
    SortOneAxis(*(data->yvec), data->yvec->size(), stackQuickSort);
    //cout << "Check y" << endl;
    Check1List(data->yvec, 0, data->yvec->size()/2);
    //cout << "Sort z" << endl;
    SortOneAxis(*(data->zvec), data->zvec->size(), stackQuickSort);
    //cout << "Check z" << endl;
    Check1List(data->zvec, 0, data->zvec->size()/2);
 
    // delete the data
    delete [] stackQuickSort;
#endif
#if 0
    sort(data->xvec->begin(), data->xvec->end());
    sort(data->yvec->begin(), data->yvec->end());
    sort(data->zvec->begin(), data->zvec->end());
#endif
#if 0
    qsort(&(data->xvec[0]), data->xvec->size(), sizeof(data->xvec[0]),
          (int(*)(const void*, const void*))(&Compare));
    Check1List(data->xvec, 0, data->xvec->size()/2);
    
    qsort(&(data->yvec[0]), data->yvec->size(), sizeof(data->yvec[0]),
          (int(*)(const void*, const void*))(&Compare));
    qsort(&(data->zvec[0]), data->zvec->size(), sizeof(data->zvec[0]),
          (int(*)(const void*, const void*))(&Compare));
#endif
  }
  else {
    // -----------------------------------------------
    // Implementation by RadixSort with 3 passes, faster by 30% or so
    // than 4-passes RadixSort, see ktbi.cpp
    cout << "Sorting by radixsort3 " << flush;
    SRadix *rb = new GALIGN16 SRadix[RXBUFS30];
    assert(rb);
    // the auxiliary array for sorting
    SItemVecRadix *aux = new GALIGN16 SItemVecRadix();
    assert(aux);
    aux->resize(cnt);
    // change axis
    for(int i = 0; i < 3; i++) {      
      // Offset to get only positive values to be sorted
      float offset = -(wBbox.Min(i));
      // Get a single array of bounds
      SItemVec *bounds = data->GetItemVec(i);
      assert(bounds);
      // sort 3 times using 10 bits in one pass
      CopyToAuxArray(*bounds, *aux);
      // take the first elem and start radix sort
      SItemRadix *start = 0;
      // first pass: bits 0-10
      RadixPassHoffset11(*aux, RXBITS30 * 0, rb, offset, &start);
      // second pass: bits 11-21
      RadixPass11(&start, cnt, RXBITS30 * 1, rb);
      // third pass: bits 22-31 (bit 32 is sign)
      RadixPassOffset10(&start, cnt, RXBITS30 * 2, rb, offset);
      // Copy the sorted data back to the array, forgetting pointer 'next'
      CopyFromAuxArray(start, *bounds);
    }
    // delete aux array
    delete aux;
    delete [] rb;
    // end of radix sort
  }

  timer.Stop();
  // ------------------------------------------
  cout << " finished in " << timer.UserTime() << " [s] - user time " << endl;

#ifdef _DEBUG
  // check if the lists are correctly sorted .. in debug
  // DEBUG << "SortAxis check\n" << flush;
  Check3List(data);
#endif

  return;
}

// test if the lists are correctly sorted
void
CKTBABuildUp::Check3List(SInputData *data)
{
#ifndef AVOIDCHECKLIST
#ifdef _DEBUG
  assert(data);
  if (data->xvec->size() != (unsigned int)data->count*2) {
    cerr << "The number of X boundaries does not match" << endl;
    abort();;
  }
  if (data->yvec->size() != (unsigned int)data->count*2) {
    cerr << "The number of Y boundaries does not match" << endl;
    abort();;
  }
  if (data->zvec->size() != (unsigned int)data->count*2) {
    cerr << "The number of Z boundaries does not match" << endl;
    abort();;
  }
  Check1List(data, 0, data->count);
  Check1List(data, 1, data->count);
  Check1List(data, 2, data->count);
#endif // _DEBUG
#endif // AVOIDCHECKLIST
  return;
}

// test if one list is correctly sorted
void
CKTBABuildUp::Check1List(SInputData *data, int axis, int countExpected)
{
#ifndef AVOIDCHECKLIST
#ifdef _DEBUG
  assert((axis >= 0) && (axis < 3));  
  SItemVec *vec = data->GetItemVec(axis);
  assert(vec);
  if (countExpected)
    Check1List(vec, axis, countExpected);
  else {
    if (vec->size() > 0) {
      cerr << "The array of objects should be of zero size"<<endl;
      abort();;
    }
  }
#endif // _DEBUG
#endif // AVOIDCHECKLIST
}

// test if one list is correctly sorted
void
CKTBABuildUp::Check1List(SItemVec *vec, int axis, int countExpected)
{
#ifndef AVOIDCHECKLIST
#ifdef _DEBUG
  int cntl,cntr; // the number of left/right boundaries
  
  // DEBUG << "Check1List starting\n";
  cntl = 0; // the number of left boundaries
  cntr = 0; // the number of right boundaries
  int cntDups = 0; // the number of duplications for current position

  // the position to check
  SItemVec::iterator prev = vec->begin();
  
  // DEBUG << p->value[i] << "\n";
  if ( (*prev).IsLeftBoundary()) {
    cntl++;
    cntDups++;
  }
  else {
    cntr++;
    cntDups--;
  }
  // the next item
  SItemVec::iterator it = vec->begin();
  it++;
  // start from the second item
  int index = 1;
  bool firstDupsProb = false;
  for ( ; it != vec->end(); it++, prev++, index++)
  {
    const SItem &p = *it;

    if (p.obj->ToBeRemoved()) {
      cout << "Object to be removed, obj = "
           << p.obj << " -- it should not happen" << endl;
    }
    
    // DEBUG << p->value[i] << "\n";
    if (p.IsLeftBoundary()) {
      cntl++;
      cntDups++;
    }
    else {
      cntr++;
      cntDups--;
    }

    if (cntDups < 0) {
      cerr << "The array is sorted in wrong way, cntDups = "
            << cntDups << " index= " << index << " indexMax="
            << countExpected*2 << endl;
      if (!firstDupsProb) {
        int imin = index-3;
        if (imin < 0) imin = 0;
        int imax = index+3;
        if (imax > countExpected*2) imax = countExpected*2;
        for (int i = imin; i < imax; i++) {
          cout << "i= " << i << "  pos = " << (*vec)[i].pos;
          if ((*vec)[i].IsLeftBoundary())
            cout << "  L  ";
          else
            cout << "  R  ";
          cout << (*vec)[i].obj << endl;
        }
      }      
      firstDupsProb = true;
    }
    
    if ( (*prev).pos > p.pos) {
      cerr << "The array is for axis=" <<(int)axis<<" incorrectly sorted\n";
      cerr << "prevPos = " << (*prev).pos << " curr->pos="
            << p.pos << "\n";
      abort();;
    }
    else {
      if ( ((*prev).pos == p.pos) &&
           ( (*prev).IsLeftBoundary()) &&
           ( (*it).IsRightBoundary()) )  {
        if (countExpected > 1) {
          cerr << "The right and left boundary are incorrectly sorted, axis = "
                << axis << "\n";
          int i = 0;
          for (SItemVec::iterator itt = vec->begin(); itt != vec->end(); itt++, i++)
            {
              cout << i << " = ";
              if ((*itt).IsLeftBoundary())
                cout << "L"; else cout << "R";    
              cout << " obj = " << (*itt).obj << " pos = " << (*itt).pos << endl;
            }
        } // if more than one object
      }
    }
  } // for all items

  if (cntl != cntr) {
    cerr << "The #left boundaries <> #right boundaries\n";
    cerr << " #left boundaries= " << cntl << " #right boundaries= "
          << cntr  << "  axis= " << (int)axis << "\n";
    abort();;
  }
    
  if (countExpected > 0) {
    if (cntl != countExpected) {
      cerr << "The number of found left boundaries = " << cntl
            << " does not equal to expected count = " << countExpected << endl;
    }
    if (cntr != countExpected) {
      cerr << "The number of found right boundaries = " << cntr
            << " does not equal to expected count = " << countExpected << endl;
    }
  } // if checking of count is allowed
  
  cerr << flush;
#endif // _DEBUG  
#endif // AVOIDCHECKLIST
}

// Init required parameters
void
CKTBABuildUp::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
CKTBABuildUp::SInputData*
CKTBABuildUp::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()->GetBoolValue("BSP.emptyCut", cutEmptySpace);
  Environment::GetSingleton()->GetBoolValue("BSP.useRadixSort", _useRadixSort);
  Environment::GetSingleton()->GetIntValue("BSP.absMaxAllowedDepth",
                                           absMaxAllowedDepth);
  Environment::GetSingleton()->GetIntValue("BSP.maxEmptyCutDepth",
                                           maxEmptyCutDepth);
  Environment::GetSingleton()->GetIntValue("BSP.algAutoTermination",
                                           algorithmAutoTermination);
  Environment::GetSingleton()->GetFloatValue("BSP.biasFreeCuts",
                                             biasFreeCuts);
#ifdef _BIDIRLISTS
  // if to clip the bounding boxes
  Environment::GetSingleton()->GetBoolValue("BSP.splitclip", splitClip);
#else
  // Without bidirectional list we cannot make split clipping
  splitClip = false;
#endif // _BIDIRLISTS

  // 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);
  }

  // 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();

  // Here create the auxiliary array
  //solidArray.reserve(objlistcnt);
  // $$JB - crashed below 
  solidArray.resize(objlistcnt);

  // Here create the first entry of boundaries for all objects
  SInputData* data = AllocNewData();
  // set the box
  data->box = wBbox;
  // which algorithm to use for break-ax
#if 1
  data->algorithmBreakAx = 0; // position based
#else  
  data->algorithmBreakAx = 1; // pointer based
#endif

#ifdef _RANDOMIZE_POSITION
  data->algorithmBreakAx = 0; // position based
#endif

  resetFlagsForBreakAx = true;
  
  assert(objlistcnt > 0);
  
  // allocate the array of boundaries for all 3 axes
  data->Alloc(data->count*2);
  // set the number of objects
  data->count = objlistcnt;
  int i = 0;
  // cout << data->xvec->size()  << "  " << data->count*2 << endl;
  // create the list of bounding boxes
  for (ObjectContainer::iterator scr = objlist->begin();
       scr != objlist->end(); scr++, i++) {
    // set the object
    solidArray[i].obj = *scr;
    solidArray[i].flags = 0;
    SSolid *solid = &(solidArray[i]);
    // create bounding box of the object
    SBBox abox;
    abox.Convert((*scr)->GetBox());    
    // copy the boundaries to the arrays
    // max boundaries as first because of stable quicksort
    SItem itemX;
    itemX.pos = abox.Min(0);
    itemX.obj = solid;
    itemX.axis = 0;
    itemX.SetLeftBoundary();
    data->xvec->push_back(itemX);

    SItem itemY;
    itemY.pos = abox.Min(1);
    itemY.obj = solid;
    itemY.axis = 1;
    itemY.SetLeftBoundary();
    data->yvec->push_back(itemY);

    SItem itemZ;
    itemZ.pos = abox.Min(2);
    itemZ.obj = solid;
    itemZ.axis = 2;
    itemZ.SetLeftBoundary();
    data->zvec->push_back(itemZ);

    // max boundaries
    itemX.pos = abox.Max(0);
    itemX.SetRightBoundary();
    data->xvec->push_back(itemX);

    itemY.pos = abox.Max(1);
    itemY.SetRightBoundary();
    data->yvec->push_back(itemY);

    itemZ.pos = abox.Max(2);
    itemZ.SetRightBoundary();
    data->zvec->push_back(itemZ);
  } // for all objects

  // cout << data->xvec->size()  << "  " << data->count*2 << endl;
  
  assert(data->xvec->size() == (unsigned int)data->count*2);
  data->xvec->resize(data->count*2);
  assert(data->yvec->size() == (unsigned int)data->count*2);
  data->yvec->resize(data->count*2);
  assert(data->zvec->size() == (unsigned int)data->count*2);
  data->zvec->resize(data->count*2);
  
  // If to use radix sort or quicksort
  //_useRadixSort = true;
  // Note that quicksort does not work for some reason !! 27/12/2005
  //_useRadixSort = false;

  // sort all the bounding boxes along the axes
  if ((initcnt = objlist->size()) != 0)
    SortAxes(data);

  // The statistics init
  cntDuplicate = 0;

  // Init the parameters from the environment file
  InitReqPref(&(data->pars));

  // 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.2 + 2.0);

      // 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*
CKTBABuildUp::BuildUp(ObjectContainer &objlist,
                      const AxisAlignedBox3 &box,
                      bool verboseF)
{
  // check the number of objects
  if (objlist.size() == 0)
    return 0; // nothing
  // cerr<<"hh44"<<endl;
  // initialize the whole box of the kd-tree and sort
  // the boundary entries
  SInputData *d = Init(&objlist, box);

  //cerr<<"hh45"<<endl;

  // copy verbose to be used consistenly further on
  this->verbose = verboseF;
  
  if (verbose)
    cout << "Building up KTB tree for " << objlist.size()
                 << " objects." << endl << flush;

  // Initialization of the first value to insert the min boxes
  d->lastMinBoxSA2 = box.SurfaceArea() * 10000.f;
  d->lastDepthForMinBoxes = -20; // for root you always put the node
  d->lastMinBoxNode = 0;
  
  // -----------------------------------------------
  // 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);  

  // ----------------------------------------------------
  // Deallocate all auxiliary data
  DeleteAuxiliaryData();

  // delete the temporary data array
  solidArray.resize(0);
  
  // the pointer to the root node
  return GetRootNode();
}

int
CKTBABuildUp::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*
CKTBABuildUp::SubDiv(SInputData *d)
{ 
#ifdef _DEBUG
  Check3List(d);
#endif

#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 1
    if ( (d->count >= minObjectsToCreateMinBox) &&
         (GetDepth() - d->lastDepthForMinBoxes >= minDepthDistanceBetweenMinBoxes) )  {
        makeMinBoxHere = true;
        d->lastDepthForMinBoxes = GetDepth();
        d->lastMinBoxSA2 = d->box.SA2();
    }
#endif
#if 0
    if (d->count >= minObjectsToCreateMinBox) {
      if ( (d->lastMinBoxSA2 >= d->box.SA2() * minSA2ratioMinBoxes) &&
           (GetDepth() - d->lastDepthForMinBoxes >= minDepthDistanceBetweenMinBoxes) ) {
        makeMinBoxHere = true;
        d->lastDepthForMinBoxes = GetDepth();
        d->lastMinBoxSA2 = d->box.SA2();
      }
    }
#endif
#if 0
    if ( (d->count >= minObjectsToCreateMinBox) &&
         (GetDepth() % minDepthDistanceBetweenMinBoxes == 0) ) {
      makeMinBoxHere = true;
      d->lastDepthForMinBoxes = GetDepth();
      d->lastMinBoxSA2 = d->box.SA2();
    }
#endif
  }

  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;
  d->cntThickness = 0;

// which calls should be used, standard or optimized
#if 1
#define CallGetSplitPlaneX GetSplitPlaneOpt2
#define CallGetSplitPlaneY GetSplitPlaneOpt2
#define CallGetSplitPlaneZ GetSplitPlaneOpt2
#endif
#if 0
// This does not work for unknown reason... VH 2006/01/08
#define CallGetSplitPlaneX GetSplitPlaneOpt3
#define CallGetSplitPlaneY GetSplitPlaneOpt3
#define CallGetSplitPlaneZ GetSplitPlaneOpt3
#endif
#if 0
#define CallGetSplitPlaneX GetSplitPlaneOptUnroll4
#define CallGetSplitPlaneY GetSplitPlaneOptUnroll4
#define CallGetSplitPlaneZ GetSplitPlaneOptUnroll4
#endif
  
  // 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
        CallGetSplitPlaneX(d->xvec, 0); // evaluate
        break;
      }
      case CKTBAxes::EE_Y_axis : {
        state.InitYaxis(d->count, d->box); // init
        CallGetSplitPlaneY(d->yvec, 1); // evaluate
        break;
      }
      case CKTBAxes::EE_Z_axis : {
        state.InitZaxis(d->count, d->box); // init
        CallGetSplitPlaneZ(d->zvec, 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.bestIterator)).pos;
      d->bestIterator = state.bestIterator;
      d->twoSplits = state.bestTwoSplits; // one or two splitting planes planed
      d->cntThickness = state.bestThickness;
      if (d->twoSplits > 1) {
        SItemVec::iterator it = state.bestIterator; it++;      
        if (it != d->GetItemVec(axis)->end())
          d->position2 = (*it).pos;
        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
          CallGetSplitPlaneX(d->xvec, 0); // evaluate
          break;
        }
        case CKTBAxes::EE_Y_axis : {
          state.InitYaxis(d->count, d->box); // init
          CallGetSplitPlaneY(d->yvec, 1); // evaluate
          break;
        }
        case CKTBAxes::EE_Z_axis : {
          state.InitZaxis(d->count, d->box); // init
          CallGetSplitPlaneZ(d->zvec, 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.bestIterator)).pos;
        d->bestIterator = state.bestIterator;
        d->twoSplits = state.bestTwoSplits; // one or two splitting planes planed
        d->cntThickness = state.bestThickness;
        if (d->twoSplits > 1) {
          SItemVec::iterator it = state.bestIterator; it++;      
          if (it != d->GetItemVec(axis)->end())
            d->position2 = (*it).pos;
          else
            d->position2 = state.box.Max(axis);
        }
      }
    }
    else {
      // no axis is required, we can pickup any we like. We test all three axis
      // and we pickup the minimum cost for all three axes.
      state.InitXaxis(d->count, d->box); // init for x axis
      CallGetSplitPlaneX(d->xvec, 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.bestIterator)).pos;
        d->bestIterator = state.bestIterator;
        d->twoSplits = state.bestTwoSplits; // one or two splitting planes planed
        d->cntThickness = state.bestThickness;
        if (d->twoSplits > 1) {
          SItemVec::iterator it = state.bestIterator;
          it++;
          if (it != d->xvec->end())
            d->position2 = (*it).pos;
          else
            d->position2 = state.box.Max().x;
        }
      }
      // -----------------------
      // now test for y axis
      state.ReinitYaxis(d->count, d->box); // init for x axis
      //state.InitYaxis(d->count, d->box); // init for x axis
      CallGetSplitPlaneY(d->yvec, 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->position = (*(state.bestIterator)).pos;
        d->bestIterator = state.bestIterator;
        d->twoSplits = state.bestTwoSplits; // one or two splitting planes planed
        d->cntThickness = state.bestThickness;
        if (d->twoSplits > 1) {
          SItemVec::iterator it = state.bestIterator;
          it++;
          if (it != d->yvec->end())
            d->position2 = (*it).pos;
          else
            d->position2 = state.box.Max().y;
        }
      }
      // -----------------------
      // now test for z axis
      state.ReinitZaxis(d->count, d->box); // init for x axis
      //state.InitZaxis(d->count, d->box); // init for x axis
      CallGetSplitPlaneZ(d->zvec, 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->position = (*(state.bestIterator)).pos;
        d->bestIterator = state.bestIterator;
        d->twoSplits = state.bestTwoSplits; // one or two splitting planes planed
        d->cntThickness = state.bestThickness;
        if (d->twoSplits > 1) {
          SItemVec::iterator it = state.bestIterator;
          it++;
          if (it != d->zvec->end())
            d->position2 = (*it).pos;
          else
            d->position2 = state.box.Max().z;
        }
      }
      // 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->algorithmBreakAx = 0;
      resetFlagsForBreakAx = true;
      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 *
CKTBABuildUp::MakeOneCut(SInputData *d)
{
// for testing accuracy of setting position from the ideal position
#ifdef _RANDOMIZE_POSITION
  assert(d->algorithmBreakAx == 0); // position based
  RandomizePosition(d->position, d->box.Min(d->axis), d->box.Max(d->axis));
#endif //_RANDOMIZE_POSITION

#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
  
#ifdef _DEBUG
  // We do not know the number of boundaries
  Check3List(d);
#endif
  
//#define _VYPIS
#ifdef _VYPIS
  DEBUG << "START: Subdivision at pos="
        << position << " axis = " << axis << "\n";
  PrintOut(axis, 1, list+axis, sec+axis, Limits::Infinity);
#endif

  // axis .. the axis perpendicular to splitting plane positioned at position
  // break active axis = split the list into two pieces
  // list[axis] .. start of the first list in axis direction .. input
  // second[axis] .. start of the second list in axis direction .. output
  // objlist .. the list of objects that are duplicated
#ifdef _DEBUG
  Check1List(d, d->axis, d->count);
  //cout << "Axis = " << axis << endl;
#endif

  // We have to allocate a new node for right child
  SInputData* rightData = AllocNewData(2*d->count);
  // Copy the basic data from parent
  rightData->CopyBasicData(d);

  // The number of objects for left children and right children
  int cntL, cntR;

//#ifdef _DEBUG
#if 1
  if (d->cntThickness < 0) {
    cerr << "Problem, d->cntThickness = " << d->cntThickness << endl;
    abort();;
  }
#endif

  // Correct for cutting off empty space and many left
  // boundaries on the splitting plane.
  SItemVec *vec = d->GetItemVec(d->axis);

  if (d->algorithmBreakAx) {
    // Use pointer-based break-ax algorithm
    if (d->bestIterator == vec->begin()) {
      assert(d->cntThickness == 0);
      d->bestIterator--;
    }
    else {
      if (d->count > 1) {
        int origThickness = d->cntThickness;
        SItemVec::iterator origIterator = d->bestIterator;
        SItemVec::iterator it = d->bestIterator;
        int caseBreak = 0;
        if ((*(it)).IsLeftBoundary()) {
          float pos = d->position;
          it--;
          int ir = 1;
          assert(it != vec->begin()-1);
          for ( ; true; ) {
            if ( (*(it)).pos < pos) {
              caseBreak = 0;
              d->bestIterator = it;
              if (ir > 1)
                d->cntThickness -= ir;
              break;
            }
            if (it == vec->begin()) {
              // cutting off, #objects on the left = 0
              d->cntThickness = 0;
              it--;
              d->bestIterator = it;
              caseBreak = 1;
              break;
            }
            if ((*(it)).IsRightBoundary()) {
              d->bestIterator = it;
              if (ir > 1)
                d->cntThickness -= ir;
              caseBreak = 2;
              break;
            }
            it--;
            ir++;
          } // for
#ifdef _DEBUG
          if (d->cntThickness < 0) {
            cerr << "Problem, d->cntThickness = " << d->cntThickness << endl;
            cerr << " origThickness = " << origThickness << " Depth = "
                  << GetDepth() << endl;
            for (SItemVec::iterator iq = it; iq != origIterator; iq++) {
              cout << "  pos = " << (*iq).pos;
              if ((*iq).IsLeftBoundary())
                cout << "  L  ";
              else
                cout << "  R  ";
              cout << (*iq).obj << endl;          
            } // for
            cout << "AAAA" << endl;
            abort();;
        }
#endif
        } // if left boundary
      } // if d->count > 1
    } // if d->vec is not the beginning of the list
  } // pointer based break-ax algorithm
    
#if 0
  DEBUG << "MakeOneCut cntL = " << cntL << " cntR = " << cntR << endl;
#endif

  // Here is the breaking the arrays into two arrays, compute the number
  // of objects on the left and on the right of the splitting plane
  int dCountTagging = 0;
  // Use either 
  if (d->algorithmBreakAx == 0)
    BreakAxPosition(d, d->axis, rightData, cntL, cntR);
  else
    BreakAx(d, d->axis, rightData, cntL, cntR);
    
#ifdef _DEBUG
  // the number of objects on the left and on the right
  int cntL_B = cntL;
  int cntR_B = cntR;
#if 0
  cout << "B - Count*2 = " << d->count*2
       << " countSum = " << cntL_B + cntR_B
       << " CntL_B = " << cntL_B
       << " cntR_B = " << cntR_B << endl;
#endif
  //Check1List(alist, axis, cntL_B, true, position);
  Check1List(d, d->axis, cntL_B);
  Check1List(rightData, d->axis, cntR_B);
  // cout << "AxisNext = " << oax0 << endl;
#endif
  
  // DEBUG << "THE CHECK at DEPTH=" << GetDepth() << endl;
  
#ifdef _VYPIS
  DEBUG << "After Break at pos="
        << position << " axis = " << axis << "\n";
  PrintOut(d, axis, d->position);
#endif

#ifdef _VYPIS
  DEBUG << "BEFORE oax0\n";
  PrintOut(d, oax0, Limits::Infinity);
#endif

  // --------------------------------------------------
  // break other two axes
  CKTBAxes::Axes oax0 = oaxes[d->axis][0];
#ifdef _DEBUG
  Check1List(d, oax0, d->count);
#endif
  
  SItemVec *vecc = d->GetItemVec(oax0);
  if (vecc->size() != 2 * d->count) {
    cout << "AAAA 1" << endl;
  }
  
  // Here break the list in the next axis
#ifdef _USE_OPTIMIZE_DIVIDE_AX
  DivideAx_I_opt(d, oax0, rightData, cntL, cntR);
#else
  DivideAx_I(d, oax0, rightData, cntL, cntR);
#endif

  if (vecc->size() != 2 * cntL) {
    cout << "AAAA 3" << endl;
  }
  if (rightData->GetItemVec(oax0)->size() != 2 * cntR) {
    cout << "AAAA 4" << endl;
    cout << "cntR * 2 = " << 2 * cntR << endl;
    cout << "vecSize = " << rightData->GetItemVec(oax0)->size() << endl;
  }
  
#ifdef _DEBUG
  // the number of objects on the left and on the right
  int cntL_I = cntL;
  int cntR_I = cntR;
#if 0
  cout << "I - Count*2 = " << count*2
       << " countSum = " << cntL_I + cntR_I
       << " CntL_B = " << cntL_I
       << " cntR_B = " << cntR_I << endl;
#endif
  
  if (cntL_I != cntL_B) {
    cerr << "FirstList - Problem with DivideAx_I, cntL_B = " << cntL_B
          << " and cntL_I = " << cntL_I << endl;
  }
  if (cntR_I != cntR_B) {
    cerr << "SecondList = Problem with DivideAx_I, cntR_B = " << cntR_B
          << " and cntR_I = " << cntR_I << endl;
  }
  
  Check1List(d, oax0, cntL_B);
  Check1List(rightData, oax0, cntR_B);

  //cout << "AxisNextNext = " << oax1 << endl;

#endif
  
#ifdef _VYPIS
  DEBUG << "AFTER DivideAx oax0\n";
  PrintOut(oax0, 3, alist + oax0, sec+oax0, 0);
#endif

#ifdef _VYPIS
  DEBUG << "BEFORE oax1\n";
  PrintOut(oax1, 1, alist + oax1, sec+oax1, Limits::Infinity);
#endif

  // Her break the list in the next next axis
  CKTBAxes::Axes oax1 = oaxes[d->axis][1];

  SItemVec *vec2 = d->GetItemVec(oax1);
  if (vec2->size() != 2 * d->count) {
    cout << "BBBB 1" << endl;
  }
  
#ifdef _USE_OPTIMIZE_DIVIDE_AX
  DivideAx_II_opt(d, oax1, rightData, cntL, cntR);
#else
  DivideAx_II(d, oax1, rightData, cntL, cntR);
#endif

  if (vec2->size() != 2 * cntL) {
    cout << "BBBB 3" << endl;
    cout << "cntL * 2 = " << 2 * cntL << endl;
    cout << "vec2size = " << vec2->size() << endl;
  }
  if (rightData->GetItemVec(oax1)->size() != 2 * cntR) {
    cout << "BBBB 4" << endl;
    cout << "cntR * 2 = " << 2 * cntR << endl;
    cout << "vecSize = " << rightData->GetItemVec(oax1)->size() << endl;
  }
  
#ifdef _DEBUG
  int cntL_II = cntL;
  int cntR_II = cntR;
#if 0
  cout << "II - Count*2 = " << count*2
       << " countSum = " << cntL_II + cntR_II
       << " CntL_B = " << cntL_II
       << " cntR_B = " << cntR_II << endl;
#endif
  if (cntL_II != cntL_B) {
    cerr << "FirstList - Problem with DivideAx_II, cntL_B = " << cntL_B
          << " and cntL_I = " << cntL_I 
          << " and cntL_II = " << cntL_II << endl; 
  }
  if (cntR_II != cntR_B) {
    cerr << "Second List - Problem with DivideAx_II, cntR_B = " << cntR_B
          << " and cntR_I = " << cntR_I
          << " and cntR_II = " << cntR_II << endl;
  }
  
  Check1List(d, oax1, cntL_B);
  Check1List(rightData, oax1, cntR_B);

  if ( (cntL_I != cntL_II) ||
       (cntR_I != cntR_II) ) {
    cout << "Problem with dividing axis II: cntL_I = " << cntL_I
         << " cntL_II = " << cntL_II
         << " cntR_I = " << cntR_I
         << " cntR_II = " << cntR_II << endl;      
  }
#endif
  
#ifdef _VYPIS
  DEBUG << "AFTER DivideAx oax1\n";
  PrintOut(oax1, 3, alist + oax1, sec+oax1, position);
#endif
  
  // 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);

#if 0  
  // If we make split clipping, then we want to reduce the boxes for
  // the objects straddling the splitting plane now.
  if ((splitClip) && (objlist.ListCount() > 0 )) {
    // if the objects are split by splitting plane
    //DEBUG << "Reduces box ax=" << axis << " position="
    // << position << "#split_objs=" << objlist.ListCount() << "\n" << flush;
    ReduceBBoxes(d, d->axis, rightData, d->position);
  }
#endif
  
  // ---------------------------------------------
  // 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);

  }

  // 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
CKTBABuildUp::GetTightBox(const SInputData &i, SBBox &tbox)
{
  // Index to the last boundary
  int cnt = 2*i.count-1;  

  tbox.MinX() = (*(i.xvec))[0].pos; 
  tbox.MinY() = (*(i.yvec))[0].pos; 
  tbox.MinZ() = (*(i.zvec))[0].pos; 
  tbox.MaxX() = (*(i.xvec))[cnt].pos; 
  tbox.MaxY() = (*(i.yvec))[cnt].pos; 
  tbox.MaxZ() = (*(i.zvec))[cnt].pos;

  assert(tbox.MinX() >= i.box.MinX());
  assert(tbox.MaxX() <= i.box.MaxX());
  assert(tbox.MinY() >= i.box.MinY());
  assert(tbox.MaxY() <= i.box.MaxY());
  assert(tbox.MinZ() >= i.box.MinZ());
  assert(tbox.MaxZ() <= i.box.MaxZ());
}


// returns a box enclosing all the objects in the node
int
CKTBABuildUp::GetEBox(const SInputData &i, SBBox &tbox)
{
  // LEFT BOX
  // initialization

  // Index to the last boundary
  int cnt = 2*i.count-1;  
  int changed = 0; // number of planes, that are changed by bounding box

  float xMin = (*(i.xvec))[0].pos;
  changed = (xMin > i.box.Min(0)) ? changed+1 : changed;    
  float xMax = (*(i.xvec))[cnt].pos;
  changed = (xMax < i.box.Max(0)) ? changed+1 : changed;    

  float yMin = (*(i.yvec))[0].pos;
  changed = (yMin > i.box.Min(1)) ? changed+1 : changed;    
  float yMax = (*(i.yvec))[cnt].pos;
  changed = (yMax < i.box.Max(1)) ? changed+1 : changed;    

  float zMin = (*(i.zvec))[0].pos;
  changed = (zMin > i.box.Min(2)) ? changed+1 : changed;    
  float zMax = (*(i.zvec))[cnt].pos;
  changed = (zMax < i.box.Max(2)) ? changed+1 : changed;    

  // Set the bounding (reduced) box
  tbox.Min() = Vector3(xMin, yMin, zMin);
  tbox.Max() = Vector3(xMax, yMax, zMax);

  // Return the number of bounding splitting planes with respect to original box
  return changed;
}


// This is only for debugging purposes, to be removed!
static
CKTBABuildUp::SSolid* objtf = (CKTBABuildUp::SSolid*)0x0;

// removes the objects specified in "tagobjlist" from boundary "list"
// it supposes the "tagobjlist" is ordered in the left boundary order
// in CKTBAxes::EE_X_axis.
void
CKTBABuildUp::RemoveObjects(SItemVec *vec, int cntObjects)
{
  assert(vec);
  assert(cntObjects);
  
  // number of removed left boundaries
  int cnt = 0;

  SItemVec::iterator curr;
#ifdef _DEBUG
  int cnt2 = 0;
  for (curr = vec->begin(); curr != vec->end(); curr++)
  {
    if (curr->obj->ToBeRemoved()) {
      //cout << cnt2 << "  " << curr - vec->begin() << endl;
      cnt2++;
    }
    //if (curr->obj == objtf)
    //  cout << "Index found cnt2 = " << cnt2 << endl;
  } // for
  if (cnt2 != 2*cntObjects) {
    cerr << "Some problem with data marking" << endl;
    abort();;
  }
#endif

  // Find the first boundary
  for (curr = vec->begin(); curr != vec->end(); curr++)
  {
    //if (curr->obj == objtf)
    //cout << "2 Index found cnt2 = " << cnt2 << endl;

    if (curr->obj->ToBeRemoved()) {
      cnt++; // the number of removed boundaries
      assert(curr->IsLeftBoundary());
      //cout << cnt << "  objToRemove=" << curr->obj << endl;
      break;
    }
  }
  // the first element to be overwritten
  SItemVec::iterator it = curr;
  curr++; 

  for ( ; curr != vec->end(); curr++) {
    //if (curr->obj == objtf) {
    //  cout << "3 Index found cnt2 = " << cnt2 << endl;

    if (curr->obj->ToBeRemoved()) {
      cnt++; // the number of removed boundaries
      //cout << cnt << "  " << curr->obj << endl;
      if (cnt == cntObjects*2) {
        curr++;
        break;
      } 
    }
    else {
      // copy the element to the right place
      *it = *curr;
      it++;
    }
  } // for

#ifdef _DEBUG
  if (cnt != 2*cntObjects) {
    cerr << "Problem with removing objects implementation" << endl;
    abort();;
  }
#endif

  // copy the rest of the list to the right place
  for ( ; curr != vec->end(); curr++, it++)
  {
    //if (curr->obj == objtf) {
    //  cout << "4 Index found cnt2 = " << cnt2 << endl;

    // copy the element to the right place
    *it = *curr;
  }
      
  // resize the vector correctly
  int oldSize = vec->size();
  assert(oldSize >= cnt);
  if (oldSize == cnt)
    vec->erase(vec->begin(), vec->end());
  else
    vec->resize(oldSize - cnt);
  assert(vec->size() % 2 == 0);
  assert(vec->size() == oldSize - cnt);

#ifdef _DEBUG    
  for (curr = vec->begin(); curr != vec->end(); curr++)
  {
    if (curr->obj->ToBeRemoved()) {
      cerr << "Implementation bug" << endl;
      abort();;
    } // if left boundary
  } // for
#endif

  //cout << "size = " << vec->size() << endl;

  return;
}

// removes the objects specified in "tagobjlist" from boundary "list"
// it supposes the "tagobjlist" is ordered in the left boundary order
// in CKTBAxes::EE_X_axis.
void
CKTBABuildUp::RemoveObjectsReset(SItemVec *vec, int cntObjects)
{
  assert(vec);
  assert(cntObjects);
  
  // number of removed left boundaries
  int cnt = 0;
  SItemVec::iterator curr;

#ifdef _DEBUG
  int cnt2 = 0;
  for (curr = vec->begin(); curr != vec->end(); curr++)
  {
    if (curr->obj->ToBeRemoved()) {
      //cout << cnt2 << "  " << curr - vec->begin() << endl;
      cnt2++;
    }
  } // for
  if (cnt2 != 2*cntObjects) {
    cerr << "Some problem with data marking" << endl;
    abort();;
  }
#endif

  // Find the first boundary
  for (curr = vec->begin(); curr != vec->end(); curr++)
  {
    if (curr->obj->ToBeRemoved()) {
      cnt++; // the number of removed boundaries
      assert(curr->IsLeftBoundary());
      curr++;
      break;
    }
  }
  SItemVec::iterator it = curr;
  it--; // the first element to be overwritten

  for ( ; curr != vec->end(); curr++) {
    if (curr->obj->ToBeRemoved()) {
      cnt++; // the number of removed boundaries
      if (curr->IsRightBoundary()) {
        curr->obj->ResetFlags();
        assert(curr->obj->flags == 0);
      }
      if (cnt == cntObjects*2) {
        curr++;
        break;
      } 
    }
    else {
      // copy the element to the right place
      *it = *curr;
      it++;
    }
  } // for
  
#ifdef _DEBUG
  if (cnt != 2*cntObjects) {
    cerr << "Problem with removing objects implementation" << endl;
    abort();;
  }
#endif

  // copy the rest of the list to the right place
  for ( ; curr != vec->end(); curr++, it++)
  {
    // copy the element to the right place
    *it = *curr;
  }
      
  // resize the vector correctly
  int oldSize = vec->size();
  assert(oldSize >= cnt);
  if (oldSize == cnt)
    vec->erase(vec->begin(), vec->end());
  else
    vec->resize(oldSize - cnt);
  assert(vec->size() % 2 == 0);
  assert(vec->size() == oldSize - cnt);

#ifdef _DEBUG    
  for (curr = vec->begin(); curr != vec->end(); curr++)
  {
    if (curr->obj->ToBeRemoved()) {
      cerr << "Implementation bug" << endl;
      abort();;
    } // if left boundary
  } // for
#endif

  //cout << "size = " << vec->size() << endl;
  
  return;
}


// make the full leaf from current node
SKTBNodeT*
CKTBABuildUp::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 = NULL;
    objlist = new ObjectContainer;

    SItemVec *vec = d->GetItemVec(0);
    SItemVec::iterator curr;    
    for (curr = vec->begin(); curr != vec->end(); curr++) {
      if ( (*curr).IsLeftBoundary()) // skip all right boundaries
        objlist->push_back( (*curr).obj->obj);
    }

    assert(objlist->size() != 0);
    // Do not delete the object list, it is used as allocated above
    SetFullLeaf(node, objlist);
  }

  // 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;
}

// split the list along the required axis
// first is the begin of the SItem list, axis is the axis, where
// to split, val is the splitting value in integer notation.
// The output of the function are two lists, first and second
// Optionally, objlist store the objects that straddle the splitting plane
void
CKTBABuildUp::BreakAx(SInputData *d, int axis,
                      SInputData *rightData,
                      int &cntLout, int &cntRout)
{
  // the number of object boundaries for left array
  int cntLeft = (d->bestIterator) - d->GetItemVec(axis)->begin() + 1;
  assert(cntLeft >= 0);
  assert(cntLeft <= 2*d->count);  
  // the number of object boundaries for right array
  int cntRight = d->count*2 - cntLeft;
  assert(cntRight >= 0);
  assert(cntRight <= 2*d->count);
  assert(d->cntThickness >= 0);
  cntLeft += d->cntThickness; // duplicated boundaries for left array
  cntRight += d->cntThickness; // duplicated boundaries for right array
  assert(cntLeft + cntRight == (d->count + d->cntThickness) * 2);

#ifdef _DEBUG
  if ( (cntLeft % 2 != 0) ||
       (cntRight % 2 != 0) ) {
    int i = 0;
    SItemVec *vec = d->GetItemVec(axis);
    cout << "d->count = " << d->count ;
    cout << "cntLeft = " << cntLeft << " cntRight" << cntRight << endl;
    cout << "bestIterator = " << (int)(d->bestIterator - vec->begin());
    cout << " cntThickness= " << d->cntThickness << " position=" << d->position << endl;
    for (SItemVec::iterator itt = vec->begin(); itt != vec->end(); itt++, i++)
    {
      cout << i << " = ";
      if ((*itt).IsLeftBoundary())
        cout << "L"; else cout << "R";
      cout << " obj = " << (*itt).obj << " pos = " << (*itt).pos << endl;
    }
    abort();;
  }
#endif
  
  // A special case, right list is empty
  if (cntRight == 0) {
    assert(d->cntThickness == 0);
    rightData->Alloc(0);
    rightData->count = 0;
    // The final setting of number of objects
    cntLout = cntLeft / 2;
    cntRout = cntRight / 2;   
    return; // nothing to do
  }

  // We have some problem that is difficult to detect, when tagging objects
  // are allowed, the flags are not always reset to zero in some cases. 
  // VH 2/1/2006. The flags are always left boundaries (=1).
  // Therefore here it is a hack to assure that the flags are definitely zero.
  if (resetFlagsForBreakAx) {
    SItemVec *vecd = d->GetItemVec(axis);
    for (SItemVec::iterator itt = vecd->begin(); itt != vecd->end(); itt++) {
      (*itt).obj->ResetFlags();
    } // for 
  } // if
  
#ifdef _DEBUG
  SItemVec *vecd = d->GetItemVec(axis);
  for (SItemVec::iterator itt = vecd->begin(); itt != vecd->end(); itt++) {
    if ((*itt).obj->Flags() != 0) {
      cout << "Init ERROR 1 in original list, flags = "
           << (*itt).obj->Flags() << " obj = " << (void*)((*itt).obj) << endl;
    }
  } // for
#endif // _DEBUG  

  assert(cntRight > 0);
  
  // Allocate the appropriate size for right array, for all 3 axes
  rightData->Alloc(cntRight);
  
  // First find create all left boundaries for right array
  SItem boundary; // create left boundary
  SItemVec *vecRight = rightData->GetItemVec(axis);

#define _USE_PUSHBACK
#ifdef _USE_PUSHBACK
  vecRight->resize(0);
#else
  vecRight->resize(cntRight);
#endif  
  if (d->cntThickness) {
    boundary.pos = d->position;
    boundary.axis = axis;
    boundary.SetLeftBoundary();
#ifdef _USE_PUSHBACK
    int i;
    for (i = 0; i < d->cntThickness; i++) {
      vecRight->push_back(boundary); // Note that the objects pointed to are not set !!!
    } // for i;
#else
    for (int i = 0; i < d->cntThickness; i++) {
      (*vecRight)[i] = boundary; // Note that the objects pointed to are not set !!!
    } // for i;
#endif
  }
  const SItemVec::iterator itendLeft = d->bestIterator + 1;
  SItemVec *vec = d->GetItemVec(axis);
  SItemVec::iterator it;
  //int cntSetLeft = 0;
  for (it = vec->begin(); it != itendLeft; it++) {
    // Mark all the items to be in the first list
    (*it).obj->SetInFirstList();
    //cntSetLeft++;
  } // for
  // The boundaries that belong definitely to the second list
  //int cntSetRight = 0;
#ifdef _USE_PUSHBACK
  const SItemVec::iterator itend = vec->end();
  for (; it != itend; it++) {
    // Mark all the items to be in the second list
    (*it).obj->SetInSecondList();
    // and copy the item to the right array
    vecRight->push_back(*it);
    //cntSetRight++;
  } // for
#else
  const int imax = vec->end() - (d->bestIterator + 1);
  int ir = d->cntThickness;
  for (int i = 0; i < imax; i++) {
    (*it).obj->SetInSecondList();
    // and copy the item to the right array
    (*vecRight)[ir] = (*it);
    ir++; it++;
  }
  assert(ir == cntRight);
#endif
  
#ifdef _DEBUG
  if (vecRight->size() != cntRight) {
    cerr << "Implementation problem vecRight->size() = "
          << vecRight->size() << "  cntRight = " << cntRight
          << " d->cntThickness = " << d->cntThickness << endl;
    abort();;
  }
#endif  
  // Only check, if right boundary is set correctly
  assert(vecRight->size() == cntRight);
  
  if (d->cntThickness) {
    // Now go only through the items on the left side of the splitting plane
    SItemVec::iterator itR = vecRight->begin(); // to set left boundaries in right array
    SItemVec::iterator itL = itendLeft; // to set new right boundaries in left array
    boundary.SetRightBoundary();
#ifdef _USE_PUSHBACK
    for (it = vec->begin(); it != itendLeft; it++) {
      if ((*it).obj->InBothLists()) {
        // set new right boundaries in left array
        (*itL) = boundary;
        (*itL).obj = (*it).obj; // set the right object
        itL++;
        // and set new left boundary in right array, only object is set
        (*itR).obj = (*it).obj;
        itR++;
      }
    } // for
#else
    int imax = itendLeft - vec->begin();
    int i;
    for (i = 0; i < imax ; i++) {
      if ((*vec)[i].obj->InBothLists()) {
        // set new right boundaries in left array
        (*itL) = boundary;
        (*itL).obj = (*vec)[i].obj; // set the right object
        itL++;
        // and set new left boundary in right array, only object is set
        (*itR).obj = (*vec)[i].obj;
        itR++;
      }
    } // for
#endif
#ifdef _DEBUG
    int sizeL = itL - vec->begin();
    assert(sizeL == cntLeft);
    int sizeR = itR - vecRight->begin();
    assert(sizeR == d->cntThickness);
#endif
  }
#ifdef _DEBUG
  else {
    vec->resize(cntLeft);    
    assert(d->cntThickness == 0);
    int cntLL = 0;
    int cntRR = 0;
    for (it = vec->begin(); it != vec->end(); it++) {
      if ((*it).obj->InBothLists()) {
        cntLL++;
        cout << "ERROR in left list " << cntLL << endl;
      }
    } // for    
    for (it = vecRight->begin(); it != vecRight->end(); it++) {
      if ((*it).obj->InBothLists()) {
        cntRR++;
        cout << "ERROR in right list " << cntRR << endl;
      }
    } // for    
  }  
#endif // _DEBUG  
  
  // The boundary on the left, unused items are not used
  vec->resize(cntLeft);

  // The final setting of size
  cntLout = cntLeft / 2;
  cntRout = cntRight / 2;

  return;
}


void
CKTBABuildUp::BreakAxPosition(SInputData *d, int axis,
                              SInputData *rightData,
                              int &cntLout, int &cntRout)
{
  // the number of object boundaries for left array is not known
  int cntLeft = 0;
  int cntRight = 0;
  d->cntThickness = 0;
  const float positionToDecide = d->position;

  // We have some problem that is difficult to detect, when tagging objects
  // are allowed, the flags are not always reset to zero in some cases. 
  // VH 2/1/2006. The flags are always left boundaries (=1).
  // Therefore here it is a hack to assure that the flags are definitely zero.
  if (resetFlagsForBreakAx) {
    SItemVec *vecd = d->GetItemVec(axis);
    for (SItemVec::iterator itt = vecd->begin(); itt != vecd->end(); itt++) {
      (*itt).obj->ResetFlags();
    } // for 
  } // if
  
#ifdef _DEBUG
  SItemVec *vecd = d->GetItemVec(axis);
  for (SItemVec::iterator itt = vecd->begin(); itt != vecd->end(); itt++) {
    if ((*itt).obj->Flags() != 0) {
      cout << "Init ERROR 2 in original list, flags = "
           << (*itt).obj->Flags() << " obj = " << (void*)((*itt).obj) << endl;
    }
  } // for
#endif // _DEBUG  

  // Allocate the appropriate size for right array, for all 3 axes
  // This is only estimated at this algorithm.
  int estimatedSize = (int)(0.6f * (float)d->count);
  if (estimatedSize < 10)
    estimatedSize = 10;
  rightData->Alloc(estimatedSize * 2);
  
  // First find create all left boundaries for right array
  SItemVec *vecRight = rightData->GetItemVec(axis);

  // Resize to 0 to start from the beginning
  vecRight->resize(0);
  SItemVec *vec = d->GetItemVec(axis);
  SItemVec::iterator it;
  const SItemVec::iterator itEnd = vec->end();
  for (it = vec->begin(); it != itEnd; it++) {
    const float &pos = (*it).pos;  
    if (pos < positionToDecide) {
      cntLeft++;
      // Mark the item to be in the first list
      (*it).obj->SetInFirstList();
    }
    else {
      if (pos > positionToDecide) {
        // It belongs to the right list
        break;
      }
      else {
        // pos == positionToDecide
        if ((*it).IsRightBoundary()) {
          // it belongs to the left list
          cntLeft++;
          (*it).obj->SetInFirstList();
        }
        else {
          // this is the left boundary
          // it belongs already to the right list
          break;
        }
      }
    }
  } // for it

  // Remember the position of the iterator, where right
  // list has to start
  SItemVec::iterator itStart = it;
  int cntDups = 0;
  SItem boundary;

  // create left boundaries for right list
  boundary.pos = d->position;
  boundary.axis = axis;
  boundary.SetLeftBoundary();
  for (; it != itEnd; it++) {
    assert((*it).pos >= positionToDecide);
    // Mark all the items to be in the second list
    if ((*it).obj->InFirstList()) {
      // and copy the newly created boundary to the right array
      boundary.obj = (*it).obj;
      vecRight->push_back(boundary);
      cntDups++;
      cntRight++;
    }
  } // for

  // Now go through the right part of the list again
  // and copy the rest of the list to the right list
  for (it = itStart; it != itEnd; it++) {
    (*it).obj->SetInSecondList();
    assert((*it).pos >= positionToDecide);
    vecRight->push_back((*it));
    cntRight++;
  } // for

  // in final pass create the new boundaries for the left list
  int i;
  SItemVec::iterator srcIt = vecRight->begin();
  for (it = itStart, i = 0 ; i < cntDups; it++, srcIt++, i++) {
    cntLeft++;
    // first copy the boundary from the beginning of the right list
    *it = *srcIt;
    // then set the right boundary in copied item
    (*it).SetRightBoundary();
    assert( (*srcIt).IsLeftBoundary());
  } // for it
  
  // The boundary on the left, unused items are not used
  vec->resize(cntLeft);
  assert(cntLeft % 2 == 0);
  assert(cntRight % 2 == 0);
  assert(cntLeft+cntRight == 2*(d->count+cntDups));
  
  // The final setting of size
  cntLout = cntLeft / 2;
  cntRout = cntRight / 2;
  // Set correctly also the number of objects
  d->cntThickness = cntDups;

#ifdef _DEBUG
  SItemVec *vecdL = d->GetItemVec(axis);
  int cntL = 0; int cntL_LB = 0; int cntL_RB = 0; int cntL_LR = 0;
  for (SItemVec::iterator itl = vecdL->begin(); itl != vecdL->end(); itl++) {
    if ((*itl).obj->InBothLists())
      cntL_LR++;
    if ((*itl).IsLeftBoundary()) cntL_LB++;
    if ((*itl).IsRightBoundary()) cntL_RB++;
    cntL++;
  } // for
  SItemVec *vecdR = rightData->GetItemVec(axis);
  int cntR = 0; int cntR_LB = 0; int cntR_RB = 0; int cntR_LR = 0;
  for (SItemVec::iterator itr = vecdR->begin(); itr != vecdR->end(); itr++) {
    if ((*itr).obj->InBothLists())
      cntR_LR++;
    if ((*itr).IsLeftBoundary()) cntR_LB++;
    if ((*itr).IsRightBoundary()) cntR_RB++;
    cntR++;
  } // for
  assert(cntL_LR == cntR_LR);
  assert(cntL_LB == cntL_RB);
  assert(cntR_LB == cntR_RB);
#endif
  
  return;
}

#if 0
// This below was an attempt to do it differently in Vienna, Dec 2007.
// VH

// ---------------------------------------------------------
// split the list along the required axis
// first is the begin of the SItem list, axis is the axis, where
// to split, val is the splitting value in integer notation.
// The output of the function are two lists, first and second
// Optionally, objlist store the objects that straddle the splitting plane
void
CKTBABuildUp::BreakAxPosition(SInputData *d, int axis,
                              SInputData *rightData,
                              int &cntLout, int &cntRout)
{
  // the number of object boundaries for left array is not known
  int cntLeft = 0;
  int cntRight = 0;
  d->cntThickness = 0;
  const float positionToDecide = d->position;

  // We have some problem that is difficult to detect, when tagging objects
  // are allowed, the flags are not always reset to zero in some cases. 
  // VH 2/1/2006. The flags are always left boundaries (=1).
  // Therefore here it is a hack to assure that the flags are definitely zero.
  if (resetFlagsForBreakAx) {
    SItemVec *vecd = d->GetItemVec(axis);
    for (SItemVec::iterator itt = vecd->begin(); itt != vecd->end(); itt++) {
      (*itt).obj->ResetFlags();
    } // for 
  } // if
  
#ifdef _DEBUG
  SItemVec *vecd = d->GetItemVec(axis);
  for (SItemVec::iterator itt = vecd->begin(); itt != vecd->end(); itt++) {
    if ((*itt).obj->Flags() != 0) {
      cout << "Init ERROR 2 in original list, flags = "
           << (*itt).obj->Flags() << " obj = " << (void*)((*itt).obj) << endl;
    }
  } // for
#endif // _DEBUG  

  // Allocate the appropriate size for right array, for all 3 axes
  // This is only estimated at this algorithm.
  int estimatedSize = (int)(0.6f * (float)d->count);
  if (estimatedSize < 10)
    estimatedSize = 10;
  assert(estimatedSize > 0);
  rightData->Alloc(estimatedSize * 2);
  
  // First find create all left boundaries for right array
  SItemVec *vecRight = rightData->GetItemVec(axis);

  // Resize to 0 to start from the beginning
  vecRight->resize(0);
  SItemVec *vec = d->GetItemVec(axis);
  SItemVec::iterator it;
  SItemVec::iterator itLeft;
  const SItemVec::iterator itEnd = vec->end();
  bool foundBoundary = false;  
  for (it = vec->begin(); it != itEnd; it++) {
    const float &pos = (*it).pos;  
    if (pos < positionToDecide) {
      cntLeft++;
      // Mark the item to be in the first list
      (*it).obj->SetInFirstList();
    }
    else {
      if (pos > positionToDecide) {
        cntRight++;
        (*it).obj->SetInSecondList();
        if (!foundBoundary) {
          foundBoundary = true;
          itLeft = it;
        }
        // It belongs to the right list
      }
      else {
        // pos == positionToDecide
        if ((*it).IsRightBoundary()) {
          // it belongs to the left list
          cntLeft++;
          (*it).obj->SetInFirstList();
        }
        else {
          // this is the left boundary
          // it belongs already to the right list
          cntRight++;
          (*it).obj->SetInSecondList();
          if (!foundBoundary) {
            foundBoundary = true;
            itLeft = it;
          }
        }
      }
    }
  } // for it

  // Remember the position of the iterator, where right
  // list has to start
  SItem boundary;
  int cntDups = 0;
  cntLeft = 0;
  cntRight = 0;
  
  // create left boundaries for right list
  boundary.pos = d->position;
  boundary.axis = axis;
  boundary.SetLeftBoundary();
  for (it = itLeft; it != itEnd; it++) {
    assert((*it).pos >= positionToDecide);
    // Mark all the items to be in the second list
    if ((*it).obj->InFirstList()) {
      // and copy the newly created boundary to the right array
      boundary.obj = (*it).obj;
      vecRight->push_back(boundary);
      cntDups++;
      cntRight++;
    }
  } // for

  // Now go through the right part of the list again
  // and copy the rest of the list to the right list
  for (it = vec->begin(); it != itEnd; it++) {
    const float &pos = (*it).pos;
    if (pos < positionToDecide) {
      // we copy the boundary to the left list
      *itLeft = *it;
      itLeft++;
      cntLeft++;
    }    
    if (pos > positionToDecide) {
      // we copy the boundary to the right list
      vecRight->push_back(*it);
      cntRight++;
    }
    else {
      if (pos == positionToDecide) {
        if ((*it).obj->InBothLists()) {
          if ((*it).IsRightBoundary()) {
            // right boundary is added to the right as
            // we already created left boundaries
            vecRight->push_back(*it);
            cntRight++;
          }
          else {
            // we copy the left boundary to the left list
            *itLeft = *it;
            itLeft++;
            cntLeft++;
          }
        }
        else {
          // This boundary belongs to only a single list
          if ((*it).obj->InFirstList()) {
            // we copy this boundary  to the left list
            *itLeft = *it;
            itLeft++;
            cntLeft++;
          }
          else 
          if ((*it).obj->InSecondList()) {
            // we copy this boundary to the right list
            vecRight->push_back(*it);
            cntRight++;     
          }
        }
      }
    }
  } // for

  
  // in final pass create the new boundaries for the left list
  int i;
  SItemVec::iterator srcIt = vecRight->begin();
  for (it = itLeft, i = 0 ; i < cntDups; it++, srcIt++, i++) {
    cntLeft++;
    // first copy the boundary from the beginning of the right list
    assert( (*srcIt).IsLeftBoundary());
    *it = *srcIt;
    // then set the right boundary in copied item
    (*it).SetRightBoundary();
    assert( (*it).IsRightBoundary());
  } // for it
  
  // The boundary on the left, unused items are not used
  vec->resize(cntLeft);
  assert(cntLeft % 2 == 0);
  assert(cntRight % 2 == 0);
  assert(cntLeft+cntRight == 2*(d->count+cntDups));
  
  // The final setting of size
  cntLout = cntLeft / 2;
  cntRout = cntRight / 2;
  // Set correctly also the number of objects
  d->cntThickness = cntDups;

#ifdef _DEBUG
  SItemVec *vecdL = d->GetItemVec(axis);
  int cntL = 0; int cntL_LB = 0; int cntL_RB = 0; int cntL_LR = 0;
  for (SItemVec::iterator itl = vecdL->begin(); itl != vecdL->end(); itl++) {
    if ((*itl).obj->InBothLists())
      cntL_LR++;
    if ((*itl).IsLeftBoundary()) cntL_LB++;
    if ((*itl).IsRightBoundary()) cntL_RB++;
    cntL++;
  } // for
  SItemVec *vecdR = rightData->GetItemVec(axis);
  int cntR = 0; int cntR_LB = 0; int cntR_RB = 0; int cntR_LR = 0;
  for (SItemVec::iterator itr = vecdR->begin(); itr != vecdR->end(); itr++) {
    if ((*itr).obj->InBothLists())
      cntR_LR++;
    if ((*itr).IsLeftBoundary()) cntR_LB++;
    if ((*itr).IsRightBoundary()) cntR_RB++;
    cntR++;
  } // for
  assert(vecdL->size() == 2 * cntLout);
  assert(vecdR->size() == 2 * cntRout);
  assert(vecdL->size() == cntL_LB + cntL_RB);
  assert(vecdR->size() == cntR_LB + cntR_RB);  
  assert(cntL_LR == cntR_LR);
  assert(cntL_LB == cntL_RB);
  assert(cntR_LB == cntR_RB);
#endif
  
  return;
}
#endif

// ------------------------------------------------------------------
// break the given list of SItem into two parts by reference axis
// and reference value, the output is in the first and second SItem
void
CKTBABuildUp::DivideAx_I(SInputData *d, int axis,
                         SInputData *rightData,
                         int &cntLout, int &cntRout)
{
  // First check, if this is not only singular case
  if (cntRout == 0) {
    assert(d->cntThickness == 0);
    return; // nothing to do
  }
  
  // the number of found boundaries on the left and on the right
  int cntLeft = 0;
  int cntRight = 0;
  
  // vector for left and right part
  SItemVec *vec = d->GetItemVec(axis);
#ifdef _DEBUG
  if (d->cntThickness == 0) {
    SItemVec::iterator it;
    for (it = vec->begin(); it != vec->end(); it++) {
      if ((*it).obj->InBothLists()) {
        cout << "ERROR in left 3 list" << endl;
      }
      if ((*it).obj->flags > 3) {
        cout << "Problem with 4 flags = " << (*it).obj->flags << endl;    
      }
    } // for    
  }  
#endif // _DEBUG  
  
  assert(vec->size() == d->count*2);
  SItemVec *vecRight = rightData->GetItemVec(axis);
  vecRight->resize(0);
  
  // Now go through source list
  SItemVec::iterator itDest = vec->begin();
  SItemVec::iterator itSrc = vec->begin();
  // traverse all boundaries, belonging only to the left part
  int i = 0;
  const int imax = d->count*2;
  for (; i != imax; itSrc++, i++) {    
    // Check if the item belongs to the second list
    if ((*itSrc).obj->InSecondList()) {
      break; // we can copy the 
    }
    // Check if the item belongs to the first list
    if ((*itSrc).obj->InFirstList()) {
      // copy the content to the first list
      // Note that *itDest = *itSrc is not necessary, since itDest = itSrc
      cntLeft++;
    }
  } // for

  // for newly copied object in the next step
  itDest = itSrc;

  // the boundaries can belong to both parts
  for (; i != imax; itSrc++, i++) {    
    // Check if the item belongs to the second list
    if ((*itSrc).obj->InSecondList()) {
      vecRight->push_back((*itSrc));
      cntRight++;
    }
    // Check if the item belongs to the first list
    if ((*itSrc).obj->InFirstList()) {
      // copy the content to the first list
      *itDest = *itSrc;
      itDest++; // and move the iterator
      cntLeft++;
    }
  } // for

  assert(cntLeft % 2 == 0);
  assert(cntRight % 2 == 0);
  assert(cntLeft == 2 * cntLout);
  assert(cntRight == 2 * cntRout);

  vec->resize(cntLeft);  

  return;
}

// break the given list of SItem into two parts by reference axis
// and reference value, the output is in the first and second SItem
void
CKTBABuildUp::DivideAx_II(SInputData *d, int axis,
                          SInputData *rightData,
                          int &cntLout, int &cntRout)
{
  // First check, if this is not only singular case
  if (cntRout == 0) {
    assert(d->cntThickness == 0);
    return; // nothing to do
  }
  
  // the number of found boundaries on the left and on the right
  int cntLeft = 0;
  int cntRight = 0;
  
  // vector for left and right part
  SItemVec *vec = d->GetItemVec(axis);
#ifdef _DEBUG
  if (d->cntThickness == 0) {
    SItemVec::iterator it;
    for (it = vec->begin(); it != vec->end(); it++) {
      if ((*it).obj->InBothLists()) {
        cout << "ERROR in left list" << endl;
      }
      if ((*it).obj->flags > 3) {
        cout << "Problem with flags = " << (*it).obj->flags << endl;      
      }
    } // for    
  }
#endif // _DEBUG  
  
  assert(vec->size() == d->count*2);
  SItemVec *vecRight = rightData->GetItemVec(axis);
  vecRight->resize(0);
  
  // Now go through the source list
  SItemVec::iterator itDest = vec->begin();
  SItemVec::iterator itSrc = vec->begin();
  // traverse all boundaries, belonging only to the left part
  int i = 0;
  const int imax = d->count*2;
  for (; i != imax; itSrc++, i++) {    
    // Check if the item belongs to the second list
    if ((*itSrc).obj->InSecondList()) {
      break; // we can copy the 
    }
    // Check if the item belongs to the first list
    if ((*itSrc).obj->InFirstList()) {
      // copy the content to the first list
      // Note that *itDest = *itSrc is not necessary, since itDest = itSrc
      cntLeft++;
    }
    // Reset flags for the next subdivision step
    if ((*itSrc).IsRightBoundary())
      (*itSrc).obj->ResetFlags();
  } // for

  // for newly copied object in the next step
  itDest = itSrc;

  // !!!!!!!!!!!!!!
  // for newly copied object in the next step
  itDest = itSrc;

  // the boundaries can belong to both parts
  for (; i != imax; itSrc++, i++) {    
    //if ((*itSrc).obj == objtf) {
    //  cout << "DivideAxII - obj found2 , i = " << i << endl;

    // Reset flags for the next subdivision step
    if ((*itSrc).IsRightBoundary()) {
      // This is the right boundary, we have to reset flags
      bool inFirstList = (*itSrc).obj->InFirstList();
      bool inSecondList = (*itSrc).obj->InSecondList();
      (*itSrc).obj->ResetFlags();
      if (inSecondList) {
        cntRight++;
        vecRight->push_back((*itSrc));
      }
        
      // Check if the item belongs to the first list
      if (inFirstList) {
        cntLeft++;
        // copy the content to the first list
        *itDest = *itSrc;
        itDest++; // and move the iterator
      }
    }
    else { // left boundary
      // Check if the item belongs to the second list
      if ((*itSrc).obj->InSecondList()) {
        cntRight++;
        vecRight->push_back((*itSrc));
      }
      // Check if the item belongs to the first list
      if ((*itSrc).obj->InFirstList()) {
        cntLeft++;
        // copy the content to the first list
        *itDest = *itSrc;
        itDest++; // and move the iterator
      }
    } // end of left boundary
  } // for
  
  assert(cntLeft % 2 == 0);
  assert(cntRight % 2 == 0);
  assert(cntLeft == 2 * cntLout);
  assert(cntRight == 2 * cntRout);
  vec->resize(cntLeft);  

#ifdef _DEBUG
  for (SItemVec::iterator itt = vec->begin(); itt != vec->end(); itt++) {
    if ((*itt).obj->Flags() != 0) {
      cout << "DivideAxII ERROR 1 in final left list, flags = "
           << (*itt).obj->Flags() << " obj = " << (void*)((*itt).obj) << endl;
      abort();;
    }
  } // for
  for (SItemVec::iterator itt = vecRight->begin(); itt != vecRight->end(); itt++) {
    if ((*itt).obj->Flags() != 0) {
      cout << "DivideAxII ERROR 1 in final right list, flags = "
           << (*itt).obj->Flags() << " obj = " << (void*)((*itt).obj) << endl;
      abort();;
    }
  } // for
#endif // _DEBUG  
  
  return;
}


// break the given list of SItem into two parts by reference axis
// and reference value, the output is in the first and second SItem
void
CKTBABuildUp::DivideAx_I_opt(SInputData *d, int axis,
                             SInputData *rightData,
                             int cntLout, int cntRout)
{
  // First check, if this is not only singular case
  if (cntRout == 0) {
    assert(d->cntThickness == 0);
    return; // nothing to do
  }
  
  // vector for left and right part
  SItemVec *vec = d->GetItemVec(axis);

#ifdef _DEBUG
  if (vec->size() != d->count*2) {
    cout << "vec->size() = " << vec->size()
         << " d->count*2 = " << d->count*2 << endl;
  }

  if (d->cntThickness == 0) {
    SItemVec::iterator it;
    for (it = vec->begin(); it != vec->end(); it++) {
      if ((*it).obj->InBothLists()) {
        cout << "ERROR in left list" << endl;
      }
      if ((*it).obj->flags > 3) {
        cout << "Problem with flags = " << (*it).obj->flags << endl;      
      }
    } // for    
  }  
#endif // _DEBUG  
  
  assert(vec->size() == d->count*2);
  SItemVec *vecRight = rightData->GetItemVec(axis);
  vecRight->resize(0);
  
  // Now go through source list
  SItemVec::iterator itDest = vec->begin();
  SItemVec::iterator itSrc = vec->begin();
#if 1
  if (vec->size() != d->count * 2) {
    cout << "Something wrong HERE vec->size="
         << vec->size() << " d->count*2 = "
         << d->count*2 << endl;
    abort();
  }
#endif  

  // traverse all boundaries, belonging only to the left part
  int i = 0;
  const int imax = d->count*2;
  for (; i != imax; itSrc++, i++) {    
    // Check if the item belongs to the second list
    if ((*itSrc).obj->InSecondList()) {
      break; // we can copy the 
    }
  } // for

  // for newly copied object in the next step
  itDest = itSrc;
  
  // the boundaries can belong to both parts
  for (; i != imax; itSrc++, i++) {
    SItem &itt = *itSrc;
    // Check if the item belongs to the second list    
    if (itt.obj->InSecondList()) {
      // and first copy the right boundary to a new list
      vecRight->push_back((*itSrc));
    }
    // Check if the item belongs to the first list
    if (itt.obj->InFirstList()) {
      // copy the content to the first list
      *itDest = *itSrc;
      itDest++; // and move the iterator
    }
  } // for
  
  // Shorten the left array by resizing
  vec->resize(cntLout*2);
  assert(vecRight->size() == cntRout*2);
  
  return;
}

// break the given list of SItem into two parts by reference axis
// and reference value, the output is in the first and second SItem
void
CKTBABuildUp::DivideAx_II_opt(SInputData *d, int axis,
                              SInputData *rightData,
                              int cntLout, int cntRout)
{
  // First check, if this is not only singular case
  if (cntRout == 0) {
    assert(d->cntThickness == 0);
    return; // nothing to do
  }
    
  // vector for left and right part
  SItemVec *vec = d->GetItemVec(axis);
  assert(vec->size() == d->count*2);
#ifdef _DEBUG
  if (d->cntThickness == 0) {
    SItemVec::iterator it;
    for (it = vec->begin(); it != vec->end(); it++) {      
      if ((*it).obj->InBothLists()) {
        cout << "ERROR in left list" << endl;
      }
      if ((*it).obj->flags > 3) {
        cout << "Problem with flags = " << (*it).obj->flags << endl;      
      }
    } // for    
  }
  {
    SItemVec::iterator ittt;
    for (ittt = vec->begin(); ittt != vec->end(); ittt++) {
      if ((*ittt).obj->ToBeRemoved()) {
        cout << "ERROR - to be removed in the left list" << endl;
      }
    }
  }
#endif // _DEBUG  
  
  assert(vec->size() == d->count*2);
  SItemVec *vecRight = rightData->GetItemVec(axis);
  vecRight->resize(0);
  
  // Now go through the source list
  SItemVec::iterator itDest = vec->begin();
  SItemVec::iterator itSrc = vec->begin();
#if 1
  if (vec->size() != d->count * 2) {
    cout << "Something wrong HERE II vec->size="
         << vec->size() << " d->count*2 = "
         << d->count*2 << endl;
    abort();
  }
#endif  
  // traverse all boundaries, belonging only to the left part
  int i = 0;
  const int imax = d->count*2;
  for (; i != imax; itSrc++, i++) {
    //if ((*itSrc).obj == objtf) {
    //  cout << "DivideAxII - obj found1 , i = " << i << endl;

    // Check if the item belongs to the second list
    if ((*itSrc).obj->InSecondList()) {
      break; // we can copy the 
    }
    // Reset flags for the next subdivision step
    if ((*itSrc).IsRightBoundary())
      (*itSrc).obj->ResetFlags();
  } // for

  // for newly copied object in the next step
  itDest = itSrc;

  // the boundaries can belong to both parts
  for (; i != imax; itSrc++, i++) {    
    //if ((*itSrc).obj == objtf) {
    //  cout << "DivideAxII - obj found2 , i = " << i << endl;

    bool inFirstList = (*itSrc).obj->InFirstList();
    bool inSecondList = (*itSrc).obj->InSecondList();

    // Reset flags for the next subdivision step
    if ((*itSrc).IsRightBoundary()) {
      // This is the right boundary, we have to reset flags
      if (inSecondList)
        vecRight->push_back((*itSrc));
      // Check if the item belongs to the first list
      if (inFirstList) {
        // copy the content to the first list
        *itDest = *itSrc;
        itDest++; // and move the iterator
      }
    }
    else { // left boundary
      // Check if the item belongs to the second list
      if (inSecondList) {
        vecRight->push_back((*itSrc));
      }
      // Check if the item belongs to the first list
      if (inFirstList) {
        // copy the content to the first list
        *itDest = *itSrc;
        itDest++; // and move the iterator
      }
    } // end of left boundary
  } // for

  // Shorten the left array by resizing
  vec->resize(cntLout*2);
  assert(vecRight->size() == cntRout*2);

#if 0
#ifdef _DEBUG
  for (SItemVec::iterator itt2 = vec->begin(); itt2 != vec->end(); itt2++) {
    if ((*itt2).obj->Flags() != 0) {
      cout << "DivideAxII ERROR 1 in final left list, flags = "
           << (*itt2).obj->Flags() << " obj = "
           << (void*)((*itt2).obj) << endl;
      abort();;
    }
  } // for
  for (SItemVec::iterator itt3 = vecRight->begin(); itt3 != vecRight->end(); itt3++) {
    if ((*itt3).obj->Flags() != 0) {
      cout << "DivideAxII ERROR 1 in final right list, flags = "
           << (*itt3).obj->Flags() << " obj = "
           << (void*)((*itt3).obj) << endl;
      abort();;
    }
  } // for
#endif // _DEBUG  
#endif
  
  return;
}

#ifdef _VYPIS
void
CKTBABuildUp::PrintOut(CKTBAxes::Axes axis, int stat, SItem **first,
                       SItem **second, float /*position*/)
{
#if 1
  int cnt = 0;
  SItem *t = *first;
  if (stat & 1) {
    DEBUG << "----FIRST--- axis=" << (int)axis << "\n";
    while (t != NULL) {
#define _VYP

#ifdef _VYP
      float v = t->value[axis];
      DEBUGW << "curr=" << t
#ifdef _BIDIRLISTS
             << " prev=" << t->prev[axis]
#endif
             << " next="
             << t->next[axis] << " val=" << v
             << " " << (t->IsRightBoundary() ? 1 : 0);
      if (t->IsRightBoundary())
        DEBUGW << "   obj= " << t->obj;
      else
        DEBUGW << "   pair= " << t->pairF;
      
      DEBUGW << endl;
#endif // _VYP
      t = t->next[axis];
      cnt++;
    }
    
    DEBUG<< "FIRST cnt= " << cnt <<"\n" << flush;
  }

  if (stat & 2) {
    // the right list
    t = *second;
    cnt = 0;
    DEBUG << "----SECOND-----\n";
    while (t != NULL) {
#ifdef _VYP
      float v = t->value[axis];
      DEBUGW << "curr=" << t
#ifdef _BIDIRLISTS
             << " prev=" << t->prev[axis]
#endif
             << " next="
            << t->next[axis] << " val=" << v
            << " " << ((t->IsRightBoundary()) ? 1 : 0);
      if (t->IsRightBoundary())
        DEBUGW << "   obj= " << t->obj;
      else
        DEBUGW << "   pair= " << t->pairF;
      DEBUGW << "\n" << flush;
#endif // _VYP
      t = t->next[axis];
      cnt++;
    }
    DEBUGW << "SECOND cnt= " << cnt <<"\n" << flush;
  }
#endif
}
#endif // _VYPIS

// --------------------------------------------------------------------
// class CKTBABuildUp::CTestAx

// The initialization for the first axis to be tested, this time *****Z axis*******
void
CKTBABuildUp::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 initialization for the first axis to be tested, this time *****Y axis*******
// This can be run independently.
void
CKTBABuildUp::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 initialization for the first axis to be tested, this time *****Z axis*******
// This can be run independently.
void
CKTBABuildUp::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 initialization for the first axis to be tested, use only in case
// when all 3 axes are tested. It can be called only when InitXaxis was called before !!!!
void
CKTBABuildUp::SSplitState::ReinitYaxis(int cnt, const SBBox &boxN)
{
  // initialize the variables, mainly for surface area estimates
  assert(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
  //int topAxis = 2;      // axis that is considered in top .. depth
  //int frontAxis = 0;    // axis that is considered in front .. height  
  // Swap the width, height, and depth
  float tS = width;
  // the size of bounding box along the axis to be split
  width = topw;
  // and along two other axes
  topw = frontw;
  frontw = tS; // copy the temporary variable
  // 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;
  //assert(areaWholeSA2 == width * SumLength + areaSplitPlane);
}

// The initialization for the first axis to be tested, use only in case
// when all 3 axes are tested. It can be called only when ReinitYaxis was called before !!!
void
CKTBABuildUp::SSplitState::ReinitZaxis(int cnt, const SBBox &boxN)
{
  // initialize the variables, mainly for surface area estimates
  assert(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
  // Swap the width, height, and depth
  float tS = width;
  // the size of bounding box along the axis to be split
  width = topw;
  // and along two other axes  
  topw = frontw;
  frontw = tS; // copy the temporary variable
  // 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;
  //assert(areaWholeSA2 == width * stateSumLength + areaSplitPlane);
}

// This is the computation of the cost using surface area heuristics
// using LINEAR EXTIMATE
void
CKTBABuildUp::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.bestIterator = state.it;
    // 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
CKTBABuildUp::EvaluateCostFreeCut(SSplitState &state)
{
  // This is assumed to work for free cut (no object intersected)
  assert(state.thickness == 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.bestIterator = state.it;
    // The number of objects whose boundaries are duplicated
    state.bestThickness = 0;
  }
  return;
}

// -------------------------------------------------------------------------------
// TRIAL TO IMPROVE for a given X-axis search for the best splitting plane position.
// Starts from start item, the rest of the information is set to 'state' variable.
void
CKTBABuildUp::GetSplitPlaneOpt(SItemVec *vec, int axisToTest)
{
#ifdef _DEBUG
  if ((vec == NULL) || (vec->size() == 0)) {
    cerr << "Trying to subdivide some node without an object" << endl;
    abort();;
  }
#endif // _DEBUG  

#if 0
  static int index = 0;
  cout << "index = " << index << endl;
  const int indexToFind = 8;
  if (index == indexToFind) {
    cout << "Index found = " << index << endl;
  }  
  index++;
#endif
  
  // some necessary space is required to cut off
  const float eps = 1.0e-6 * state.width;
  const float MinPosition = state.box.Min(axisToTest);
  const float MaxPosition = state.box.Max(axisToTest);
  //float MinPositionAccept = MinPosition + eps;
  //float MaxPositionAccept = MaxPosition - eps;

  // wherefrom to start
  SItemVec::iterator curr = vec->begin();
  SItemVec::iterator next = vec->begin() + 1;
  // Set the type of splitting by this function, which is in this case
  // not overwritten in Evaluate() function
  state.bestTwoSplits = 1;
  float val = (*curr).pos;
  float nval;
  float pval = val;

  // Evaluate the first possible position, cutting off empty
  // space on the left of the splitting plane
  state.position = val - MinPosition;
  if (state.position > 0.f) {
    state.position2 = next->pos - MinPosition;
    state.it = curr;
    // Here evaluate the cost of surface area heuristics
    EvaluateCostFreeCut(state);
  }
  
  //-------------------- TEST LOOP ---------------------------------------
  // make evaluation for each splitting position
  const int imax = vec->size()-1;
  int ii;
  for (ii = 0; ii < imax; ii++)
  {
    nval = next->pos;

#if 0
    if (axisToTest == 2) {
      if ((val > 0.866) && (val < 0.870)) {       
        cout << "val = " << val;
        cout << "  nval = " << nval;
        if (curr->IsRightBoundary())
          cout << " R ";
        else
          cout << " L ";
        cout << " thickness = " << state.thickness << endl;
        
      }
    }
#endif    
    // update left box and rightbox properties
    if (curr->IsRightBoundary()) { // we are on the right boundary of an object
      state.cntRight--;
      state.thickness--;
      // Check possibly the position
      if ( (val != nval) ||
           ((curr->IsRightBoundary()) && (next->IsLeftBoundary())) )
        {
          state.position = val - MinPosition;
          state.position2 = nval - MinPosition;
          state.it = curr;
          // Here evaluate the cost of surface area heuristics
          EvaluateCost(state);
        }      
    }
    else {
      // the left boundary .. enter the object from left

      // Check possibly the position
      if ( (pval != val)
           // || ((curr->IsRightBoundary()) && (next->IsLeftBoundary()))
           )
        {
          state.position = val - MinPosition;
          state.position2 = nval - MinPosition;
          state.it = curr;
          // Here evaluate the cost of surface area heuristics
          EvaluateCost(state);
        }    
      
      state.cntLeft++;
      state.thickness++;
    }
        
    curr = next; // pointer to the boundary
    next++;
    pval = val;
    val = nval; // value of splitting plane
  } // while 

  //cout << "i = " << i << endl;
  
  assert(state.cntLeft == state.cntAll);
  state.cntRight--;
  assert(state.cntRight == 0);
  state.thickness--;
  assert(state.thickness == 0);  
  
  // Evaluate last possible position 
  state.position = val - MinPosition;
  if (state.position < MaxPosition) {
    state.position2 = state.width;
    state.it = curr;
    // Here evaluate the cost of surface area heuristics
    EvaluateCostFreeCut(state);
  }
   
  // In this case the best result is kept in 'state' variable
  return;
} // CTestAx::GetSplitPlaneOpt (for X, Y, Z)

// -------------------------------------------------------------------------------
// TRIAL TO IMPROVE for a given X/Y/Z-axis search for the best splitting plane position.
// Starts from start item, the rest of the information is set to 'state' variable.
void
CKTBABuildUp::GetSplitPlaneOpt2(SItemVec *vec, int axisToTest)
{
#ifdef _DEBUG
  if ((vec == NULL) || (vec->size() == 0)) {
    cerr << "Trying to subdivide some node without an object" << endl;
    abort();;
  }
#endif // _DEBUG  
  
  // some necessary space is required to cut off
  const float eps = 1.0e-6 * state.width;
  const float MinPosition = state.box.Min(axisToTest);
  const float MaxPosition = state.box.Max(axisToTest);
  //float MinPositionAccept = MinPosition + eps;
  //float MaxPositionAccept = MaxPosition - eps;

  // wherefrom to start
  SItemVec::iterator curr = vec->begin();
  SItemVec::iterator next = vec->begin() + 1;
  // Set the type of splitting by this function, which is in this case
  // not overwritten in Evaluate() function
  state.bestTwoSplits = 1;
  state.thickness = 0;
  float val = (*curr).pos;
  float nval;
  float pval = val;

//#if 1
#ifdef _DEBUG_COSTFUNCTION
  static int indexToFind = 0;
  static int indexSS = 0;
  if ( (indexSS == indexToFindSS) &&
       (!_alreadyDebugged)) {
    cout << "Debugging cost function, index = " << indexSS << endl;
    InitCostStream(indexSS, state.cntAll,
                   (val-state.box.Min(axisToTest))/state.width,
                   //(val-MinPosition)/state.width,
                   //(MinPosition)/state.width,
                   (state.box.Max(axisToTest)-MinPosition)/state.width);
  }
  indexSS++;
#endif
  
  // Evaluate the first possible position, cutting off empty
  // space on the left of the splitting plane
  state.position = val - MinPosition;
  if (state.position > 0.f) {
    state.position2 = next->pos - MinPosition;
    state.it = curr;
    // Here evaluate the cost of surface area heuristics
    EvaluateCostFreeCut(state);
  }
  
  //-------------------- TEST LOOP ---------------------------------------
  // make evaluation for each splitting position
  const int imax = vec->size()-1;
  int ii;
  for (ii = 1; ii <= imax; ii++)
  {
    nval = (*vec)[ii].pos;
    // update left box and rightbox properties
    if (curr->IsRightBoundary()) {
      // the right boundary of an object
      state.cntRight--;
      state.thickness--;
      
      // Check possibly the position
      if ( (val != nval) ||
           ((curr->IsRightBoundary()) && (next->IsLeftBoundary())) )
        {
          if (state.thickness >= 0) {
            state.position = val - MinPosition;
            state.position2 = nval - MinPosition;
            state.it = curr;
            // Here evaluate the cost of surface area heuristics
            EvaluateCost(state);
          }
        }
    }
    else {
      // the left boundary .. enter the object from left

      // Check possibly the position
      if (pval != val)
      {
        if (state.thickness >= 0) {
          state.position = val - MinPosition;
          state.position2 = nval - MinPosition;
          state.it = curr;
          // Here evaluate the cost of surface area heuristics
          EvaluateCost(state);
        }
      }      
      state.cntLeft++;
      state.thickness++;
    }
        
    curr = next; // pointer to the boundary
    pval = val;
    val = nval; // value of splitting plane
    next++;
  } // while 

  //cout << "i = " << i << endl;
  
  assert(state.cntLeft == state.cntAll);
  state.cntRight--;
  assert(state.cntRight == 0);
  state.thickness--;
  assert(state.thickness == 0);  

#if 0
  if (state.thickness < 0) {
    cout << "SSSomething wrong happened here at end\n" << endl;
  }
#endif      

#ifdef _DEBUG
  if (curr != vec->end()-1) {
    cerr << "Implementation bug in pointers" << endl;
    cout << "curr = " << curr - vec->begin()
         << " vec->end()-1 = " << vec->end()-1 - vec->begin() << endl;
    abort();;
  }
#endif
  
  // Evaluate last possible position 
  state.position = val - MinPosition;
  if (state.position < MaxPosition) {
    state.position2 = state.width;
    state.it = curr;
    // Here evaluate the cost of surface area heuristics
    EvaluateCostFreeCut(state);
  }
  
#ifdef _DEBUG_COSTFUNCTION
  CloseCostStream();
#endif
  
  // In this case the best result is kept in 'state' variable
  return;
} // CTestAx::GetSplitPlaneOpt (for X, Y, Z)

// -------------------------------------------------------------------------------
// TRIAL TO IMPROVE for a given X/Y/Z-axis search for the best splitting plane position.
// Using unrolling possibly understandable for inteligent compiler.
// Starts from start item, the rest of the information is set to 'state' variable.
void
CKTBABuildUp::GetSplitPlaneOpt3(SItemVec *vec, int axisToTest)
{
#ifdef _DEBUG
  if ((vec == NULL) || (vec->size() == 0)) {
    cerr << "Trying to subdivide some node without an object" << endl;
    abort();;
  }
#endif // _DEBUG  

#if 0
  static int index = 0;
  cout << "index = " << index << endl;
  const int indexToFind = 27;
  if (index == indexToFind) {
    cout << "Index found = " << index << endl;
  }  
  index++;
#endif
  
  // some necessary space is required to cut off
  const float eps = 1.0e-6 * state.width;
  const float MinPosition = state.box.Min(axisToTest);
  const float MaxPosition = state.box.Max(axisToTest);
  //float MinPositionAccept = MinPosition + eps;
  //float MaxPositionAccept = MaxPosition - eps;

  // wherefrom to start
  SItemVec::iterator curr = vec->begin();
  SItemVec::iterator next = vec->begin() + 1;
  // Set the type of splitting by this function, which is in this case
  // not overwritten in Evaluate() function
  state.bestTwoSplits = 1;
  float val = (*curr).pos;
  // value at previous and next position
  float pval, nval;
  pval = MinPosition;

  // Evaluate the first possible position, cutting off empty
  // space on the left of the splitting plane
  state.position = val - MinPosition;
  // The first boundary must be left
  assert(curr->IsLeftBoundary());
  if (state.position > 0.f) {    
    state.position2 = next->pos - MinPosition;
    state.it = curr;
    // Here evaluate the cost of surface area heuristics
    EvaluateCostFreeCut(state);
  } // ------------------------------
  // Update for the next iteration
  state.cntLeft++;
  state.thickness++;
  pval = val;
  
  //-------------------- TEST LOOP ---------------------------------------
  // make evaluation for each splitting position, minus the first and
  // the last one
  int imax = vec->size()-2;
  // Setting unrolling
  const int MaxUNROLL = 4;
  const int loops = imax / MaxUNROLL;
  const int remainder = imax % MaxUNROLL;
  // There is some bug, when using this version, the boundaries are
  // incorrectly sorted.
  assert(remainder < MaxUNROLL);
  int ii = 1;
  if (loops > 0) {
    // The outer loop
    for (int l = 0; l < loops; l++, ii += MaxUNROLL)
    {
      // This has to be unrolled by compiler such as Intel Compiler or GCC4.1
      // The number of loops is known in advance !
      for (int qq = 0; qq < MaxUNROLL; qq++)
      {
        curr = vec->begin() + ii + qq;
        next = curr + 1;
        #if 1
        val = (*curr).pos;
        nval = (*next).pos;
        #else
        val = (*vec)[ii+qq].pos;
        nval = (*vec)[ii+qq+1].pos;
        #endif
        
        // update left box and rightbox properties
        if (curr->IsRightBoundary()){// we are on the right boundary of an object
          state.cntRight--;
          state.thickness--;
          // Check possibly the position
          if ( (val != nval) ||
               ((curr->IsRightBoundary()) && (next->IsLeftBoundary())) )
          {
            state.position = val - MinPosition;
            state.position2 = nval - MinPosition;
            state.it = curr;
            // Here evaluate the cost of surface area heuristics
            EvaluateCost(state);
            // It does make sense to change boundary value
            pval = (*vec)[ii+qq].pos; 
          }
        }
        else {
          // the left boundary .. enter the object from left
        
          // Check possibly the position
          if (pval != val)
          {
            state.position = val - MinPosition;
            state.position2 = nval - MinPosition;
            state.it = curr;
            // Here evaluate the cost of surface area heuristics
            EvaluateCost(state);
            // It makes sense to change boundary value
            pval = (*vec)[ii+qq].pos;
          }
          state.cntLeft++;
          state.thickness++;
        } // left or right boundary      
      } // for qq
    } // for ii

    // Set the correct pointer for the next iteration
    curr = next; next++;
    pval = val;
  }
  else {
    //cout << "No loop" << endl;
    pval = val;
    curr = next;
    next++;
  }

  // The rest of the loop
  if (ii <= imax) {
    // Set the correct pointer  
    for ( ;ii <= imax; ii++) {
      nval = curr->pos;
      // update left box and rightbox properties
      if (curr->IsRightBoundary()) { // we are on the right boundary of an object
        state.cntRight--;
        state.thickness--;
        // Check possibly the position
        if ( (val != nval) ||
             ((curr->IsRightBoundary()) && (next->IsLeftBoundary())) )
        {
          state.position = val - MinPosition;
          state.position2 = nval - MinPosition;
          state.it = curr;
          // Here evaluate the cost of surface area heuristics
          EvaluateCost(state);
        }
      }
      else {
        // the left boundary .. enter the object from left
      
        // Check possibly the position
        if (pval != val)
        {
          state.position = val - MinPosition;
          state.position2 = nval - MinPosition;
          state.it = curr;
          // Here evaluate the cost of surface area heuristics
          EvaluateCost(state);
        }
        state.cntLeft++;
        state.thickness++;
      } // if right/left boundary
    
      curr = next; // pointer to the boundary
      pval = val;
      val = nval; // value of splitting plane
      next++;
    } // for qq

    // For the last evaluation
  }
  else {
    if (!loops) {
      curr = next;
      val = nval;
    }
  }

  //cout << "i = " << i << endl;

#ifdef _DEBUG  
  if (curr != vec->end()-1) {
    cerr << "Implementation bug in pointers" << endl;
    cout << "curr = " << curr - vec->begin()
         << "vec->end()-1 = " << vec->end()-1 - vec->begin() << endl;
    abort();;
  }
#endif
  
  assert(state.cntLeft == state.cntAll);
  state.cntRight--;
  assert(state.cntRight == 0);
  state.thickness--;
  assert(state.thickness == 0);
  
  // Evaluate last possible position
  state.position = val - MinPosition;
  if (state.position < MaxPosition) {
    state.position2 = state.width;
    state.it = curr;
    // Here evaluate the cost of surface area heuristics
    EvaluateCostFreeCut(state);
  }
   
  // In this case the best result is kept in 'state' variable
  return;
} // CTestAx::GetSplitPlaneOpt3 (for X, Y, Z)

#if 0
// Here is some bug, that is clearly visible for tree.nff !! The resulting
// tree is much less efficient. 3/1/2006 VH.
// -------------------------------------------------------------------------------
// TRIAL TO IMPROVE for a given X,Y,Z-axis search for the best splitting plane position.
// Starts from start item, the rest of the information is set to 'state' variable.
// It does not work for gcc compiler better than GetSplitPlaneOpt
void
CKTBABuildUp::GetSplitPlaneOptUnroll4(SItemVec *vec, int axisToTest)
{
#ifdef _DEBUG
  if ((vec == NULL) || (vec->size() == 0)) {
    cerr << "Trying to subdivide some node without an object" << endl;
    abort();;
  }
#endif // _DEBUG  

#if 0
  static int index = 0;
  cout << "index = " << index << endl;
  const int indexToFind = 8;
  if (index == indexToFind) {
    cout << "Index found = " << index << endl;
  }  
  index++;
#endif
  
  // some necessary space is required to cut off
  const float eps = 1.0e-6 * state.width;
  const float MinPosition = state.box.Min(axisToTest);
  const float MaxPosition = state.box.Max(axisToTest);
  //float MinPositionAccept = MinPosition + eps;
  //float MaxPositionAccept = MaxPosition - eps;

  // wherefrom to start
  // Set the type of splitting by this function, which is in this case
  // not overwritten in Evaluate() function
  state.bestTwoSplits = 1;
  float val4 = MinPosition;
  
  //-------------------- TEST LOOP ---------------------------------------
  // make evaluation for each splitting position

  // the first position was already evaluated and the last is also a special case
  const int imax = vec->size() - 1;
  int cntDebug = 0;
  int imax4 = 4*(imax/4);
  int imax4rest = imax % 4;
  int i;
  // This is manual unrolling
  if (imax4 >= 4) {
  for(i = 0; i < imax4; i += 4)
  {
    // The evaluation I
    cntDebug++;
    float pval1 = val4;
    SItem* curr1 = &((*vec)[i+0]); float val1 = (*vec)[i+0].pos;
    SItem* next1 = &((*vec)[i+1]); float nval1 = (*vec)[i+1].pos;
    if (curr1->IsRightBoundary()) {
      state.cntRight--; state.thickness--;
      if ( (val1 != nval1) ||
           ((curr1->IsRightBoundary()) && (next1->IsLeftBoundary())) ) {
        state.position = val1 - MinPosition; state.position2 = nval1 - MinPosition;
        state.it = vec->begin() + i; EvaluateCost(state);
      }
    }
    else {
      if (pval1 != val1) {
          state.position = val1 - MinPosition; state.position2 = nval1 - MinPosition;
          state.it = vec->begin() + i; EvaluateCost(state);
      }
      state.cntLeft++; state.thickness++;
    }
    // The evaluation II
    cntDebug++;
    float pval2 = (*vec)[i+0].pos;
    SItem* curr2 = &((*vec)[i+1]); float val2 = (*vec)[i+1].pos;
    SItem* next2 = &((*vec)[i+2]); float nval2 = (*vec)[i+2].pos;
    if (curr2->IsRightBoundary()) {
      state.cntRight--; state.thickness--;
      if ( (val2 != nval2) ||
           ((curr2->IsRightBoundary()) && (next2->IsLeftBoundary())) ) {
        state.position = val2 - MinPosition; state.position2 = nval2 - MinPosition;
        state.it = vec->begin() + i + 1; EvaluateCost(state);
      }
    }
    else {
      if (pval2 != val2) {
          state.position = val2 - MinPosition; state.position2 = nval2 - MinPosition;
          state.it = vec->begin() + i + 1; EvaluateCost(state);
      }
      state.cntLeft++; state.thickness++;
    }
    // The evaluation III
    cntDebug++;
    float pval3 = (*vec)[i+1].pos;
    SItem* curr3 = &((*vec)[i+2]); float val3 = (*vec)[i+2].pos;
    SItem* next3 = &((*vec)[i+3]); float nval3 = (*vec)[i+3].pos;
    if (curr3->IsRightBoundary()) {
      state.cntRight--; state.thickness--;
      if ( (val3 != nval3) ||
           ((curr3->IsRightBoundary()) && (next3->IsLeftBoundary())) ) {
        state.position = val3 - MinPosition; state.position2 = nval3 - MinPosition;
        state.it = vec->begin() + i + 2; EvaluateCost(state);
      }
    }
    else {
      if (pval3 != val3) {
          state.position = val3 - MinPosition; state.position2 = nval3 - MinPosition;
          state.it = vec->begin() + i + 2; EvaluateCost(state);
      }
      state.cntLeft++; state.thickness++;
    }
    // The evaluation IV
    cntDebug++;
    float pval4 = (*vec)[i+2].pos;
    SItem* curr4 = &((*vec)[i+3]); val4 = (*vec)[i+3].pos;
    SItem* next4 = &((*vec)[i+4]); float nval4 = (*vec)[i+4].pos;
    if (curr4->IsRightBoundary()) {
      state.cntRight--; state.thickness--;
      if ( (val4 != nval4) ||
           ((curr4->IsRightBoundary()) && (next4->IsLeftBoundary())) ) {
        state.position = val4 - MinPosition; state.position2 = nval4 - MinPosition;
        state.it = vec->begin() + i + 3; EvaluateCost(state);
      }
    }
    else {
      if (pval4 != val4) {
          state.position = val4 - MinPosition; state.position2 = nval4 - MinPosition;
          state.it = vec->begin() + i + 3; EvaluateCost(state);
      }
      state.cntLeft++; state.thickness++;
    }
  } // for, unrolled loop by factor 4
  } // if more than 4 evaluations
  
  for(i = imax4; i < imax; i ++)
  {
    cntDebug++;
    float pvalI = val4;
    SItem* currI = &((*vec)[i+0]); float valI = (*vec)[i+0].pos;
    SItem* nextI = &((*vec)[i+1]); float nvalI = (*vec)[i+1].pos;
    if (currI->IsRightBoundary()) {
      state.cntRight--; state.thickness--;
      if ( (valI != nvalI) ||
           ((currI->IsRightBoundary()) && (nextI->IsLeftBoundary())) ) {
        state.position = valI - MinPosition; state.position2 = nvalI - MinPosition;
        state.it = vec->begin() + i; EvaluateCost(state);
      }
    }
    else {
      if (pvalI != valI) {
          state.position = valI - MinPosition; state.position2 = nvalI - MinPosition;
          state.it = vec->begin() + i; EvaluateCost(state);
      }
      state.cntLeft++; state.thickness++;
    }
    val4 = valI;
  } // for i
  
  //cout << "i = " << i << endl;
  assert(cntDebug == imax);
  
  assert(state.cntLeft == state.cntAll);
  state.cntRight--;
  assert(state.cntRight == 0);
  state.thickness--;
  assert(state.thickness == 0);  
  
  // Evaluate last possible position 
  state.position = (*vec)[imax-1].pos - MinPosition;
  if (state.position < MaxPosition) {
    state.position2 = state.width;
    state.it = vec->end() - 1;
    // Here evaluate the cost of surface area heuristics
    EvaluateCost(state);
  }
   
  // In this case the best result is kept in 'state' variable
  return;
} // CTestAx::GetSplitPlaneX,Y,Z
#endif

}