source: GTP/trunk/Lib/Vis/Preprocessing/src/MutualVisibility.cpp @ 2601

#include <assert.h>
#include <stack>
#include "KdTree.h"
#include "AxisAlignedBox3.h"
#include "Ray.h"
#include "MutualVisibility.h"
#include "Exporter.h"
#include "Mesh.h"
#include "Triangle3.h"
#include "SceneGraph.h"

using namespace std;

namespace GtpVisibilityPreprocessor {

void
RayShaft::Init(
               const Rectangle3 &source,
               const Rectangle3 &target)
{
  mDepth = 0;
  mSource = source;
  mTarget = target;
}


Vector3
RayShaft::GetIntersectionPoint(const int rayIndex,
                               const int depth) const
{
  Vector3 origin, direction;
  Ray ray;
  GetRaySegment(rayIndex, ray);
  if (depth >= mSamples[rayIndex].mIntersections.size()) {
    cerr<<"depth of sample out of limits"<<endl;
    exit(1);
  }
  return ray.Extrap(mSamples[rayIndex].mIntersections[depth].mT);
}

void
RayShaft::GetRaySegment(const int i, Ray &ray) const
{
  Vector3 origin, direction;
  GetRay(i, origin, direction);
  ray.Init(origin, direction, Ray::LINE_SEGMENT);
  if ( mSamples[i].IsValid() ) {
    origin = ray.Extrap(mSamples[i].mMinT);
    direction = ray.Extrap(mSamples[i].mMaxT) - origin;
    ray.Init(origin, direction, Ray::LINE_SEGMENT);
  }
}

void
RayShaft::GetRay(const int rayIndex,
                 Vector3 &origin,
                 Vector3 &direction) const
{
  
  assert(rayIndex < 4);
  
  origin = mSource.mVertices[rayIndex];
  direction = mTarget.mVertices[rayIndex] - origin;
}

void
MutualVisibilitySampler::PerformSplit(
                                      const RayShaft &sample,
                                      const bool splitSource,
                                      const int axis,
                                      RayShaft &sample1,
                                      RayShaft &sample2
                                      )
{
  

  // split the triangles

  if (splitSource) {
    sample1.mTarget = sample.mTarget;
    sample2.mTarget = sample.mTarget;
    sample.mSource.Split(
                         axis,
                         sample1.mSource,
                         sample2.mSource);
  } else {
    
    sample1.mSource = sample.mSource;
    sample2.mSource = sample.mSource;
    
    sample.mTarget.Split(
                         axis,
                         sample1.mTarget,
                         sample2.mTarget);
  }
  
  // split the intersections
  switch (axis) {
  case 0:
    sample1.mSamples[0].mIntersections = sample.mSamples[0].mIntersections;
    sample1.mSamples[3].mIntersections = sample.mSamples[3].mIntersections;
    
    sample2.mSamples[1].mIntersections = sample.mSamples[1].mIntersections;
    sample2.mSamples[2].mIntersections = sample.mSamples[2].mIntersections;
    break;

  case 1:
    sample1.mSamples[0].mIntersections = sample.mSamples[0].mIntersections;
    sample1.mSamples[1].mIntersections = sample.mSamples[1].mIntersections;

    sample2.mSamples[2].mIntersections = sample.mSamples[2].mIntersections;
    sample2.mSamples[3].mIntersections = sample.mSamples[3].mIntersections;
    break;
  }

  // the intersections for the new shaft rays will be established
  // later
  sample1.mDepth = sample2.mDepth = sample.mDepth+1;
}

float
MutualVisibilitySampler::GetSpatialAngle(const RayShaft &shaft,
                                         const Vector3 &point
                                         )
{
  const int sampleIndices[]={
    0,1,2,
    0,2,3
  };
  
  float sum = 0.0f;
  int i, j=0;

  if (!shaft.IsValid())
    return 2.0f*mSolidAngleThreshold;

  for (i=0; i < 2; i++) {
    Triangle3 triangle(shaft.GetIntersectionPoint(sampleIndices[j++], 0),
                       shaft.GetIntersectionPoint(sampleIndices[j++], 0),
                       shaft.GetIntersectionPoint(sampleIndices[j++], 0));
    sum += triangle.GetSpatialAngle(point);
  }

  return sum;
}


void
MutualVisibilitySampler::ComputeError(RayShaft &shaft)
{
  // evaluate minimal error which can be achieved by more precise evaluation
  // if this is above the threshold do not proceed further

  // check whether the samples pierce the same mesh
  Intersectable::NewMail();
  int i, j;
  for (i=0; i < 4; i++) {
    RaySample *sample = &shaft.mSamples[i];
    for (j=0; j < sample->mIntersections.size(); j++)
      sample->mIntersections[j].mObject->Mail();
  }

  //  int id = Mailable::GetThreadId();
  for (i=0; i < 4; i++) {
    RaySample *sample = &shaft.mSamples[i];
    for (j=0; j < sample->mIntersections.size(); j++) {
      if (sample->mIntersections[j].mObject->IncMail() == 4) {
                cerr<<"T";
                shaft.mError = 0.0f;
                return;
      }
    }
  }
    
  
  float maxAngle = GetSpatialAngle(shaft,
                                   shaft.mTarget.GetCenter());
  
  for (i=0; i < 3; i++) {
    float angle = GetSpatialAngle(shaft,
                                  shaft.mTarget.mVertices[i]);
    if (angle > maxAngle)
      maxAngle = angle;
  }

  maxAngle = MAX_FLOAT;
  shaft.mError = maxAngle;
}


void
MutualVisibilitySampler::ConstructInitialSamples2(
                                                  const AxisAlignedBox3 &source,
                                                  const AxisAlignedBox3 &target,
                                                  vector<RayShaft *> &samples
                                                  )
{
  // get all rectangles potentially visible from the source box
  int i;
  int sourceMask = 0;
  for (i=0; i < 8; i++)
    sourceMask |= source.GetFaceVisibilityMask(target.GetVertex(i));

  // now for each visble source face find all visible target faces
  for (i=0; i < 6; i++) {
    Rectangle3 sourceFace = source.GetFace(i);
    if ( sourceMask &(1<<i) ) {
      int mask = target.GetFaceVisibilityMask(sourceFace);
      // construct triangle samples for all visible rectangles
      for (int j=0; j < 6; j++)
        if (mask & (1<<j)) {
          AddInitialSamples2(sourceFace, target.GetFace(j), samples);
        }
    }
  }
}

void
MutualVisibilitySampler::ConstructInitialSamples3(
                                                  const AxisAlignedBox3 &source,
                                                  const AxisAlignedBox3 &target,
                                                  vector<RayShaft *> &samples
                                                  )
{
  // get all rectangles potentially visible from the source box
  int i;
  int sourceMask = 0;
  for (i=0; i < 8; i++)
    sourceMask |= source.GetFaceVisibilityMask(target.GetVertex(i));

  // now for each visble source face find all visible target faces
  int face;
  for (face=0; face < 6; face++) {
    if ( sourceMask & (1<<face) ) {
      Rectangle3 sourceRect = source.GetFace(face);

      Vector3 targetCenter = target.Center();
      
      Vector3 normal = sourceRect.GetNormal();
      
      Plane3 sourcePlane(normal, sourceRect.GetVertex(0));
      
      Plane3 targetPlane(normal, target.GetVertex(0));

      int i;
      for (i=1; i < 8; i++) {
        Vector3 v = target.GetVertex(i);
        if (targetPlane.Distance(v) < 0)
          targetPlane = Plane3(normal, v);
      }
  
      Vector3 xBasis = Normalize(sourceRect.GetVertex(1) - sourceRect.GetVertex(0));
      Vector3 yBasis = Normalize(sourceRect.GetVertex(3) - sourceRect.GetVertex(0));

      // cast rays between the centers of the boxes
      Vector3 targetRCenter = targetPlane.FindIntersection(sourceRect.GetCenter(),
                                                           targetCenter);
      
      Rectangle3 targetRect;
      
      float targetDist[4];
      targetDist[0] = targetDist[1] = targetDist[2] = targetDist[3] = 0.0f;
      
      // cast rays between corresponding vertices of the boxes
      int j;
      int intersections=0;
      for (i=0; i < 4; i++)
        for (j=0; j < 8; j++) {
          Vector3 v;
          Vector3 diff;
          bool coplanar;
          float dist;
          
          v = targetPlane.FindIntersection(sourceRect.GetVertex(i),
                                           target.GetVertex(j),
                                           NULL,
                                           &coplanar);
          if (!coplanar) {
            // evaluate target and 
            diff = targetRCenter - v;
            dist = DotProd(diff, xBasis);
            if (dist < targetDist[0])
              targetDist[0] = dist;
            if (dist > targetDist[1])
              targetDist[1] = dist;

            dist = DotProd(diff, yBasis);

            if (dist < targetDist[2])
              targetDist[2] = dist;
            if (dist > targetDist[3])
              targetDist[3] = dist;
            intersections++;
          }
        }
      
      if (intersections>=4) {
        targetRect.mVertices[0] = targetRCenter + targetDist[0]*xBasis + targetDist[2]*yBasis;
        targetRect.mVertices[1] = targetRCenter + targetDist[1]*xBasis + targetDist[2]*yBasis;
        targetRect.mVertices[2] = targetRCenter + targetDist[1]*xBasis + targetDist[3]*yBasis;
        targetRect.mVertices[3] = targetRCenter + targetDist[0]*xBasis + targetDist[3]*yBasis;
        
        //      cout<<sourceRect<<targetRect<<endl;
        AddInitialSamples(sourceRect, targetRect, samples);
      }
    }
  }
}

void
MutualVisibilitySampler::ConstructInitialSamples(
                                                 const AxisAlignedBox3 &source,
                                                 const AxisAlignedBox3 &target,
                                                 vector<RayShaft *> &samples
                                                 )
{
  Vector3 sourceCenter = source.Center();
  Vector3 targetCenter = target.Center();
  Vector3 normal = Normalize(sourceCenter - targetCenter );
  
  Plane3 sourcePlane(normal, source.GetVertex(0));
  int i;
  for (i=1; i < 8; i++) {
    Vector3 v = source.GetVertex(i);
    if (sourcePlane.Distance(v) > 0)
      sourcePlane = Plane3(normal, v);
  }
  
  Plane3 targetPlane(normal, target.GetVertex(0));
  for (i=1; i < 8; i++) {
    Vector3 v = target.GetVertex(i);
    if (targetPlane.Distance(v) < 0)
      targetPlane = Plane3(normal, v);
  }

  
  Vector3 xBasis = CrossProd(Vector3(0,1,0), normal);

  if (Magnitude(xBasis) > 1e-6)
    xBasis.Normalize();
  else {
    xBasis = Normalize(CrossProd(Vector3(0,0,1), normal));
  }

  Vector3 yBasis = Normalize( CrossProd(normal, xBasis) );
  // cast rays between the centers of the boxes
  Vector3 targetRCenter = targetPlane.FindIntersection(sourceCenter,
                                                       targetCenter);
  
  Vector3 sourceRCenter = sourcePlane.FindIntersection(sourceCenter,
                                                       targetCenter);
  

  Rectangle3 sourceRect;
  Rectangle3 targetRect;


  if (0) {
  float scale = Magnitude(source.Size())*0.7f;
  sourceRect.mVertices[0] = sourceRCenter - scale*xBasis - scale*yBasis;
  sourceRect.mVertices[1] = sourceRCenter + scale*xBasis - scale*yBasis;
  sourceRect.mVertices[2] = sourceRCenter + scale*xBasis + scale*yBasis;
  sourceRect.mVertices[3] = sourceRCenter - scale*xBasis + scale*yBasis;

  scale = Magnitude(target.Size())*0.7f;
  targetRect.mVertices[0] = targetRCenter - scale*xBasis - scale*yBasis;
  targetRect.mVertices[1] = targetRCenter + scale*xBasis - scale*yBasis;
  targetRect.mVertices[2] = targetRCenter + scale*xBasis + scale*yBasis;
  targetRect.mVertices[3] = targetRCenter - scale*xBasis + scale*yBasis;
  }


  float sourceDist[4];
  float targetDist[4];
  sourceDist[0] = sourceDist[1] = sourceDist[2] = sourceDist[3] = 0.0f;
  targetDist[0] = targetDist[1] = targetDist[2] = targetDist[3] = 0.0f;

  // cast rays between corresponding vertices of the boxes
  int j;
  for (i=0; i < 8; i++)
    for (j=0; j < 8; j++) {
      Vector3 v;
      Vector3 diff;
      bool coplanar;
      float dist;
      v = sourcePlane.FindIntersection(source.GetVertex(i),
                                       target.GetVertex(j),
                                       NULL,
                                       &coplanar);
      if (!coplanar) {
        // evaluate source and 
        diff = sourceRCenter - v;
        dist = DotProd(diff, xBasis);
        if (dist < sourceDist[0])
          sourceDist[0] = dist;
        if (dist > sourceDist[1])
          sourceDist[1] = dist;

        dist = DotProd(diff, yBasis);
        if (dist < sourceDist[2])
          sourceDist[2] = dist;
        if (dist > sourceDist[3])
          sourceDist[3] = dist;
      }
      
      v = targetPlane.FindIntersection(source.GetVertex(i),
                                       target.GetVertex(j),
                                       NULL,
                                       &coplanar);
      if (!coplanar) {
        // evaluate target and 
        diff = targetRCenter - v;
        dist = DotProd(diff, xBasis);
        if (dist < targetDist[0])
          targetDist[0] = dist;
        if (dist > targetDist[1])
          targetDist[1] = dist;
        dist = DotProd(diff, yBasis);
        if (dist < targetDist[2])
          targetDist[2] = dist;
        if (dist > targetDist[3])
          targetDist[3] = dist;
      }
    }

  sourceRect.mVertices[0] = sourceRCenter + sourceDist[0]*xBasis + sourceDist[2]*yBasis;
  sourceRect.mVertices[1] = sourceRCenter + sourceDist[1]*xBasis + sourceDist[2]*yBasis;
  sourceRect.mVertices[2] = sourceRCenter + sourceDist[1]*xBasis + sourceDist[3]*yBasis;
  sourceRect.mVertices[3] = sourceRCenter + sourceDist[0]*xBasis + sourceDist[3]*yBasis;

  targetRect.mVertices[0] = targetRCenter + targetDist[0]*xBasis + targetDist[2]*yBasis;
  targetRect.mVertices[1] = targetRCenter + targetDist[1]*xBasis + targetDist[2]*yBasis;
  targetRect.mVertices[2] = targetRCenter + targetDist[1]*xBasis + targetDist[3]*yBasis;
  targetRect.mVertices[3] = targetRCenter + targetDist[0]*xBasis + targetDist[3]*yBasis;


  cout<<sourceRect<<targetRect<<endl;
  AddInitialSamples(sourceRect, targetRect, samples);
}

void
MutualVisibilitySampler::AddInitialSamples(
                                           const Rectangle3 &sourceRect,
                                           const Rectangle3 &targetRect,
                                           vector<RayShaft *> &samples
                                           )
{

  RayShaft *sample = new RayShaft(sourceRect, 
                                  targetRect);
  samples.push_back(sample);
}

void
MutualVisibilitySampler::AddInitialSamples2(
                                            const Rectangle3 &sourceRect,
                                            const Rectangle3 &targetRect,
                                            vector<RayShaft *> &samples
                                            )
{
  // align the rectangles properly
  Rectangle3 sRect, tRect;
  if (DotProd(sourceRect.GetNormal(), targetRect.GetNormal()) < -0.99f) {
    int i;
    for (i=0; i < 4; i++) {
      tRect.mVertices[i] = targetRect.mVertices[( 7 - i )%4];
    }
    
    RayShaft *sample = new RayShaft(sourceRect, 
                                    tRect);
    samples.push_back(sample);
  }
  
  return;
  
  float minDist = MAX_FLOAT;
  int startI;
  int startJ;
  int i, j;

  
  for (i=0; i < 4; i++) {
    for (j=0; j < 4; j++) {
      float dist = Distance(sourceRect.mVertices[i], targetRect.mVertices[j]);
      if (dist < minDist) {
        minDist = dist;
        startI = i;
        startJ = j;
      }
    }
  }
  for (i=0; i < 4; i++) {
    sRect.mVertices[i] = sourceRect.mVertices[(startI + i )%4];
    tRect.mVertices[i] = targetRect.mVertices[(4 + startJ - i )%4];
  }
  
  RayShaft *sample = new RayShaft(sRect, 
                                  tRect);
  samples.push_back(sample);
}


void
MutualVisibilitySampler::ExportShafts(vector<RayShaft *> &shafts,
                                      const bool singleFile)
{
  static int id = 0;
  char filename[64];
  if (id > 20)
    return;
  
  Exporter *exporter = NULL;
  for (int i=0; i < shafts.size(); i++) {
    if (!exporter) {
      if (singleFile)
        sprintf(filename, "shafts-single-%04d.x3d", id++);
      else
        sprintf(filename, "shafts%04d-%02d.x3d", id++, i);
      exporter = Exporter::GetExporter(filename);
    }
    
    exporter->SetWireframe();
    // exporter->ExportScene(mSceneGraph->GetRoot());

    exporter->ExportBox(mSource);
    exporter->ExportBox(mTarget);

    exporter->SetFilled();


    RayShaft *shaft = shafts[i];
    Mesh *mesh = new Mesh;
    mesh->AddRectangle(shaft->mSource);
    mesh->AddRectangle(shaft->mTarget);
    vector<Ray *> rays;
    for (int j=0; j < 4; j++) {
      Ray *ray = new Ray;
      shaft->GetRaySegment(j, *ray);
      rays.push_back(ray);
    }
    
    Material m = RandomMaterial();
    exporter->SetForcedMaterial(m);
    MeshInstance mi(mesh);
    exporter->ExportIntersectable(&mi);
    exporter->ExportRays(rays, -1.0f, m.mDiffuseColor);
    if (!singleFile) {
      delete exporter;
      exporter = NULL;
    }
  }
  if (exporter)
    delete exporter;
  
}

int
MutualVisibilitySampler::CastRays(RayShaft &shaft)
{
  Ray ray;
  int i;

  for (i=0; i < 4; i++)
    if (!shaft.mSamples[i].IsProcessed()) {
      Vector3 origin, direction;
      shaft.GetRay(i, origin, direction);
      // determine intersections with the boxes
      ray.Init(origin, direction, Ray::LINE_SEGMENT);
      float stmin, stmax = 0.0f, ttmin=1.0f, ttmax;
      bool valid = true;
      
      if (mUseBoxes) {
        if (mSource.GetMinMaxT(ray, &stmin, &stmax) &&
            mTarget.GetMinMaxT(ray, &ttmin, &ttmax)) {
          shaft.mSamples[i].mMinT = stmax;
          shaft.mSamples[i].mMaxT = ttmin;
          origin = ray.Extrap(stmax);
          direction = ray.Extrap(ttmin) - origin;
          // reinit the ray
          ray.Init(origin, direction, Ray::LINE_SEGMENT);
        } else
          valid = false;
      } else {
        shaft.mSamples[i].mMinT = 0.0f;
        shaft.mSamples[i].mMaxT = 1.0f;
      }
      if (valid) {
        if (!mKdTree->CastRay(ray)) {
          cerr<<"V"<<endl;
          return VISIBLE;
        }
        shaft.mSamples[i].mIntersections = ray.intersections;
        cerr<<"I";
      } else {
        cerr<<"X";
        shaft.mSamples[i].SetInvalid();
      }
    }
  return INVISIBLE;
}

int
MutualVisibilitySampler::ComputeVisibility()
{
  int result = INVISIBLE;

  vector<RayShaft *> shafts;
  ConstructInitialSamples3(mSource, mTarget, shafts);
        
  if (1)
    ExportShafts(shafts, false);

  stack<RayShaft *> shaftStack;
  int i;
  for (i=0; i < shafts.size(); i++)
    shaftStack.push(shafts[i]);

  shafts.clear();
  Ray ray;
  
// now process the shafts as long as we have something to do
  while (!shaftStack.empty()) {
    RayShaft *shaft = shaftStack.top();
    shaftStack.pop();
    
//      // cast a new ray
//      int triangleSplitEdge = SetupExtremalRay(sample, source, ray);
    
    if (CastRays(*shaft) == VISIBLE) {
      result = VISIBLE;
      break;
    }
                
    // compute error ....
    ComputeError(*shaft);
    cout<<shaft->mDepth<<"|";
    if (shaft->IsValid())
      shafts.push_back(shaft);

    if (shaft->mDepth < 10 &&
                                shaft->mError > mSolidAngleThreshold) {
      
      // generate 2 new samples
      RayShaft *newSamples[2];
      newSamples[0] = new RayShaft;
      newSamples[1] = new RayShaft;
      
      // chose what to split
      bool splitSource = shaft->mSource.GetArea() > shaft->mTarget.GetArea();
      int axis;
      if (splitSource) {
                                axis = shaft->mSource.DominantAxis();
      } else {
                                axis = shaft->mTarget.DominantAxis();
      }
      
      PerformSplit(*shaft, splitSource, axis, *newSamples[0], *newSamples[1]);
      //      delete shaft;
      shaftStack.push(newSamples[0]);
      shaftStack.push(newSamples[1]);
    } else {
      // store a terminal shaft
    }

  }
  
  while (!shaftStack.empty()) {
    RayShaft *shaft = shaftStack.top();
    shaftStack.pop();
    delete shaft;
  }

  if (0)
    ExportShafts(shafts, true);

  for (i=0; i < shafts.size(); i++) {
    delete shafts[i];
  }

  return result;
}

MutualVisibilitySampler::MutualVisibilitySampler(SceneGraph *sceneGraph,
                                                                                                 KdTree *kdTree,
                                                                                                 const AxisAlignedBox3 &source,
                                                                                                 const AxisAlignedBox3 &target,
                                                                                                 const float solidAngleThreshold)
{
        mSceneGraph = sceneGraph;
        mKdTree = kdTree;
        mSource = source;
        mTarget = target;
        mUseBoxes = true;
        mSolidAngleThreshold = solidAngleThreshold;
}


int
ComputeBoxVisibility(SceneGraph *sceneGraph,
                                         KdTree *kdTree,
                                         const AxisAlignedBox3 &source,
                                         const AxisAlignedBox3 &target,
                                         const float solidAngleThreshold)
{
        MutualVisibilitySampler
                sampler(sceneGraph, kdTree, source, target, solidAngleThreshold);

        int visibility = sampler.ComputeVisibility();

        return visibility;

}

}