ntl_bsptree.cpp

Go to the documentation of this file.

/******************************************************************************
 *
 * El'Beem - Free Surface Fluid Simulation with the Lattice Boltzmann Method
 * Copyright 2003-2006 Nils Thuerey
 *
 * Tree container for fast triangle intersects
 *
 *****************************************************************************/


#include "ntl_bsptree.h"
#include "utilities.h"

#include <algorithm>

int globalSortingAxis;
vector<ntlVec3Gfx> *globalSortingPoints;

#define TREE_DOUBLEI 300

/* try axis selection? */
bool chooseAxis = 0;
/* do median search? */
int doSort = 0;


class BSPNode {
    public:
        BSPNode() {};

        ntlVec3Gfx min,max;              /* AABB for node */
        vector<ntlTriangle *> *members;  /* stored triangles */
        BSPNode *child[2]; /* pointer to children nodes */
        char axis;                  /* division axis */
        char cloneVec;              /* is this vector a clone? */

        inline bool isLeaf() const { 
            return (child[0] == NULL); 
        }
};


class BSPStackElement {
    public:
        BSPNode *node;
        gfxReal mindist, maxdist;
};

class BSPStack {
    public:
        int stackPtr;
        BSPStackElement elem[ BSP_STACK_SIZE ];
};

class TriangleBBox {
    public:
        ntlVec3Gfx start, end;
};


/******************************************************************************
 * calculate tree statistics
 *****************************************************************************/
void calcStats(BSPNode *node, int depth, int &noLeafs, gfxReal &avgDepth, gfxReal &triPerLeaf,int &totalTris)
{
    if(node->members != NULL) {
        totalTris += node->members->size();
    }
    //depth = 15; // DBEUG!

    if( (node->child[0]==NULL) && (node->child[1]==NULL) ) {
        // leaf
        noLeafs++;
        avgDepth += depth;
        triPerLeaf += node->members->size();
    } else {
        for(int i=0;i<2;i++) 
        calcStats(node->child[i], depth+1, noLeafs, avgDepth, triPerLeaf, totalTris);
    }
}



/******************************************************************************
 * triangle comparison function for median search 
 *****************************************************************************/
bool lessTriangleAverage(const ntlTriangle *x, const ntlTriangle *y)
{
    return x->getAverage(globalSortingAxis) < y->getAverage(globalSortingAxis);
}


/******************************************************************************
 * triangle AABB intersection
 *****************************************************************************/
bool ntlTree::checkAABBTriangle(ntlVec3Gfx &min, ntlVec3Gfx &max, ntlTriangle *tri)
{
    // test only BB of triangle
    TriangleBBox *bbox = &mpTBB[ tri->getBBoxId() ];
    if( bbox->end[0]   < min[0] ) return false;
    if( bbox->start[0] > max[0] ) return false;
    if( bbox->end[1]   < min[1] ) return false;
    if( bbox->start[1] > max[1] ) return false;
    if( bbox->end[2]   < min[2] ) return false;
    if( bbox->start[2] > max[2] ) return false;
    return true;
}







/******************************************************************************
 * Default constructor
 *****************************************************************************/
ntlTree::ntlTree() :
  mStart(0.0), mEnd(0.0), mMaxDepth( 5 ), mMaxListLength( 5 ), mpRoot( NULL) ,
  mpNodeStack( NULL), mpVertices( NULL ), mpVertNormals( NULL ), mpTriangles( NULL ),
  mCurrentDepth(0), mCurrentNodes(0), mTriDoubles(0)
{
  errFatal( "ntlTree","Uninitialized BSP Tree!\n",SIMWORLD_INITERROR );
  return;
}


/******************************************************************************
 * Constructor with init
 *****************************************************************************/
//ntlTree::ntlTree(int depth, int objnum, vector<ntlVec3Gfx> *vertices, vector<ntlVec3Gfx> *normals, vector<ntlTriangle> *trilist) :
ntlTree::ntlTree(int depth, int objnum, ntlScene *scene, int triFlagMask) :
  mStart(0.0), mEnd(0.0), mMaxDepth( depth ), mMaxListLength( objnum ), mpRoot( NULL) ,
  mpNodeStack( NULL), mpTBB( NULL ),
    mTriangleMask( 0xFFFF ),
  mCurrentDepth(0), mCurrentNodes(0), mTriDoubles(0)
{  
    // init scene data pointers
    mpVertices = scene->getVertexPointer();
    mpVertNormals = scene->getVertexNormalPointer();
    mpTriangles = scene->getTrianglePointer();
    mTriangleMask = triFlagMask;

  if(mpTriangles == NULL) {
    errFatal( "ntlTree Cons","no triangle list!\n",SIMWORLD_INITERROR);
    return;
  }
  if(mpTriangles->size() == 0) {
    warnMsg( "ntlTree::ntlTree","No triangles ("<< mpTriangles->size()  <<")!\n");
        mStart = mEnd = ntlVec3Gfx(0,0,0);
    return;
  }
  if(depth>=BSP_STACK_SIZE) {
    errFatal( "ntlTree::ntlTree","Depth to high ("<< mMaxDepth  <<")!\n", SIMWORLD_INITERROR );
    return;
  }

  /* check triangles (a bit inefficient, but we dont know which vertices belong
     to this tree), and generate bounding boxes */
    mppTriangles = new vector<ntlTriangle *>;
    int noOfTriangles = mpTriangles->size();
    mpTBB = new TriangleBBox[ noOfTriangles ];
    int bbCount = 0;
  mStart = mEnd = (*mpVertices)[ mpTriangles->front().getPoints()[0] ];
    //errMsg("TreeDebug","Start");
  for (vector<ntlTriangle>::iterator iter = mpTriangles->begin();
       iter != mpTriangles->end(); 
       iter++ ) {
        //errorOut(" d "<< convertFlags2String((int)(*iter).getFlags()) <<" "<< convertFlags2String( (int)mTriangleMask)<<" add? "<<( ((int)(*iter).getFlags() & (int)mTriangleMask) != 0 ) );
        // discard triangles that dont match mask
        if( ((int)(*iter).getFlags() & (int)mTriangleMask) == 0 ) {
            continue;
        }

        // test? TODO
        ntlVec3Gfx tnormal = (*mpVertNormals)[ (*iter).getPoints()[0] ]+
            (*mpVertNormals)[ (*iter).getPoints()[1] ]+
            (*mpVertNormals)[ (*iter).getPoints()[2] ];
        ntlVec3Gfx triangleNormal = (*iter).getNormal();
        if( equal(triangleNormal, ntlVec3Gfx(0.0)) ) continue;
        if( equal(       tnormal, ntlVec3Gfx(0.0)) ) continue;
        // */

        ntlVec3Gfx bbs, bbe;
        //errMsg("TreeDebug","Triangle");
        for(int i=0;i<3;i++) {
            int index = (*iter).getPoints()[i];
            ntlVec3Gfx tp = (*mpVertices)[ index ];
            //errMsg("TreeDebug","  Point "<<i<<" = "<<tp<<" ");
            if(tp[0] < mStart[0]) mStart[0]= tp[0];
            if(tp[0] > mEnd[0])   mEnd[0]= tp[0];
            if(tp[1] < mStart[1]) mStart[1]= tp[1];
            if(tp[1] > mEnd[1])   mEnd[1]= tp[1];
            if(tp[2] < mStart[2]) mStart[2]= tp[2];
            if(tp[2] > mEnd[2])   mEnd[2]= tp[2];
            if(i==0) {
                bbs = bbe = tp; 
            } else {
                if( tp[0] < bbs[0] ) bbs[0] = tp[0];
                if( tp[0] > bbe[0] ) bbe[0] = tp[0];
                if( tp[1] < bbs[1] ) bbs[1] = tp[1];
                if( tp[1] > bbe[1] ) bbe[1] = tp[1];
                if( tp[2] < bbs[2] ) bbs[2] = tp[2];
                if( tp[2] > bbe[2] ) bbe[2] = tp[2];
            }
        }
        mppTriangles->push_back( &(*iter) );
        //errMsg("TreeDebug","Triangle "<<(*mpVertices)[(*iter).getPoints()[0]]<<" "<<(*mpVertices)[(*iter).getPoints()[1]]<<" "<<(*mpVertices)[(*iter).getPoints()[2]]<<" ");

        // add BB
        mpTBB[ bbCount ].start = bbs;
        mpTBB[ bbCount ].end = bbe;
        (*iter).setBBoxId( bbCount );
        bbCount++;
  }
    
    

  /* slighlty enlarge bounding tolerance for tree 
     to avoid problems with triangles paralell to slabs */
  mStart -= ntlVec3Gfx( getVecEpsilon() );
  mEnd   += ntlVec3Gfx( getVecEpsilon() );

  /* init root node and stack */
  mpNodeStack = new BSPStack;
  mpRoot = new BSPNode;
  mpRoot->min = mStart;
  mpRoot->max = mEnd;
  mpRoot->axis = AXIS_X;
  mpRoot->members = mppTriangles;
    mpRoot->child[0] = mpRoot->child[1] = NULL;
    mpRoot->cloneVec = 0;
    globalSortingPoints = mpVertices;
    mpTriDist = new char[ mppTriangles->size() ];
    mNumNodes = 1;
    mAbortSubdiv = 0;

  /* create tree */
  debugOutInter( "Generating BSP Tree...  (Nodes "<< mCurrentNodes <<
                        ", Depth "<<mCurrentDepth<< ") ", 2, 2000 );
  subdivide(mpRoot, 0, AXIS_X);
  debMsgStd("ntlTree::ntlTree",DM_MSG,"Generated Tree: Nodes "<< mCurrentNodes <<
                             ", Depth "<<mCurrentDepth<< " with "<<noOfTriangles<<" triangles", 2 );

    delete [] mpTriDist;
    delete [] mpTBB;
    mpTriDist = NULL;
    mpTBB = NULL;

    /* calculate some stats about tree */
    int noLeafs = 0;
    gfxReal avgDepth = 0.0;
    gfxReal triPerLeaf = 0.0;
    int totalTris = 0;
    
    calcStats(mpRoot,0, noLeafs, avgDepth, triPerLeaf, totalTris);
    avgDepth /= (gfxReal)noLeafs;
    triPerLeaf /= (gfxReal)noLeafs;
    debMsgStd("ntlTree::ntlTree",DM_MSG,"Tree ("<<doSort<<","<<chooseAxis<<") Stats: Leafs:"<<noLeafs<<", avgDepth:"<<avgDepth<<
            ", triPerLeaf:"<<triPerLeaf<<", triDoubles:"<<mTriDoubles<<", totalTris:"<<totalTris
            <<" nodes:"<<mNumNodes
            //<<" T"<< (totalTris%3)  // 0=ich, 1=f, 2=a
            , 2 );

    if(mAbortSubdiv) {
        errMsg("ntlTree::ntlTree","Aborted... "<<mNumNodes);
    deleteNode(mpRoot);
        mpRoot = NULL;
    }
}

/******************************************************************************
 * Destructor
 *****************************************************************************/
ntlTree::~ntlTree()
{
  /* delete tree, and all members except for the root node */
  deleteNode(mpRoot);
  if(mpNodeStack) delete mpNodeStack;
}


/******************************************************************************
 * subdivide tree
 *****************************************************************************/
void ntlTree::subdivide(BSPNode *node, int depth, int axis)
{
  int nextAxis=0; /* next axis to partition */
    int allTriDistSet = (1<<0)|(1<<1); // all mpTriDist flags set?
    //errorOut(" "<<node<<" depth:"<<depth<<" m:"<<node->members->size() <<"  "<<node->min<<" - "<<node->max );

  if(depth>mCurrentDepth) mCurrentDepth = depth;
  node->child[0] = node->child[1] = NULL;
    if( ( (int)node->members->size() > mMaxListLength) &&
            (depth < mMaxDepth ) 
            && (node->cloneVec<10)
            && (!mAbortSubdiv)
            ) {

        gfxReal planeDiv = 0.499999;    // position of plane division

        // determine next subdivision axis
        int newaxis = 0;
        gfxReal extX = node->max[0]-node->min[0];
        gfxReal extY = node->max[1]-node->min[1];
        gfxReal extZ = node->max[2]-node->min[2];

        if( extY>extX  ) {
            if( extZ>extY ) {
                newaxis = 2;
            } else {
                newaxis = 1;
            }
        } else {
            if( extZ>extX ) {
                newaxis = 2;
            } else {
                newaxis = 0;
            }
        }
        axis = node->axis = newaxis;

        // init child nodes
        for( int i=0; i<2; i++) {
            /* status output */
            mCurrentNodes++;
            if(mCurrentNodes % 13973 ==0) {
                debugOutInter( "NTL Generating BSP Tree ("<<doSort<<","<<chooseAxis<<") ...  (Nodes "<< mCurrentNodes <<
                        ", Depth "<<mCurrentDepth<< ") " , 2, 2000);
            }

            /* create new node */
            node->child[i] = new BSPNode;
            node->child[i]->min = node->min;
            node->child[i]->max = node->max;
            node->child[i]->max = node->max;
            node->child[i]->child[0] = NULL;
            node->child[i]->child[1] = NULL;
            node->child[i]->members = NULL;
            nextAxis = (axis+1)%3;
            node->child[i]->axis = nextAxis;
            mNumNodes++;
            // abort when using 256MB only for tree...
            if(mNumNodes*sizeof(BSPNode)> 1024*1024*512) mAbortSubdiv = 1;

            /* current division plane */
            if(!i) {
                node->child[i]->min[axis] = node->min[axis];
                node->child[i]->max[axis] = node->min[axis] + planeDiv*
                    (node->max[axis]-node->min[axis]);
            } else {
                node->child[i]->min[axis] = node->min[axis] + planeDiv*
                    (node->max[axis]-node->min[axis]);
                node->child[i]->max[axis] = node->max[axis];
            }
        }


        /* process the two children */
        int thisTrisFor[2] = {0,0};
        int thisTriDoubles[2] = {0,0};
        for(int t=0;t<(int)node->members->size();t++) mpTriDist[t] = 0;
        for( int i=0; i<2; i++) {
            /* distribute triangles */
            int t  = 0;
            for (vector<ntlTriangle *>::iterator iter = node->members->begin();
                    iter != node->members->end(); iter++ ) {

                /* add triangle, check bounding box axis */
                TriangleBBox *bbox = &mpTBB[ (*iter)->getBBoxId() ];
                bool isintersect = true;
                if( bbox->end[axis]   < node->child[i]->min[axis] ) isintersect = false;
                else if( bbox->start[axis] > node->child[i]->max[axis] ) isintersect = false;
                if(isintersect) {
                    // add flag to vector 
                    mpTriDist[t] |= (1<<i);
                    // count no. of triangles for vector init
                    thisTrisFor[i]++;
                }

                if(mpTriDist[t] == allTriDistSet) {
                    thisTriDoubles[i]++;
                    mTriDoubles++; // TODO check for small geo tree??
                }
                t++;
            } /* end of loop over all triangles */
        } // i

        /* distribute triangles */
        bool haveCloneVec[2] = {false, false};
        for( int i=0; i<2; i++) {
            node->child[i]->members = new vector<ntlTriangle *>( thisTrisFor[i] );
            node->child[i]->cloneVec = 0;
        }

        int tind0 = 0;
        int tind1 = 0;
        if( (!haveCloneVec[0]) || (!haveCloneVec[1]) ){
            int t  = 0; // triangle index counter
            for (vector<ntlTriangle *>::iterator iter = node->members->begin();
                    iter != node->members->end(); iter++ ) {
                if(!haveCloneVec[0]) {
                    if( (mpTriDist[t] & 1) == 1) {
                        (*node->child[0]->members)[tind0] = (*iter); // dont use push_back for preinited size!
                        tind0++;
                    }
                }
                if(!haveCloneVec[1]) {
                    if( (mpTriDist[t] & 2) == 2) {
                        (*node->child[1]->members)[tind1] = (*iter); // dont use push_back for preinited size!
                        tind1++;
                    }
                }
                t++;
            } /* end of loop over all triangles */
        }

        // subdivide children
        for( int i=0; i<2; i++) {
            /* recurse */
            subdivide( node->child[i], depth+1, nextAxis );
        }

        /* if we are here, this are childs, so we dont need the members any more... */
        /* delete unecessary members */
        if( (!haveCloneVec[0]) && (!haveCloneVec[1]) && (node->cloneVec == 0) ){ 
            delete node->members; 
        }
        node->members = NULL;

    } /* subdivision necessary */
}

/******************************************************************
 * triangle intersection with triangle pointer,
 * returns t,u,v by references 
 */
#if GFX_PRECISION==1
// float values
#define RAY_TRIANGLE_EPSILON (1e-07)

#else 
// double values
#define RAY_TRIANGLE_EPSILON (1e-15)

#endif 


/******************************************************************************
 * intersect ray with BSPtree
 *****************************************************************************/
inline void ntlRay::intersectTriangle(vector<ntlVec3Gfx> *mpV, ntlTriangle *tri, gfxReal &t, gfxReal &u, gfxReal &v) const
{
  /* (cf. moeller&haines, page 305) */
  t = GFX_REAL_MAX;
  ntlVec3Gfx  e0 = (*mpV)[ tri->getPoints()[0] ];
  ntlVec3Gfx  e1 = (*mpV)[ tri->getPoints()[1] ] - e0;
  ntlVec3Gfx  e2 = (*mpV)[ tri->getPoints()[2] ] - e0;
  ntlVec3Gfx  p  = cross( mDirection, e2 );
  gfxReal divisor  = dot(e1, p);    
  if((divisor > -RAY_TRIANGLE_EPSILON)&&(divisor < RAY_TRIANGLE_EPSILON)) return;
      
  gfxReal invDivisor  = 1/divisor;
  ntlVec3Gfx  s  = mOrigin - e0;
  u  = invDivisor * dot(s, p);
  if( (u<0.0-RAY_TRIANGLE_EPSILON) || (u>1.0+RAY_TRIANGLE_EPSILON) ) return;
      
  ntlVec3Gfx  q  = cross( s,e1 );
  v  = invDivisor * dot(mDirection, q);
  if( (v<0.0-RAY_TRIANGLE_EPSILON) || ((u+v)>1.0+RAY_TRIANGLE_EPSILON) ) return;
      
  t = invDivisor * dot(e2, q);      
}
void ntlTree::intersect(const ntlRay &ray, gfxReal &distance, 
        ntlVec3Gfx &normal, 
        ntlTriangle *&tri, 
        int flags, bool forceNonsmooth) const
{
  gfxReal mint = GFX_REAL_MAX;  /* current minimal t */
  ntlVec3Gfx  retnormal;       /* intersection (interpolated) normal */
    gfxReal mintu=0.0, mintv=0.0;    /* u,v for min t intersection */

  BSPNode *curr, *nearChild, *farChild; /* current node and children */
  gfxReal  planedist, mindist, maxdist;
  ntlVec3Gfx   pos;

    ntlTriangle *hit = NULL;
    tri = NULL;

  ray.intersectCompleteAABB(mStart,mEnd,mindist,maxdist);

  if((maxdist < 0.0) ||
         (!mpRoot) ||
     (mindist == GFX_REAL_MAX) ||
     (maxdist == GFX_REAL_MAX) ) {
    distance = -1.0;
    return;
  }
  mindist -= getVecEpsilon();
  maxdist += getVecEpsilon();

  /* stack init */
  mpNodeStack->elem[0].node = NULL;
  mpNodeStack->stackPtr = 1;

  curr = mpRoot;  
  mint = GFX_REAL_MAX;
  while(curr != NULL) {

    while( !curr->isLeaf() ) {
      planedist = distanceToPlane(curr, curr->child[0]->max, ray );
      getChildren(curr, ray.getOrigin(), nearChild, farChild );

            // check ray direction for small plane distances
      if( (planedist>-getVecEpsilon() )&&(planedist< getVecEpsilon() ) ) {
                // ray origin on intersection plane
                planedist = 0.0;
                if(ray.getDirection()[curr->axis]>getVecEpsilon() ) {
                    // larger coords
                    curr = curr->child[1];
                } else if(ray.getDirection()[curr->axis]<-getVecEpsilon() ) {
                    // smaller coords
                    curr = curr->child[0];
                } else {
                    // paralell, order doesnt really matter are min/max/plane ok?
                    mpNodeStack->elem[ mpNodeStack->stackPtr ].node    = curr->child[0];
                    mpNodeStack->elem[ mpNodeStack->stackPtr ].mindist = planedist;
                    mpNodeStack->elem[ mpNodeStack->stackPtr ].maxdist = maxdist;
                    (mpNodeStack->stackPtr)++;
                    curr    = curr->child[1];
                    maxdist = planedist;
                }
            } else {
                // normal ray
                if( (planedist>maxdist) || (planedist<0.0-getVecEpsilon() ) ) {
                    curr = nearChild;
                } else if(planedist < mindist) {
                    curr = farChild;
                } else {
                    mpNodeStack->elem[ mpNodeStack->stackPtr ].node    = farChild;
                    mpNodeStack->elem[ mpNodeStack->stackPtr ].mindist = planedist;
                    mpNodeStack->elem[ mpNodeStack->stackPtr ].maxdist = maxdist;
                    (mpNodeStack->stackPtr)++;

                    curr    = nearChild;
                    maxdist = planedist;
                }
            } 
    }
    
    
    /* intersect with current node */
    for (vector<ntlTriangle *>::iterator iter = curr->members->begin();
                 iter != curr->members->end(); iter++ ) {

            /* check for triangle flags before intersecting */
            if((!flags) || ( ((*iter)->getFlags() & flags) > 0 )) {

                if( ((*iter)->getLastRay() == ray.getID() )&&((*iter)->getLastRay()>0) ) {
                    // was already intersected...
                } else {
                    // we still need to intersect this triangle
                    gfxReal u=0.0,v=0.0, t=-1.0;
                    ray.intersectTriangle( mpVertices, (*iter), t,u,v);
                    (*iter)->setLastRay( ray.getID() );
                    
                    if( (t > 0.0) && (t<mint) )  {
                        mint = t;     
                        hit = (*iter);
                        mintu = u; mintv = v;
                    }
                }

            } // flags check
    }

    /* check if intersection is valid */
    if( (mint>0.0) && (mint < GFX_REAL_MAX) ) {
      pos = ray.getOrigin() + ray.getDirection()*mint;

      if( (pos[0] >= curr->min[0]) && (pos[0] <= curr->max[0]) &&
                    (pos[1] >= curr->min[1]) && (pos[1] <= curr->max[1]) &&
                    (pos[2] >= curr->min[2]) && (pos[2] <= curr->max[2]) ) 
            {

                if(forceNonsmooth) {
                    // calculate triangle normal
                    ntlVec3Gfx e0,e1,e2;
                    e0 = (*mpVertices)[ hit->getPoints()[0] ];
                    e1 = (*mpVertices)[ hit->getPoints()[1] ];
                    e2 = (*mpVertices)[ hit->getPoints()[2] ];
                    retnormal = cross( -(e2-e0), (e1-e0) );
                } else {
                    // calculate interpolated normal
                    retnormal = (*mpVertNormals)[ hit->getPoints()[0] ] * (1.0-mintu-mintv)+
                        (*mpVertNormals)[ hit->getPoints()[1] ]*mintu +
                        (*mpVertNormals)[ hit->getPoints()[2] ]*mintv;
                }
                normalize(retnormal);
                normal = retnormal;
                distance = mint;
                tri = hit;
                return;
      }
    }    

    (mpNodeStack->stackPtr)--;
    curr    = mpNodeStack->elem[ mpNodeStack->stackPtr ].node;
    mindist = mpNodeStack->elem[ mpNodeStack->stackPtr ].mindist;
    maxdist = mpNodeStack->elem[ mpNodeStack->stackPtr ].maxdist;
  } /* traverse tree */

    if(mint == GFX_REAL_MAX) {
        distance = -1.0;
    } else {
        // intersection outside the BSP bounding volumes might occur due to roundoff...
        //retnormal = (*mpVertNormals)[ hit->getPoints()[0] ] * (1.0-mintu-mintv)+ (*mpVertNormals)[ hit->getPoints()[1] ]*mintu + (*mpVertNormals)[ hit->getPoints()[2] ]*mintv;
        if(forceNonsmooth) {
            // calculate triangle normal
            ntlVec3Gfx e0,e1,e2;
            e0 = (*mpVertices)[ hit->getPoints()[0] ];
            e1 = (*mpVertices)[ hit->getPoints()[1] ];
            e2 = (*mpVertices)[ hit->getPoints()[2] ];
            retnormal = cross( -(e2-e0), (e1-e0) );
        } else {
            // calculate interpolated normal
            retnormal = (*mpVertNormals)[ hit->getPoints()[0] ] * (1.0-mintu-mintv)+
                (*mpVertNormals)[ hit->getPoints()[1] ]*mintu +
                (*mpVertNormals)[ hit->getPoints()[2] ]*mintv;
        }

        normalize(retnormal);
        normal = retnormal;
        distance = mint;
        tri = hit;
    }
    return;
}

inline void ntlRay::intersectTriangleX(vector<ntlVec3Gfx> *mpV, ntlTriangle *tri, gfxReal &t, gfxReal &u, gfxReal &v) const
{
  /* (cf. moeller&haines, page 305) */
  t = GFX_REAL_MAX;
  ntlVec3Gfx  e0 = (*mpV)[ tri->getPoints()[0] ];
  ntlVec3Gfx  e1 = (*mpV)[ tri->getPoints()[1] ] - e0;
  ntlVec3Gfx  e2 = (*mpV)[ tri->getPoints()[2] ] - e0;

  //ntlVec3Gfx  p  = cross( mDirection, e2 );
  //ntlVector3Dim<Scalar> cp( (-), (- (1.0 *v[2])), ((1.0 *v[1]) -) );
  ntlVec3Gfx  p(0.0, -e2[2], e2[1]);

  gfxReal divisor  = dot(e1, p);    
  if((divisor > -RAY_TRIANGLE_EPSILON)&&(divisor < RAY_TRIANGLE_EPSILON)) return;
      
  gfxReal invDivisor  = 1/divisor;
  ntlVec3Gfx  s  = mOrigin - e0;
  u  = invDivisor * dot(s, p);
  if( (u<0.0-RAY_TRIANGLE_EPSILON) || (u>1.0+RAY_TRIANGLE_EPSILON) ) return;
      
  ntlVec3Gfx  q  = cross( s,e1 );
  //v  = invDivisor * dot(mDirection, q);
  v  = invDivisor * q[0];
  if( (v<0.0-RAY_TRIANGLE_EPSILON) || ((u+v)>1.0+RAY_TRIANGLE_EPSILON) ) return;
      
  t = invDivisor * dot(e2, q);
}
void ntlTree::intersectX(const ntlRay &ray, gfxReal &distance, 
        ntlVec3Gfx &normal, 
        ntlTriangle *&tri, 
        int flags, bool forceNonsmooth) const
{
  gfxReal mint = GFX_REAL_MAX;  /* current minimal t */
  ntlVec3Gfx  retnormal;       /* intersection (interpolated) normal */
    gfxReal mintu=0.0, mintv=0.0;    /* u,v for min t intersection */

  BSPNode *curr, *nearChild, *farChild; /* current node and children */
  gfxReal  planedist, mindist, maxdist;
  ntlVec3Gfx   pos;

    ntlTriangle *hit = NULL;
    tri = NULL;

  ray.intersectCompleteAABB(mStart,mEnd,mindist,maxdist); // +X

  if((maxdist < 0.0) ||
         (!mpRoot) ||
     (mindist == GFX_REAL_MAX) ||
     (maxdist == GFX_REAL_MAX) ) {
    distance = -1.0;
    return;
  }
  mindist -= getVecEpsilon();
  maxdist += getVecEpsilon();

  /* stack init */
  mpNodeStack->elem[0].node = NULL;
  mpNodeStack->stackPtr = 1;

  curr = mpRoot;  
  mint = GFX_REAL_MAX;
  while(curr != NULL) { // +X

    while( !curr->isLeaf() ) {
      planedist = distanceToPlane(curr, curr->child[0]->max, ray );
      getChildren(curr, ray.getOrigin(), nearChild, farChild );

            // check ray direction for small plane distances
      if( (planedist>-getVecEpsilon() )&&(planedist< getVecEpsilon() ) ) {
                // ray origin on intersection plane
                planedist = 0.0;
                if(ray.getDirection()[curr->axis]>getVecEpsilon() ) {
                    // larger coords
                    curr = curr->child[1];
                } else if(ray.getDirection()[curr->axis]<-getVecEpsilon() ) {
                    // smaller coords
                    curr = curr->child[0];
                } else {
                    // paralell, order doesnt really matter are min/max/plane ok?
                    mpNodeStack->elem[ mpNodeStack->stackPtr ].node    = curr->child[0];
                    mpNodeStack->elem[ mpNodeStack->stackPtr ].mindist = planedist;
                    mpNodeStack->elem[ mpNodeStack->stackPtr ].maxdist = maxdist;
                    (mpNodeStack->stackPtr)++;
                    curr    = curr->child[1];
                    maxdist = planedist;
                }
            } else {
                // normal ray
                if( (planedist>maxdist) || (planedist<0.0-getVecEpsilon() ) ) {
                    curr = nearChild;
                } else if(planedist < mindist) {
                    curr = farChild;
                } else {
                    mpNodeStack->elem[ mpNodeStack->stackPtr ].node    = farChild;
                    mpNodeStack->elem[ mpNodeStack->stackPtr ].mindist = planedist;
                    mpNodeStack->elem[ mpNodeStack->stackPtr ].maxdist = maxdist;
                    (mpNodeStack->stackPtr)++;

                    curr    = nearChild;
                    maxdist = planedist;
                }
            } 
    } // +X
    
    
    /* intersect with current node */
    for (vector<ntlTriangle *>::iterator iter = curr->members->begin();
                 iter != curr->members->end(); iter++ ) {

            /* check for triangle flags before intersecting */
            if((!flags) || ( ((*iter)->getFlags() & flags) > 0 )) {

                if( ((*iter)->getLastRay() == ray.getID() )&&((*iter)->getLastRay()>0) ) {
                    // was already intersected...
                } else {
                    // we still need to intersect this triangle
                    gfxReal u=0.0,v=0.0, t=-1.0;
                    ray.intersectTriangleX( mpVertices, (*iter), t,u,v);
                    (*iter)->setLastRay( ray.getID() );
                    
                    if( (t > 0.0) && (t<mint) )  {
                        mint = t;     
                        hit = (*iter);
                        mintu = u; mintv = v;
                    }
                }

            } // flags check
    } // +X

    /* check if intersection is valid */
    if( (mint>0.0) && (mint < GFX_REAL_MAX) ) {
      pos = ray.getOrigin() + ray.getDirection()*mint;

      if( (pos[0] >= curr->min[0]) && (pos[0] <= curr->max[0]) &&
                    (pos[1] >= curr->min[1]) && (pos[1] <= curr->max[1]) &&
                    (pos[2] >= curr->min[2]) && (pos[2] <= curr->max[2]) ) 
            {

                if(forceNonsmooth) {
                    // calculate triangle normal
                    ntlVec3Gfx e0,e1,e2;
                    e0 = (*mpVertices)[ hit->getPoints()[0] ];
                    e1 = (*mpVertices)[ hit->getPoints()[1] ];
                    e2 = (*mpVertices)[ hit->getPoints()[2] ];
                    retnormal = cross( -(e2-e0), (e1-e0) );
                } else {
                    // calculate interpolated normal
                    retnormal = (*mpVertNormals)[ hit->getPoints()[0] ] * (1.0-mintu-mintv)+
                        (*mpVertNormals)[ hit->getPoints()[1] ]*mintu +
                        (*mpVertNormals)[ hit->getPoints()[2] ]*mintv;
                }
                normalize(retnormal);
                normal = retnormal;
                distance = mint;
                tri = hit;
                return;
      }
    }     // +X

    (mpNodeStack->stackPtr)--;
    curr    = mpNodeStack->elem[ mpNodeStack->stackPtr ].node;
    mindist = mpNodeStack->elem[ mpNodeStack->stackPtr ].mindist;
    maxdist = mpNodeStack->elem[ mpNodeStack->stackPtr ].maxdist;
  } /* traverse tree */

    if(mint == GFX_REAL_MAX) {
        distance = -1.0;
    } else {

        // intersection outside the BSP bounding volumes might occur due to roundoff...
        if(forceNonsmooth) {
            // calculate triangle normal
            ntlVec3Gfx e0,e1,e2;
            e0 = (*mpVertices)[ hit->getPoints()[0] ];
            e1 = (*mpVertices)[ hit->getPoints()[1] ];
            e2 = (*mpVertices)[ hit->getPoints()[2] ];
            retnormal = cross( -(e2-e0), (e1-e0) );
        } else {
            // calculate interpolated normal
            retnormal = (*mpVertNormals)[ hit->getPoints()[0] ] * (1.0-mintu-mintv)+
                (*mpVertNormals)[ hit->getPoints()[1] ]*mintu +
                (*mpVertNormals)[ hit->getPoints()[2] ]*mintv;
        }

        normalize(retnormal);
        normal = retnormal;
        distance = mint;
        tri = hit;
    } // +X
    return;
}



/******************************************************************************
 * distance to plane function for nodes
 *****************************************************************************/
gfxReal ntlTree::distanceToPlane(BSPNode *curr, ntlVec3Gfx plane, ntlRay ray) const
{
  return ( (plane[curr->axis]-ray.getOrigin()[curr->axis]) / ray.getDirection()[curr->axis] );
}


/******************************************************************************
 * return ordering of children nodes relatice to origin point
 *****************************************************************************/
void ntlTree::getChildren(BSPNode *curr, ntlVec3Gfx origin, BSPNode *&node_near, BSPNode *&node_far) const 
{
  if(curr->child[0]->max[ curr->axis ] >= origin[ curr->axis ]) {
    node_near = curr->child[0];
    node_far = curr->child[1];
  } else {
    node_near = curr->child[1];
    node_far = curr->child[0];
  }
}


/******************************************************************************
 * delete a node of the tree with all sub nodes
 *  dont delete root members 
 *****************************************************************************/
void ntlTree::deleteNode(BSPNode *curr) 
{
    if(!curr) return;

  if(curr->child[0] != NULL)
    deleteNode(curr->child[0]);
  if(curr->child[1] != NULL)
    deleteNode(curr->child[1]);

  if(curr->members != NULL) delete curr->members;
  delete curr;
}



/******************************************************************
 * intersect only front or backsides
 * currently unused
 */
inline void ntlRay::intersectTriangleFront(vector<ntlVec3Gfx> *mpV, ntlTriangle *tri, gfxReal &t, gfxReal &u, gfxReal &v) const
{
  t = GFX_REAL_MAX;
  ntlVec3Gfx  e0 = (*mpV)[ tri->getPoints()[0] ];
  ntlVec3Gfx  e1 = (*mpV)[ tri->getPoints()[1] ] - e0;
  ntlVec3Gfx  e2 = (*mpV)[ tri->getPoints()[2] ] - e0;
  ntlVec3Gfx  p  = cross( mDirection, e2 );
  gfxReal a  = dot(e1, p);  
  //if((a > -RAY_TRIANGLE_EPSILON)&&(a < RAY_TRIANGLE_EPSILON)) return;
  if(a < RAY_TRIANGLE_EPSILON) return; // cull backsides
      
  gfxReal f  = 1/a;
  ntlVec3Gfx  s  = mOrigin - e0;
  u  = f * dot(s, p);
  if( (u<0.0-RAY_TRIANGLE_EPSILON) || (u>1.0+RAY_TRIANGLE_EPSILON) ) return;
      
  ntlVec3Gfx  q  = cross( s,e1 );
  v  = f * dot(mDirection, q);
  if( (v<0.0-RAY_TRIANGLE_EPSILON) || ((u+v)>1.0+RAY_TRIANGLE_EPSILON) ) return;
      
  t = f * dot(e2, q);      
}
inline void ntlRay::intersectTriangleBack(vector<ntlVec3Gfx> *mpV, ntlTriangle *tri, gfxReal &t, gfxReal &u, gfxReal &v) const
{
  t = GFX_REAL_MAX;
  ntlVec3Gfx  e0 = (*mpV)[ tri->getPoints()[0] ];
  ntlVec3Gfx  e1 = (*mpV)[ tri->getPoints()[1] ] - e0;
  ntlVec3Gfx  e2 = (*mpV)[ tri->getPoints()[2] ] - e0;
  ntlVec3Gfx  p  = cross( mDirection, e2 );
  gfxReal a  = dot(e1, p);  
  //if((a > -RAY_TRIANGLE_EPSILON)&&(a < RAY_TRIANGLE_EPSILON)) return;
  if(a > -RAY_TRIANGLE_EPSILON) return; // cull frontsides
      
  gfxReal f  = 1/a;
  ntlVec3Gfx  s  = mOrigin - e0;
  u  = f * dot(s, p);
  if( (u<0.0-RAY_TRIANGLE_EPSILON) || (u>1.0+RAY_TRIANGLE_EPSILON) ) return;
      
  ntlVec3Gfx  q  = cross( s,e1 );
  v  = f * dot(mDirection, q);
  if( (v<0.0-RAY_TRIANGLE_EPSILON) || ((u+v)>1.0+RAY_TRIANGLE_EPSILON) ) return;
      
  t = f * dot(e2, q);      
}