source: orxonox.OLD/orxonox/trunk/src/lib/graphics/spatial_separation/quadtree_node.cc @ 4929

/*
   orxonox - the future of 3D-vertical-scrollers

   Copyright (C) 2004 orx

   This program is free software; you can redistribute it and/or modify
   it under the terms of the GNU General Public License as published by
   the Free Software Foundation; either version 2, or (at your option)
   any later version.

### File Specific:
   main-programmer: Patrick Boenzli
   co-programmer: ...
*/

#define DEBUG_SPECIAL_MODULE DEBUG_MODULE_SPATIAL_SEPARATION

#include "quadtree_node.h"

#include "quadtree.h"
#include "material.h"
#include "abstract_model.h"
#include "list.h"
#include "vector.h"

using namespace std;


/**
 *  standard constructor
 */
QuadtreeNode::QuadtreeNode (sTriangleExt** triangles, int numTriangles,
                            const float* pVertices, int numVertices,
                            Quadtree* quadtree, QuadtreeNode* parent,
                            Rectangle* rect, int treeDepth, const int maxDepth, int index
                           )
{
  /* save all important variables localy */
  this->pTriangles = triangles;
  this->numTriangles = numTriangles;
  this->pVertices = pVertices;
  this->numVertices = numVertices;
  this->quadtree = quadtree;
  this->parent = parent;
  this->pDimension = rect;
  this->treeDepth = treeDepth;
  this->maxDepth = maxDepth;
  this->indexNode = index;

  /* debug output */
  for( int i = 0; i < this->treeDepth; ++i)
    PRINT(3)(" |");
  PRINT(3)(" | +-| (Event) Building Node Nr. %i Depth: %i/%i, pointer: %p\n", this->indexNode, treeDepth, maxDepth, this);

  for( int i = 0; i < this->treeDepth; ++i)
    PRINT(3)(" |");
  PRINT(3)(" | +-| (II) Rectangle Center (%5.2f, %5.2f), half axis length: %5.2f\n",
  this->pDimension->getCenter()->x, this->pDimension->getCenter()->z, this->pDimension->getAxis());

  this->init();
}


/**
 *  standard constructor
 */
QuadtreeNode::QuadtreeNode(modelInfo* pModelInfo, Quadtree* quadtree, const int maxDepth)
{
  /* save all important variables localy */
  this->pModelInfo = pModelInfo;
  this->quadtree = quadtree;
  this->maxDepth = maxDepth;
  this->treeDepth = 0;
  this->indexNode = 0;

  /* create an array of triangle references */
  this->pTriangles = new sTriangleExt*[this->pModelInfo->numTriangles];
  this->numTriangles = this->pModelInfo->numTriangles;
  this->pVertices = this->pModelInfo->pVertices;
  this->numVertices = this->pModelInfo->numVertices;
  for( int i = 0; i < this->pModelInfo->numTriangles; ++i)
    this->pTriangles[i] = &this->pModelInfo->pTriangles[i];

  /* debug output */
  for( int i = 0; i < this->treeDepth; ++i)
    PRINT(3)(" |");
  PRINT(3)(" | +-| (Event) Building Node Nr. %i Depth: %i/%i, pointer: %p\n", this->indexNode, treeDepth, maxDepth, this);

  /* set some important variables */
  this->pDimension = this->getDimFromModel();

  this->init();
}


/**
 *  takes the rest of the initialisation process
 */
void QuadtreeNode::init()
{
  this->setClassID(CL_QUADTREE_NODE, "QuadtreeNode");

  /* init the rest of the variables for both init types */
  this->offset = 0.0f;
  this->nodeIter = -1;
  this->bDraw = false;

  this->parent = NULL;
  this->nodeA = NULL;
  this->nodeB = NULL;
  this->nodeC = NULL;
  this->nodeD = NULL;
  this->nodes = new QuadtreeNode*[4];
  for(int i = 0; i < 4; ++i)
    this->nodes[i] = NULL;

  /* now separate the nodes */
  if( this->treeDepth < this->maxDepth)
    this->separateNode();
}


/**
 *  standard deconstructor
 */
QuadtreeNode::~QuadtreeNode ()
{
  if( this->nodeA != NULL)
    delete this->nodeA;
  if( this->nodeB != NULL)
    delete this->nodeB;
  if( this->nodeC != NULL)
    delete this->nodeC;
  if( this->nodeD != NULL)
    delete this->nodeD;
}



/**
 *  this functions builds up a hash table containing all leafs of the Quadtree in a sorted array
 * @param nodeList the nodelist array to add them
 * @param index the current index in the array

  The algorithm used for this purpose is home-brown. its not to fast but and the nodes are not always in the right
  order. this is why there will be needed a quicksort later on.
 */
void QuadtreeNode::buildHashTable(QuadtreeNode** nodeList, int* index)
{
  if( this->nodeIter == -1)
    this->nodeIter = *index;

  /*              offset              #of elements in a row            #of rows in a quadtree          */
  int threshold = this->nodeIter + (int)pow(2, this->maxDepth) * (int)pow(2, maxDepth - treeDepth - 1);
  int loopLimit = (*index < threshold)?2:4;

  /* is it a leaf? */
  if( this->treeDepth < this->maxDepth)
    for(int i = (*index < threshold)?0:2; i < loopLimit; ++i)
      this->nodes[i]->buildHashTable(nodeList, index);
  else
    nodeList[(*index)++] = this;
}



/**
 *  gives the signal to separate the model into a quadtree
 * @param treeDepth the max depth, the steps to go if treeDept == 0 leaf reached

 * @todo ATTENION: This function is currently not used and not implemented, but would be a nice addon
 */
void QuadtreeNode::separateNode(float minLength)
{
  /* dimension calculation & limit checking */
  if( minLength <= this->pDimension->getAxis())
    return;

  /* node separation */
//   this->separateNode();
//   this->nodeA->separateNode(minLength);
//   this->nodeB->separateNode(minLength);
//   this->nodeC->separateNode(minLength);
//   this->nodeD->separateNode(minLength);
}


/**
 *  gives the signal to separate the model into a quadtree
 * @param treeDepth the max depth, the steps to go if treeDept == 0 leaf reached
 */
void QuadtreeNode::separateNode()
{
  /* separate the four regions into the four lists */
  tList<sTriangleExt*>*           listA = new tList<sTriangleExt*>();    //!< triangle list of nodeA
  tList<sTriangleExt*>*           listB = new tList<sTriangleExt*>();    //!< triangle list of nodeB
  tList<sTriangleExt*>*           listC = new tList<sTriangleExt*>();    //!< triangle list of nodeC
  tList<sTriangleExt*>*           listD = new tList<sTriangleExt*>();    //!< triangle list of nodeD
  const float*                    pVert;                                 //!< pointer to the vertices
  const Vector*                   rectCenter;                            //!< vector to the center of the rect

  rectCenter = this->pDimension->getCenter();
  for( int i = 0; i < this->numTriangles; ++i)
  {
    for( int j = 0; j < 3; ++j)
    {
      pVert = &this->pVertices[this->pTriangles[i]->indexToVertices[j]];
      if( pVert[0] > rectCenter->x + this->offset && pVert[2] > rectCenter->z + this->offset)
        listA->add(&this->pTriangles[i]);
      if( pVert[0] < rectCenter->x + this->offset && pVert[2] > rectCenter->z + this->offset)
        listB->add(&this->pTriangles[i]);
      if( pVert[0] < rectCenter->x + this->offset && pVert[2] < rectCenter->z + this->offset)
        listC->add(&this->pTriangles[i]);
      if( pVert[0] > rectCenter->x + this->offset && pVert[2] < rectCenter->z + this->offset)
        listD->add(&this->pTriangles[i]);
    }
  }
  for( int i = 0; i < treeDepth; ++i)
    PRINT(3)(" |");
  PRINT(3)(" | +-| (II) Quadtree Counts - separating: A: %i, B: %i, C: %i, D: %i\n", listA->getSize(), listB->getSize(), listC->getSize(), listD->getSize());


  /* Separating into to the triangle arrays */
  sTriangleExt**                 pTriA;                                 //!< Triangle array A
  sTriangleExt**                 pTriB;                                 //!< Triangle array B
  sTriangleExt**                 pTriC;                                 //!< Triangle array C
  sTriangleExt**                 pTriD;                                 //!< Triangle array D
  int                            lenA;                                  //!< length array A
  int                            lenB;                                  //!< length array B
  int                            lenC;                                  //!< length array C
  int                            lenD;                                  //!< length array D
  tIterator<sTriangleExt*>*      iterator;                              //!< iterator for the list iterations
  sTriangleExt**                 tempTri;                               //!< temp save place for triangle pointer
  int                            counter;                               //!< counter for the while loops

  lenA = listA->getSize();
  lenB = listB->getSize();
  lenC = listC->getSize();
  lenD = listD->getSize();

  pTriA = new sTriangleExt*[listA->getSize()];
  pTriB = new sTriangleExt*[listB->getSize()];
  pTriC = new sTriangleExt*[listC->getSize()];
  pTriD = new sTriangleExt*[listD->getSize()];

  counter = 0;
  iterator = listA->getIterator();
  tempTri = iterator->nextElement();
  while( tempTri)
  {
    pTriA[counter] = *tempTri;
    tempTri = iterator->nextElement();
    ++counter;
  }
  counter = 0;
  iterator = listB->getIterator();
  tempTri = iterator->nextElement();
  while( tempTri)
  {
    pTriB[counter] = *tempTri;
    tempTri = iterator->nextElement();
    ++counter;
  }
  counter = 0;
  iterator = listC->getIterator();
  tempTri = iterator->nextElement();
  while( tempTri)
  {
    pTriC[counter] = *tempTri;
    tempTri = iterator->nextElement();
    ++counter;
  }
  counter = 0;
  iterator = listD->getIterator();
  tempTri = iterator->nextElement();
  while( tempTri)
  {
    pTriD[counter] = *tempTri;
    tempTri = iterator->nextElement();
    ++counter;
  }

  /* now do cleanup */
  delete listA;
  delete listB;
  delete listC;
  delete listD;
  delete iterator;


  /* now create the rectangle dimensions */
  Vector                     v;                                                             //!< temp saving place
  Rectangle*                 rA;                                                            //!< new size of the node A
  Rectangle*                 rB;                                                            //!< new size of the node B
  Rectangle*                 rC;                                                            //!< new size of the node C
  Rectangle*                 rD;                                                            //!< new size of the node D


  v.x = this->pDimension->getCenter()->x + this->pDimension->getAxis() / 2.0f;
  v.y = 0.0;
  v.z = this->pDimension->getCenter()->z + this->pDimension->getAxis() / 2.0f;
  rA = new Rectangle(v, this->pDimension->getAxis() / 2.0f);
  v.z = this->pDimension->getCenter()->z - this->pDimension->getAxis() / 2.0f;
  rB = new Rectangle(v, this->pDimension->getAxis() / 2.0f);
  v.x = this->pDimension->getCenter()->x - this->pDimension->getAxis() / 2.0f;
  rC = new Rectangle(v, this->pDimension->getAxis() / 2.0f);
  v.z = this->pDimension->getCenter()->z + this->pDimension->getAxis() / 2.0f;
  rD = new Rectangle(v, this->pDimension->getAxis() / 2.0f);

  /* now create the new nodes  */
  this->nodeA = new QuadtreeNode(pTriA, lenA, this->pVertices, this->numVertices, this->quadtree, this, rA, this->treeDepth + 1, this->maxDepth, (this->treeDepth + 1) * 10 + 0);
  this->nodeB = new QuadtreeNode(pTriB, lenB, this->pVertices, this->numVertices, this->quadtree, this, rB, this->treeDepth + 1, this->maxDepth, (this->treeDepth + 1) * 10 + 1);
  this->nodeC = new QuadtreeNode(pTriC, lenC, this->pVertices, this->numVertices, this->quadtree, this, rC, this->treeDepth + 1, this->maxDepth, (this->treeDepth + 1) * 10 + 2);
  this->nodeD = new QuadtreeNode(pTriD, lenD, this->pVertices, this->numVertices, this->quadtree, this, rD, this->treeDepth + 1, this->maxDepth, (this->treeDepth + 1) * 10 + 3);

  /* map the array references, this is for faster and automatical interfacing  \todo: use only array */
  this->nodes[0] = this->nodeA;
  this->nodes[1] = this->nodeB;
  this->nodes[2] = this->nodeC;
  this->nodes[3] = this->nodeD;
}


/**
 *  gets the maximal dimension of a model
 * @return the dimension of the AbstractModel as a Rectangle

   The rectangle is x-z axis aligned. ATTENTION: if there are any vertices in the model, that exceed the
   size of 999999.0, there probably will be some errors in the dimensions calculations. Write an email to
   patrick@orxonox.ethz.ch if you will ever encounter a model bigger than 999999.0 units, I will realy be
   happy to see orxonox used to extensivly :)
 */
Rectangle* QuadtreeNode::getDimFromModel()
{
  float            maxX, maxY;                       //!< the maximal coordinates axis
  float            minX, minY;                       //!< minimal axis coorindates
  const float*     pVertices;                        //!< pointer to the current vertices

  maxX = -999999; maxY = -999999;
  minX =  999999; minY =  999999;
  /* get maximal/minimal x/y */
  for( int i = 0; i < this->numTriangles; ++i)
  {
    for( int j = 0; j < 3; ++j)
    {

      pVertices = &this->pVertices[this->pTriangles[i]->indexToVertices[j]];
      if( pVertices[0] > maxX)
        maxX = pVertices[0];
      if( pVertices[2] > maxY)
        maxY = pVertices[2];

      if( pVertices[0] < minX)
        minX = pVertices[0];
      if( pVertices[2] < minY)
        minY = pVertices[2];
    }
  }

  Rectangle* rect = new Rectangle();
  rect->setCenter((maxX + minX) / 2.0f, 0.0f, (maxY + minY) / 2.0f); /* this is little strange, since y is in opengl the up vector */
  rect->setAxis(fmax(((fabs(maxX) + fabs(minX)) / 2.0f), ((fabs(maxY) + fabs(minY)) / 2.0f)));

  for( int i = 0; i < this->treeDepth; ++i)
    PRINT(3)(" |");
  PRINT(3)(" | +-| (II) Rectangle Dimension  (%5.2f, %5.2f) to (%5.2f, %5.2f), Center (%5.2f, %5.2f)\n", minX, minY, maxX, maxY, rect->getCenter()->x, rect->getCenter()->z, rect->getAxis());
  return rect;
}


 /**
 * checks if a point is included in this quadtree
 * @param v the vector to be checked
 * @returns true if the vector is included
  */
bool QuadtreeNode::includesPoint(const Vector& v)
{
  Vector center = *this->pDimension->getCenter();
  float ax = this->pDimension->getAxis();

  if( v.x > center.x - ax && v.x < center.x + ax &&
      v.z > center.z - ax && v.z < center.z + ax )
  {
    this->bDraw = true;
    return true;
  }
  return false;
}


sTriangleExt* QuadtreeNode::getTriangle(const Vector& position)
{

}

bool QuadtreeNode::pointInTriangle(const Vector&p, const Vector& a, const Vector& b, const Vector& c)
{
  if( this->sameSide(p, a, b, c) && this->sameSide(p, b, a, c) && sameSide(p, c, a, b))
    return true;
  return false;
  /*
  function PointInTriangle(p, a,b,c)
  if SameSide(p,a, b,c) and SameSide(p,b, a,c)
  and SameSide(p,c, a,b) then return true
  else return false
  */
}


bool QuadtreeNode::sameSide(const Vector& p1, const Vector&p2, const Vector& a, const Vector& b)
{
  Vector cp1 = (b - a).cross(p1 - a);
  Vector cp2 = (b - a).cross(p2 - a);

  if( unlikely(cp1.dot(cp2) >= 0)) return true;
  return false;
}


/**
 *  draws all the debug quadtree squares
 */
void QuadtreeNode::drawTree() const
{
  if( this->treeDepth == this->maxDepth)
  {
    Vector t1 = *this->pDimension->getCenter();
    float ax = this->pDimension->getAxis();
    float h = 50.0f;

    this->quadtree->getMaterial(this->indexNode)->select();
    glBegin(GL_QUADS);
    glVertex3f(t1.x + ax, h - this->treeDepth * 10.0f, t1.z + ax);
    glVertex3f(t1.x - ax, h - this->treeDepth * 10.0f, t1.z + ax);
    glVertex3f(t1.x - ax, h - this->treeDepth * 10.0f, t1.z - ax);
    glVertex3f(t1.x + ax, h - this->treeDepth * 10.0f, t1.z - ax);
    glEnd();
  }


  if( this->nodeA != NULL)
    this->nodeA->drawTree();
  if( this->nodeB != NULL)
    this->nodeB->drawTree();
  if( this->nodeC != NULL)
    this->nodeC->drawTree();
  if( this->nodeD != NULL)
    this->nodeD->drawTree();
}


/**
 *  draws only this quadtree square
 */
void QuadtreeNode::draw() const
{
  if( likely(!bDraw))
    return;
  Vector t1 = *this->pDimension->getCenter();
  float ax = this->pDimension->getAxis();
  float h = 70.0f;

  glBegin(GL_QUADS);
  this->quadtree->getMaterial(this->indexNode)->select();

  glVertex3f(t1.x + ax, h, t1.z + ax);
  glVertex3f(t1.x - ax, h, t1.z + ax);
  glVertex3f(t1.x - ax, h, t1.z - ax);
  glVertex3f(t1.x + ax, h, t1.z - ax);

  glEnd();

}