source: GTP/trunk/Lib/Vis/Preprocessing/src/havran/testrt.cpp @ 2629

#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 "Triangle3.h"
#include "IntersectableWrapper.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();

void
_SortRays2(SimpleRayContainer &rays,
           const int l,
           const int r,
           const int depth,
           float box[12])
{
  // pick-up a pivot
  int axis;
  
  float maxDiff = -1.0f;
  // get the largest axis
  int offset = 0;
  int i;

  //const int batchsize = 16384;
  //const int batchsize = 8192;
  //const int batchsize = 128;
  const int batchsize = 4;

  //if (r - l < 16*batchsize)
  //    offset = 3;
        
  //  if (depth%2==0)
  //    offset = 3;
  
  for (i=offset; i < offset + 6; i++) {
    float diff = box[i + 6] - box[i];
    assert(diff >= 0.f);
    if (diff > maxDiff) {
      // Find the maximum
      maxDiff = diff;
      axis = i;
    }
  }

  //  cout<<depth<<" "<<axis<<" "<<l<<" "<<r<<endl;
  
  i=l;
  int j=r;

  float x = (box[axis] + box[axis+6])*0.5f;
  //  float x = rays[(l+r)/2].GetParam(axis);
  do {
        while(i<j && rays[i].GetParam(axis) < x)
          i++;
        while(i<j && x < rays[j].GetParam(axis))
          j--;
        
        if (i <= j) {
          swap(rays[i], rays[j]);
          i++;
          j--;
        }
  } while (i<=j);

  
  if (l + batchsize < j ) {
        // set new max
        float save = box[axis+6];
        box[axis+6] = x;
        _SortRays2(rays, l, j, depth+1, box);
        box[axis+6] = save;
  } else {
        //      for (int k=0; k < 6; k++)
        //        cout<<k<<" "<<box[k]<<" - "<<box[k+6]<<endl;
  }
  
  if (i + batchsize < r) {
    // set new min
    box[axis] = x;
    _SortRays2(rays, i, r, depth+1, box);
  } else {
        //      for (int k=0; k < 6; k++)
        //        cout<<k<<" "<<box[k]<<" - "<<box[k+6]<<endl;
  }     
}

void
SortRays2(SimpleRayContainer &rays, const AxisAlignedBox3 &box)
{
  const float sizeBox = Magnitude(box.Diagonal());
  // This is some size of the 
  const float sizeDir = 0.1f * sizeBox;
  
  float b[12]={
    box.Min().x,
    box.Min().y,
    box.Min().z,
    -sizeDir,
    -sizeDir,
    -sizeDir,
    box.Max().x,
    box.Max().y,
    box.Max().z,
    sizeDir,
    sizeDir,
    sizeDir
  };

  
  _SortRays2(rays, 0, (int)rays.size()-1, 0, b);

  return;
}                                       


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;
  float 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);
  //dir = -dir;
#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;

  int id = preprocessor->mObjects.size() + 1;
    
  //We add here a few objects
  ObjectContainer dynObjects;
  Vector3 baseVec = origin + dir * 2.0f;
  Triangle3 tr1(baseVec, baseVec + Vector3(1, 0, 0),
                baseVec + Vector3(0, 1, 0));           
  TriangleIntersectable ti1(tr1);
  dynObjects.push_back(&ti1);
  ti1.mId = id+1;
  
  tr1.Init(baseVec, baseVec + Vector3(1, 0, 0),
           baseVec + Vector3(0, 0, 1));        
  TriangleIntersectable ti2(tr1);
  dynObjects.push_back(&ti2);
  ti2.mId = id+2;

  tr1.Init(baseVec, baseVec + Vector3(0, 1, 0),
           baseVec + Vector3(0, 0, 1));        
  TriangleIntersectable ti3(tr1);
  dynObjects.push_back(&ti3);
  ti3.mId = id+3;

#if 1
  // This is required for cam.SnapImagePacket2
  // otherwise dynamic object cannot be identified!
  preprocessor->mObjects.push_back(&ti1);
  preprocessor->mObjects.push_back(&ti2);
  preprocessor->mObjects.push_back(&ti3);
#endif

  if (0) {
    Matrix4x4 mat;
    mat = IdentityMatrix();
    mat = mat * TranslationMatrix(Vector3(-1, 0, 0));
    preprocessor->mRayCaster->AddDynamicObjecs(dynObjects, mat);
  
#if 1
    // ray by ray
    cam.SnapImage("test-rays.tga",
                  preprocessor->mRayCaster,
                  bboxOrig,
                  preprocessor->mSceneGraph);
#endif
#if 1
    // using ray packets
    cam.SnapImage2("test-oneDir.tga",
                   preprocessor->mRayCaster,
                   bboxOrig,
                   preprocessor->mSceneGraph);
#endif
    
#if 1
#ifdef _USE_HAVRAN_SSE
    // using ray packets
    cam.SnapImagePacket("test-packet.tga",
                        preprocessor->mRayCaster,
                        bboxOrig,
                        preprocessor->mSceneGraph);
#endif
#endif    
#if 1
    // using ray packets
    cam.SnapImagePacket2("test-packet4.tga",
                         preprocessor->mRayCaster,
                         bboxOrig,
                         preprocessor->mSceneGraph);
#endif
  }
  else {
    cout << "Computing animation" << endl;    
    for (int i = 0; i < 20; i++) {
      preprocessor->mRayCaster->DeleteDynamicObjects();
      Matrix4x4 mat;
      mat = IdentityMatrix();
      mat = mat * TranslationMatrix(Vector3(-0.1*(float)i, 0, 0));
      preprocessor->mRayCaster->AddDynamicObjecs(dynObjects, mat);
      char name[200];      
#if 1
    // ray by ray
      sprintf(name, "test-rays-%03d.tga", i);
      cam.SnapImage(name,
                    preprocessor->mRayCaster,
                    bboxOrig,
                    preprocessor->mSceneGraph);
#endif
#if 1
      // using ray packets
      sprintf(name, "test-oneDir-%03d.tga", i);
      cam.SnapImage2(name,
                     preprocessor->mRayCaster,
                     bboxOrig,
                     preprocessor->mSceneGraph);
#endif

#if 1
#ifdef _USE_HAVRAN_SSE  
      // using ray packets
      sprintf(name, "test-packet-%03d.tga", i);
      cam.SnapImagePacket(name,
                          preprocessor->mRayCaster,
                          bboxOrig,
                          preprocessor->mSceneGraph);
#endif
#endif
#if 1
      // using ray packets
      sprintf(name, "test-packet4-%03d.tga", i);
      cam.SnapImagePacket2(name,
                           preprocessor->mRayCaster,
                           bboxOrig,
                           preprocessor->mSceneGraph);
#endif
    } // for
  } // animation

  
  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);
  SimpleRayContainer 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 << "Press a key to start ray shooting" << endl;
  getchar();
  cout << "Ray shooting " << cntRays << " rays started - "
       << (castDoubleRays ? " double " : " single ")
       << " dir " << endl;
  
  AxisAlignedBox3 bboxOrig = preprocessor->mSceneGraph->GetBox();
  
  cout << "Sorting rays " << endl;

  CTimer timer;
  timer.Reset();
  timer.Start();
  long t1 = GetTime();

  //SortRays2(rays, bboxOrig);

  timer.Stop();
  long t2 = GetTime();
  cout<<"\n#SORTING_TIME = ";
  cout << TimeDiff(t1, t2)<<" [mikrosec]"
       << " userTime = " << timer.UserTime() << " realTime = "
       << timer.RealTime() << endl;

  cout << "Starting to shoot " << cntRays * mult << " rays" << endl;
  timer.Start();
  
  RayCaster *raycaster = preprocessor->mRayCaster;
  VssRayContainer vssRays;

//#define DEBUGRESULTS

#ifdef DEBUGRESULTS
  int cntIntelYesWeNo = 0;
  int cntIntelNoWeYes = 0;
  int cntIntelOurDistMismatch = 0;
  bool printOut = false;
  double sumDistIntel = 0.f;
  double sumDistOurAlg = 0.f;
  double sumDistAbsDiff = 0.f;
#endif

  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)) > 0.1f)
           )
        {
          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();
  t2 = GetTime();
  cout<<"\n#SORTING + 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);
  SimpleRayContainer 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();

#undef DEBUGRESULTS
//#define DEBUGRESULTS

#ifdef DEBUGRESULTS
  int cntIntelYesWeNo = 0;
  int cntIntelNoWeYes = 0;
  int cntIntelOurDistMismatch = 0;
  bool printOut = false;
  double sumDistIntel = 0.;
  double 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;
}

// This is for testing RT implementation
void
TestRT_4_fromFile(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);
  SimpleRayContainer rays;
  SimpleRay rayTest;
  vector<RESult> results;
  vector<AxisAlignedBox3> boxes;

  int cntRays = 0;
  for (int i = 0; cntRays < cntMaxRays;) {
    int ch = fgetc(fp);
    switch (ch) {
    case 'I': { // two-sided rays of 16
      Vector3 minv, maxv;
      if (fscanf(fp, "%f %f %f %f %f %f\n",
                 &minv.x, &minv.y, &minv.z,
                 &maxv.x, &maxv.y, &maxv.z) != 6) {
        cerr << "Problem parsing the ray file" << endl;
        cerr << "EXITING for ray order" << i << endl;
        goto FINISH;
      }
      boxes.push_back(AxisAlignedBox3(minv, maxv));      

      for (int j = 0; j < 4; 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\n",
                   &order, &ox, &oy, &oz, &dx, &dy, &dz,
                   &hitA, &hitAT) != 9) {
          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, 0, -1.f));
        cntRays++;
        i++;
      }
      break;
    }
    default: {
      cerr << "Not yet implemented or end of file" << endl;
      goto FINISH;
    }
    } // switch
  } // for i

FINISH:
  
  fclose(fp);
  cout << "Starting to shoot " << cntRays << " 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.f;
  double sumDistOurAlg = 0.f;
  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();
  Vector3 boxMin, boxMax;

  bool printOut = false;
  
  SimpleRayContainer raysToTest;
  int boxI = 0;
  for (int i = 0; i < cntRays - 4; i+= 4, boxI++) {
    Vector3 origin4[4];
    Vector3 direction4[4];
    int     result4[4];
    float   dist4[4];
    boxMin = boxes[boxI].Min();
    boxMax = boxes[boxI].Max();
    for (int j = 0; j < 4; j++) {
      int o = i+j;
      origin4[j] = rays[o].mOrigin;
      direction4[j] = rays[o].mDirection;
    }
    raycaster->CastRaysPacket4(boxMin, boxMax,
                               origin4, direction4,
                               result4, dist4);
    if (printOut) {
      printf("I %4.7f %4.7f %4.7f %4.7f %4.7f %4.7f\n",
             boxMin.x, boxMin.y, boxMin.z, boxMax.x, boxMax.y, boxMax.z);
    }

    for (int j = 0; j < 4; j++) {
#if 0
      if (result4[j] == 0) {
        int res = raycaster->CastRay(rays[i+j],
                                     vssRays,
                                     bboxOrig,
                                     false,
                                     true);
        if (res) {
          float tdist = SimpleRay::IntersectionRes[0].tdist;
          Vector3 point = rays[i+j].Extrap(tdist);
          AxisAlignedBox3 testbox(boxMin, boxMax);
          
          if (testbox.IsInside(point)) {
            cout << "Error in the algorithm - computed not in the box, but"
                 << " it is later found in the box" << endl;
            cout << " j = " << j << endl;
            cout << " box = " << testbox << endl;
            cout << " point = " << point << endl;
            raycaster->CastRaysPacket4(boxMin, boxMax,
                               origin4, direction4,
                               result4, dist4);
          }
        }
      }
#endif      
      
      if (printOut) {
        printf("%d %4.7f %4.7f %4.7f %4.7f %4.7f %4.7f %d %4.7f\n",
               i+j,
               origin4[j].x,
               origin4[j].y,
               origin4[j].z,
               direction4[j].x,
               direction4[j].y,
               direction4[j].z,
               (result4[j] != -1) ? 1 : 0,
               (result4[j] != -1) ? dist4[j] : 0);
      }
    } // for j    
  } // for i

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