source: GTP/trunk/Lib/Vis/Preprocessing/src/MeshKdTree.cpp @ 752

#include <stack>
#include <algorithm>
#include <queue>
#include "Environment.h"
#include "Mesh.h"
#include "MeshKdTree.h"

float MeshKdTree::mSplitBorder;
int MeshKdTree::mTermMaxDepth;
int MeshKdTree::mTermMinCost;
float MeshKdTree::mMaxCostRatio;
float MeshKdTree::mCt_div_ci;
int MeshKdTree::mSplitMethod;

MeshKdTree::MeshKdTree(Mesh *mesh):mMesh(mesh)
{
  mRoot = new MeshKdLeaf;
  mSplitCandidates = NULL;
}

void
MeshKdTree::ParseEnvironment()
{
  environment->GetIntValue("MeshKdTree.Termination.maxDepth", mTermMaxDepth);
  environment->GetIntValue("MeshKdTree.Termination.minCost", mTermMinCost);
  environment->GetFloatValue("MeshKdTree.Termination.maxCostRatio", mMaxCostRatio);
  environment->GetFloatValue("MeshKdTree.Termination.ct_div_ci", mCt_div_ci);
  environment->GetFloatValue("MeshKdTree.splitBorder", mSplitBorder);
  
  char splitType[64];
  environment->GetStringValue("MeshKdTree.splitMethod", splitType);
  
  mSplitMethod = SPLIT_SPATIAL_MEDIAN;
  if (strcmp(splitType, "spatialMedian") == 0)
    mSplitMethod = SPLIT_SPATIAL_MEDIAN;
  else
    if (strcmp(splitType, "objectMedian") == 0)
      mSplitMethod = SPLIT_OBJECT_MEDIAN;
    else
      if (strcmp(splitType, "SAH") == 0)
        mSplitMethod = SPLIT_SAH;
      else {
        cerr<<"Wrong kd split type "<<splitType<<endl;
        exit(1);
      }
}



int
MeshKdTree::SelectPlane(MeshKdLeaf *leaf,
                        const AxisAlignedBox3 &box,
                        float &position
                        )
{
  int axis = -1;
  
  switch (mSplitMethod)
    {
    case SPLIT_SPATIAL_MEDIAN: {
      axis = box.Size().DrivingAxis();
      position = (box.Min()[axis] + box.Max()[axis])*0.5f;
      break;
    }
    case SPLIT_SAH: {
      int objectsBack, objectsFront;
      float costRatio;
      bool mOnlyDrivingAxis = false;
      if (mOnlyDrivingAxis) {
        axis = box.Size().DrivingAxis();
        costRatio = BestCostRatio(leaf,
                                  box,
                                  axis,
                                  position,
                                  objectsBack,
                                  objectsFront);
      } else {
        costRatio = MAX_FLOAT;
        for (int i=0; i < 3; i++) {
          float p;
          float r = BestCostRatio(leaf,
                                  box,
                                  i,
                                  p,
                                  objectsBack,
                                  objectsFront);
          if (r < costRatio) {
            costRatio = r;
            axis = i;
            position = p;
          }
        }
      }
      
      if (costRatio > mMaxCostRatio) {
        //      cout<<"Too big cost ratio "<<costRatio<<endl;
        axis = -1;
      }
      break;
    }
    
    }
  return axis;
}

MeshKdNode *
MeshKdTree::SubdivideNode(
                          MeshKdLeaf *leaf,
                          MeshKdInterior *parent,
                          const AxisAlignedBox3 &box,
                          const int depth,
                          AxisAlignedBox3 &backBBox,
                          AxisAlignedBox3 &frontBBox
                          )
{
  
  if (TerminationCriteriaMet(leaf, depth))
    return leaf;
  
  float position;
  // select subdivision axis
  int axis = SelectPlane( leaf, box, position );
  
  if (axis == -1) {
    return leaf;
  }
  
  // add the new nodes to the tree
  MeshKdInterior *node = new MeshKdInterior;
  
  node->mAxis = axis;
  node->mPosition = position;
  
  backBBox = box;
  frontBBox = box;
  
  // first count ray sides
  
  backBBox.SetMax(axis, position);
  frontBBox.SetMin(axis, position);

  vector<int>::const_iterator fi;
  vector<int> objectsFront, objectsBack;
  
  for ( fi = leaf->mFaces.begin();
        fi != leaf->mFaces.end();
        fi++) {
    // determine the side of this ray with respect to the plane
    AxisAlignedBox3 box = mMesh->GetFaceBox(*fi);

    if (box.Max(axis) > position ) 
      objectsFront.push_back(*fi);
    
    if (box.Min(axis) < position )
      objectsBack.push_back(*fi);
  }
  
  MeshKdLeaf *back = new MeshKdLeaf(objectsBack);
  MeshKdLeaf *front = new MeshKdLeaf(objectsFront);

  // replace a link from node's parent
  if (  parent )
    parent->ReplaceChildLink(leaf, node);
  
  // and setup child links
  node->SetupChildLinks(back, front);
  
  delete leaf;
  return node;
}




void
MeshKdTree::SortSplitCandidates(
                            MeshKdLeaf *node,
                            const int axis
                            )
{
  mSplitCandidates->clear();
  
  int requestedSize = 2*(int)node->mFaces.size();
  // creates a sorted split candidates array
  if (mSplitCandidates->capacity() > 500000 &&
      requestedSize < (int)(mSplitCandidates->capacity()/10) ) {
    delete mSplitCandidates;
    mSplitCandidates = new vector<SortableEntry>;
  }
  
  mSplitCandidates->reserve(requestedSize);
  
  vector<int>::const_iterator fi;
  // insert all queries 
  for(fi = node->mFaces.begin();
      fi < node->mFaces.end();
      fi++) {
    
    AxisAlignedBox3 box = mMesh->GetFaceBox(*fi);
    
    mSplitCandidates->push_back(SortableEntry(SortableEntry::FACE_MIN,
                                             box.Min(axis),
                                             *fi)
                               );
    
    
    mSplitCandidates->push_back(SortableEntry(SortableEntry::FACE_MAX,
                                             box.Max(axis),
                                             *fi)
                               );
  }
  
  stable_sort(mSplitCandidates->begin(), mSplitCandidates->end());
}


float
MeshKdTree::BestCostRatio(
                          MeshKdLeaf *node,
                          const AxisAlignedBox3 &box,
                          const int axis,
                          float &position,
                          int &objectsBack,
                          int &objectsFront
                          )
{

  SortSplitCandidates(node, axis);
  
  // go through the lists, count the number of objects left and right
  // and evaluate the following cost funcion:
  // C = ct_div_ci  + (ol + or)/queries

  int objectsLeft = 0, objectsRight = (int)node->mFaces.size();
  
  float minBox = box.Min(axis);
  float maxBox = box.Max(axis);
  float boxArea = box.SurfaceArea();
  
  float minBand = minBox + mSplitBorder*(maxBox - minBox);
  float maxBand = minBox + (1.0f - mSplitBorder)*(maxBox - minBox);
  
  float minSum = 1e20f;
  vector<SortableEntry>::const_iterator ci;

  for(ci = mSplitCandidates->begin();
      ci != mSplitCandidates->end();
      ci++) {
    switch ((*ci).type) {
    case SortableEntry::FACE_MIN:
      objectsLeft++;
      break;
    case SortableEntry::FACE_MAX:
      objectsRight--;
      break;
    }
    
    if ((*ci).value > minBand && (*ci).value < maxBand) {
      AxisAlignedBox3 lbox = box;
      AxisAlignedBox3 rbox = box;
      lbox.SetMax(axis, (*ci).value);
      rbox.SetMin(axis, (*ci).value);

      float sum = objectsLeft*lbox.SurfaceArea() + objectsRight*rbox.SurfaceArea();
      
      //      cout<<"pos="<<(*ci).value<<"\t q=("<<ql<<","<<qr<<")\t r=("<<rl<<","<<rr<<")"<<endl;
      //      cout<<"cost= "<<sum<<endl;
      
      if (sum < minSum) {
        minSum = sum;
        position = (*ci).value;
        
        objectsBack = objectsLeft;
        objectsFront = objectsRight;
      }
    }
  }
  
  float oldCost = (float)node->mFaces.size();
  float newCost = mCt_div_ci + minSum/boxArea;
  float ratio = newCost/oldCost;

#if 0
  cout<<"===================="<<endl;
  cout<<"costRatio="<<ratio<<" pos="<<position<<" t="<<(position - minBox)/(maxBox - minBox)
      <<"\t o=("<<objectsBack<<","<<objectsFront<<")"<<endl;
#endif
  return ratio;
}

int
MeshKdTree::CastRay(
                                        Ray &ray,
                                        MeshInstance *instance
                                        )
{
  int hits = 0;
  
  stack<RayTraversalData> tStack;
  
  float maxt = 1e6;
  float mint = 0;
  
  AxisAlignedBox3 box = GetBox();

  if (!box.GetMinMaxT(ray, &mint, &maxt))
    return 0;

  if (mint < 0)
    mint = 0;
  

  if (ray.GetType() == Ray::LOCAL_RAY &&
          ray.intersections.size() && ray.intersections[0].mT < mint) {
        return 0;
  }
  
  maxt += Limits::Threshold;
  
  Vector3 entp = ray.Extrap(mint);
  Vector3 extp = ray.Extrap(maxt);
  
  MeshKdNode *node = mRoot;
  MeshKdNode *farChild;
  float position;
  int axis;

  while (1) {
    if (!node->IsLeaf()) {
      MeshKdInterior *in = (MeshKdInterior *) node;
      position = in->mPosition;
      axis = in->mAxis;
                        
      if (entp[axis] <= position) {
                                if (extp[axis] <= position) {
                                        node = in->mBack;
                                        // cases N1,N2,N3,P5,Z2,Z3
                                        continue;
                                } else {
                                        // case N4
                                        node = in->mBack;
                                        farChild = in->mFront;
                                }
      }
      else {
                                if (position <= extp[axis]) {
                                        node = in->mFront;
                                        // cases P1,P2,P3,N5,Z1
                                        continue;
                                } else {
                                        node = in->mFront;
                                        farChild = in->mBack;
                                        // case P4
                                }
      }
      // $$ modification 3.5.2004 - hints from Kamil Ghais
      // case N4 or P4
      float tdist = (position - ray.GetLoc(axis)) / ray.GetDir(axis);
      tStack.push(RayTraversalData(farChild, extp, maxt));
      extp = ray.GetLoc() + ray.GetDir()*tdist;
      maxt = tdist;
    } else {
      // compute intersection with all objects in this leaf
      MeshKdLeaf *leaf = (MeshKdLeaf *) node;
      //      cout<<"leaf mfaces size="<<leaf->mFaces.size()<<endl<<flush;
      hits += instance->CastRay(ray, leaf->mFaces);
      
      if (ray.GetType() == Ray::LOCAL_RAY && ray.intersections.size())
                                if (ray.intersections[0].mT <= maxt) {
                                        break;
                                }
      
      // get the next node from the stack
      if (tStack.empty())
                                break;
      
      entp = extp;
      mint = maxt;

      if (ray.GetType() == Ray::LINE_SEGMENT && mint > 1.0f)
                                break;
                        
      RayTraversalData &s  = tStack.top();
      node = s.mNode;
      extp = s.mExitPoint;
      maxt = s.mMaxT;
      tStack.pop();
    }
  }
  
  
  return hits;
}

bool
MeshKdTree::TerminationCriteriaMet(const MeshKdLeaf *leaf, const int depth)
{
  //  cerr<<"\n OBJECTS="<<leaf->mObjects.size()<<endl;
  return
    (leaf->mFaces.size() <= mTermMinCost) ||
    (depth >= mTermMaxDepth);
  
}

bool
MeshKdTree::Construct()
{
  if (!mSplitCandidates)
    mSplitCandidates = new vector<SortableEntry>;
  
  // first construct a leaf that will get subdivide
  MeshKdLeaf *leaf = (MeshKdLeaf *) mRoot;

  mRoot = Subdivide(TraversalData(leaf, NULL, GetBox(), 0));
  
  // remove the allocated array
  delete mSplitCandidates;
  mSplitCandidates = NULL;
  
  return true;
}


MeshKdNode *
MeshKdTree::Subdivide(const TraversalData &tdata)
{
  MeshKdNode *result = NULL;
  
  priority_queue<TraversalData> tStack;
  //  stack<STraversalData> tStack;
  
  tStack.push(tdata);
  AxisAlignedBox3 backBox, frontBox;

  
  while (!tStack.empty()) {

    TraversalData data = tStack.top();
    tStack.pop();
    
    MeshKdNode *node = SubdivideNode((MeshKdLeaf *) data.mNode,
                                     data.mParent,
                                     data.mBox,
                                     data.mDepth,
                                     backBox,
                                     frontBox
                                     );
    if (result == NULL)
      result = node;
    
    if (!node->IsLeaf()) {

      MeshKdInterior *interior = (MeshKdInterior *) node;
      // push the children on the stack
      tStack.push(TraversalData(interior->mBack, interior, backBox, data.mDepth+1));
      tStack.push(TraversalData(interior->mFront, interior, frontBox, data.mDepth+1));
      
    }
  }
  
  return result;

}