Index: /GTP/trunk/Lib/Vis/Preprocessing/src/havran/ktbf2trv.cpp
===================================================================
--- /GTP/trunk/Lib/Vis/Preprocessing/src/havran/ktbf2trv.cpp	(revision 2595)
+++ /GTP/trunk/Lib/Vis/Preprocessing/src/havran/ktbf2trv.cpp	(revision 2595)
@@ -0,0 +1,617 @@
+// ===================================================================
+// $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
+
+#ifdef _USE_HAVRAN_SSE  
+#ifdef __SSE__
+
+// Even faster - about 125,500 rays per second for single dir and 164,000 rps
+// for double dir !
+int
+CKTBTraversal::FindNearestI_16oneDir(SimpleRayContainer &rays, int offset,
+				     int copyOffset)
+{
+  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 + copyOffset].intersectable =
+      results[i].intersectable;
+    SimpleRay::IntersectionRes[i + copyOffset].tdist =
+      results[i].tdist;
+  } // for i
+  return 0;
+}
+  
+#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; };
+    GALIGN16 int inters[4];
+    inters[0] = inters[1] = inters[2] = inters[3] = 1;
+    SimpleRay sray[4];
+    int maxIntersections = 4;
+    unsigned int inters32 = 0xf;
+    for (int i = 0; i < 4; i++) {
+      SimpleRay::IntersectionRes[i].intersectable = 0;
+      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) ) {
+	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] + Limits::Small;
+	      // 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[i].maxt);
+		rp.SetObject(i, SimpleRay::IntersectionRes[i].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;
+      } // if leaf
+      // 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
+
+  return;
+}
+#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 { unsigned int activeMask[4]; __m128 activeMask_4; };
+    GALIGN16 union { unsigned int 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++) {
+      SimpleRay::IntersectionRes[i].intersectable = 0;
+      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] + Limits::Small;
+#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		
+	      {
+		// object was intersected
+		rp.SetT(i, SimpleRay::IntersectionRes[i].maxt);
+		rp.SetObject(i, SimpleRay::IntersectionRes[i].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
+
+  return;
+}
+#endif
+
+// This code allows to specify the box where the ray should be traversed!
+void
+CKTBTraversal::FindNearestI(RayPacket2x2 &rp, Vector3 &boxmin, Vector3 &boxmax)
+{
+  static AxisAlignedBox3 localbox;
+  localbox.SetMin(boxmin);
+  localbox.SetMax(boxmax);
+
+  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 { unsigned int activeMask[4]; __m128 activeMask_4; };
+    GALIGN16 union { unsigned int 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++) {
+      SimpleRay::IntersectionRes[i].intersectable = 0;
+      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)
+#undef _NOT_STORE_MINT	
+//#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] + Limits::Small;
+#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[i].maxt);
+		rp.SetObject(i, SimpleRay::IntersectionRes[i].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, localbox);
+    rp.SetObject(i, SimpleRay::IntersectionRes[0].intersectable);
+    rp.SetT(i, SimpleRay::IntersectionRes[0].tdist);
+    // SimpleRay::IntersectionRes[0].intersectable->GetNormal(0);
+  } // for
+}
+
+#endif // __SSE__
+#endif // _USE_HAVRAN_SSE  
+
+#endif //  TRV00F
+
+} // namespace
+
Index: /GTP/trunk/Lib/Vis/Preprocessing/src/havran/testrt.cpp
===================================================================
--- /GTP/trunk/Lib/Vis/Preprocessing/src/havran/testrt.cpp	(revision 2595)
+++ /GTP/trunk/Lib/Vis/Preprocessing/src/havran/testrt.cpp	(revision 2595)
@@ -0,0 +1,857 @@
+#define USE_THREADS 0
+
+#ifdef UNICODE
+#undef UNICODE
+#endif
+
+#define NOMINMAX
+#ifdef __WINDOWS__
+#include <windows.h>
+#ifdef _CRT_SET
+#include <crtdbg.h>
+#endif // _CRT_SET
+#endif
+#include <cstdio>
+
+#include "Camera.h"
+#include "PreprocessorFactory.h"
+#include "Parser.h"
+#include "Environment.h"
+#include "MeshKdTree.h"
+#include "Preprocessor.h"
+#include "common.h"
+#include "PreprocessorThread.h"
+#include "ObjExporter.h"
+#include "SceneGraph.h"
+#include "GlobalLinesRenderer.h"
+#include "RayCaster.h"
+#include "timer.h"
+#include "raypack.h"
+
+#include "ViewCellsManager.h"
+
+#ifdef USE_QT  
+	#include "QtPreprocessorThread.h"
+	#include "QtGlViewer.h"
+	#include "QtGlRenderer.h"
+#else
+	#if USE_THREADS
+		#include "BoostPreprocessorThread.h"
+	#endif
+#endif
+
+#include "ResourceManager.h"
+#include "GlRenderer.h"
+
+
+#define USE_EXE_PATH false
+
+
+using namespace GtpVisibilityPreprocessor;
+
+//Preprocessor *preprocessor = NULL;
+extern GlRendererWidget *rendererWidget;
+//GlobalLinesRenderer *globalLinesRenderer = NULL;
+
+// DLL function signature
+typedef GlRendererWidget *(*importFunction)(Preprocessor *);
+
+
+// Flag if the ray should be single sided (one direction)
+// or double sided (shooting ray in both directions)
+static bool castDoubleRays;
+
+extern void Cleanup();
+
+static string ReplaceSuffix(const string &filename, const string &a, const string &b)
+{
+	string result = filename;
+
+	int pos = (int)filename.rfind(a, (int)filename.size() - 1);
+	if (pos == filename.size() - a.size()) 
+	{
+		result.replace(pos, a.size(), b);
+	}
+
+	return result;
+}
+
+
+static int SplitFilenames(const string &str, vector<string> &filenames)
+{
+	int pos = 0;
+
+	while(1) {
+		int npos = (int)str.find(';', pos);
+		
+		if (npos < 0 || npos - pos < 1)
+			break;
+		filenames.push_back(string(str, pos, npos - pos));
+		pos = npos + 1;
+	}
+	
+	filenames.push_back(string(str, pos, str.size() - pos));
+	return (int)filenames.size();
+}
+
+
+static string GetInternFilename(const string &filename, const string newSuffix)
+{
+	vector<string> filenames;
+	const int files = SplitFilenames(filename, filenames);
+
+	vector<string>::const_iterator sit, sit_end = filenames.end();
+	string kdFilename;
+
+	int i = 0;
+	for (sit = filenames.begin(); sit != sit_end; ++ sit, ++ i)
+	{
+		string currentFile = *sit;
+		string strippedFilename;
+
+		if (i == 0)
+		{	
+			strippedFilename = currentFile;
+		}
+		else
+		{
+			char *str = StripPath(currentFile.c_str());
+			strippedFilename = string(str);
+
+			delete [] str;
+		}
+		
+		string suffix("_");
+
+		if (i == (int)filenames.size() - 1)
+		{
+			suffix = newSuffix;
+		}
+
+		if (strstr(strippedFilename.c_str(), ".x3d")) 
+		{
+			kdFilename += ReplaceSuffix(strippedFilename, ".x3d", suffix);
+		}
+        else if (strstr(strippedFilename.c_str(), ".dat"))
+		{
+			kdFilename += ReplaceSuffix(strippedFilename, ".dat", suffix);
+		} 
+		else if (strstr(strippedFilename.c_str(), ".obj"))
+		{
+			kdFilename += ReplaceSuffix(strippedFilename, ".obj", suffix);
+		}
+		else
+		{
+			cerr << "Error: Currently unsupported format for kd, filename " << currentFile << endl;
+		}
+	}
+
+	//cout << "kdfilename: " << kdFilename << endl;
+	return kdFilename;
+}
+
+
+// -------------------------------------------------------
+// Written by Vlastimil Havran
+
+// This is for testing RT implementation
+void
+TestRTcamera(int argc, char **argv)
+{
+  int returnCode = 0;
+
+  InitTiming();
+  Debug.open("debug.log");
+
+  Environment::GetSingleton()->Parse(argc, argv, USE_EXE_PATH);
+  MeshKdTree::ParseEnvironment();
+
+  char buff[128];
+  Environment::GetSingleton()->GetStringValue("Preprocessor.type", buff);
+  string preprocessorType(buff);
+
+  if (!(preprocessor = PreprocessorFactory::CreatePreprocessor(preprocessorType)))
+  {
+    Environment::DelSingleton();
+    cerr << "Unknown preprocessor type" << endl;
+    exit(1);
+  }
+
+
+  Environment::GetSingleton()->GetStringValue("Scene.filename", buff);
+  string filename(buff);
+
+  const string externKdTree = ReplaceSuffix(filename, ".obj", ".kdf");
+  const string internKdTree = GetInternFilename(filename, preprocessor->mLoadMeshes ?
+						".kdm" : ".kdt");
+
+  if (preprocessor->InitRayCast(externKdTree, internKdTree))
+  {
+    cout << "ray casting initialized!" << endl;
+  }
+  else
+  {
+    cout << "ray casting initialization failed!" << endl;
+    Cleanup();
+    exit(1);
+  }
+  
+  //Debug << "using pvs type " << PVS_TYPE << endl;
+
+  /////////////
+  //-- load scene 
+  
+  if (!preprocessor->LoadScene(filename))
+  {
+    cout << "loading file " << filename << " failed" << endl;
+    Cleanup();
+    exit(1);
+  }
+	
+  ////////////
+  //-- initialize external ray caster
+  
+  if (preprocessor->LoadInternKdTree(internKdTree))
+  {
+    cout << "intern kd tree loaded!" << endl;
+  }
+  else
+  {
+    cout << "loading intern kd tree failed!" << endl;
+    Cleanup();
+    exit(1);
+  }
+  
+  // export objects as obj
+  if (preprocessor->mExportObj)
+  {
+    if (strstr(filename.c_str(), ".obj"))
+      {
+	cerr << "already in obj format" << endl;
+	if (0) 	preprocessor->ExportObj("test.obj", preprocessor->mObjects);
+      }
+    else
+      {
+	
+	const string objname = GetInternFilename(filename, ".obj");
+	
+	cout << "exporting scene to " << objname << endl;
+	bool success = preprocessor->ExportObj(objname, preprocessor->mObjects);
+	
+	
+	if (success)
+	  cout << "finished exporting obj" << endl;
+	else
+	  cerr << "error exporting " << objname << endl;
+      }
+  }
+
+  int width = 1000;
+  int height = 500;
+  float fieldOfView = 115.f;
+  Camera cam(width, height, fieldOfView);
+  
+  AxisAlignedBox3 bboxOrig = preprocessor->mSceneGraph->GetBox();
+  AxisAlignedBox3 bbox = bboxOrig;
+  int sizeDiag = Magnitude(bbox.Diagonal());
+  bbox.Enlarge(sizeDiag * 1.5f);
+
+  Vector3 origin, dir;
+
+//#define WIEN1
+#define WIEN2
+//#define ARENA1
+    
+#ifdef WIEN1
+  // 1099.9 183.0 -387
+  origin = Vector3(1099.9f, 183.0f, -387.0f);
+  dir = Vector3(-0.6f, 0.0001f, -0.8f);
+#define DIREXISTS
+#endif  
+#ifdef WIEN2
+  // 935.6 215.8 -1012.3
+  origin = Vector3(935.6, 215.8, -1012.4);
+  dir = Vector3(0.01f, 0.01f, 1.0f);
+#define DIREXISTS
+#endif
+#ifdef ARENA1
+  origin = Vector3(22.16, 19.63, -950.44);
+  dir = Vector3(0.9, 0.001f, -0.5f);
+#define DIREXISTS
+#endif  
+
+  cam.SetPosition(origin);
+
+#ifndef DIREXISTS
+  Vector3 center = bbox.Center();
+  dir = center - origin;
+#endif
+  
+  dir.Normalize();
+  cam.SetDirection(dir);
+  cout << "Computing image\n" << endl;
+
+#if 1
+  // ray by ray
+  cam.SnapImage("test-rays.tga",
+		preprocessor->mRayCaster,
+		bboxOrig,
+		preprocessor->mSceneGraph);
+#endif
+#if 1
+  // using ray packets
+  cam.SnapImagePacket("test-packet.tga",
+		      preprocessor->mRayCaster,
+		      bboxOrig,
+		      preprocessor->mSceneGraph);
+#endif
+#if 1
+  // using ray packets
+  cam.SnapImage2("test-oneDir.tga",
+		 preprocessor->mRayCaster,
+		 bboxOrig,
+		 preprocessor->mSceneGraph);
+#endif
+  
+  cout << "Done\n" << endl;
+  return;
+}
+
+struct RESult {
+  int hitA;
+  float hitAT;
+  int hitB;
+  float hitBT;
+  RESult(int hA, float tA, int hB, float tB):
+    hitA(hA), hitAT(tA), hitB(hB), hitBT(tB) { }
+};
+
+// This is for testing RT implementation
+void
+TestRTfromFile(int argc, char **argv)
+{
+  int returnCode = 0;
+
+  InitTiming();
+  Debug.open("debug.log");
+
+  Environment::GetSingleton()->Parse(argc, argv, USE_EXE_PATH);
+  MeshKdTree::ParseEnvironment();
+
+  char buff[128];
+  Environment::GetSingleton()->GetStringValue("Preprocessor.type", buff);
+  string preprocessorType(buff);
+
+  if (!(preprocessor = PreprocessorFactory::CreatePreprocessor(preprocessorType)))
+  {
+    Environment::DelSingleton();
+    cerr << "Unknown preprocessor type" << endl;
+    exit(1);
+  }
+
+
+  Environment::GetSingleton()->GetStringValue("Scene.filename", buff);
+  string filename(buff);
+
+  const string externKdTree = ReplaceSuffix(filename, ".obj", ".kdf");
+  const string internKdTree = GetInternFilename(filename, preprocessor->mLoadMeshes ?
+						".kdm" : ".kdt");
+
+  if (preprocessor->InitRayCast(externKdTree, internKdTree))
+  {
+    cout << "ray casting initialized!" << endl;
+  }
+  else
+  {
+    cout << "ray casting initialization failed!" << endl;
+    Cleanup();
+    exit(1);
+  }
+  
+  //Debug << "using pvs type " << PVS_TYPE << endl;
+
+  /////////////
+  //-- load scene 
+  
+  if (!preprocessor->LoadScene(filename))
+  {
+    cout << "loading file " << filename << " failed" << endl;
+    Cleanup();
+    exit(1);
+  }
+	
+  ////////////
+  //-- initialize external ray caster
+  
+  if (preprocessor->LoadInternKdTree(internKdTree))
+  {
+    cout << "intern kd tree loaded!" << endl;
+  }
+  else
+  {
+    cout << "loading intern kd tree failed!" << endl;
+    Cleanup();
+    exit(1);
+  }
+  
+  // export objects as obj
+  if (preprocessor->mExportObj)
+  {
+    if (strstr(filename.c_str(), ".obj"))
+    {
+      cerr << "already in obj format" << endl;
+      if (0) 	preprocessor->ExportObj("test.obj", preprocessor->mObjects);
+    }
+    else
+    {
+      const string objname = GetInternFilename(filename, ".obj");
+	
+      cout << "exporting scene to " << objname << endl;
+      bool success = preprocessor->ExportObj(objname, preprocessor->mObjects);	
+	
+      if (success)
+	cout << "finished exporting obj" << endl;
+      else
+	cerr << "error exporting " << objname << endl;
+    }
+  }
+
+  Environment::GetSingleton()->GetStringValue("Rays.file", buff);
+  FILE *fp = fopen(buff, "rb");
+  if (!fp) {
+    cerr << "ERROR: file " << buff << " cannot be opened for reading" << endl;
+    cerr << "EXITING" << endl;
+    exit(3);
+  }
+  cout << "File " << buff << " was opened successfully" << endl;
+  int cntMaxRays = 100000;
+  Environment::GetSingleton()->GetIntValue("Rays.cnt", cntMaxRays);
+  vector<SimpleRay> rays;
+  SimpleRay rayTest;
+  vector<RESult> results;
+  
+  int cntRays = 0;
+  for (int i = 0; cntRays < cntMaxRays; i++) {
+    int ch = fgetc(fp);
+    switch (ch) {
+    case 'G': { // two-sided rays of 16
+      for (int j = 0; j < 16; j++) {
+	int order;
+	float ox, oy, oz, dx, dy, dz;
+	int hitA; float hitAT;
+	int hitB; float hitBT;	
+	if (fscanf(fp, "%d %f %f %f %f %f %f %d %f %d %f\n",
+		   &order, &ox, &oy, &oz, &dx, &dy, &dz, &hitA, &hitAT, &hitB, &hitBT) != 11) {
+	  cerr << "Problem parsing the ray file" << endl;
+	  cerr << "EXITING for ray order" << order << endl;
+	  goto FINISH;
+	}
+	Vector3 mPosition(ox, oy, oz);
+	Vector3 mDirection(dx, dy, dz);
+	rayTest.Set(mPosition, mDirection, 0, 1.0, true);
+	rays.push_back(rayTest);
+	results.push_back(RESult(hitA, hitAT, hitB, hitBT));
+	cntRays++;
+      }
+      break;
+    }
+    case 'H': // one-sided rays of 16
+      cerr << "Not yet implemented " << endl; abort();
+      
+    case 'D': // two-sided ray
+      cerr << "Not yet implemented " << endl; abort();
+      
+    case 'S': // one-sided ray
+      cerr << "Not yet implemented " << endl; abort();
+    } // switch
+  } // for i
+
+FINISH:
+  fclose(fp);
+
+  Environment::GetSingleton()->GetBoolValue("TestDoubleRays", castDoubleRays);
+
+  double mult = 1.0;
+  if (castDoubleRays)
+    mult = 2.0;
+  
+  cout << "Starting to shoot " << cntRays * mult << " rays" << endl;
+  
+  RayCaster *raycaster = preprocessor->mRayCaster;
+  VssRayContainer vssRays;
+  AxisAlignedBox3 bboxOrig = preprocessor->mSceneGraph->GetBox();
+
+//#define DEBUGRESULTS
+
+#ifdef DEBUGRESULTS
+  int cntIntelYesWeNo = 0;
+  int cntIntelNoWeYes = 0;
+  int cntIntelOurDistMismatch = 0;
+  bool printOut = false;
+  double sumDistIntel = 0.;
+  doublecca sumDistOurAlg = 0.;
+  double sumDistAbsDiff = 0.f;
+#endif
+
+  cout << "Press a key to start ray shooting" << endl;
+  getchar();
+  cout << "Ray shooting " << cntRays << " rays started - "
+       << (castDoubleRays ? " double " : " single ")
+       << " dir " << endl;
+
+  long t1 = GetTime();
+  CTimer timer;
+  timer.Reset();
+  timer.Start();
+  
+  SimpleRayContainer raysToTest;
+  for (int i = 0; i < cntRays - 16; i++) {
+#if 0
+    int res = raycaster->CastRay(rays[i],
+				 vssRays,
+				 bboxOrig,
+				 castDoubleRays, // castDoubleRay,
+				 false); // pruneInvalidRays
+#else
+    raysToTest.erase(raysToTest.begin(), raysToTest.end());
+    for (int j = 0; j < 16; j++, i++) {
+      raysToTest.push_back(rays[i]);
+    }
+#if 0
+    for (int j = 0; j < 16; j++) {
+      cout << "orig = " << raysToTest[j].mOrigin << " dir = "
+	   << raysToTest[j].mDirection << endl;
+    }
+#endif    
+        
+    raycaster->CastRays16(raysToTest,
+			  vssRays,
+			  bboxOrig,
+			  castDoubleRays,
+			  false);
+#endif
+    
+#ifdef DEBUGRESULTS
+    if (!castDoubleRays) {
+      float T = rays[i].IntersectionRes[0].tdist;
+      if ( (res != results[i].hitA) ||
+	   ((res)&&(results[i].hitA)&& (fabs(T - results[i].hitAT)) > 2.0f)
+	   )
+	{
+	  if (printOut)
+	    cout << " i = " << i << " ";
+	  if ((res != 0) && (results[i].hitA == 0)) {
+	    cntIntelNoWeYes++;
+	    if (printOut)
+	      cout << "Intel no intersection, our alg intersection at t = " << T << endl;
+	  }
+	  else {
+	    if ((res = 0) && (results[i].hitA)) {
+	      cntIntelYesWeNo++;
+	      if (printOut)
+		cout << "Intel intersection at = " << results[i].hitAT
+		     << " , our alg no intersection = " << endl;
+	      
+	    }
+	    else {
+	      cntIntelOurDistMismatch++;
+	      sumDistOurAlg += T;
+	      sumDistIntel += results[i].hitAT;
+	      sumDistAbsDiff += fabs(T - results[i].hitAT);
+	      if (printOut)
+		cout << "Intel intersection at = " << results[i].hitAT
+		     << " , our alg intersection at = " << T << endl;	    
+	    }
+	  }
+	}
+    } // double rays
+    else {
+      // checking for results of double rays - not yet implemented
+    }
+#endif    
+  }
+
+  timer.Stop();
+  long t2 = GetTime();
+  cout<<"\n#RAY_CAST_TIME = ";
+  cout << TimeDiff(t1, t2)<<" [mikrosec]"
+       << " userTime = " << timer.UserTime() << " realTime = "
+       << timer.RealTime() << endl;
+    ;
+  cout << "Rays shot per milisecond [userTimer] = "
+       << ((double)cntRays * mult/(double)timer.UserTime()) / 1000.f << endl;
+
+#ifdef DEBUGRESULTS  
+  cout << "cntIntelYesWeNo = " << cntIntelYesWeNo << endl;
+  cout << "cntIntelNoWeYes = " << cntIntelNoWeYes << endl;
+  cout << "cntIntelOurDistMismatch = " << cntIntelOurDistMismatch++ << endl;
+  cout << " sumDistIntel = " << sumDistIntel/(double)cntIntelOurDistMismatch << endl;
+  cout << " sumDistOur = " << sumDistOurAlg/(double)cntIntelOurDistMismatch << endl;
+  cout << " sumAbsDiffDist = " << sumDistAbsDiff/(double)cntIntelOurDistMismatch
+       << endl;
+#endif  
+  cout << "Done\n" << endl;
+  return;
+}
+
+//-------------------------------------------------------------------------
+// This is for testing RT implementation using ray packets
+void
+TestRTfromFilePackets(int argc, char **argv)
+{
+  int returnCode = 0;
+
+  InitTiming();
+  Debug.open("debug.log");
+
+  Environment::GetSingleton()->Parse(argc, argv, USE_EXE_PATH);
+  MeshKdTree::ParseEnvironment();
+
+  char buff[128];
+  Environment::GetSingleton()->GetStringValue("Preprocessor.type", buff);
+  string preprocessorType(buff);
+
+  if (!(preprocessor = PreprocessorFactory::CreatePreprocessor(preprocessorType)))
+  {
+    Environment::DelSingleton();
+    cerr << "Unknown preprocessor type" << endl;
+    exit(1);
+  }
+
+
+  Environment::GetSingleton()->GetStringValue("Scene.filename", buff);
+  string filename(buff);
+
+  const string externKdTree = ReplaceSuffix(filename, ".obj", ".kdf");
+  const string internKdTree = GetInternFilename(filename, preprocessor->mLoadMeshes ?
+						".kdm" : ".kdt");
+
+  if (preprocessor->InitRayCast(externKdTree, internKdTree))
+  {
+    cout << "ray casting initialized!" << endl;
+  }
+  else
+  {
+    cout << "ray casting initialization failed!" << endl;
+    Cleanup();
+    exit(1);
+  }
+  
+  //Debug << "using pvs type " << PVS_TYPE << endl;
+
+  /////////////
+  //-- load scene 
+  
+  if (!preprocessor->LoadScene(filename))
+  {
+    cout << "loading file " << filename << " failed" << endl;
+    Cleanup();
+    exit(1);
+  }
+	
+  ////////////
+  //-- initialize external ray caster
+  
+  if (preprocessor->LoadInternKdTree(internKdTree))
+  {
+    cout << "intern kd tree loaded!" << endl;
+  }
+  else
+  {
+    cout << "loading intern kd tree failed!" << endl;
+    Cleanup();
+    exit(1);
+  }
+  
+  // export objects as obj
+  if (preprocessor->mExportObj)
+  {
+    if (strstr(filename.c_str(), ".obj"))
+    {
+      cerr << "already in obj format" << endl;
+      if (0) 	preprocessor->ExportObj("test.obj", preprocessor->mObjects);
+    }
+    else
+    {
+      const string objname = GetInternFilename(filename, ".obj");
+	
+      cout << "exporting scene to " << objname << endl;
+      bool success = preprocessor->ExportObj(objname, preprocessor->mObjects);	
+	
+      if (success)
+	cout << "finished exporting obj" << endl;
+      else
+	cerr << "error exporting " << objname << endl;
+    }
+  }
+
+  Environment::GetSingleton()->GetStringValue("Rays.file", buff);
+  FILE *fp = fopen(buff, "rb");
+  if (!fp) {
+    cerr << "ERROR: file " << buff << " cannot be opened for reading" << endl;
+    cerr << "EXITING" << endl;
+    exit(3);
+  }
+  cout << "File " << buff << " was opened successfully" << endl;
+  int cntMaxRays = 100000;
+  Environment::GetSingleton()->GetIntValue("Rays.cnt", cntMaxRays);
+  vector<SimpleRay> rays;
+  SimpleRay rayTest;
+  vector<RESult> results;
+  
+  int cntRays = 0;
+  for (int i = 0; cntRays < cntMaxRays; i++) {
+    int ch = fgetc(fp);
+    switch (ch) {
+    case 'G': { // two-sided rays of 16
+      for (int j = 0; j < 16; j++) {
+	int order;
+	float ox, oy, oz, dx, dy, dz;
+	int hitA; float hitAT;
+	int hitB; float hitBT;	
+	if (fscanf(fp, "%d %f %f %f %f %f %f %d %f %d %f\n",
+		   &order, &ox, &oy, &oz, &dx, &dy, &dz, &hitA, &hitAT, &hitB, &hitBT) != 11) {
+	  cerr << "Problem parsing the ray file" << endl;
+	  cerr << "EXITING for ray order" << order << endl;
+	  goto FINISH;
+	}
+	Vector3 mPosition(ox, oy, oz);
+	Vector3 mDirection(dx, dy, dz);
+	rayTest.Set(mPosition, mDirection, 0, 1.0, true);
+	rays.push_back(rayTest);
+	results.push_back(RESult(hitA, hitAT, hitB, hitBT));
+	cntRays++;
+      }
+      break;
+    }
+    case 'H': // one-sided rays of 16
+      cerr << "Not yet implemented " << endl; abort();
+      
+    case 'D': // two-sided ray
+      cerr << "Not yet implemented " << endl; abort();
+      
+    case 'S': // one-sided ray
+      cerr << "Not yet implemented " << endl; abort();
+    } // switch
+  } // for i
+FINISH:
+  fclose(fp);
+  
+  Environment::GetSingleton()->GetBoolValue("TestDoubleRays", castDoubleRays);
+
+  double mult = 1.0;
+  if (castDoubleRays)
+    mult = 2.0;
+
+  cout << "Starting to shoot " << cntRays * mult << " rays" << endl;
+  
+  RayCaster *raycaster = preprocessor->mRayCaster;
+  VssRayContainer vssRays;
+  AxisAlignedBox3 bboxOrig = preprocessor->mSceneGraph->GetBox();
+
+//#define DEBUGRESULTS
+
+#ifdef DEBUGRESULTS
+  int cntIntelYesWeNo = 0;
+  int cntIntelNoWeYes = 0;
+  int cntIntelOurDistMismatch = 0;
+  bool printOut = false;
+  double sumDistIntel = 0.;
+  doublecca sumDistOurAlg = 0.;
+  double sumDistAbsDiff = 0.f;
+#endif
+
+  cout << "Press a key to start ray shooting" << endl;
+  getchar();
+  cout << "Ray packs shooting " << cntRays << " rays started - "
+       << (castDoubleRays ? " double " : " single ")
+       << " dir " << endl;
+
+  long t1 = GetTime();
+  CTimer timer;
+  timer.Reset();
+  timer.Start();
+
+#ifdef _USE_HAVRAN_SSE
+#ifdef __SSE__  
+  RayPacket2x2 raysPack;
+  
+  for (int i = 0; i < cntRays - 16; i++) {
+    for (int j = 0; j < 4; j++, i++) {
+      raysPack.SetLoc(j, rays[i].mOrigin);
+      raysPack.SetDir(j, rays[i].mDirection);
+    } // for
+#if 0
+    for (int j = 0; j < 16; j++) {
+      cout << "orig = " << raysToTest[j].mOrigin << " dir = "
+	   << raysToTest[j].mDirection << endl;
+    }
+#endif
+    raysPack.ComputeDirSign();
+    raycaster->CastRaysPacket2x2(raysPack, castDoubleRays);
+    
+#ifdef DEBUGRESULTS
+    if (!castDoubleRays) {
+      float T = rays[i].IntersectionRes[0].tdist;
+      if ( (res != results[i].hitA) ||
+	   ((res)&&(results[i].hitA)&& (fabs(T - results[i].hitAT)) > 2.0f)
+	   )
+	{
+	  if (printOut)
+	    cout << " i = " << i << " ";
+	  if ((res != 0) && (results[i].hitA == 0)) {
+	    cntIntelNoWeYes++;
+	    if (printOut)
+	      cout << "Intel no intersection, our alg intersection at t = " << T << endl;
+	  }
+	  else {
+	    if ((res = 0) && (results[i].hitA)) {
+	      cntIntelYesWeNo++;
+	      if (printOut)
+		cout << "Intel intersection at = " << results[i].hitAT
+		     << " , our alg no intersection = " << endl;
+	      
+	    }
+	    else {
+	      cntIntelOurDistMismatch++;
+	      sumDistOurAlg += T;
+	      sumDistIntel += results[i].hitAT;
+	      sumDistAbsDiff += fabs(T - results[i].hitAT);
+	      if (printOut)
+		cout << "Intel intersection at = " << results[i].hitAT
+		     << " , our alg intersection at = " << T << endl;	    
+	    }
+	  }
+	}
+    } // double rays
+    else {
+      // checking for results of double rays - not yet implemented
+    }
+#endif    
+  }
+
+#endif // __SSE__ 
+#endif // _USE_HAVRAN_SSE
+
+  timer.Stop();
+  long t2 = GetTime();
+  cout<<"\n#RAY_CAST_TIME = ";
+  cout << TimeDiff(t1, t2)<<" [mikrosec]"
+       << " userTime = " << timer.UserTime() << " realTime = "
+       << timer.RealTime() << endl;
+    ;
+  cout << "Rays shot per milisecond [userTimer] = "
+       << ((double)cntRays * mult/(double)timer.UserTime()) / 1000.f << endl;
+
+#ifdef DEBUGRESULTS  
+  cout << "cntIntelYesWeNo = " << cntIntelYesWeNo << endl;
+  cout << "cntIntelNoWeYes = " << cntIntelNoWeYes << endl;
+  cout << "cntIntelOurDistMismatch = " << cntIntelOurDistMismatch++ << endl;
+  cout << " sumDistIntel = " << sumDistIntel/(double)cntIntelOurDistMismatch << endl;
+  cout << " sumDistOur = " << sumDistOurAlg/(double)cntIntelOurDistMismatch << endl;
+  cout << " sumAbsDiffDist = " << sumDistAbsDiff/(double)cntIntelOurDistMismatch
+       << endl;
+#endif  
+  cout << "Done\n" << endl;
+  return;
+}
Index: /GTP/trunk/Lib/Vis/Preprocessing/src/havran/testrt.h
===================================================================
--- /GTP/trunk/Lib/Vis/Preprocessing/src/havran/testrt.h	(revision 2595)
+++ /GTP/trunk/Lib/Vis/Preprocessing/src/havran/testrt.h	(revision 2595)
@@ -0,0 +1,16 @@
+
+#ifndef _TESTRT_H_
+#define _TESTRT_H_
+
+// This is for testing RT implementation
+void
+TestRTcamera(int argc, char **argv);
+
+void
+TestRTfromFile(int argc, char **argv);
+
+void
+TestRTfromFilePackets(int argc, char **argv);
+
+#endif // _TESTRT_H_
+
