// ===================================================================
// $Id: $
//
// ktbtrav.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 "ktbtrav.h"
#include "Intersectable.h"

namespace GtpVisibilityPreprocessor {
  
//------------------------------------------------------------------
// Robust statistically optimized algorithm for CKTB ray-traversal
// original version 025. This algorithm was published at Journal of
// Graphics Tools, 1997, Havran et al.

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

#ifdef TRV000
int
CKTBTraversal::FindNearestI(const SimpleRay &ray) 
{
  // 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)) ||       
       (tmin >= tmax) ||
       (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

#ifdef _COMPUTE_INVERTED_DIR_KTB  
  Vector3 invertedDir;
  invertedDir.x = 1.0f / ray.mDirection.x;
  invertedDir.y = 1.0f / ray.mDirection.y;
  invertedDir.z = 1.0f / ray.mDirection.z;
#endif // _COMPUTE_INVERTED_DIR_KTB  

  // start from the root node
  SKTBNodeT *currNode = root;

  struct SStackElem *entp, *extp;
  GetStackPointers(entp, extp);
  //struct SStackElem *entp = &(stack[0]);
  //struct SStackElem *extp = &(stack[1]);

  // exit point setting for the box and the ray
  extp->x = ray.mOrigin.x + ray.mDirection.x * tmax;
  extp->y = ray.mOrigin.y + ray.mDirection.y * tmax;
  extp->z = ray.mOrigin.z + ray.mDirection.z * tmax;
  extp->nodep = NULL;
  extp->prev = NULL;
  extp->t = tmax;

  // entry point setting for the box and the ray
  entp->nodep = NULL;
  entp->prev = NULL;
  if (tmin > 0.0) { // external node
    entp->x = ray.mOrigin.x + ray.mDirection.x * tmin;
    entp->y = ray.mOrigin.y + ray.mDirection.y * tmin;
    entp->z = ray.mOrigin.z + ray.mDirection.z * tmin;
    entp->t = tmin;
  }
  else { // internal node
    entp->x = ray.mOrigin.x;
    entp->y = ray.mOrigin.y;
    entp->z = ray.mOrigin.z;
    entp->t = 0.0;
  }
  SKTBNodeT *farChild;

  // DEBUG << "A: c= " << currNode << "\n" << flush ;
 
  // traverse through whole CKTB tree
  for (;;) {
#ifndef _ALLOC_EMPTY_LEAVES
    for (;;) {
    // label for continuing the traversal
#else
    // we have to check the node
    // current node is not the leaf, empty leaves are NULL pointers
    while (currNode) {
#endif

#ifdef _DEBUGKTB
      cout << "i = " << ib << " node = " << (void*)currNode
	   << " depth=" << depth << endl;
      ib++;
#endif      
      
#ifdef __TRAVERSAL_STATISTICS
      allNodesTraversed++;
#endif // __TRAVERSAL_STATISTICS
      // the position of the splitting plane
      float splitVal = GetSplitValue(currNode);
      // decision based on the axis given by splitting plane
      switch (GetNodeType(currNode) & 0x7) {
	// -------------------------------------------------
	case CKTBAxes::EE_X_axis: {
#ifdef _DEBUGKTB
	  cout << "X-axis v=" << splitVal << endl;
	  depth++;
#endif
	  if (entp->x <= splitVal) {
	    if (extp->x <= splitVal) {
	      currNode = GetLeft(currNode);
	      // cases N1,N2,N3,P5,Z2,Z3
	      continue;
	    }
	    // case N4
	    farChild = GetRight(currNode);
	    currNode = GetLeft(currNode);
	  }
          else {
	    if (splitVal <= extp->x) {
	      currNode = GetRight(currNode);
	      continue; // cases P1,P2,P3,N5,Z1
	    }
	    farChild = GetLeft(currNode); // case P4
	    currNode = GetRight(currNode);
	  }
#ifdef _DEBUGKTB
	  cout << "X-Push right node = " << farChild << endl;
#endif	  
          // case N4 or P4
#ifdef _COMPUTE_INVERTED_DIR_KTB
	  float tdist = (splitVal - ray.mOrigin.x) * invertedDir.x;
#else  // _COMPUTE_INVERTED_DIR_KTB
	  float tdist = (splitVal - ray.mOrigin.x) / ray.mDirection.x;
#endif // _COMPUTE_INVERTED_DIR_KTB
	  
	  struct SStackElem *tmp = extp;
	  if (++extp == entp)
	    extp++;
#if defined(_KTBEXTSTACK) && defined(_DEBUGKTB)
	  extp->dummy = depth+1;
#endif
	  extp->prev = tmp;
	  extp->nodep = farChild;
	  extp->t = tdist;
	  extp->x = splitVal;
	  extp->y = ray.mOrigin.y +  tdist * ray.mDirection.y;
	  extp->z = ray.mOrigin.z +  tdist * ray.mDirection.z;
	  continue;
        }
	// -------------------------------------------------
	case CKTBAxes::EE_Y_axis: {
#ifdef _DEBUGKTB
	  cout << "Y-axis v=" << splitVal << endl;
	  depth++;
#endif
	  if (entp->y <= splitVal) {
	    if (extp->y <= splitVal) {
	      currNode = GetLeft(currNode); 
	      //case N1,N2,N3,P5,Z2,Z3
	      continue;
	    }
	    // case N4
	    farChild = GetRight(currNode);
	    currNode = GetLeft(currNode);
	  }
          else {
	    if (splitVal <= extp->y) {
	      currNode = GetRight(currNode);
	      continue; // case P1,P2,P3,N5
	    }
	    farChild = GetLeft(currNode); // case P4
	    currNode = GetRight(currNode);
	  }
#ifdef _DEBUGKTB
	  cout << "Y-Push right node = " << farChild << endl;
#endif	  
          // case N4 or P4
#ifdef _COMPUTE_INVERTED_DIR_KTB
	  float tdist = (splitVal - ray.mOrigin.y) * invertedDir.y;
#else  // _COMPUTE_INVERTED_DIR_KTB
	  float tdist = (splitVal - ray.mOrigin.y) / ray.mDirection.y;
#endif // _COMPUTE_INVERTED_DIR_KTB

	  struct SStackElem *tmp = extp;
	  if (++extp == entp)
	    extp++;
#if defined(_KTBEXTSTACK) && defined(_DEBUGKTB)
	  extp->dummy = depth+1;
#endif	  	  
	  extp->prev = tmp;
	  extp->nodep = farChild;
	  extp->t = tdist;
	  extp->x = ray.mOrigin.x + tdist * ray.mDirection.x;
	  extp->y = splitVal;
	  extp->z = ray.mOrigin.z + tdist * ray.mDirection.z;
	  continue;
        }
	// -------------------------------------------------
	case CKTBAxes::EE_Z_axis: {
#ifdef _DEBUGKTB
	  cout << "Z-axis v=" << splitVal << endl;
	  depth++;
#endif
	  if (entp->z <= splitVal) {
	    if (extp->z <= splitVal) {
	      currNode = GetLeft(currNode);
	      //case N1,N2,N3,P5,Z2,Z3
	      continue;
	    }
	    // case N4
	    farChild = GetRight(currNode);
	    currNode = GetLeft(currNode);
  	  }
          else {
	    if (splitVal <= extp->z) {
	      currNode = GetRight(currNode);
	      continue; // case P1,P2,P3,N5
	    }
	    farChild = GetLeft(currNode); // case P4
	    currNode = GetRight(currNode);
          }
#ifdef _DEBUGKTB
	  cout << "Z-Push right node = " << farChild << endl;
#endif	  
          // case N4 or P4
#ifdef _COMPUTE_INVERTED_DIR_KTB
	  float tdist = (splitVal - ray.mOrigin.z) * invertedDir.z;
#else  // _COMPUTE_INVERTED_DIR_KTB
	  float tdist = (splitVal - ray.mOrigin.z) / ray.mDirection.z;
#endif // _COMPUTE_INVERTED_DIR_KTB

	  struct SStackElem *tmp = extp;
          if (++extp == entp)
	    extp++;
#if defined(_KTBEXTSTACK) && defined(_DEBUGKTB)
	  extp->dummy = depth+1;
#endif	  
	  
	  extp->prev = tmp;
	  extp->nodep = farChild;
	  extp->t = tdist;
	  extp->x = ray.mOrigin.x + tdist * ray.mDirection.x;
	  extp->y = ray.mOrigin.y + tdist * ray.mDirection.y;
	  extp->z = splitVal;
	  continue;
        }	  
        case CKTBAxes::EE_Leaf:
	{ // test objects for intersection
#ifdef _DEBUGKTB
	  cout << "Leaf " << endl;
	  depth++;
#endif
	  goto TEST_OBJECTS;
	}
        case CKTBAxes::EE_Link:{
#ifdef _DEBUGKTB
	  cout << "Link " << endl;
#endif 	  
	  currNode = GetLinkNode(currNode);
	  // cout << "Link node was accessed" << endl;
	  continue;
        }
	case CKTBAxes::EE_X_axisBox: {
#ifdef _DEBUGKTB
	  cout << "X-axis Box v=" << splitVal << endl;
	  depth++;
#endif
	  if (entp->x <= splitVal) {
	    if (extp->x <= splitVal) {
	      currNode = GetLeftMinBox(currNode);
	      // cases N1,N2,N3,P5,Z2,Z3
	      continue;
	    }
	    // case N4
	    farChild = GetRight(currNode);
	    currNode = GetLeftMinBox(currNode);
	  }
          else {
	    if (splitVal <= extp->x) {
	      currNode = GetRight(currNode);
	      continue; // cases P1,P2,P3,N5,Z1
	    }
	    farChild = GetLeftMinBox(currNode); // case P4
	    currNode = GetRight(currNode);
	  }
#ifdef _DEBUGKTB
	  cout << "X-Push (Box) right node = " << farChild << endl;
#endif	  
          // case N4 or P4
#ifdef _COMPUTE_INVERTED_DIR_KTB
	  float tdist = (splitVal - ray.mOrigin.x) * invertedDir.x;
#else  // _COMPUTE_INVERTED_DIR_KTB
	  float tdist = (splitVal - ray.mOrigin.x) / ray.mDirection.x;
#endif // _COMPUTE_INVERTED_DIR_KTB
	  
	  struct SStackElem *tmp = extp;
	  if (++extp == entp)
	    extp++;
#if defined(_KTBEXTSTACK) && defined(_DEBUGKTB)
	  extp->dummy = depth+1;
#endif
	  extp->prev = tmp;
	  extp->nodep = farChild;
	  extp->t = tdist;
	  extp->x = splitVal;
	  extp->y = ray.mOrigin.y +  tdist * ray.mDirection.y;
	  extp->z = ray.mOrigin.z +  tdist * ray.mDirection.z;
	  continue;
        }
	// -------------------------------------------------
	case CKTBAxes::EE_Y_axisBox: {
#ifdef _DEBUGKTB
	  cout << "Y-axis Box v=" << splitVal << endl;
	  depth++;
#endif
	  if (entp->y <= splitVal) {
	    if (extp->y <= splitVal) {
	      currNode = GetLeftMinBox(currNode); 
	      //case N1,N2,N3,P5,Z2,Z3
	      continue;
	    }
	    // case N4
	    farChild = GetRight(currNode);
	    currNode = GetLeftMinBox(currNode);
	  }
          else {
	    if (splitVal <= extp->y) {
	      currNode = GetRight(currNode);
	      continue; // case P1,P2,P3,N5
	    }
	    farChild = GetLeftMinBox(currNode); // case P4
	    currNode = GetRight(currNode);
	  }
#ifdef _DEBUGKTB
	  cout << "Y-Push (Box) right node = " << farChild << endl;
#endif	  
          // case N4 or P4
#ifdef _COMPUTE_INVERTED_DIR_KTB
	  float tdist = (splitVal - ray.mOrigin.y) * invertedDir.y;
#else  // _COMPUTE_INVERTED_DIR_KTB
	  float tdist = (splitVal - ray.mOrigin.y) / ray.mDirection.y;
#endif // _COMPUTE_INVERTED_DIR_KTB

	  struct SStackElem *tmp = extp;
	  if (++extp == entp)
	    extp++;
#if defined(_KTBEXTSTACK) && defined(_DEBUGKTB)
	  extp->dummy = depth+1;
#endif	  	  
	  extp->prev = tmp;
	  extp->nodep = farChild;
	  extp->t = tdist;
	  extp->x = ray.mOrigin.x + tdist * ray.mDirection.x;
	  extp->y = splitVal;
	  extp->z = ray.mOrigin.z + tdist * ray.mDirection.z;
	  continue;
        }
	// -------------------------------------------------
	case CKTBAxes::EE_Z_axisBox: {
#ifdef _DEBUGKTB
	  cout << "Z-axis Box v=" << splitVal << endl;
	  depth++;
#endif
	  if (entp->z <= splitVal) {
	    if (extp->z <= splitVal) {
	      currNode = GetLeftMinBox(currNode);
	      //case N1,N2,N3,P5,Z2,Z3
	      continue;
	    }
	    // case N4
	    farChild = GetRight(currNode);
	    currNode = GetLeftMinBox(currNode);
  	  }
          else {
	    if (splitVal <= extp->z) {
	      currNode = GetRight(currNode);
	      continue; // case P1,P2,P3,N5
	    }
	    farChild = GetLeftMinBox(currNode); // case P4
	    currNode = GetRight(currNode);
          }
#ifdef _DEBUGKTB
	  cout << "Z-Push (Box) right node = " << farChild << endl;
#endif	  
          // case N4 or P4
#ifdef _COMPUTE_INVERTED_DIR_KTB
	  float tdist = (splitVal - ray.mOrigin.z) * invertedDir.z;
#else  // _COMPUTE_INVERTED_DIR_KTB
	  float tdist = (splitVal - ray.mOrigin.z) / ray.mDirection.z;
#endif // _COMPUTE_INVERTED_DIR_KTB

	  struct SStackElem *tmp = extp;
          if (++extp == entp)
	    extp++;
#if defined(_KTBEXTSTACK) && defined(_DEBUGKTB)
	  extp->dummy = depth+1;
#endif	  
	  
	  extp->prev = tmp;
	  extp->nodep = farChild;
	  extp->t = tdist;
	  extp->x = ray.mOrigin.x + tdist * ray.mDirection.x;
	  extp->y = ray.mOrigin.y + tdist * ray.mDirection.y;
	  extp->z = splitVal;
	  continue;
        }	  
      } // switch
    } // while current node is not the leaf

    // leaf can be empty or full at this point
TEST_OBJECTS:

#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 = extp->t;
      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	
	RestoreStackPointers();
	return 1;
      }
    }
#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
    entp = extp;
    currNode = entp->nodep;
#ifdef _DEBUGKTB
#ifdef _KTBEXTSTACK
    depth = entp->dummy;
    cout << "Popped node at depth = " << depth << endl;
#endif	  
#endif

    // DEBUG << "Pop c= " << currNode << "\n" << flush ;
    if (currNode == NULL) {
#ifdef _DEBUGKTB
	cout << "NO HIT - EXIT " << endl; 
#endif 	

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

      // no objects found along the ray path
      RestoreStackPointers();
      return 0;
    }
    extp = extp->prev;
  } // for(;;)
} // FindNearestI
#endif // TRV000

#ifdef TRV001
int
CKTBTraversal::FindNearestI(const SimpleRay &ray)
{
  // 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)) ||
       (tmin >= tmax) ||
       (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

#ifdef _COMPUTE_INVERTED_DIR_KTB  
  Vector3 invertedDir;
  invertedDir.x = 1.0f / ray.mDirection.x;
  invertedDir.y = 1.0f / ray.mDirection.y;
  invertedDir.z = 1.0f / ray.mDirection.z;
#endif // _COMPUTE_INVERTED_DIR_KTB  

  // start from the root node
  SKTBNodeT *currNode = root;

  struct SStackElem *entp, *extp;
  GetStackPointers(entp, extp);
  //struct SStackElem *entp = &(stack[0]);
  //struct SStackElem *extp = &(stack[1]);

  // exit point setting for the box and the ray
  extp->x = ray.mOrigin.x + ray.mDirection.x * tmax;
  extp->y = ray.mOrigin.y + ray.mDirection.y * tmax;
  extp->z = ray.mOrigin.z + ray.mDirection.z * tmax;
  extp->nodep = NULL;
  extp->prev = NULL;
  extp->t = tmax;

  // entry point setting for the box and the ray
  entp->nodep = NULL;
  entp->prev = NULL;
  if (tmin > 0.0) { // external node
    entp->x = ray.mOrigin.x + ray.mDirection.x * tmin;
    entp->y = ray.mOrigin.y + ray.mDirection.y * tmin;
    entp->z = ray.mOrigin.z + ray.mDirection.z * tmin;
    entp->t = tmin;
  }
  else { // internal node
    entp->x = ray.mOrigin.x;
    entp->y = ray.mOrigin.y;
    entp->z = ray.mOrigin.z;
    entp->t = 0.0;
  }
  SKTBNodeT *farChild;

  // DEBUG << "A: c= " << currNode << "\n" << flush ;
 
  // traverse through whole CKTB tree
  for (;;) {
#ifndef _ALLOC_EMPTY_LEAVES
    for (;;) {
    // label for continuing the traversal
#else
    // we have to check the node
    // current node is not the leaf, empty leaves are NULL pointers
    while (currNode) {
#endif

#ifdef _DEBUGKTB
      cout << "i = " << ib << " node = " << (void*)currNode
	   << " depth=" << depth << endl;
      ib++;
#endif      
      
#ifdef __TRAVERSAL_STATISTICS
      allNodesTraversed++;
#endif // __TRAVERSAL_STATISTICS
      // the position of the splitting plane
      float splitVal = GetSplitValue(currNode);
      // decision based on the axis given by splitting plane
      switch (GetNodeType(currNode) & 0x07) {
	// -------------------------------------------------
	case CKTBAxes::EE_X_axis: {
#ifdef _DEBUGKTB
	  cout << "X-axis v=" << splitVal << endl;
	  depth++;
#endif
	  if (entp->x <= splitVal) {
	    if (extp->x <= splitVal) {
	      currNode = GetLeft(currNode);
	      // cases N1,N2,N3,P5,Z2,Z3
	      continue;
	    }
	    // case N4
	    farChild = GetRight(currNode);
	    currNode = GetLeft(currNode);
	  }
          else {
	    if (splitVal <= extp->x) {
	      currNode = GetRight(currNode);
	      continue; // cases P1,P2,P3,N5,Z1
	    }
	    farChild = GetLeft(currNode); // case P4
	    currNode = GetRight(currNode);
	  }
#ifdef _DEBUGKTB
	  cout << "X-Push right node = " << farChild << endl;
#endif	  
          // case N4 or P4
#ifdef _COMPUTE_INVERTED_DIR_KTB
	  float tdist = (splitVal - ray.mOrigin.x) * invertedDir.x;
#else  // _COMPUTE_INVERTED_DIR_KTB
	  float tdist = (splitVal - ray.mOrigin.x) / ray.mDirection.x;
#endif // _COMPUTE_INVERTED_DIR_KTB
	  
	  struct SStackElem *tmp = extp;
	  if (++extp == entp)
	    extp++;
#if defined(_KTBEXTSTACK) && defined(_DEBUGKTB)
	  extp->dummy = depth+1;
#endif
	  extp->prev = tmp;
	  extp->nodep = farChild;
	  extp->t = tdist;
	  extp->x = splitVal;
	  extp->y = ray.mOrigin.y +  tdist * ray.mDirection.y;
	  extp->z = ray.mOrigin.z +  tdist * ray.mDirection.z;
	  continue;
        }
	// -------------------------------------------------
	case CKTBAxes::EE_Y_axis: {
#ifdef _DEBUGKTB
	  cout << "Y-axis v=" << splitVal << endl;
	  depth++;
#endif
	  if (entp->y <= splitVal) {
	    if (extp->y <= splitVal) {
	      currNode = GetLeft(currNode); 
	      //case N1,N2,N3,P5,Z2,Z3
	      continue;
	    }
	    // case N4
	    farChild = GetRight(currNode);
	    currNode = GetLeft(currNode);
	  }
          else {
	    if (splitVal <= extp->y) {
	      currNode = GetRight(currNode);
	      continue; // case P1,P2,P3,N5
	    }
	    farChild = GetLeft(currNode); // case P4
	    currNode = GetRight(currNode);
	  }
#ifdef _DEBUGKTB
	  cout << "Y-Push right node = " << farChild << endl;
#endif	  
          // case N4 or P4
#ifdef _COMPUTE_INVERTED_DIR_KTB
	  float tdist = (splitVal - ray.mOrigin.y) * invertedDir.y;
#else  // _COMPUTE_INVERTED_DIR_KTB
	  float tdist = (splitVal - ray.mOrigin.y) / ray.mDirection.y;
#endif // _COMPUTE_INVERTED_DIR_KTB

	  struct SStackElem *tmp = extp;
	  if (++extp == entp)
	    extp++;
#if defined(_KTBEXTSTACK) && defined(_DEBUGKTB)
	  extp->dummy = depth+1;
#endif	  	  
	  extp->prev = tmp;
	  extp->nodep = farChild;
	  extp->t = tdist;
	  extp->x = ray.mOrigin.x + tdist * ray.mDirection.x;
	  extp->y = splitVal;
	  extp->z = ray.mOrigin.z + tdist * ray.mDirection.z;
	  continue;
        }
	// -------------------------------------------------
	case CKTBAxes::EE_Z_axis: {
#ifdef _DEBUGKTB
	  cout << "Z-axis v=" << splitVal << endl;
	  depth++;
#endif
	  if (entp->z <= splitVal) {
	    if (extp->z <= splitVal) {
	      currNode = GetLeft(currNode);
	      //case N1,N2,N3,P5,Z2,Z3
	      continue;
	    }
	    // case N4
	    farChild = GetRight(currNode);
	    currNode = GetLeft(currNode);
  	  }
          else {
	    if (splitVal <= extp->z) {
	      currNode = GetRight(currNode);
	      continue; // case P1,P2,P3,N5
	    }
	    farChild = GetLeft(currNode); // case P4
	    currNode = GetRight(currNode);
          }
#ifdef _DEBUGKTB
	  cout << "Z-Push right node = " << farChild << endl;
#endif	  
          // case N4 or P4
#ifdef _COMPUTE_INVERTED_DIR_KTB
	  float tdist = (splitVal - ray.mOrigin.z) * invertedDir.z;
#else  // _COMPUTE_INVERTED_DIR_KTB
	  float tdist = (splitVal - ray.mOrigin.z) / ray.mDirection.z;
#endif // _COMPUTE_INVERTED_DIR_KTB

	  struct SStackElem *tmp = extp;
          if (++extp == entp)
	    extp++;
#if defined(_KTBEXTSTACK) && defined(_DEBUGKTB)
	  extp->dummy = depth+1;
#endif	  
	  
	  extp->prev = tmp;
	  extp->nodep = farChild;
	  extp->t = tdist;
	  extp->x = ray.mOrigin.x + tdist * ray.mDirection.x;
	  extp->y = ray.mOrigin.y + tdist * ray.mDirection.y;
	  extp->z = splitVal;
	  continue;
        }	  
        case CKTBAxes::EE_Leaf:
	{ // test objects for intersection
#ifdef _DEBUGKTB
	  cout << "Leaf " << endl;
	  depth++;
#endif
	  goto TEST_OBJECTS;
	}
        case CKTBAxes::EE_Link:{
#ifdef _DEBUGKTB
	  cout << "Link " << endl;
#endif 	  
	  currNode = GetLinkNode(currNode);
	  // cout << "Link node was accessed" << endl;
	  continue;
        }
      } // switch
    } // while current node is not the leaf

    // leaf can be empty or full at this point
TEST_OBJECTS:

#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 = extp->t;
      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	
	RestoreStackPointers();
	return 1;
      }
    }
#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
    entp = extp;
    currNode = entp->nodep;
#ifdef _DEBUGKTB
#ifdef _KTBEXTSTACK
    depth = entp->dummy;
    cout << "Popped node at depth = " << depth << endl;
#endif	  
#endif

    // DEBUG << "Pop c= " << currNode << "\n" << flush ;
    if (currNode == NULL) {
#ifdef _DEBUGKTB
	cout << "NO HIT - EXIT " << endl; 
#endif 	

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

      // no objects found along the ray path
      RestoreStackPointers();
      return 0;
    }
    extp = extp->prev;
  } // for(;;)
} // FindNearestI

#endif // TRV001

#if defined(TRV000)||defined(TRV001)||defined(TRV002)||defined(TRV003)||defined(TRV004)||defined(TRV007)
int
CKTBTraversal::FindNearestI_16oneDir(SimpleRayContainer &rays)
{
  assert("NYI");
}
#endif

#if defined(TRV000)||defined(TRV001)||defined(TRV002)||defined(TRV003)||defined(TRV004)||defined(TRV006)||defined(TRV007)||defined(TRV00F)
int
CKTBTraversal::FindNearestI_16twoDir(SimpleRayContainer &rays)
{
  assert("NYI");
  return 0;
}
#endif

#ifdef __SSE__
#if defined(TRV000)||defined(TRV001)||defined(TRV002)||defined(TRV003)||defined(TRV004)||defined(TRV005)
// The same operations for packets of rays, if implemented by
// a particular ASDS, otherwise it is emulated by decomposition
// of a packet to individual rays and traced individually.
void
CKTBTraversal::FindNearestI(RayPacket2x2 &raypack)
{
  for (int i = 0; i < 4; i++) {
    cout << " orig = " << raypack.GetLoc(i) << " dir = "
	 << raypack.GetDir(i) << endl;
  }
  // cout << "Not yet implemented" << endl;
}
#endif
#endif // __SSE__
 
} // namespace