source: GTP/trunk/Lib/Vis/Preprocessing/src/havran/ktbftrav.cpp @ 2583

// ===================================================================
// $Id: $
//
// ktbftrav.cpp
//
// class: CKTBTraversal
//
// REPLACEMENT_STRING
//
// Copyright by Vlastimil Havran, 2007 - email to "vhavran AT seznam.cz"
// Initial coding by Vlasta Havran, February 2007 (copy from kdrtrav.cpp)

// GOLEM headers
#include "ktbconf.h"
#include "ktbtrav.h"
#include "Intersectable.h"

namespace GtpVisibilityPreprocessor {

#ifdef TRV00F

#if 0
// the depth of traversal when kd-trees are nested. The global (the highest
// level) kd-tree is at the depth 0.
int
CKTBTraversal::traversalDepth = 0;

// sets the stack pointers that are used for the traversal.
void
CKTBTraversal::GetStackPointers(struct SStackElem *&entryPointer,
                                struct SStackElem *&exitPointer)
{
  assert(traversalDepth < MAX_NESTING);
  
  int index = traversalDepth * MAX_HEIGHT;
  entryPointer = &(stack[index]);
  exitPointer = &(stack[index + 1]);

  traversalDepth++;  
  return;
}

// sets the stack pointers that are used for the traversal.
void
CKTBTraversal::RestoreStackPointers()
{
  assert(traversalDepth >= 0);
  traversalDepth--;  
  return;
}


// default constructor
CKTBTraversal::CKTBTraversal()
{
  bbox = AxisAlignedBox3(Vector3(MAXFLOAT),
                         Vector3(-MAXFLOAT));
  root = 0;
  epsilon = 0;

#ifdef __TRAVERSAL_STATISTICS
  _allNodesTraversed = 0L;
  _fullLeavesTraversed = 0L;
  _emptyLeavesTraversed = 0L;
#endif
}
  
// Here we find the node (preferably minbox node) containing
// the point
const SKTBNodeT*
CKTBTraversal::Locate(const Vector3 &position)
{
  const SKTBNodeT *current, *next = root;
  const SKTBNodeT *returnNode = 0;

  do {
    current = next;
    switch (GetNodeType(current)) {
      // -------------------------------------------------
      case CKTBAxes::EE_X_axis: {
        if (position[CKTBAxes::EE_X_axis] < current->splitPlane)
          next =  GetLeft(current);
        else
          next = GetRight(current);
        break;
      }
      case CKTBAxes::EE_Y_axis: {
        if (position[CKTBAxes::EE_Y_axis] < current->splitPlane)
          next =  GetLeft(current);
        else
          next = GetRight(current);
        break;
      }
      case CKTBAxes::EE_Z_axis: {
        if (position[CKTBAxes::EE_Z_axis] < current->splitPlane)
          next =  GetLeft(current);
        else
          next = GetRight(current);
        break;
      }
      case CKTBAxes::EE_X_axisBox: {
        if (position[CKTBAxes::EE_X_axis] < current->splitPlane)
          next =  GetLeft(current);
        else
          next = GetRight(current);
        returnNode = current;
        break;
      }
      case CKTBAxes::EE_Y_axisBox: {
        if (position[CKTBAxes::EE_Y_axis] < current->splitPlane)
          next =  GetLeft(current);
        else
          next = GetRight(current);
        returnNode = current;
        break;
      }
      case CKTBAxes::EE_Z_axisBox: {
        if (position[CKTBAxes::EE_Z_axis] < current->splitPlane)
          next =  GetLeft(current);
        else
          next = GetRight(current);
        returnNode = current;
        break;
      }
      case CKTBAxes::EE_Leaf: {
        next = 0; // finishing
        break;
      }
      case CKTBAxes::EE_Link:{
        next = GetLinkNode(current);
        // cout << "Link node was accessed" << endl;
        break;
      }
    } // switch
  }
  while (next != NULL);

  if (returnNode)
    return returnNode;

  // return the last (leaf node) visited on the path
  return current;
}
#endif
  
  
#if 1
int
CKTBTraversal::FindNearestI(const SimpleRay &ray)
{
#if 0
  static int counter = 0;
  counter++;
  bool debug = false;
  if (counter == 530) {
    debug = true;
    cout << "COUNTER = " << counter << endl;
    cout << "DEBUG starts" << endl;    
  }
#endif
  
  // passing through parameters
  float tmin, tmax;
  
  // test if the whole CKTB tree is missed by the input ray
  if ( (!root) ||
       (!bbox.ComputeMinMaxT(ray.mOrigin, ray.mDirection, &tmin, &tmax)) ||
       (tmax <= tmin) ||
       (tmax <= 0.f) ) {
    SimpleRay::IntersectionRes[0].intersectable = 0;
    return 0; // no object can be intersected
  }

//#define _DEBUGKTB
#ifdef _DEBUGKTB
  int ib = 0;
  int depth = 0; 
#endif
  
#ifdef __TRAVERSAL_STATISTICS
  int allNodesTraversed = 0L;
  int fullLeavesTraversed = 0L;
  int emptyLeavesTraversed = 0L;
#endif // __TRAVERSAL_STATISTICS

  Vector3 invertedDir;
  invertedDir.x = 1.0f / ray.mDirection.x;
  invertedDir.y = 1.0f / ray.mDirection.y;
  invertedDir.z = 1.0f / ray.mDirection.z;
  
  // start from the root node
  if (tmin < 0.f)
    tmin = 0.f;

  int index = 1;
  stack3[1].nodep = root;
  stack3[1].tmax = tmax;
  tmax = tmin;
  SKTBNodeT * childNodes[2];
  int RayDirs[3];
  RayDirs[0] = ray.mDirection.x < 0.f ? 1 : 0;
  RayDirs[1] = ray.mDirection.y < 0.f ? 1 : 0;
  RayDirs[2] = ray.mDirection.z < 0.f ? 1 : 0;
  
  // we have to check the node
  // current node is not the leaf, empty leaves are NULL pointers
  while (index) {
    register SKTBNodeT *currNode = stack3[index].nodep;
    tmin = tmax;
    tmax = stack3[index].tmax;
#if 0
    if (debug) {
      cout << "node = " << (void*)currNode
           << " tmin = " << tmin << " tmax = " << tmax
           << endl;
    }
#endif
    
    assert(tmin <= tmax);
    
#ifdef __TRAVERSAL_STATISTICS
    allNodesTraversed++;
#endif // __TRAVERSAL_STATISTICS
    register const int nodeType = GetNodeType(currNode);
        
    if (nodeType < CKTBAxes::EE_Leaf) {
      float tval = (GetSplitValue(currNode) - ray.mOrigin[nodeType]);
      tval *= invertedDir[nodeType];
      SKTBNodeT *near, *far;
      childNodes[0] = GetLeft(currNode);
      childNodes[1] = GetRight(currNode);
      int rayDir = RayDirs[nodeType];
      near = childNodes[rayDir];
      far = childNodes[rayDir ^ 0x1];
      
      stack3[index].nodep = far;
      // stack3[index].tmax = tmax; // not necessary, this is already there !
      // int c = tval < tmax ? 1 : 0;
      index += tval < tmax ? 1 : 0;
      stack3[index].nodep = near;
      stack3[index].tmax = Min(tval, tmax);
      tmax = tmin;
      // int d = tval < tmin ? -1 : 0;
      index += tval < tmin ? -1 : 0;
    }
    else {
      if (nodeType == CKTBAxes::EE_Leaf) {
        // test objects for intersection
#ifdef _DEBUGKTB
        cout << "Leaf " << endl;
        depth++;
#endif
#ifdef _DEBUGKTB
        DEBUG << "currNode = " << currNode << " entp.t = " << entp->t
              << " extp.t = " << extp->t << endl;
#endif
        if (!IsEmptyLeaf_(currNode)) {
#ifdef _DEBUGKTB
          cout << "Full leaf at depth= " << depth << endl; 
#endif      

#ifdef __TRAVERSAL_STATISTICS
          fullLeavesTraversed++;
#endif // __TRAVERSAL_STATISTICS
          // test the objects in the full leaf against the ray
          
          SimpleRay::IntersectionRes[0].maxt = stack3[index].tmax;
#if 0
          // using subroutine
          if (TestFullLeaf(ray, currNode))
#else
          // Avoiding function call by copying the code
          const ObjectContainer * const list = GetObjList(currNode); 
          int intersected = 0;
          // iterate the whole list and find out the nearest intersection
          ObjectContainer::const_iterator sc_end = list->end();
          for (ObjectContainer::const_iterator sc = list->begin(); sc != sc_end; sc++) {
            // if the intersection realy lies in the node        
            intersected += ((*sc)->CastSimpleRay(ray));
          } // for all objects
          if (intersected)
#endif
          {
#ifdef _DEBUGKTB
            cout << "Full leaf HIT " << endl; 
#endif  
              
#ifdef __TRAVERSAL_STATISTICS
            _allNodesTraversed += allNodesTraversed;
            _fullLeavesTraversed += fullLeavesTraversed;
            _emptyLeavesTraversed += emptyLeavesTraversed;
#endif // __TRAVERSAL_STATISTICS
            
            // signed distance should be already set in TestFullLeaf
            // the first object intersected was found   
            return 1;
          }
        } // full leaf
#ifdef __TRAVERSAL_STATISTICS
        else {
#ifdef _DEBUGKTB
          cout << "Empty leaf at depth= " << depth << endl; 
#endif      
          emptyLeavesTraversed++;
        }
#endif // __TRAVERSAL_STATISTICS

#ifdef _DEBUGKTB
        cout << "Pop the node" << endl;
#endif      
    
        // pop farChild from the stack
        // restore the current values
        index--;
        continue;
      }
      else {
        assert(nodeType == CKTBAxes::EE_Link);
#ifdef _DEBUGKTB
        cout << "Link " << endl;
#endif
        stack3[index].nodep = GetLinkNode(currNode);
        // cout << "Link node was accessed" << endl;
        continue;
      }
    }
  } // while current node is not the leaf

#ifdef __TRAVERSAL_STATISTICS
  _allNodesTraversed += allNodesTraversed;
  _fullLeavesTraversed += fullLeavesTraversed;
  _emptyLeavesTraversed += emptyLeavesTraversed;
#endif // __TRAVERSAL_STATISTICS

  // no objects found along the ray path
  return 0;
} // FindNearestI - single ray
#endif

#if 1
int
CKTBTraversal::FindNearestI(const SimpleRay &ray, const AxisAlignedBox3 &localbox)
{
#if 0
  static int counter = 0;
  counter++;
  bool debug = false;
  if (counter == 530) {
    debug = true;
    cout << "COUNTER = " << counter << endl;
    cout << "DEBUG starts" << endl;    
  }
#endif
  
  // passing through parameters
  float tmin, tmax;
  
  // test if the whole CKTB tree is missed by the input ray
  if ( (!root) ||
       (!localbox.ComputeMinMaxT(ray.mOrigin, ray.mDirection, &tmin, &tmax)) ||
       (tmax <= tmin) ||
       (tmax <= 0.f) ) {
    SimpleRay::IntersectionRes[0].intersectable = 0;
    return 0; // no object can be intersected
  }

//#define _DEBUGKTB
#ifdef _DEBUGKTB
  int ib = 0;
  int depth = 0; 
#endif
  
#ifdef __TRAVERSAL_STATISTICS
  int allNodesTraversed = 0L;
  int fullLeavesTraversed = 0L;
  int emptyLeavesTraversed = 0L;
#endif // __TRAVERSAL_STATISTICS

  Vector3 invertedDir;
  invertedDir.x = 1.0f / ray.mDirection.x;
  invertedDir.y = 1.0f / ray.mDirection.y;
  invertedDir.z = 1.0f / ray.mDirection.z;
  
  // start from the root node
  if (tmin < 0.f)
    tmin = 0.f;

  int index = 1;
  stack3[1].nodep = root;
  stack3[1].tmax = tmax;
  tmax = tmin;
  SKTBNodeT * childNodes[2];
  int RayDirs[3];
  RayDirs[0] = ray.mDirection.x < 0.f ? 1 : 0;
  RayDirs[1] = ray.mDirection.y < 0.f ? 1 : 0;
  RayDirs[2] = ray.mDirection.z < 0.f ? 1 : 0;
  
  // we have to check the node
  // current node is not the leaf, empty leaves are NULL pointers
  while (index) {
    register SKTBNodeT *currNode = stack3[index].nodep;
    tmin = tmax;
    tmax = stack3[index].tmax;
#if 0
    if (debug) {
      cout << "node = " << (void*)currNode
           << " tmin = " << tmin << " tmax = " << tmax
           << endl;
    }
#endif
    
    assert(tmin <= tmax);
    
#ifdef __TRAVERSAL_STATISTICS
    allNodesTraversed++;
#endif // __TRAVERSAL_STATISTICS
    register const int nodeType = GetNodeType(currNode);
    if (nodeType < CKTBAxes::EE_Leaf) {
      float tval = (GetSplitValue(currNode) - ray.mOrigin[nodeType]);
      tval *= invertedDir[nodeType];
      SKTBNodeT *near, *far;
      childNodes[0] = GetLeft(currNode);
      childNodes[1] = GetRight(currNode);
      int rayDir = RayDirs[nodeType];
      near = childNodes[rayDir];
      far = childNodes[rayDir ^ 0x1];
      
      stack3[index].nodep = far;
      // stack3[index].tmax = tmax; // not necessary, this is already there !
      // int c = tval < tmax ? 1 : 0;
      index += tval < tmax ? 1 : 0;
      stack3[index].nodep = near;
      stack3[index].tmax = Min(tval, tmax);
      tmax = tmin;
      // int d = tval < tmin ? -1 : 0;
      index += tval < tmin ? -1 : 0;
    }
    else {
      if (nodeType == CKTBAxes::EE_Leaf) {
        // test objects for intersection
#ifdef _DEBUGKTB
        cout << "Leaf " << endl;
        depth++;
#endif
#ifdef _DEBUGKTB
        DEBUG << "currNode = " << currNode << " entp.t = " << entp->t
              << " extp.t = " << extp->t << endl;
#endif
        if (!IsEmptyLeaf_(currNode)) {
#ifdef _DEBUGKTB
          cout << "Full leaf at depth= " << depth << endl; 
#endif      

#ifdef __TRAVERSAL_STATISTICS
          fullLeavesTraversed++;
#endif // __TRAVERSAL_STATISTICS
          // test the objects in the full leaf against the ray
          
          SimpleRay::IntersectionRes[0].maxt = stack3[index].tmax;
#if 0
          // using subroutine
          if (TestFullLeaf(ray, currNode))
#else
          // Avoiding function call by copying the code
          const ObjectContainer * const list = GetObjList(currNode); 
          int intersected = 0;
          // iterate the whole list and find out the nearest intersection
          ObjectContainer::const_iterator sc_end = list->end();
          for (ObjectContainer::const_iterator sc = list->begin(); sc != sc_end; sc++) {
            // if the intersection realy lies in the node        
            intersected += ((*sc)->CastSimpleRay(ray));
          } // for all objects
          if (intersected)
#endif
          {
#ifdef _DEBUGKTB
            cout << "Full leaf HIT " << endl; 
#endif  
              
#ifdef __TRAVERSAL_STATISTICS
            _allNodesTraversed += allNodesTraversed;
            _fullLeavesTraversed += fullLeavesTraversed;
            _emptyLeavesTraversed += emptyLeavesTraversed;
#endif // __TRAVERSAL_STATISTICS
            
            // signed distance should be already set in TestFullLeaf
            // the first object intersected was found   
            return 1;
          }
        } // full leaf
#ifdef __TRAVERSAL_STATISTICS
        else {
#ifdef _DEBUGKTB
          cout << "Empty leaf at depth= " << depth << endl; 
#endif      
          emptyLeavesTraversed++;
        }
#endif // __TRAVERSAL_STATISTICS

#ifdef _DEBUGKTB
        cout << "Pop the node" << endl;
#endif      
    
        // pop farChild from the stack
        // restore the current values
        index--;
        continue;
      }
      else {
        assert(nodeType == CKTBAxes::EE_Link);
#ifdef _DEBUGKTB
        cout << "Link " << endl;
#endif
        stack3[index].nodep = GetLinkNode(currNode);
        // cout << "Link node was accessed" << endl;
        continue;
      }
    }
  } // while current node is not the leaf

#ifdef __TRAVERSAL_STATISTICS
  _allNodesTraversed += allNodesTraversed;
  _fullLeavesTraversed += fullLeavesTraversed;
  _emptyLeavesTraversed += emptyLeavesTraversed;
#endif // __TRAVERSAL_STATISTICS

  // no objects found along the ray path
  return 0;
} // FindNearestI - single ray
#endif
  

// ---------------------------------------------------------------------------
// This is an attempt using SSE instructions - not successfull 31/12/2007
#if 0
int
CKTBTraversal::FindNearestI(const SimpleRay &ray)
{
#if 0
  static int counter = 0;
  counter++;
  bool debug = false;
  if (counter == 530) {
    debug = true;
    cout << "COUNTER = " << counter << endl;
    cout << "DEBUG starts" << endl;    
  }
#endif
  
  // passing through parameters
  float tmin, tmax;
  
  // test if the whole CKTB tree is missed by the input ray
  if ( (!root) ||
       (!bbox.ComputeMinMaxT(ray.mOrigin, ray.mDirection, &tmin, &tmax)) ||
       (tmax <= tmin) ||
       (tmax <= 0.f) ) {
    SimpleRay::IntersectionRes[0].intersectable = 0;
    return 0; // no object can be intersected
  }

//#define _DEBUGKTB
#ifdef _DEBUGKTB
  int ib = 0;
  int depth = 0; 
#endif
  
#ifdef __TRAVERSAL_STATISTICS
  int allNodesTraversed = 0L;
  int fullLeavesTraversed = 0L;
  int emptyLeavesTraversed = 0L;
#endif // __TRAVERSAL_STATISTICS

  Vector3 invertedDir;
  invertedDir.x = 1.0f / ray.mDirection.x;
  invertedDir.y = 1.0f / ray.mDirection.y;
  invertedDir.z = 1.0f / ray.mDirection.z;
  
  // start from the root node
  if (tmin < 0.f)
    tmin = 0.f;

  int index = 1;
  stack3[1].nodep = root;
  stack3[1].tmax = tmax;
  tmax = tmin;
  SKTBNodeT * childNodes[2];
  int RayDirs[3];
  RayDirs[0] = ray.mDirection.x < 0.f ? 1 : 0;
  RayDirs[1] = ray.mDirection.y < 0.f ? 1 : 0;
  RayDirs[2] = ray.mDirection.z < 0.f ? 1 : 0;
  
  // we have to check the node
  // current node is not the leaf, empty leaves are NULL pointers
  while (index) {
    SKTBNodeT *currNode = stack3[index].nodep;
    tmin = tmax;
    tmax = stack3[index].tmax;
#if 0
    if (debug) {
      cout << "node = " << (void*)currNode
           << " tmin = " << tmin << " tmax = " << tmax
           << endl;
    }
#endif
    
    assert(tmin <= tmax);
    
#ifdef __TRAVERSAL_STATISTICS
    allNodesTraversed++;
#endif // __TRAVERSAL_STATISTICS
    const unsigned int nodeType = GetNodeType(currNode);
    if (nodeType < CKTBAxes::EE_Leaf) {
      float tval = (GetSplitValue(currNode) - ray.mOrigin[nodeType]);
      tval *= invertedDir[nodeType];
      SKTBNodeT *near, *far;
      childNodes[0] = GetLeft(currNode);
      childNodes[1] = GetRight(currNode);
      int rayDir = RayDirs[nodeType];
      near = childNodes[rayDir];
      far = childNodes[rayDir ^ 0x1];
      // This code is slower than above !!!!!
      stack3[index].nodep = far;
      // stack3[index].tmax = tmax; // this is already there, not necessary!
      __m128 tsim = _mm_set_ss(tval);
      // index += tval < tmax ? 1 : 0;
      __m128 tother = _mm_set_ss(tmax);
      index += _mm_ucomilt_ss(tsim, tother);
      stack3[index].nodep = near;
      // stack3[index].tmax = Min(tval, tmax);
      _mm_store_ss(&(stack3[index].tmax), _mm_min_ps(tsim, tother));
      tmax = tmin;
      // index += tval < tmin ? -1 : 0;
      __m128 tother2 = _mm_set_ss(tmin);
      index -= _mm_ucomilt_ss(tsim, tother2);
    }
    else {
      if (nodeType == CKTBAxes::EE_Leaf) {
        // test objects for intersection
#ifdef _DEBUGKTB
        cout << "Leaf " << endl;
        depth++;
#endif
#ifdef _DEBUGKTB
        DEBUG << "currNode = " << currNode << " entp.t = " << entp->t
              << " extp.t = " << extp->t << endl;
#endif
        if (!IsEmptyLeaf_(currNode)) {
#ifdef _DEBUGKTB
          cout << "Full leaf at depth= " << depth << endl; 
#endif      

#ifdef __TRAVERSAL_STATISTICS
          fullLeavesTraversed++;
#endif // __TRAVERSAL_STATISTICS
          // test the objects in the full leaf against the ray
          
          SimpleRay::IntersectionRes[0].maxt = stack3[index].tmax;
          if (TestFullLeaf(ray, currNode)) {
#ifdef _DEBUGKTB
            cout << "Full leaf HIT " << endl; 
#endif  
              
#ifdef __TRAVERSAL_STATISTICS
            _allNodesTraversed += allNodesTraversed;
            _fullLeavesTraversed += fullLeavesTraversed;
            _emptyLeavesTraversed += emptyLeavesTraversed;
#endif // __TRAVERSAL_STATISTICS
            
            // signed distance should be already set in TestFullLeaf
            // the first object intersected was found   
            return 1;
          }
        } // full leaf
#ifdef __TRAVERSAL_STATISTICS
        else {
#ifdef _DEBUGKTB
          cout << "Empty leaf at depth= " << depth << endl; 
#endif      
          emptyLeavesTraversed++;
        }
#endif // __TRAVERSAL_STATISTICS

#ifdef _DEBUGKTB
        cout << "Pop the node" << endl;
#endif      
    
        // pop farChild from the stack
        // restore the current values
        index--;
        continue;
      }
      else {
        assert(nodeType == CKTBAxes::EE_Link);
#ifdef _DEBUGKTB
        cout << "Link " << endl;
#endif
        stack3[index].nodep = GetLinkNode(currNode);
        // cout << "Link node was accessed" << endl;
        continue;
      }
    }
  } // while current node is not the leaf

#ifdef __TRAVERSAL_STATISTICS
  _allNodesTraversed += allNodesTraversed;
  _fullLeavesTraversed += fullLeavesTraversed;
  _emptyLeavesTraversed += emptyLeavesTraversed;
#endif // __TRAVERSAL_STATISTICS

  // no objects found along the ray path
  return 0;
} // FindNearestI - single ray
#endif

#if 0
// Reasonably fast - about 101,500 rays per second for single dir!
// It allows fast switching context from one ray to the next ray so it it
// virtually independent of memory latency !
int
CKTBTraversal::FindNearestI_16oneDir(SimpleRayContainer &rays, int offset)
{
  // passing through parameters
  int cntRays = 0;
  if (!root)
    return 0;

  // The auxiliary variables to be precomputed
  static float invertedDir[cntMaxRays * 4];
  static float rayOrig[cntMaxRays * 4];
  static int rayDirs[cntMaxRays * 4];
  static int indexRay[cntMaxRays];
  static int indexArray[cntMaxRays];
  static int indexStack[cntMaxRays];
  const int LOG2_MAX_HEIGHT = 5;
  const int MAX_HEIGHT = 1 << LOG2_MAX_HEIGHT;
  assert(MAX_HEIGHT == 32);
  static struct SStackElem3 stackA[cntMaxRays * MAX_HEIGHT];
  static float tmaxArray[cntMaxRays];
  int cntHits = 0;
  
  float tmin, tmax;
  for (int i = 0; i < cntMaxRays; i++) {
  // test if the whole CKTB tree is missed by the input ray
    if ((!bbox.ComputeMinMaxT(rays[i+offset].mOrigin,
                              rays[i+offset].mDirection,
                              &tmin, &tmax)) ||
        (tmax <= tmin) ||
        (tmax <= 0.f) ) {
      SimpleRay::IntersectionRes[i].intersectable = 0;
    }
    else {
      int indexR = (cntRays << 2);
      rayOrig[indexR + 0] = rays[i+offset].mOrigin.x;
      rayOrig[indexR + 1] = rays[i+offset].mOrigin.y;
      rayOrig[indexR + 2] = rays[i+offset].mOrigin.z;
      //rayOrig[indexR + 3] = 0.f;
      invertedDir[indexR + 0] = 1.0f / (rays[i+offset].mDirection.x);
      invertedDir[indexR + 1] = 1.0f / (rays[i+offset].mDirection.y);
      invertedDir[indexR + 2] = 1.0f / (rays[i+offset].mDirection.z);
      //invertedDir[indexR + 2] = 0.f;
      rayDirs[indexR + 0] = rays[i+offset].mDirection.x < 0.f ? 1 : 0;
      rayDirs[indexR + 1] = rays[i+offset].mDirection.y < 0.f ? 1 : 0;
      rayDirs[indexR + 2] = rays[i+offset].mDirection.z < 0.f ? 1 : 0;
      //rayDirs[indexR + 3] = 0; 
      indexRay[cntRays] = i; // the index to the ray
      indexArray[cntRays] = cntRays; // the index to the array
      int indexS = (cntRays << LOG2_MAX_HEIGHT) + 1;
      indexStack[cntRays] = indexS; // the index in the stack
      stackA[indexS].nodep = root; // we start from the root
      stackA[indexS].tmax = tmax; // maximum distance
      if (tmin < 0.f) tmin = 0.f;
      tmaxArray[cntRays] = tmin;
      cntRays++;
    }
  }

//#define _DEBUGKTB
#ifdef _DEBUGKTB
  int ib = 0;
  int depth = 0;
#endif
  
#ifdef __TRAVERSAL_STATISTICS
  int allNodesTraversed = 0L;
  int fullLeavesTraversed = 0L;
  int emptyLeavesTraversed = 0L;
#endif // __TRAVERSAL_STATISTICS
  
  SKTBNodeT * childNodes[2];

#define PREF_DEFAULT _MM_HINT_T0
  
  // we have to check the node
  // current node is not the leaf, empty leaves are NULL pointers
  while (cntRays) {
    // we assume that all the nodes are interior nodes
    for (int i = 0; i < cntRays; i++) {
      // which indices to array should be used
      int indexA = indexArray[i];
      float tmin = tmaxArray[indexA];

      // the stack indexing is here
      int indexSA = indexStack[indexA];
      SKTBNodeT *currNode = stackA[indexSA].nodep;
      
#if 0
      if (debug) {
        cout << "node = " << (void*)currNode
             << " tmin = " << tmin << " tmax = " << tmax
             << endl;
      }
#endif

#if 0
      if (tmin > tmax) {
        cout << "PROBLEM tmin = " << tmin << " tmax = " << tmax;
        cout << endl;
      }
#endif      
      assert(tmin <= tmax);
    
#ifdef __TRAVERSAL_STATISTICS
      allNodesTraversed++;
#endif // __TRAVERSAL_STATISTICS
      const unsigned int nodeType = (GetNodeType(currNode)) & 0x7;
      if (nodeType < CKTBAxes::EE_Leaf) {
        int indexRayOD = (indexA << 2) + nodeType;
        float tval = (GetSplitValue(currNode) - rayOrig[indexRayOD])
          * invertedDir[indexRayOD];
        SKTBNodeT *near, *far;
        childNodes[0] = GetLeft(currNode);
        childNodes[1] = GetRight(currNode);
        int rayDir = rayDirs[indexRayOD];
        near = childNodes[rayDir];
        far = childNodes[rayDir ^ 0x1];
        
        stackA[indexSA].nodep = far; // store far node
        //stackA[indexSA].tmax = tmax; // with correct dist - the same as before
        float tmax = tmaxArray[indexA] = stackA[indexSA].tmax;
        int c = tval < tmax ? 1 : 0;
        indexSA += c;
        stackA[indexSA].nodep = near; // store near node
        stackA[indexSA].tmax = Min(tval, tmax); // with correct dist
        int d = tval < tmin ? 1 : 0;
        indexSA -= d;
        // This is prefetching - so the next time we have it
        // in the cache !
        GPREFETCH(stackA[indexSA].nodep, PREF_DEFAULT);
        // store tmax and index to the stack
        indexStack[indexA] = indexSA;
        tmaxArray[indexA] = tmin;       
      }
      else {
        if (nodeType == CKTBAxes::EE_Leaf) {
          // test objects for intersection
#ifdef _DEBUGKTB
          cout << "Leaf " << endl;
          depth++;
#endif
#ifdef _DEBUGKTB
          DEBUG << "currNode = " << currNode << " entp.t = " << entp->t
                << " extp.t = " << extp->t << endl;
#endif
          float tmax = tmaxArray[indexA] = stackA[indexSA].tmax;
          if (!IsEmptyLeaf_(currNode)) {
#ifdef _DEBUGKTB
            cout << "Full leaf at depth= " << depth << endl; 
#endif      

#ifdef __TRAVERSAL_STATISTICS
            fullLeavesTraversed++;
#endif // __TRAVERSAL_STATISTICS
          // test the objects in the full leaf against the ray
          
            // which ray is processed
            int indexR = indexRay[indexA];
            SimpleRay::IntersectionRes[indexR].maxt = tmax;
            if (TestFullLeaf(rays[indexR+offset], currNode, indexR)) {

              // we remove the ray from the calculation
              indexArray[i] = indexArray[cntRays-1];
              cntRays--; // we decrease the number of rays
              cntHits++;
#ifdef _DEBUGKTB
              cout << "Full leaf HIT " << endl; 
#endif  
              
#ifdef __TRAVERSAL_STATISTICS
              _allNodesTraversed += allNodesTraversed;
              _fullLeavesTraversed += fullLeavesTraversed;
              _emptyLeavesTraversed += emptyLeavesTraversed;
#endif // __TRAVERSAL_STATISTICS            
            
              // signed distance should be already set in TestFullLeaf
              // the first object intersected was found 
              continue;
            }
          } // full leaf
#ifdef __TRAVERSAL_STATISTICS
          else {
#ifdef _DEBUGKTB
            cout << "Empty leaf at depth= " << depth << endl; 
#endif      
            emptyLeavesTraversed++;
          }
#endif // __TRAVERSAL_STATISTICS

#ifdef _DEBUGKTB
          cout << "Pop the node" << endl;
#endif      
    
          // pop farChild from the stack
          // restore the current values
          --indexSA;
          // This is bits 0,1,2,3,4,5 - the stack depth = 32 !
          if ( (indexSA & 0x1f) == 0) {
            // we remove the ray from the calculation
            indexArray[i] = indexArray[cntRays-1];
            cntRays--; // we decrease the number of rays
          }
          else {
            indexStack[indexA] = indexSA;
            // we prefetch the data to be accessible
            // the next time
            GPREFETCH(stackA[indexSA].nodep, PREF_DEFAULT);
          }
          continue;
        }
        else {
          assert(nodeType == CKTBAxes::EE_Link);
#ifdef _DEBUGKTB
          cout << "Link " << endl;
#endif
          stackA[indexSA].nodep = GetLinkNode(currNode);
          // cout << "Link node was accessed" << endl;
          continue;
        }
      } // empty leaf or link
    } // for all active rays
  } // while cntRays

#ifdef __TRAVERSAL_STATISTICS
  _allNodesTraversed += allNodesTraversed;
  _fullLeavesTraversed += fullLeavesTraversed;
  _emptyLeavesTraversed += emptyLeavesTraversed;
#endif // __TRAVERSAL_STATISTICS

  // no objects found along the ray path
  return cntHits;
}
#endif

#ifdef __SSE__

#if 1
// Even faster - about 125,500 rays per second for single dir and 164 rps
// for double dir !
int
CKTBTraversal::FindNearestI_16oneDir(SimpleRayContainer &rays, int offset)
{
  static RayPacket2x2 raypack;
  struct SResultI {
    Intersectable *intersectable;
    float          tdist;    
  };
  static SResultI results[16];
    
  for (int i = 0; i < 4; i++) {
    int k = i * 4 + offset;
    for (int j = 0; j < 4; j++, k++) {
      raypack.SetLoc(j, rays[k].mOrigin);
      raypack.SetDir(j, rays[k].mDirection);      
    }
    // Here either use ray packet traversal or
    // casting individual rays
    FindNearestI(raypack);
    k = i * 4;
    for (int j = 0; j < 4; j++, k++) {
      results[k].intersectable = raypack.GetObject(j);
      results[k].tdist = raypack.GetT(j);
    } // for j
  } // for i

  // Copy the results to the output array
  for (int i = 0; i < 16; i++) {
    SimpleRay::IntersectionRes[i].intersectable =
      results[i].intersectable;
    SimpleRay::IntersectionRes[i].tdist =
      results[i].tdist;
  } // for i
  return 0;
}
#endif
  
#if 0
// This code works well 1/1/2008 - 11:00
// The same operations for packets of rays for the same signs,
// otherwise it is emulated by decomposition
// of a packet to individual rays and traced individually.
void
CKTBTraversal::FindNearestI(RayPacket2x2 &rp)
{
  int m1 = _mm_movemask_ps(rp.dx4);
  if ((m1 == 0)||(m1 == 15)) {
  m1 = _mm_movemask_ps(rp.dy4);
  if ((m1 == 0)||(m1 == 15)) {
  m1 = _mm_movemask_ps(rp.dz4);
  if ((m1 == 0)||(m1 == 15)) {
    rp.Init();
    // all the signs for 4 rays are the same, use
    // ray packet traversal
    // Compute min and max distances
    GALIGN16 union { float tmin4[4]; __m128 tmin_4; };
    GALIGN16 union { float tmax4[4]; __m128 tmax_4; };
    SimpleRay sray[4];
    int maxIntersections = 4;
    GALIGN16 union { int inters[4]; __m128 inters_4; };
    inters[0] = inters[1] = inters[2] = inters[3] = 1;
    unsigned int inters32 = 0xf;
    for (int i = 0; i < 4; i++) {
      bbox.ComputeMinMaxT(rp.GetLoc(i), rp.GetDir(i), &(tmin4[i]), &(tmax4[i]));
      if ( (tmin4[i] >= tmax4[i]) ||
           (tmax4[i] < 0.f) ) {
        inters[i] = 0; // finished
        inters32 &= ~(1 << i); // bit zero when ray is invalid
        maxIntersections--;
      }      
      if (tmin4[i] < 0.f)
        tmin4[i] = 0.f;
      sray[i].mOrigin = rp.GetLoc(i);
      sray[i].mDirection = rp.GetDir(i);
    } // for i
    if (maxIntersections == 0)
      return;

    SKTBNodeT * childNodes[2];
    int RayDirs[3];
    RayDirs[0] = (rp.dx[0] > 0.f) ? 1 : 0;
    RayDirs[1] = (rp.dy[0] > 0.f) ? 1 : 0;
    RayDirs[2] = (rp.dz[0] > 0.f) ? 1 : 0;
    //int activeMask=_mm_movemask_ps(_mm_cmplt_ps( tmin_4, tmax_4 ))&inters32;
    int activeMask = inters32;
    int indexStack = 0;
    SKTBNodeT *currNode = root;
    unsigned int k = GetNodeType(currNode);
    for (;;) {      
      while (k < CKTBAxes::EE_Leaf) {
        // the 3 operations below can be brought down to 3 simple float
        // calculations by precomputing min/max of the inverse dir
        const __m128 node_split = _mm_set_ps1(GetSplitValue(currNode));
        const __m128 t4 =
          _mm_mul_ps(_mm_sub_ps(node_split, rp.orig[k]), rp.idir[k]);
        childNodes[0] = GetLeft(currNode);
        childNodes[1] = GetRight(currNode);
        int rayDir = RayDirs[k];
        SKTBNodeT *far = childNodes[rayDir];
        if (!(_mm_movemask_ps(_mm_cmpgt_ps(t4, tmin_4)) & activeMask))
        {
          currNode = far;
          k = GetNodeType(currNode);
          continue;
        }
        currNode = childNodes[rayDir ^ 0x1]; // this is near node
        k = GetNodeType(currNode);      
        if (! (_mm_movemask_ps(_mm_cmplt_ps( t4, tmax_4)) & activeMask))
          continue;

        // pop far node to the stack
        stack4[indexStack].nodep = far;
        stack4[indexStack].tmax_4 = tmax_4;
        stack4[indexStack].tmin_4 = _mm_max_ps(t4, tmin_4);
        // stack4[indexStack].mask = activeMask;
        indexStack++;
        
        tmax_4 = _mm_min_ps(t4, tmax_4); 
        activeMask &= _mm_movemask_ps(_mm_cmplt_ps( tmin_4, tmax_4 ));
      } // while this is an interior node

      // either a leaf or a link
      if (k == CKTBAxes::EE_Leaf) {
        // test objects for intersection
        if (!IsEmptyLeaf_(currNode)) {
          // cout << "Full leaf" << endl;
          
          // test the objects in the full leaf against the ray    
          for (int i = 0; i < 4; i++) {
            if (inters[i] ) {
              // no intersection so far !
              SimpleRay::IntersectionRes[i].maxt = tmax4[i];
              // Test only rays that were not finished
              if (TestFullLeaf(sray[i], currNode, i)) {
                // intersection for this ray found
                inters[i] = 0;
                inters32 &= ~(1 << i);
                rp.SetT(i, SimpleRay::IntersectionRes[0].maxt);
                rp.SetObject(i, SimpleRay::IntersectionRes[0].intersectable);           
                // signed distance should be already set in TestFullLeaf
                // the first object intersected was found
                if (--maxIntersections == 0)
                  return;
              }
            } // if this ray did not hit the triangle so far
          } // for all 4 rays
        } // full leaf
        // pop farChild from the stack
        // restore the current values
        // update the minimum distance since we traverse to the next one

        if (indexStack == 0)
          return;
        indexStack--;
        currNode = stack4[indexStack].nodep;
        k = GetNodeType(currNode);
        tmin_4 = stack4[indexStack].tmin_4;
        tmax_4 = stack4[indexStack].tmax_4;
        activeMask = _mm_movemask_ps(_mm_cmple_ps( tmin_4, tmax_4 )) & inters32;
        continue;
      }
      // cout << "Link node was accessed" << endl;
      assert(k == CKTBAxes::EE_Link);
      currNode = GetLinkNode(currNode);
      k = GetNodeType(currNode);
    } // for
    return;
  }}}

  // Trace ray by ray
  SimpleRay ray;
  for (int i = 0; i < 4; i++) {
    ray.mOrigin = rp.GetLoc(i);
    ray.mDirection = rp.GetDir(i);
    FindNearestI(ray);
    rp.SetObject(i, SimpleRay::IntersectionRes[0].intersectable);
    rp.SetT(i, SimpleRay::IntersectionRes[0].tdist);
    // SimpleRay::IntersectionRes[0].intersectable->GetNormal(0);
  } // for
}
#endif


#if 1
// This code also works well 1/1/2008 - 14:00
// Using mask of 128-bits width - the code works as well, only a bit
// faster than the code above
void
CKTBTraversal::FindNearestI(RayPacket2x2 &rp)
{
  int m1 = _mm_movemask_ps(rp.dx4);
  if ((m1 == 0)||(m1 == 15)) {
  m1 = _mm_movemask_ps(rp.dy4);
  if ((m1 == 0)||(m1 == 15)) {
  m1 = _mm_movemask_ps(rp.dz4);
  if ((m1 == 0)||(m1 == 15)) {
    rp.Init();
    // all the signs for 4 rays are the same, use
    // ray packet traversal
    // Compute min and max distances
    GALIGN16 union { float tmin4[4]; __m128 tmin_4; };
    GALIGN16 union { float tmax4[4]; __m128 tmax_4; };
    GALIGN16 union { float activeMask[4]; __m128 activeMask_4; };
    GALIGN16 union { float liveMask[4]; __m128 liveMask_4; };
    liveMask[0] = liveMask[1] = liveMask[2] = liveMask[3] = 0xffffffff;

    GALIGN16 SimpleRay sray[4];
    int maxIntersections = 4;
    // unsigned int inters32 = 0xf;
    for (int i = 0; i < 4; i++) {
      rp.SetObject(i, 0);
      bbox.ComputeMinMaxT(rp.GetLoc(i), rp.GetDir(i), &(tmin4[i]), &(tmax4[i]));
      if ( (tmin4[i] >= tmax4[i]) ||
           (tmax4[i] < 0.f) ) {
        liveMask[i] = 0; // finished
        // inters32 &= ~(1 << i); // bit zero when ray is invalid
        maxIntersections--;
      }
      if (tmin4[i] < 0.f)
        tmin4[i] = 0.f;
      sray[i].mOrigin = rp.GetLoc(i);
      sray[i].mDirection = rp.GetDir(i);
    } // for i
    if (maxIntersections == 0)
      return;

    // This is the mask 128 bits witdth
    //activeMask_4 =
    //  _mm_and_ps(_mm_cmple_ps(tmin_4, tmax_4),
    //           _mm_cmplt_ps(tmax_4, _mm_setzero_ps()));
    activeMask_4 = liveMask_4;

    SKTBNodeT * childNodes[2];
    int RayDirs[4];
    RayDirs[0] = (rp.dx[0] > 0.f) ? 1 : 0;
    RayDirs[1] = (rp.dy[0] > 0.f) ? 1 : 0;
    RayDirs[2] = (rp.dz[0] > 0.f) ? 1 : 0;
    int indexStack = 0;
    SKTBNodeT *currNode = root;
    unsigned int k = GetNodeType(currNode);
    for (;;) {
      // traverse until we find a leaf
      while (k < CKTBAxes::EE_Leaf) {
        // the 3 operations below can be brought down to 3 simple float
        // calculations by precomputing min/max of the inverse dir
        // const __m128 node_split = ;
        const __m128 t4 =
          _mm_mul_ps(_mm_sub_ps(_mm_set_ps1(GetSplitValue(currNode)),
                                rp.orig[k]), rp.idir[k]);
        childNodes[0] = GetLeft(currNode);
        childNodes[1] = GetRight(currNode);
        int rayDir = RayDirs[k];
        SKTBNodeT *far = childNodes[rayDir];
        if (_mm_movemask_ps(_mm_and_ps(_mm_cmpge_ps(t4, tmin_4),
                                       activeMask_4))) {          
          currNode = far;
          k = GetNodeType(currNode);
          continue;
        }

        currNode = childNodes[rayDir ^ 0x1]; // this is near node
        k = GetNodeType(currNode);      
        if (_mm_movemask_ps(_mm_and_ps(_mm_cmple_ps(t4, tmax_4),
                                       activeMask_4)))
          continue;

        // pop far node to the stack
        stack4[indexStack].nodep = far;
        stack4[indexStack].tmax_4 = tmax_4;

// Uncomenting this macro is unsafe!
// Not convinced if for packet of 4 rays we can say that since when
// one ray is different than the others, it could bring to wrong state
// It is surely true for one ray when tmin < t < tmax, but for a packet
// of rays this condition can be true only for a single ray
//   tmin4 = max(t4, tmin4) = min(t4, tmax4)
//#define _NOT_STORE_MINT

#ifdef _NOT_STORE_MINT  
#else
        // store mint onto the stack
        stack4[indexStack].tmin_4 = _mm_max_ps(t4, tmin_4);
#endif  
        // stack4[indexStack].mask = activeMask;
        indexStack++;
        
        tmax_4 = _mm_min_ps(t4, tmax_4); 
        activeMask_4 = _mm_cmplt_ps( tmin_4, tmax_4 );
      } // while this is an interior node

      // either a leaf or a link
      if (k == CKTBAxes::EE_Leaf) {
        // test objects for intersection
        if (!IsEmptyLeaf_(currNode)) {
          // cout << "Full leaf" << endl;
          
          // test the objects in the full leaf against the ray    
          for (int i = 0; i < 4; i++) {
            if (liveMask[i] ) {
              // no intersection so far !
              SimpleRay::IntersectionRes[i].maxt = tmax4[i];
#if 0
              // Using subroutine
              // Test only rays that were not finished
              if (TestFullLeaf(sray[i], currNode, i))
#else
              // avoiding one call
              const ObjectContainer * const list = GetObjList(currNode); 
              int intersected = 0;
              // iterate the whole list and find out the nearest intersection
              ObjectContainer::const_iterator sc_end = list->end();
              for (ObjectContainer::const_iterator sc = list->begin(); sc != sc_end; sc++) {
                // if the intersection realy lies in the node        
                intersected |= ((*sc)->CastSimpleRay(sray[i], i));
              } // for all objects
              if (intersected)
#endif
              {
                rp.SetT(i, SimpleRay::IntersectionRes[0].maxt);
                rp.SetObject(i, SimpleRay::IntersectionRes[0].intersectable);
                // signed distance should be already set in TestFullLeaf
                // the first object intersected was found
                if (--maxIntersections == 0)
                  return;
                // inters32 &= ~(1 << i);
                liveMask[i] = 0;
              }
            } // if this ray did not hit the triangle so far
          } // for all 4 rays
        } // full leaf

        // pop farChild from the stack
        // restore the current values
        // update the minimum distance since we traverse to the next one
        do {
          if (indexStack == 0)
            return;
          indexStack--;
          currNode = stack4[indexStack].nodep;
          k = GetNodeType(currNode);
#ifdef _NOT_STORE_MINT
          // this is an attempt !
          tmin_4 = tmax_4;
#else
          // This surrely works
          tmin_4 = stack4[indexStack].tmin_4;
#endif    
          tmax_4 = stack4[indexStack].tmax_4;
          activeMask_4 = _mm_and_ps(_mm_cmple_ps( tmin_4, tmax_4 ), liveMask_4);
        }
        while (_mm_movemask_ps(activeMask_4) == 0);
      }
      else {
        // cout << "Link node was accessed" << endl;
        assert(k == CKTBAxes::EE_Link);
        currNode = GetLinkNode(currNode);
        k = GetNodeType(currNode);
      }
    } // for(;;)
    return;
  }}}

  // Trace ray by ray
  SimpleRay ray;
  for (int i = 0; i < 4; i++) {
    ray.mOrigin = rp.GetLoc(i);
    ray.mDirection = rp.GetDir(i);
    FindNearestI(ray);
    rp.SetObject(i, SimpleRay::IntersectionRes[0].intersectable);
    rp.SetT(i, SimpleRay::IntersectionRes[0].tdist);
    // SimpleRay::IntersectionRes[0].intersectable->GetNormal(0);
  } // for
}
#endif

#if 1
// This code also works well 1/1/2008 - 14:00
// Using mask of 128-bits width - the code works as well, only a bit
// faster than the code above
void
CKTBTraversal::FindNearestI(RayPacket2x2 &rp, Vector3 &boxmin, Vector3 &boxmax)
{

  static AxisAlignedBox3 localbox;
  localbox.SetMin(boxmin);
  localbox.SetMax(boxmax);

#define USE_SIMPLE_VERSION 1

#if !USE_SIMPLE_VERSION
  int m1 = _mm_movemask_ps(rp.dx4);
  if ((m1 == 0)||(m1 == 15)) {
  m1 = _mm_movemask_ps(rp.dy4);
  if ((m1 == 0)||(m1 == 15)) {
  m1 = _mm_movemask_ps(rp.dz4);
  if ((m1 == 0)||(m1 == 15)) {
    rp.Init();
    
    // all the signs for 4 rays are the same, use
    // ray packet traversal
    // Compute min and max distances
    GALIGN16 union { float tmin4[4]; __m128 tmin_4; };
    GALIGN16 union { float tmax4[4]; __m128 tmax_4; };
    GALIGN16 union { float activeMask[4]; __m128 activeMask_4; };
    GALIGN16 union { float liveMask[4]; __m128 liveMask_4; };
    liveMask[0] = liveMask[1] = liveMask[2] = liveMask[3] = 0xffffffff;

    GALIGN16 SimpleRay sray[4];
    int maxIntersections = 4;
    // unsigned int inters32 = 0xf;
    for (int i = 0; i < 4; i++) {
      rp.SetObject(i, 0);
      localbox.ComputeMinMaxT(rp.GetLoc(i), rp.GetDir(i), &(tmin4[i]), &(tmax4[i]));
      if ( (tmin4[i] >= tmax4[i]) ||
           (tmax4[i] < 0.f) ) {
        liveMask[i] = 0; // finished
        // inters32 &= ~(1 << i); // bit zero when ray is invalid
        maxIntersections--;
      }
      if (tmin4[i] < 0.f)
        tmin4[i] = 0.f;
      sray[i].mOrigin = rp.GetLoc(i);
      sray[i].mDirection = rp.GetDir(i);
    } // for i
    if (maxIntersections == 0)
      return;

    // This is the mask 128 bits witdth
    //activeMask_4 =
    //  _mm_and_ps(_mm_cmple_ps(tmin_4, tmax_4),
    //           _mm_cmplt_ps(tmax_4, _mm_setzero_ps()));
    activeMask_4 = liveMask_4;

    SKTBNodeT * childNodes[2];
    int RayDirs[4];
    RayDirs[0] = (rp.dx[0] > 0.f) ? 1 : 0;
    RayDirs[1] = (rp.dy[0] > 0.f) ? 1 : 0;
    RayDirs[2] = (rp.dz[0] > 0.f) ? 1 : 0;
    int indexStack = 0;
    SKTBNodeT *currNode = root;
    unsigned int k = GetNodeType(currNode);
    for (;;) {
      // traverse until we find a leaf
      while (k < CKTBAxes::EE_Leaf) {
        // the 3 operations below can be brought down to 3 simple float
        // calculations by precomputing min/max of the inverse dir
        // const __m128 node_split = ;
        const __m128 t4 =
          _mm_mul_ps(_mm_sub_ps(_mm_set_ps1(GetSplitValue(currNode)),
                                rp.orig[k]), rp.idir[k]);
        childNodes[0] = GetLeft(currNode);
        childNodes[1] = GetRight(currNode);
        int rayDir = RayDirs[k];
        SKTBNodeT *far = childNodes[rayDir];
        if (_mm_movemask_ps(_mm_and_ps(_mm_cmpge_ps(t4, tmin_4),
                                       activeMask_4))) {          
          currNode = far;
          k = GetNodeType(currNode);
          continue;
        }

        currNode = childNodes[rayDir ^ 0x1]; // this is near node
        k = GetNodeType(currNode);      
        if (_mm_movemask_ps(_mm_and_ps(_mm_cmple_ps(t4, tmax_4),
                                       activeMask_4)))
          continue;

        // pop far node to the stack
        stack4[indexStack].nodep = far;
        stack4[indexStack].tmax_4 = tmax_4;

// Uncomenting this macro is unsafe!
// Not convinced if for packet of 4 rays we can say that since when
// one ray is different than the others, it could bring to wrong state
// It is surely true for one ray when tmin < t < tmax, but for a packet
// of rays this condition can be true only for a single ray
//   tmin4 = max(t4, tmin4) = min(t4, tmax4)
//#define _NOT_STORE_MINT

#ifdef _NOT_STORE_MINT  
#else
        // store mint onto the stack
        stack4[indexStack].tmin_4 = _mm_max_ps(t4, tmin_4);
#endif  
        // stack4[indexStack].mask = activeMask;
        indexStack++;
        
        tmax_4 = _mm_min_ps(t4, tmax_4); 
        activeMask_4 = _mm_cmplt_ps( tmin_4, tmax_4 );
      } // while this is an interior node

      // either a leaf or a link
      if (k == CKTBAxes::EE_Leaf) {
        // test objects for intersection
        if (!IsEmptyLeaf_(currNode)) {
          // cout << "Full leaf" << endl;
          
          // test the objects in the full leaf against the ray    
          for (int i = 0; i < 4; i++) {
            if (liveMask[i] ) {
              // no intersection so far !
              SimpleRay::IntersectionRes[i].maxt = tmax4[i];
#if 0
              // Using subroutine
              // Test only rays that were not finished
              if (TestFullLeaf(sray[i], currNode, i))
#else
              // avoiding one call
              const ObjectContainer * const list = GetObjList(currNode); 
              int intersected = 0;
              // iterate the whole list and find out the nearest intersection
              ObjectContainer::const_iterator sc_end = list->end();
              for (ObjectContainer::const_iterator sc = list->begin(); sc != sc_end; sc++) {
                // if the intersection realy lies in the node        
                intersected |= ((*sc)->CastSimpleRay(sray[i], i));
              } // for all objects
              if (intersected)
#endif
              {
                rp.SetT(i, SimpleRay::IntersectionRes[0].maxt);
                rp.SetObject(i, SimpleRay::IntersectionRes[0].intersectable);
                // signed distance should be already set in TestFullLeaf
                // the first object intersected was found
                if (--maxIntersections == 0)
                  return;
                // inters32 &= ~(1 << i);
                liveMask[i] = 0;
              }
            } // if this ray did not hit the triangle so far
          } // for all 4 rays
        } // full leaf

        // pop farChild from the stack
        // restore the current values
        // update the minimum distance since we traverse to the next one
        do {
          if (indexStack == 0)
            return;
          indexStack--;
          currNode = stack4[indexStack].nodep;
          k = GetNodeType(currNode);
#ifdef _NOT_STORE_MINT
          // this is an attempt !
          tmin_4 = tmax_4;
#else
          // This surrely works
          tmin_4 = stack4[indexStack].tmin_4;
#endif    
          tmax_4 = stack4[indexStack].tmax_4;
          activeMask_4 = _mm_and_ps(_mm_cmple_ps( tmin_4, tmax_4 ), liveMask_4);
        }
        while (_mm_movemask_ps(activeMask_4) == 0);
      }
      else {
        // cout << "Link node was accessed" << endl;
        assert(k == CKTBAxes::EE_Link);
        currNode = GetLinkNode(currNode);
        k = GetNodeType(currNode);
      }
    } // for(;;)
    return;
  }}}
#endif
  // Trace ray by ray
  SimpleRay ray;
  for (int i = 0; i < 4; i++) {
    ray.mOrigin = rp.GetLoc(i);
    ray.mDirection = rp.GetDir(i);
    FindNearestI(ray, localbox);
    rp.SetObject(i, SimpleRay::IntersectionRes[0].intersectable);
    rp.SetT(i, SimpleRay::IntersectionRes[0].tdist);
    // SimpleRay::IntersectionRes[0].intersectable->GetNormal(0);
  } // for
}
#endif

#endif // __SSE__

#endif //  TRV00F

} // namespace