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

/*
   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"
#include "debug.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;

  if( this->pTriangles)
    delete [] this->pTriangles;

  if( this->pDimension)
    delete this->pDimension;
}



/**
 *  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
  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->firstElement();
  while( tempTri)
  {
    pTriA[counter] = *tempTri;
    tempTri = iterator->nextElement();
    ++counter;
  }
  counter = 0;
  iterator = listB->getIterator();
  tempTri = iterator->firstElement();
  while( tempTri)
  {
    pTriB[counter] = *tempTri;
    tempTri = iterator->nextElement();
    ++counter;
  }
  counter = 0;
  iterator = listC->getIterator();
  tempTri = iterator->firstElement();
  while( tempTri)
  {
    pTriC[counter] = *tempTri;
    tempTri = iterator->nextElement();
    ++counter;
  }
  counter = 0;
  iterator = listD->getIterator();
  tempTri = iterator->firstElement();
  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) const
{
  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 )
    return true;
  return false;
}



float QuadtreeNode::getHeight(const Vector& position) const
{
  Vector a, b, c;
  sTriangleExt* tri;

  for( int i = 0; i < this->numTriangles; ++i)
  {
    a = *(sVec3D*)&this->pVertices[this->pTriangles[i]->indexToVertices[0]];
    b = *(sVec3D*)&this->pVertices[this->pTriangles[i]->indexToVertices[1]];
    c = *(sVec3D*)&this->pVertices[this->pTriangles[i]->indexToVertices[2]];

    if( unlikely(this->pointInTriangle(position, a, b, c)))
    {
      tri = this->pTriangles[i];
      break;
    }
  }

  /* calculate height out of the data collected above */

}


/**
 *  get triangles that includes the position
 * @param position the position that is going to be checked
 * @returns the triangle in which the position is included, NULL if there is no such triangle

 There is some random behaviour if there are more than one triangle at the same y
 coordinate. At the moment the function just takes the first triangle, that included the
 vector
 */
sTriangleExt* QuadtreeNode::getTriangle(const Vector& position) const
{
  Vector a, b, c;
  PRINTF(0)("Get Triangle, %i\n", this->numTriangles);

  for( int i = 0; i < numTriangles; ++i)
    {

      a = *(sVec3D*)&this->pVertices[this->pTriangles[i]->indexToVertices[0]];
      b = *(sVec3D*)&this->pVertices[this->pTriangles[i]->indexToVertices[1]];
      c = *(sVec3D*)&this->pVertices[this->pTriangles[i]->indexToVertices[2]];

      if( unlikely(this->pointInTriangle(position, a, b, c)))
        return this->pTriangles[i];

    }
  return NULL;
}


/**
 *  checks if the point is in the triangle
 * @param point to be checked
 * @param rectangle edge a
 * @param rectangle edge b
 * @param rectangle edge c
 * @returns true if the point is inside
 */
bool QuadtreeNode::pointInTriangle(const Vector&p, const Vector& a, const Vector& b, const Vector& c) const
{

  PRINTF(0)("p: (%f, %f, %f)\n", p.x, p.y, p.z);
  PRINTF(0)("a: (%f, %f, %f)\n", a.x, a.y, a.z);
  PRINTF(0)("b: (%f, %f, %f)\n", b.x, b.y, b.z);
  PRINTF(0)("c: (%f, %f, %f)\n", c.x, c.y, c.z);

  if( this->sameSide(p, a, b, c) && this->sameSide(p, b, a, c) && sameSide(p, c, a, b))
    return true;
  return false;
}


/**
 *  checks if two points are on the same side
 * @param point one to be checked
 * @param point two to be checked
 * @param begining point of the line
 * @param end of the line
 */
bool QuadtreeNode::sameSide(const Vector& p1, const Vector&p2, const Vector& a, const Vector& b) const
{
  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(!this->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();
}