// 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
					 )
{
#ifdef USE_HAVRAN_RAYCASTER

  //int hit_triangles[16];
  //float dist[16];
  Vector3 normals[16];
  Vector3 min = sbox.Min();
  Vector3 max = sbox.Max();
  
  int packets = (int)rays.size() / 16;
  
  int i, 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];
  }
#endif // USE_HAVRAN_RAYCASTER

  return;
}

void HavranDynRayCaster::CastRays(
			       SimpleRayContainer &rays,
			       VssRayContainer &vssRays,
			       const AxisAlignedBox3 &sbox,
			       const bool castDoubleRay,
			       const bool pruneInvalidRays )
{
  int buckets = (int)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 

  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_RAYCASTER 

}
#endif // _USE_HAVRAN_SSE

 
} // the namespace