source: orxonox.OLD/trunk/src/lib/graphics/spatial_separation/quadtree.cc @ 7194

/*
   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.h"

#include "quadtree_node.h"
#include "vector.h"

#include "material.h"
#include "debug.h"

using namespace std;

#define QUADTREE_MATERIAL_COUNT       4

/**
 *  standard constructor
 */
Quadtree::Quadtree (const modelInfo* pModelInfo, const int treeDepth)
{
  this->setClassID(CL_QUADTREE, "Quadtree");
  this->pModelInfo = pModelInfo;
  this->treeDepth = treeDepth;

  /* initialize the materials for debug draw */
  this->materials = new Material*[QUADTREE_MATERIAL_COUNT];
  for(int i = 0; i < QUADTREE_MATERIAL_COUNT; ++i)
  {
    materials[i] = new Material();
    materials[i]->setIllum(3);
  }
  materials[0]->setAmbient(0.0, 0.3, 0.0);
  materials[1]->setAmbient(0.4, 0.0, 0.2);
  materials[2]->setAmbient(1.0, 0.0, 0.0);
  materials[3]->setAmbient(5.0, 3.0, 1.0);

  /* build the tree */
  this->rootNode = new QuadtreeNode(this->pModelInfo, this, this->treeDepth);

  /* make an array with access to the leafs of the Quad-Tree */
  this->nodes = new QuadtreeNode*[(int)pow(4, treeDepth)];
  int index = 0; //new int; *index = 0; // !!changed by bensch!!
  for(int i = 0; i < (int)pow(2, treeDepth); ++i)
  {
    this->rootNode->buildHashTable(this->nodes, &index);
  }
  /* sort and revert the hash table to fit the right position */
  this->sortHashTable(this->nodes);
  this->revertHashTable(this->nodes);

  /* define some important and often used variables */
  this->quadLength = this->nodes[0]->getDimension()->getAxis() * 2.0f;
  Rectangle* r = this->rootNode->getDimension();
  float xOff = r->getCenter().x - r->getAxis();
  float yOff = r->getCenter().z - r->getAxis();
  this->offset = new Vector();
  this->offset->x = xOff;
  this->offset->z = yOff;
  this->maxIndex = (int)pow(2, this->treeDepth);
}


/**
 *  standard deconstructor
 */
Quadtree::~Quadtree ()
{
  // delete what has to be deleted here
  if (this->materials != NULL)
  {
    for (unsigned int i = 0; i < QUADTREE_MATERIAL_COUNT; i++)
      delete this->materials[i];
    delete[] this->materials;
  }

  delete this->offset;

  delete this->rootNode;
  delete [] this->nodes;
}


/**
 *  this function rotates the array and mirrors it in the middle
 * @param nodes: the nodes to translate

  since the original matrix is counted from the right upper edge to the right lower one, we have to reorganize the elements
  to be able to easily correlate the hashtable array indexes with the coorindates.
 */
void Quadtree::revertHashTable(QuadtreeNode** nodes)
{
  int                  len         = (int)pow(2, this->treeDepth);          //!< the length of a quadtree side
  int                  iterator    = 0;                                     //!< iterator used for mapping
  QuadtreeNode*        tmpNode     = NULL;                                  //!< temp saving place
  int                  offset      = 0;                                     //!< offset used in the calc

  for(int i = len - 1; i >= 0; --i)
  {
    for(int j = len - 1; j >= 0; --j)
    {
      offset = j * len + i;
      /* only swap the elements in one direction */
      if( offset > iterator)
      {
        tmpNode = nodes[offset];
        nodes[offset] = nodes[iterator];
        nodes[iterator] = tmpNode;
      }
      ++iterator;
    }
  }
}


/**
 *  sorts the hash table using their positions
 * @param nodes the nodes to use
 */
void Quadtree::sortHashTable(QuadtreeNode** nodes)
{
  int                  len         = (int)pow(2, this->treeDepth);          //!< the length of a quadtree side
  float                a;                                                   //!< temp place for float a
  float                b;                                                   //!< temp place for float b
  QuadtreeNode*        tmpNode;                                             //!< tmp place for a QuadtreeNode

  for( int i = 0; i < len; ++i)
  {
    for( int j = 0; j < len; ++j)
    {
      for( int k = j + 1; k < len; ++k)
      {
        a = this->nodes[i * len + j]->getDimension()->getCenter().z;
        b = this->nodes[i * len + k]->getDimension()->getCenter().z;

        if( b > a)
        {
          tmpNode = this->nodes[i * len + j];
          this->nodes[i * len + j] = this->nodes[i * len + k];
          this->nodes[i * len + k] = tmpNode;
        }
      }
    }
  }

}


/**
 *  maps a position to a quadtree
 * @param position the position so look for
 * @return the quadtree

  this function will return the quadtree that contains the position
 */
QuadtreeNode* Quadtree::getQuadtreeFromPosition(const Vector& position) const
{
  /* shift into model space */
  Vector v = position - *this->offset;
  /* map */
  int i = (int)(v.x / quadLength);
  int j = (int)(v.z / quadLength);

  /* check if object is still inside the terrain - this check is not complete @todo check all 4 bounds */
  if( i < this->maxIndex && j < this->maxIndex && i >= 0 && j >= 0)
    return this->nodes[i + j * this->maxIndex];


  PRINTF(4)("Object has left terrain\n");
  return NULL;
}


/**
 *  maps a position to a triangle
 * @param position the position so look for
 * @return the triangle

  this function will return the quadtree that contains the position
 */
sTriangleExt* Quadtree::getTriangleFromPosition(const Vector& position) const
{
  QuadtreeNode* q = this->getQuadtreeFromPosition(position);

  if( likely(q != NULL))
    return q->getTriangle(position - *this->offset);
  return NULL;
}


/**
 *  draws the debug quadtree boxes around the model
 */
void Quadtree::drawTree() const
{
  //this->rootNode->drawTree();
  for(int i = 0; i < (int)pow(4, this->treeDepth); ++i)
  {
    this->nodes[i]->draw();
  }
}