Index: GTP/trunk/Lib/Vis/Preprocessing/src/HavranDynRayCaster.cpp
===================================================================
--- GTP/trunk/Lib/Vis/Preprocessing/src/HavranDynRayCaster.cpp	(revision 2630)
+++ GTP/trunk/Lib/Vis/Preprocessing/src/HavranDynRayCaster.cpp	(revision 2630)
@@ -0,0 +1,939 @@
+
+// Written by Vlastimil Havran, December 2007
+
+// This macro allows to use the ray shooting written by
+// Vlastimil Havran, 2007-2008
+
+
+#include "VssRay.h"
+#include "KdTree.h"
+#include "Preprocessor.h"
+#include "IntersectableWrapper.h"
+
+#ifdef USE_HAVRAN_RAYCASTER
+#include "ktbconf.h"
+#include "timer.h"
+#include "ktball.h"
+#include "ktb.h"
+#ifdef _USE_HAVRAN_SSE
+
+#include "raypack.h"
+#endif // _USE_HAVRAN_SSE
+#endif // USE_HAVRAN_RAYCASTER
+#include "HavranRayCaster.h"
+
+#define DEBUG_RAYCAST 0
+
+namespace GtpVisibilityPreprocessor {
+
+#ifdef _USE_HAVRAN_SSE
+// static rays
+GALIGN16 RayPacket2x2
+HavranDynRayCaster::raypack_t;
+#endif // _USE_HAVRAN_SSE
+  
+HavranDynRayCaster::HavranDynRayCaster(const Preprocessor &preprocessor):
+  HavranRayCaster(preprocessor), mDynKtbtree(0),
+  dynobjects(0), dynamicFlag(false)
+{
+#ifdef USE_HAVRAN_RAYCASTER 
+#ifdef _USE_HAVRAN_SSE
+  if ( ((int)(&raypack_t)) & 0xf != 0) {
+    cerr << "The ray pack data type not aligned on 16 Bytes boundary" << endl;
+    cerr << "Address is " << (void*)(&raypack_t) << endl;
+    cerr << "Reorganize your data structure to make it aligned" << endl;
+    abort();
+  }
+#endif
+#endif
+}
+  
+
+HavranDynRayCaster::~HavranDynRayCaster()
+{
+#ifdef USE_HAVRAN_RAYCASTER
+  DeleteDynamicObjects();
+#endif // USE_HAVRAN_RAYCASTER
+}
+
+void
+HavranDynRayCaster::DeleteDynamicObjects()
+{
+#ifdef USE_HAVRAN_RAYCASTER
+  delete mDynKtbtree; mDynKtbtree = 0;
+  delete dynobjects; dynobjects = 0;
+  dynamicFlag = false;
+#endif // USE_HAVRAN_RAYCASTER
+}
+
+void
+HavranDynRayCaster::AddDynamicObjecs(const ObjectContainer &objects, const Matrix4x4 &m)
+{
+#ifdef USE_HAVRAN_RAYCASTER
+  if (dynobjects)
+    DeleteDynamicObjects();
+
+  dynamicFlag = false;
+
+  if (objects.size()) {
+    dynamicFlag = true;
+    dynobjects = new ObjectContainer(objects);
+    assert(dynobjects);
+    // kd-tree built over the objects
+    mDynKtbtree = new CKTB;
+    mDynKtbtree->BuildUp(*dynobjects);
+
+    // The matrix to transform the objects
+    UpdateDynamicObjects(m);
+  }
+#endif // USE_HAVRAN_RAYCASTER  
+}
+
+void
+HavranDynRayCaster::UpdateDynamicObjects(const Matrix4x4 &m)
+{
+#ifdef USE_HAVRAN_RAYCASTER
+  matTr = m; 
+  matTr_inv = Invert(matTr);
+#endif // USE_HAVRAN_RAYCASTER
+}
+
+// Using packet of 4 rays supposing that these are coherent
+// We give a box to which each ray is clipped to before the
+// ray shooting is computed !
+void HavranDynRayCaster::CastRaysPacket4(const Vector3 &minBox,
+					 const Vector3 &maxBox,
+					 const Vector3 origin4[],
+					 const Vector3 direction4[],
+					 int     result4[],
+					 float   dist4[])
+{
+#ifdef USE_HAVRAN_RAYCASTER 
+#ifdef _USE_HAVRAN_SSE  
+  for (int i = 0; i < 4; i++) {
+    result4[i] = -1;
+    result4_t[4] = -1;
+    raypack.SetLoc(i, origin4[i]);
+    raypack.SetDir(i, direction4[i]);
+  }
+  
+  // 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.
+  mKtbtree->FindNearestI(raypack, minBox, maxBox);
+  
+  for (int i = 0; i < 4; i++) {
+    // if ray intersects an object, we set the pointer to
+    // this object
+    Intersectable* intersectable = raypack.GetObject(i);
+    if (intersectable) {
+      result4[i] = intersectable->mId;
+      dist4[i] = raypack.GetT(i);
+    }
+  }
+
+  if (dynamicFlag) {
+    // The ray pack to be transformed
+    raypack_t = raypack;
+    raypack_t.ApplyTransform(matTr_inv);
+    
+    mDynKtbtree->FindNearestI(raypack_t, minBox, maxBox);
+    
+    for (int i = 0; i < 4; i++) {
+      // if ray intersects an object, we set the pointer to
+      // this object
+      Intersectable* intersectable = raypack_t.GetObject(i);
+      if (intersectable) {
+	float t = raypack_t.GetT(i);
+	if ( ( (result4[i] != -1) && (t < dist4[i]) ) ||
+	     (result4[i] == -1) ) {
+	  // update, dynamic object is closer
+	  dist4[i] = t;
+	  result4[i] = intersectable->mId;
+	}
+      }
+    } // for i
+  } // dynamic flag
+    
+  return;
+#else // _USE_HAVRAN_SSE
+  // Compute the result ray by ray
+  SimpleRay sray;
+  for (int i = 0; i < 4; i++) {
+    result4[i] = -1; // no intersection
+    sray.mOrigin = origin4[i];
+    sray.mDirection = direction4[i];
+    mKtbtree->FindNearestI(sray, minBox, maxBox);
+    if (SimpleRay::IntersectionRes[0].intersectable) {
+      // This is object ID - is this the triangle index ???
+      result4[i] = SimpleRay::IntersectionRes[0].intersectable->mId;
+      dist4[i] = SimpleRay::IntersectionRes[0].tdist;
+    }
+
+    if (dynamicFlag) {
+      // Now apply transform
+      ApplyTransform(sray);
+      mDynKtbtree->FindNearestI(sray, minBox, maxBox);
+      Intersectable *objdyn = SimpleRay::IntersectionRes[0].intersectable;
+      if (objdyn) {
+	float t = SimpleRay::IntersectionRes[0].tdist;
+	if ( ((result4[i] != -1) && (t < dist4[i])) ||
+	     (result4[i] == -1) ) {
+	  // = update, dynamic object is closer
+	  dist4[i] = t;
+	  result4[i] = objdyn->mId;
+	}
+      }
+    } // dynamic flag
+  } // for i;
+  
+#endif // _USE_HAVRAN_SSE  
+#endif // USE_HAVRAN_RAYCASTER 
+}
+
+
+int HavranDynRayCaster::CastRay(const SimpleRay &simpleRay,
+				VssRayContainer &vssRays,
+				const AxisAlignedBox3 &box,
+				const bool castDoubleRay,
+				const bool pruneInvalidRays)
+{
+#ifdef USE_HAVRAN_RAYCASTER 
+
+  // rays forwards - TESTED 21/1/2008 VH (only for forward rays)
+
+  int hits = 0;
+  Intersection hitA(simpleRay.mOrigin), hitB(simpleRay.mOrigin);
+    
+  // ray.mFlags &= ~Ray::CULL_BACKFACES;
+  bool result;
+  hitA.mObject = 0;
+  if ((result = mKtbtree->FindNearestI(simpleRay)))  {
+    hitA.mObject = SimpleRay::IntersectionRes[0].intersectable;
+    tdist[0] = SimpleRay::IntersectionRes[0].tdist;
+    hitA.mPoint = simpleRay.Extrap(tdist[0]);
+    hitA.mNormal = SimpleRay::IntersectionRes[0].intersectable->GetNormal(0);
+    //hitA.mNormal = (dynamic_cast< TriangleIntersectable *>
+    //		    (hitA.mObject))->GetNormal(0);
+  }
+
+  if (dynamicFlag) {
+    bool result2 = false;
+    // For a dynamic object
+    sray_t = simpleRay;
+    // Now apply transform
+    ApplyTransform(sray_t);
+    if (mDynKtbtree->FindNearestI(sray_t)) {
+      float t = SimpleRay::IntersectionRes[0].tdist;
+      if  ( ((hitA.mObject) && (t < tdist[0])) ||
+	  (!hitA.mObject) ) {
+	result = result2 = true; // dynamic object hit
+	// There is an intersection closer than the previous one
+	hitA.mObject = SimpleRay::IntersectionRes[0].intersectable;
+	tdist[0] = SimpleRay::IntersectionRes[0].tdist;
+	hitA.mPoint = simpleRay.Extrap(tdist[0]);
+	hitA.mNormal = hitA.mObject->GetNormal(0);
+	// We have to transform the normal
+	hitA.mNormal = TransformNormal(matTr_inv, hitA.mNormal);      
+      }
+    }
+    if (result && (!result2)) {
+      // We put back the result from hitA.mObject for ray tracing
+      SimpleRay::IntersectionRes[0].intersectable = hitA.mObject;
+      SimpleRay::IntersectionRes[0].tdist = tdist[0];
+    }
+  }
+  
+  bool resultB = false;
+  if (castDoubleRay) {
+    bool result2 = false;
+    // casting rays backward
+    Vector3 *v = (Vector3*)(&simpleRay.mDirection);
+    *v = -(*v);
+    // ray.mFlags &= ~Ray::CULL_BACKFACES;
+    if (mKtbtree->FindNearestI(simpleRay))  {
+      resultB = true;
+      hitB.mObject = SimpleRay::IntersectionRes[0].intersectable;
+      assert(hitB.mObject);
+      float tdist = SimpleRay::IntersectionRes[0].tdist;
+      hitB.mPoint = simpleRay.Extrap(tdist);
+      hitB.mNormal = hitB.mObject->GetNormal(0);
+    }
+    
+    if (dynamicFlag) {
+      // For a dynamic object
+      sray_t = simpleRay;
+      // Now apply transform
+      ApplyTransform(sray_t);
+      bool result2;
+      if ( mDynKtbtree->FindNearestI(sray_t)) {
+	assert(SimpleRay::IntersectionRes[0].intersectable);
+	float t = SimpleRay::IntersectionRes[0].tdist;
+	if ( ((hitB.mObject) && (t < tdist[0])) ||
+	     (!hitB.mObject) ) {
+	  // There is an intersection closer than the previous one
+	  resultB = result2 = true; // dynamic object hit
+	  hitB.mObject = SimpleRay::IntersectionRes[0].intersectable;
+	  tdist[0] = SimpleRay::IntersectionRes[0].tdist;
+	  hitB.mPoint = simpleRay.Extrap(tdist[0]);
+	  hitB.mNormal = hitB.mObject->GetNormal(0);
+	  // We have to transform the normal
+	  hitB.mNormal = TransformNormal(matTr_inv, hitB.mNormal);      
+	}
+      }
+      if ((resultB) && (!result2)) {
+	  // For visualization
+	  SimpleRay::IntersectionRes[0].intersectable = hitB.mObject;
+	  SimpleRay::IntersectionRes[0].tdist = tdist[0];
+	}      
+    } // dynamicFlag
+    
+    // restore the direction to the original
+    *v = -simpleRay.mDirection;
+  } // castDoubleRay
+
+#ifdef _PROCESS_RAY  
+  // This code is also in IntelRayCaster.cpp
+  return ProcessRay(
+		    simpleRay,
+		    hitA,
+		    hitB,
+		    vssRays,
+		    box,
+		    castDoubleRay,
+		    pruneInvalidRays
+		    );
+#else // _PROCESS_RAY
+  if (castDoubleRay)
+    return resultB;
+  return result;
+#endif // _PROCESS_RAY  
+
+#else
+  return 0;
+#endif // USE_HAVRAN_RAYCASTER   
+}
+
+void HavranDynRayCaster::CastRays16(SimpleRayContainer &rays,
+				 VssRayContainer &vssRays,
+				 const AxisAlignedBox3 &sbox,
+				 const bool castDoubleRay,
+				 const bool pruneInvalidRays)
+{
+  CastRays16(rays, 0, vssRays, sbox, castDoubleRay, pruneInvalidRays);
+}
+
+
+void HavranDynRayCaster::CastRays16(SimpleRayContainer &rays,
+				 int offset,
+				 VssRayContainer &vssRays,
+				 const AxisAlignedBox3 &sbox,
+				 const bool castDoubleRays,
+				 const bool pruneInvalidRays)
+{
+#ifdef USE_HAVRAN_RAYCASTER
+  
+#if DEBUG_RAYCAST
+  Debug << "C16 " << flush;
+#endif
+
+  // Use special algorithm for 16 rays at once
+  if (castDoubleRays) {
+    // Here we decompose shooting into two phases
+
+    // First we delete the results from casting dynamic objects
+    if (dynamicFlag) {
+      for (int i = 0; i < 32; i++)
+	objI[i] = 0;
+    }
+    
+    // Here we shoot first backward rays and forward ones
+    SimpleRayContainer::iterator sit = rays.begin() + offset; 
+    SimpleRayContainer::const_iterator sit_end = rays.begin() + offset + 16;
+    for ( ; sit != sit_end; ++ sit) 
+    {
+      (*sit).mDirection = - (*sit).mDirection;
+    }
+    // backward rays to be shot - saving with offset 16
+
+#ifdef _USE_HAVRAN_SSE  
+    mKtbtree->SetOffset(0);
+    mKtbtree->FindNearestI_16oneDir(rays, offset, 16);
+#else
+    mKtbtree->SetOffset(16);
+    mKtbtree->FindNearestI_16oneDirNoSSE(rays, offset);
+#endif // _USE_HAVRAN_SSE
+
+    if (dynamicFlag) {
+      for (int i = 0; i < 16; i++) {
+	// store the origin and direction of the ray
+	orig[i] = rays[offset+i].mOrigin;
+	dirs[i] = rays[offset+i].mDirection;
+
+	objI[i+16] = SimpleRay::IntersectionRes[i+16].intersectable;
+	if (objI[i+16])
+	  normal[i+16] = objI[i+16]->GetNormal(0);
+	tdist[i+16] = SimpleRay::IntersectionRes[i+16].tdist;
+	ApplyTransform(rays[offset+i]);
+      } // for
+      // Now shoot the ray with dynamic object
+#ifdef _USE_HAVRAN_SSE  
+      mDynKtbtree->SetOffset(0);
+      mDynKtbtree->FindNearestI_16oneDir(rays, offset, 16);
+#else
+      mDynKtbtree->SetOffset(16);
+      mDynKtbtree->FindNearestI_16oneDirNoSSE(rays, offset);
+#endif // _USE_HAVRAN_SSE
+      for (int i = 0; i < 16; i++) {
+	Intersectable *intersectable =
+	  SimpleRay::IntersectionRes[i+16].intersectable;
+	if (intersectable) {
+	  float t = SimpleRay::IntersectionRes[i+16].tdist;
+	  if  ( ((objI[i+16]) && (t < tdist[i+16])) ||
+		(!objI[i+16]) ) {
+	    // There is an intersection closer than the previous one
+	    objI[i+16] = intersectable;
+	    tdist[i+16] = SimpleRay::IntersectionRes[i+16].tdist;
+	    normal[i+16] = intersectable->GetNormal(0);
+	    // We have to transform the normal
+	    normal[i+16] = TransformNormal(matTr_inv, normal[i+16]);      
+	  } // if update
+	} // if intersected dynamic object
+	else {
+	  // we copy back the static object - only for visualization
+	  SimpleRay::IntersectionRes[i+16].intersectable = objI[i+16];
+	  SimpleRay::IntersectionRes[i+16].tdist = tdist[i+16];	  
+ 	}
+      } // for 
+    } // dynamic flag
+    
+    // set the ray direction to original direction
+    if (dynamicFlag) {
+      sit = rays.begin() + offset;
+      int i = 0;
+      for ( ; sit != sit_end; ++sit, ++i) 
+      {
+	rays[offset+i].mOrigin = orig[i];
+	rays[offset+i].mDirection = -dirs[i];
+      }
+    }
+    else {
+      // no dynamic objects
+      sit = rays.begin() + offset; 
+      for ( ; sit != sit_end; ++ sit) 
+      {
+	(*sit).mDirection = - (*sit).mDirection;
+      }
+    }
+    // forward rays to be shot
+#ifdef _USE_HAVRAN_SSE
+    mKtbtree->SetOffset(0);
+    mKtbtree->FindNearestI_16oneDir(rays, offset, 0);
+#else
+    mKtbtree->SetOffset(0);
+    mKtbtree->FindNearestI_16oneDirNoSSE(rays, offset);
+#endif // _USE_HAVRAN_SSE
+
+    if (dynamicFlag) {
+      for (int i = 0; i < 16; i++) {
+	objI[i] = SimpleRay::IntersectionRes[i].intersectable;
+	if (objI[i])
+	  normal[i] = objI[i]->GetNormal(0);
+	tdist[i] = SimpleRay::IntersectionRes[i].tdist;
+	ApplyTransform(rays[offset+i]);
+      } // for
+      // Now shoot the ray with dynamic object
+#ifdef _USE_HAVRAN_SSE  
+      mDynKtbtree->SetOffset(0);
+      mDynKtbtree->FindNearestI_16oneDir(rays, offset, 0);
+#else
+      mDynKtbtree->SetOffset(0);
+      mDynKtbtree->FindNearestI_16oneDirNoSSE(rays, offset);
+#endif // _USE_HAVRAN_SSE
+      for (int i = 0; i < 16; i++) {
+	Intersectable *intersectable
+	  = SimpleRay::IntersectionRes[i].intersectable;
+	if (intersectable) {
+	  float t = SimpleRay::IntersectionRes[i].tdist;
+	  if  ( ((objI[i]) && (t < tdist[i])) ||
+		(!objI[i]) ) {
+	    // There is an intersection closer than the previous one
+	    objI[i] = intersectable;
+	    tdist[i] = SimpleRay::IntersectionRes[i].tdist;
+	    normal[i] = objI[i]->GetNormal(0);
+	    // We have to transform the normal
+	    normal[i] = TransformNormal(matTr_inv, normal[i]);
+	  } // update
+	} // intersection
+	// we have to recover the ray
+	rays[offset+i].mOrigin = orig[i];
+	rays[offset+i].mDirection = -dirs[i];	
+      } // for i
+    } // dynamic flag
+  } // cast double rays
+  else {
+    // ONLY forward rays - TESTED 21/1/2007 VH
+
+    // First we delete the results from casting dynamic objects
+    if (dynamicFlag) {
+      for (int i = 0; i < 16; i++) {
+	objI[i] = 0;
+	orig[i] = rays[offset+i].mOrigin;
+	dirs[i] = rays[offset+i].mDirection;	
+      }
+    }
+
+    // Shoot all 16 rays  at the same time using a special algorithm
+    mKtbtree->SetOffset(0);
+#ifdef _USE_HAVRAN_SSE  
+    mKtbtree->FindNearestI_16oneDir(rays, offset, 0);    
+#else
+    mKtbtree->FindNearestI_16oneDirNoSSE(rays, offset);    
+#endif // _USE_HAVRAN_SSE
+
+    // Here we must add dynamic objects
+    if (dynamicFlag) {
+      for (int i = 0; i < 16; i++) {
+	objI[i] = SimpleRay::IntersectionRes[i].intersectable;
+	if (objI[i])
+	  normal[i] = objI[i]->GetNormal(0);
+	tdist[i] = SimpleRay::IntersectionRes[i].tdist;
+	ApplyTransform(rays[offset+i]);
+      } // for
+      // Now shoot the ray with dynamic object
+#ifdef _USE_HAVRAN_SSE  
+      mDynKtbtree->SetOffset(0);
+      mDynKtbtree->FindNearestI_16oneDir(rays, offset, 0);
+#else
+      mDynKtbtree->SetOffset(0);
+      mDynKtbtree->FindNearestI_16oneDirNoSSE(rays, offset);
+#endif // _USE_HAVRAN_SSE
+      for (int i = 0; i < 16; i++) {
+	Intersectable *intersectable
+	  = SimpleRay::IntersectionRes[i].intersectable;
+	if (intersectable) {
+	  float t = SimpleRay::IntersectionRes[i].tdist;
+	  if  ( ((objI[i]) && (t < tdist[i])) ||
+		(!objI[i]) ) {	    
+	    // There is an intersection closer than the previous one
+	    objI[i] = intersectable;
+	    tdist[i] = SimpleRay::IntersectionRes[i].tdist;
+	    normal[i] = objI[i]->GetNormal(0);
+	    // We have to transform the normal
+	    normal[i] = TransformNormal(matTr_inv, normal[i]);
+	  } // update
+	} // intersection
+	// we have to recover the rays back
+	rays[offset+i].mOrigin = orig[i];
+	rays[offset+i].mDirection = dirs[i];	
+      } // for i
+    } // dynamic flag
+  }
+  
+  if (dynamicFlag) {
+    for (int i=0, k=offset; i < 16; i++, k++) 
+    {
+      Intersection hitA(rays[k].mOrigin), hitB(rays[k].mOrigin);
+
+#if DEBUG_RAYCAST
+      Debug<<"FH\n"<<flush;
+#endif
+
+      Intersectable *intersectable = objI[i];
+      // For visualization needs only - forward rays
+      SimpleRay::IntersectionRes[i].intersectable = objI[i];
+      SimpleRay::IntersectionRes[i].tdist = tdist[i];
+      if (intersectable)
+      {
+	hitA.mObject = intersectable;
+	// Get the normal of that face
+	hitA.mNormal = normal[i];
+	
+	//-rays[index+i].mDirection; // $$ temporary
+	hitA.mPoint = rays[k].Extrap(tdist[i]);
+      }
+    
+#if DEBUG_RAYCAST
+      Debug<<"BH\n"<<flush;
+#endif
+
+      if (castDoubleRays) 
+      {
+	Intersectable *intersectable = objI[i+16];
+
+	if (intersectable)
+	{ 
+	  hitB.mObject = intersectable;
+	  hitB.mNormal = normal[i+16];
+	  hitB.mPoint = rays[k].Extrap(-tdist[i+16]);
+	}
+      }
+    
+#if DEBUG_RAYCAST
+      Debug<<"PR\n"<<flush;
+#endif
+
+#ifdef _PROCESS_RAY  
+      ProcessRay(rays[k],
+		 hitA,
+		 hitB,
+		 vssRays,
+		 sbox,
+		 castDoubleRays,
+		 pruneInvalidRays
+		 );
+#endif
+    } // for
+  }
+  else {
+    // no dynamic objects
+    for (int i=0, k=offset; i < 16; i++, k++) 
+    {
+      Intersection hitA(rays[k].mOrigin), hitB(rays[k].mOrigin);
+
+#if DEBUG_RAYCAST
+      Debug<<"FH\n"<<flush;
+#endif
+
+      Intersectable *intersectable
+	= SimpleRay::IntersectionRes[i].intersectable;
+
+      if (intersectable)
+      {
+	hitA.mObject = intersectable;
+	// Get the normal of that face
+	hitA.mNormal = intersectable->GetNormal(0);
+	
+	//-rays[index+i].mDirection; // $$ temporary
+	float tdist = SimpleRay::IntersectionRes[i].tdist;
+	hitA.mPoint = rays[k].Extrap(tdist);
+      }
+      
+#if DEBUG_RAYCAST
+      Debug<<"BH\n"<<flush;
+#endif
+
+      if (castDoubleRays) 
+      {
+	Intersectable *intersectable =
+	  SimpleRay::IntersectionRes[i+16].intersectable;
+	
+	if (intersectable)
+	{ 
+	  hitB.mObject = intersectable;
+	  hitB.mNormal = intersectable->GetNormal(0);;
+	  float tdist = SimpleRay::IntersectionRes[16+i].tdist;
+	  hitB.mPoint = rays[k].Extrap(-tdist);
+	}
+      }
+    
+#if DEBUG_RAYCAST
+      Debug<<"PR\n"<<flush;
+#endif
+
+#ifdef _PROCESS_RAY  
+      ProcessRay(rays[k],
+		 hitA,
+		 hitB,
+		 vssRays,
+		 sbox,
+		 castDoubleRays,
+		 pruneInvalidRays
+		 );
+#endif
+    } // for
+  } // dynamic objects
+
+  
+#if DEBUG_RAYCAST
+  Debug<<"C16F\n"<<flush;
+#endif
+
+#endif // USE_HAVRAN_RAYCASTER 
+}
+
+
+
+void
+HavranDynRayCaster::CastSimpleForwardRays(
+					  SimpleRayContainer &rays,
+					  const AxisAlignedBox3 &sbox
+					 )
+{
+  int hit_triangles[16];
+  float dist[16];
+  Vector3 normals[16];
+  Vector3 min = sbox.Min();
+  Vector3 max = sbox.Max();
+  
+  int packets = rays.size() / 16;
+  
+  int i, j, k = 0;
+  Vector3 dir;
+  
+  // By groups of rays
+  for (i=0; i < packets; i++) {
+    int offset = i * 16;
+    mKtbtree->FindNearestI_16oneDir(rays, offset, 0);
+
+    if (dynamicFlag) {
+      for (int i = 0; i < 16; i++) {
+	orig[i] = rays[i].mOrigin;
+	dirs[i] = rays[i].mDirection;
+	
+	tdist[i] = SimpleRay::IntersectionRes[i].tdist;
+	objI[i] = SimpleRay::IntersectionRes[i].intersectable;
+	// for dynamic objects
+	ApplyTransform(rays[i]);
+      } // for
+
+      // shoot the rays against dynamic objects
+      mDynKtbtree->FindNearestI_16oneDir(rays, offset, 0);
+      // and combine the results
+      for (int i = 0; i < 16; i++) {
+	float t = SimpleRay::IntersectionRes[i].tdist;
+	if (SimpleRay::IntersectionRes[i].intersectable) {
+	  if  ( ((objI[i]) && (t < tdist[i])) ||
+		(!objI[i]) ) {
+	    // There is an intersection closer than the previous one
+	    objI[i] = SimpleRay::IntersectionRes[i].intersectable;
+	    tdist[i] = SimpleRay::IntersectionRes[i].tdist;
+	  } // update
+	} // intersection
+	else {
+	  // Only for visualization
+	  SimpleRay::IntersectionRes[i].intersectable = objI[i];
+	  SimpleRay::IntersectionRes[i].tdist = tdist[i];
+	}
+	// recover ray origin and direction
+	rays[i].mOrigin = orig[i];
+	rays[i].mDirection = dirs[i];	
+      } // for
+
+    } // dynamic objects
+    
+    // ??? What to do with the results ? These are
+    // not used at the moment also in IntelRayCaster.cpp
+  } // for
+
+
+  i = 0;
+  int k2 = k;
+  for (; k < rays.size(); k++, i++) {
+    double normal[3];
+    // Intersect with the static scene
+    bool result = mKtbtree->FindNearestI(rays[k]);
+    tdist[i] = SimpleRay::IntersectionRes[0].tdist;
+    objI[i] = SimpleRay::IntersectionRes[0].intersectable;
+    
+    if (dynamicFlag) {
+      orig[i] = rays[k].mOrigin;
+      dirs[i] = rays[k].mDirection;
+
+      // Intersect with the dynamic object
+      ApplyTransform(rays[k]);
+      bool result2 = false;
+      if (mDynKtbtree->FindNearestI(rays[k])) {
+	float t = SimpleRay::IntersectionRes[0].tdist;
+	if  ( ((objI[i]) && (t < tdist[i])) ||
+	      (!objI[i]) ) {
+	  // There is an intersection closer than the previous one
+	  result2 = true;
+	  objI[i] = SimpleRay::IntersectionRes[0].intersectable;
+	  tdist[i] = t;
+	}
+      }
+      
+      // recover ray origin and direction
+      rays[k].mOrigin = orig[i];
+      rays[k].mDirection = dirs[i];	
+    } // dynamic objects
+    
+    // ??? What to do with the results ? These are
+    // not used at the moment also in IntelRayCaster.cpp
+  }
+  // The result will be in the array
+  i = 0;
+  for (; k2 < rays.size(); k2++, i++) {
+    SimpleRay::IntersectionRes[i].intersectable = objI[i];
+    SimpleRay::IntersectionRes[i].tdist = tdist[i];
+  }
+
+  return;
+}
+
+void HavranDynRayCaster::CastRays(
+			       SimpleRayContainer &rays,
+			       VssRayContainer &vssRays,
+			       const AxisAlignedBox3 &sbox,
+			       const bool castDoubleRay,
+			       const bool pruneInvalidRays )
+{
+  int buckets = rays.size()/16;
+  int offset = 0;
+
+#if 0
+  int time = GetTime();
+  CastSimpleForwardRays(rays, sbox);
+  cout<<1e-3*2*rays.size()/TimeDiff(time, GetTime())<<" Mrays/sec"<<endl;
+#endif
+
+  // Cast only by 16 rays at once
+  for (int i=0; i < buckets; i++, offset+=16) {
+    CastRays16(rays, offset, vssRays, sbox,
+	       castDoubleRay, pruneInvalidRays);
+
+    if ((int)rays.size() > 100000 && i % (100000/16) == 0)
+      cout<<"\r"<<offset<<"/"<<(int)rays.size()<<"\r";
+  }
+
+  // Cast the rest of the rays
+  for (; offset < (int)rays.size(); offset++)
+    CastRay(rays[offset], vssRays, sbox, castDoubleRay, pruneInvalidRays);
+
+  return;
+}
+
+#ifdef _USE_HAVRAN_SSE
+// BUG1 41579  196948 1064111
+// BUG2 254    1672   10869
+  
+// Just for testing concept
+void
+HavranDynRayCaster::CastRaysPacket2x2(RayPacket2x2 &raysPack,
+				      bool castDoubleRay,
+				      const bool pruneInvalidRays)
+{
+#ifdef USE_HAVRAN_RAYCASTER 
+#ifdef _USE_HAVRAN_SSE
+
+  if (castDoubleRay) {
+    // cast forward rays
+    mKtbtree->FindNearestI(raysPack);
+
+    if (dynamicFlag) {
+      // The ray pack to be transformed
+      raypack_t = raysPack;
+      raypack.ApplyTransform(matTr_inv);
+    
+      mDynKtbtree->FindNearestI(raypack_t);
+    
+      for (int i = 0; i < 4; i++) {
+	// if ray intersects an object, we set the pointer to
+	// this object
+	Intersectable* intersectable = raypack_t.GetObject(i);
+	if (intersectable) {
+	  Intersectable *o = raysPack.GetObject(i);
+	  if ( ((o) && (raypack_t.GetT(i) < tdist[i])) ||
+	       (!o) ) {
+	    // update, dynamic object is closer
+	    raysPack.SetT(i, raypack_t.GetT(i));
+	    raysPack.SetObject(i, raypack_t.GetObject(i));
+	  } // if update is required
+	} // if intersectable
+      } // for i
+    } // dynamic object    
+
+    // Double (=opposite) direction
+    for (int i = 0; i < 4; i++)
+      raysPack.SetDir(i, -raysPack.GetDir(i));
+    // cast backward rays
+    mKtbtree->FindNearestI(raysPack);
+
+    if (dynamicFlag) {
+
+      // The ray pack to be transformed
+      raypack_t = raysPack;
+      raypack_t.ApplyTransform(matTr_inv);
+    
+      mDynKtbtree->FindNearestI(raypack_t);
+    
+      for (int i = 0; i < 4; i++) {
+	// if ray intersects an object, we set the pointer to
+	// this object
+	Intersectable* intersectable = raypack_t.GetObject(i);
+	if (intersectable) {
+	  Intersectable *o = raysPack.GetObject(i);
+	  if ( ((o) && (raypack_t.GetT(i) < tdist[i])) ||
+	       (!o) ) {
+	    // update, dynamic object is closer
+	    raysPack.SetT(i, raypack_t.GetT(i));
+	    raysPack.SetObject(i, raypack_t.GetObject(i));
+	  } // if update is required
+	} // if intersectable
+      } // for i
+    } // dynamic object    
+    
+    // reverse the rays back
+    for (int i = 0; i < 4; i++)
+      raysPack.SetDir(i, -raysPack.GetDir(i));
+  }
+  else {
+    // ONLY forward rays - TESTED 21/1/2007 VH
+
+    mKtbtree->FindNearestI(raysPack);
+
+    if (dynamicFlag) {
+      // The ray pack to be transformed
+      raypack_t = raysPack;
+      raypack_t.ApplyTransform(matTr_inv);
+    
+      mDynKtbtree->FindNearestI(raypack_t);
+    
+      for (int i = 0; i < 4; i++) {
+	// if ray intersects an object, we set the pointer to
+	// this object
+	Intersectable* intersectable = raypack_t.GetObject(i);
+	if (intersectable) {
+	  Intersectable *o = raysPack.GetObject(i);
+	  if ( ((o) && (raypack_t.GetT(i) < tdist[i])) ||
+	       (!o) ) {
+	    // update, dynamic object is closer
+	    raysPack.SetT(i, raypack_t.GetT(i));
+	    raysPack.SetObject(i, raypack_t.GetObject(i));
+	  } // if update is required
+	} // if intersectable
+      } // for i
+    } // dynamic object    
+    
+#if 0
+    // Only verification of correctness by casting single rays
+    static int cntBugs = 0;
+    SimpleRay ray;
+    int cntErrors = 0, cntDistErrors = 0;
+    bool newBug = false;
+    for (int i = 0; i < 4; i++) {
+      ray.mOrigin = raysPack.GetLoc(i);
+      ray.mDirection = raysPack.GetDir(i);
+      mKtbtree->FindNearestI(ray);
+      if (raysPack.GetObject(i) != SimpleRay::IntersectionRes[0].intersectable) {
+	float dist = (raysPack.GetT(i) - SimpleRay::IntersectionRes[0].tdist);
+	if (fabs(dist) > 0.001f) {
+	  cntErrors++; newBug = true;
+	  cntBugs++;
+	  cout << " BUG1 d= " << dist;
+	}
+      }
+      else {
+	float dist = 0.f;
+	if (raysPack.GetObject(i) && SimpleRay::IntersectionRes[0].intersectable)
+	  if (fabs((dist=(fabs (raysPack.GetT(i) - SimpleRay::IntersectionRes[0].tdist)))) > 1.f) {
+	    cntDistErrors++; newBug = true; cntBugs++;
+	    cout << " BUG2 distdf= " << dist ;	    
+	  }
+      }
+    } // for
+    if (newBug) cout << " CB= " << cntBugs << "\n";
+#endif
+  }
+
+  return;
+#endif // _USE_HAVRAN_SSE
+#endif // USE_HAVRAN_RAYCASTER 
+}
+
+  
+#endif // _USE_HAVRAN_SSE
+
+
+} // the namespace
Index: GTP/trunk/Lib/Vis/Preprocessing/src/SG08/run_test_vienna
===================================================================
--- GTP/trunk/Lib/Vis/Preprocessing/src/SG08/run_test_vienna	(revision 2620)
+++ GTP/trunk/Lib/Vis/Preprocessing/src/SG08/run_test_vienna	(revision 2630)
@@ -7,4 +7,6 @@
 COMMAND="../scripts/preprocessor.sh -preprocessor_quit_on_finish+ -preprocessor_use_gl_renderer- -preprocessor_evaluate_filter- -preprocessor_detect_empty_viewspace+ -samples_per_evaluation=5000000 -samples_per_pass=1000000 -total_samples=12000000 -view_cells_use_kd_pvs+ -preprocessor_ray_cast_method=0"
 
+# FINAL TIME PLOT
+COMMAND="../scripts/preprocessor.sh -preprocessor_quit_on_finish+ -preprocessor_use_gl_renderer- -preprocessor_evaluate_filter- -preprocessor_detect_empty_viewspace+ -samples_per_evaluation=5000000 -samples_per_pass=1000000 -total_samples=4000000000 -total_time=7200 -view_cells_use_kd_pvs+"
 
 SCENE=../data/vienna/vienna_cropped.obj
