source: GTP/trunk/Lib/Vis/Preprocessing/src/HavranRayCaster.cpp @ 2686

// 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"
#include "Environment.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
HavranRayCaster::raypack;
#endif // _USE_HAVRAN_SSE

  
HavranRayCaster::HavranRayCaster(const Preprocessor &preprocessor):
  RayCaster(preprocessor), mKtbtree(0)
{
#ifdef USE_HAVRAN_RAYCASTER 
  mKtbtree = new CKTB();
#endif  
}
  

HavranRayCaster::~HavranRayCaster()
{
#ifdef USE_HAVRAN_RAYCASTER 
  delete mKtbtree;
  mKtbtree = 0;
#endif // USE_HAVRAN_RAYCASTER
}

void
HavranRayCaster::Build(ObjectContainer &objlist)
{
#ifdef USE_HAVRAN_RAYCASTER 
  char buff[256];
  Environment::GetSingleton()->GetStringValue("Scene.filename", buff);
  string filename(buff);
  const string kdfile = ReplaceSuffix(filename, ".obj", ".kdh");

  if (!ImportBinTree(kdfile, objlist)) {
        cout << "\nKd-tree for Havran ray caster imported."<<endl<<flush;
  }
  else {
    CTimer timer;
    cout << "\nBuilding up kd-tree for Havran ray caster ..."<<endl<<flush;
    
    timer.Start();
    mKtbtree->BuildUp(objlist);
    timer.Stop();
    cout << "\nBuilding up kd-tree is finished, user time = "
         << timer.UserTime() << " real time = " << timer.RealTime()
         << endl;
    ExportBinTree(kdfile);
  }
#endif
}

bool
HavranRayCaster::ExportBinTree(const string &filename)
{
#ifdef USE_HAVRAN_RAYCASTER 
  return mKtbtree->ExportBinTree(filename);
#else
        return false;
#endif
}

bool
HavranRayCaster::ImportBinTree(const string &filename, ObjectContainer &objects)
{
#ifdef USE_HAVRAN_RAYCASTER 
        return mKtbtree->ImportBinTree(filename, objects);
#else
        return false;
#endif
}

  
// 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 HavranRayCaster::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;
    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;
    if ( (intersectable = raypack.GetObject(i)) ) {
      // $$JB this is very hacky - > should be replaced with fetching the index of the triangle
      // $$VH - this is object ID - is this the triangle index ???
      result4[i] = intersectable->mId;
      dist4[i] = raypack.GetT(i);
    }
  }
  
  return;
#else // _USE_HAVRAN_SSE
  // Compute the result ray by ray
  SimpleRay sray;
  for (int i = 0; i < 4; i++) {
    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;
    }
    else {
      result4[i] = -1; // no intersection
    }
  } // for i;
  
#endif // _USE_HAVRAN_SSE  
#endif // USE_HAVRAN_RAYCASTER 
}


int HavranRayCaster::CastRay(const SimpleRay &simpleRay,
                             VssRayContainer &vssRays,
                             const AxisAlignedBox3 &box,
                             const bool castDoubleRay,
                             const bool pruneInvalidRays)
{
#ifdef USE_HAVRAN_RAYCASTER 

  int hits = 0;
  Intersection hitA(simpleRay.mOrigin), hitB(simpleRay.mOrigin);
  
  // inside test for bounding box
  // enlarge box slightly so the view point fits for sure
  //  AxisAlignedBox3 sbox = box;
  //  sbox.Enlarge(Vector3(-Limits::Small));
  // $$ JB moved here from Validate routine

//   if (!box.IsInside(simpleRay.mOrigin)) {
//      cout<<"out of box "<<simpleRay.mOrigin<<" "<<box<<endl;
//      return 0;
//   }
  
  // ray.mFlags &= ~Ray::CULL_BACKFACES;
  bool result;
  if ((result = mKtbtree->FindNearestI(simpleRay)))  {
    hitA.mObject = SimpleRay::IntersectionRes[0].intersectable;
    float tdist = SimpleRay::IntersectionRes[0].tdist;
    hitA.mPoint = simpleRay.Extrap(tdist);
    hitA.mNormal = SimpleRay::IntersectionRes[0].intersectable->GetNormal(0);
    //hitA.mNormal = (dynamic_cast< TriangleIntersectable *>
    //              (hitA.mObject))->GetNormal(0);
  }
  
  
  if (castDoubleRay) {
    Vector3 *v = (Vector3*)(&simpleRay.mDirection);
    *v = -(*v);
    // ray.mFlags &= ~Ray::CULL_BACKFACES;
    if (mKtbtree->FindNearestI(simpleRay))  {
      hitB.mObject = SimpleRay::IntersectionRes[0].intersectable;
      float tdist = SimpleRay::IntersectionRes[0].tdist;
      hitB.mPoint = simpleRay.Extrap(tdist);
      hitB.mNormal = SimpleRay::IntersectionRes[0].intersectable->GetNormal(0);
      //hitB.mNormal = (dynamic_cast< TriangleIntersectable *>
      //              (hitB.mObject))->GetNormal(0);
    }
    // restore the direction to the original
    *v = -simpleRay.mDirection;
  }

#ifdef _PROCESS_RAY  
  // This code is also in IntelRayCaster.cpp
  return ProcessRay(
                    simpleRay,
                    hitA,
                    hitB,
                    vssRays,
                    box,
                    castDoubleRay,
                    pruneInvalidRays
                    );
#else // _PROCESS_RAY
  return result;
#endif // _PROCESS_RAY  

#else
  return 0;
#endif // USE_HAVRAN_RAYCASTER   
}

void HavranRayCaster::CastRays16(SimpleRayContainer &rays,
                                 VssRayContainer &vssRays,
                                 const AxisAlignedBox3 &sbox,
                                 const bool castDoubleRay,
                                 const bool pruneInvalidRays)
{
  CastRays16(rays, 0, vssRays, sbox, castDoubleRay, pruneInvalidRays);
}


void HavranRayCaster::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

#if 1
  // This is shooting ray by ray
  SimpleRayContainer::const_iterator sit, sit_end = rays.end();

  // no acceleration for ray bundles implemented right now
  for (sit = rays.begin(); sit != sit_end; ++ sit) 
  {
    CastRay(*sit, vssRays, sbox, castDoubleRays, pruneInvalidRays);
  }
#else
  // Use special algorithm for 16 rays at once
  if (castDoubleRays) {

#if 0 // ------------------------------------------------
    // Special routine to cast double sided rays
    mKtbtree->SetOffset(0);
#ifdef _USE_HAVRAN_SSE  
    mKtbtree->FindNearestI_16twoDir(rays);
#else
    mKtbtree->FindNearestI_16twoDirNoSSE(rays);    
#endif
#else // -------------------------------------------------
    // Here we decompose shooting into two phases

    // 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
    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  
#endif // ------------------------------------------------
  } // cast double rays
  else {
    // ONLY single rays
    // 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  
  }
#endif
  
  
  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 *intersect = SimpleRay::IntersectionRes[i].intersectable;

    if (intersect)
    {
      hitA.mObject = intersect;
      // Get the normal of that face
      hitA.mNormal = intersect->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 *intersect =
        SimpleRay::IntersectionRes[i+16].intersectable;

      if (intersect)
      { 
        hitB.mObject = intersect;
        hitB.mNormal = intersect->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

  
#if DEBUG_RAYCAST
  Debug<<"C16F\n"<<flush;
#endif

#endif // USE_HAVRAN_RAYCASTER 
}



void
HavranRayCaster::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 = 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);
    // ??? What to do with the results ? These are
    // not used at the moment also in IntelRayCaster.cpp
  } // for


  for (; k < rays.size(); k++) {
    //double normal[3];
    mKtbtree->FindNearestI(rays[k]);
    // ??? What to do with the results ? These are
    // not used at the moment also in IntelRayCaster.cpp
  }

#endif // USE_HAVRAN_RAYCASTER 
  return;
}

void HavranRayCaster::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);

        preprocessor->UpdateDynamicObjects();
    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
HavranRayCaster::CastRaysPacket2x2(RayPacket2x2 &raysPack,
                                   bool castDoubleRay,
                                   const bool pruneInvalidRays)
{
#ifdef USE_HAVRAN_RAYCASTER 
#ifdef _USE_HAVRAN_SSE

  if (castDoubleRay) {
    // cast forward rays
    mKtbtree->FindNearestI(raysPack);
    for (int i = 0; i < 4; i++)
      raysPack.SetDir(i, -raysPack.GetDir(i));
    // cast backward rays
    mKtbtree->FindNearestI(raysPack);
    // reverse the rays back
    for (int i = 0; i < 4; i++)
      raysPack.SetDir(i, -raysPack.GetDir(i));
  }
  else {
    // only rays forwards
    mKtbtree->FindNearestI(raysPack);
#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