/*
   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_COLLISION

#include "obb_tree_node.h"
#include "list.h"
#include "obb.h"
#include "obb_tree.h"
#include "vector.h"
#include "abstract_model.h"

#include <math.h>

#include "stdincl.h"

#include "lin_alg.h"




using namespace std;

OBBTree*  OBBTreeNode::obbTree = NULL;

float**  OBBTreeNode::coMat = NULL;
float**  OBBTreeNode::eigvMat = NULL;
float*   OBBTreeNode::eigvlMat = NULL;
int*     OBBTreeNode::rotCount = NULL;

/**
   \brief standard constructor
 */
OBBTreeNode::OBBTreeNode ()
{
  this->setClassID(CL_OBB_TREE_NODE, "OBBTreeNode");
  this->nodeLeft = NULL;
  this->nodeRight = NULL;

  if(coMat == NULL)
  {
    coMat = new float*[4];
    for(int i = 0; i < 4; i++)
      coMat[i] = new float[4];
  }
  if(eigvMat == NULL)
  {
    eigvMat = new float*[4];
    for(int i = 0; i < 4; i++)
      eigvMat[i] = new float[4];
  }
  if( eigvlMat == NULL)
  {
    eigvlMat = new float[4];
  }
  if( rotCount == NULL)
    rotCount = new int;

  this->sphereObj = gluNewQuadric();
}


/**
   \brief standard deconstructor
 */
OBBTreeNode::~OBBTreeNode ()
{
  // delete what has to be deleted here
}



/**
   \brief creates a new BVTree or BVTree partition
   \param depth: how much more depth-steps to go: if == 1 don't go any deeper!
   \param verticesList: the list of vertices of the object - each vertices triple is interpreted as a triangle
 */
void OBBTreeNode::spawnBVTree(const int depth, sVec3D *verticesList, const int length)
{
  PRINT(0)("\n");
  this->treeIndex = this->obbTree->getID();
  PRINTF(0)("OBB Depth: %i, tree index: %i, numVertices: %i\n", depth, treeIndex, length);
  this->depth = depth;


  this->bvElement = new OBB();
  this->bvElement->vertices = verticesList;
  this->bvElement->numOfVertices = length;
  PRINTF(3)("Created OBBox\n");
  this->calculateBoxCovariance(this->bvElement, verticesList, length);
  PRINTF(3)("Calculated attributes1\n");
  this->calculateBoxEigenvectors(this->bvElement, verticesList, length);
  PRINTF(3)("Calculated attributes2\n");
  this->calculateBoxAxis(this->bvElement, verticesList, length);
  PRINTF(3)("Calculated attributes3\n");


  if( likely( this->depth > 0))
  {
    this->forkBox(this->bvElement);


    if(this->tmpLen1 > 0)
    {
      OBBTreeNode* node1 = new OBBTreeNode();
      this->nodeLeft = node1;
      this->nodeLeft->spawnBVTree(depth - 1, this->tmpVert1, this->tmpLen1);
    }
    else
    {
      PRINTF(0)("Aboarding tree walk: less than 3 vertices left\n");
    }

    if( this->tmpLen2 > 0)
    {
      OBBTreeNode* node2 = new OBBTreeNode();
      this->nodeRight = node2;
      this->nodeRight->spawnBVTree(depth - 1, this->tmpVert2, this->tmpLen2);
    }
    else
    {
      PRINTF(0)("Aboarding tree walk: less than 3 vertices left\n");
    }

  }
}



void OBBTreeNode::calculateBoxCovariance(OBB* box, sVec3D* verticesList, int length)
{
  float     facelet[length];                         //!< surface area of the i'th triangle of the convex hull
  float     face;                                    //!< surface area of the entire convex hull
  Vector    centroid[length];                        //!< centroid of the i'th convex hull
  Vector    center;                                  //!< the center of the entire hull
  Vector    p, q, r;                                 //!< holder of the polygon data, much more conveniant to work with Vector than sVec3d
  Vector    t1, t2;                                  //!< temporary values
  float     covariance[3][3];                        //!< the covariance matrix
  int       mode = 3;                                //!< mode = 0: vertex soup, no connections, mode = 1: 3 following verteces build a triangle

  this->numOfVertices = length;
  this->vertices = verticesList;


  if( likely(mode == 0))
  {
    /* fist compute all the convex hull face/facelets and centroids */
    for(int i = 0; i < length; i+=3)          /* FIX-ME-QUICK: hops of 3, array indiscontinuity*/
    {
      p = verticesList[i];
      q = verticesList[i + 1];
      r = verticesList[i + 2];

      t1 = p - q; t2 = p - r;

      /* finding the facelet surface via cross-product */
      facelet[i] = 0.5f * fabs( t1.cross(t2).len() );
      /* update the entire convex hull surface */
      face += facelet[i];

      /* calculate the cetroid of the hull triangles */
      centroid[i] = (p + q + r) * 1/3;
      /* now calculate the centroid of the entire convex hull, weighted average of triangle centroids */
      center += centroid[i] * facelet[i];
    }
    /* take the average of the centroid sum */
    center /= face;
    PRINTF(3)("-- Calculated Center\n");


    /* now calculate the covariance matrix - if not written in three for-loops, it would compute faster: minor */
    for(int j = 0; j < 3; ++j)
    {
      for(int k = 0; k < 3; ++k)
      {
        for(int i = 0; i < length; i+=3)
        {
          p = verticesList[i];
          q = verticesList[i + 1];
          r = verticesList[i + 2];

          covariance[j][k] = facelet[i] / (12.0f * face) * (9.0f * centroid[i][j] * centroid[i][k] + p[j] * p[k] +
              q[j] * q[k] + r[j] * r[k]) - center[j] * center[k];
        }
      }
    }
    PRINTF(3)("-- Calculated Covariance\n");
  }
  else if( mode == 1)
  {
    for( int i = 0; i < length; i+=3)          /* FIX-ME-QUICK: hops of 3, array indiscontinuity*/
    {
      p = verticesList[i];
      q = verticesList[i + 1];
      r = verticesList[i + 2];

      centroid[i] = (p + q + r) / 3.0f;
      center += centroid[i];
    }
    center /= length;

    for( int j = 0; j < 3; ++j)
    {
      for( int k = 0; k < 3; ++k)
      {
        for( int i = 0; i < length; i+=3)
        {
          p = verticesList[i];
          q = verticesList[i +1];
          r = verticesList[i + 2];

          covariance[j][k] = p[j] * p[k] + q[j] * q[k] + r[j] + r[k];
        }
        covariance[j][k] /= (3.0f * length);
      }
    }
    PRINTF(3)("-- Calculated Covariance\n");
  }
  else if( mode == 2)
  {
    /* fist compute all the convex hull face/facelets and centroids */
    for(int i = 0; i < length; i+=3)          /* FIX-ME-QUICK: hops of 3, array indiscontinuity*/
    {
      p = verticesList[i];
      q = verticesList[i + 1];
      r = verticesList[i + 2];

      t1 = p - q; t2 = p - r;

      /* finding the facelet surface via cross-product */
      facelet[i] = 0.5f * fabs( t1.cross(t2).len() );
      /* update the entire convex hull surface */
      face += facelet[i];

      /* calculate the cetroid of the hull triangles */
      centroid[i] = (p + q + r) * 1/3;
      /* now calculate the centroid of the entire convex hull, weighted average of triangle centroids */
      center += centroid[i] * facelet[i];
    }
    /* take the average of the centroid sum */
    center /= face;
    PRINTF(3)("-- Calculated Center\n");

    for( int j = 0; j < 3; ++j)
    {
      for( int k = 0; k < 3; ++k)
      {
        for( int i = 0; i < length; i+=3)
        {
          p = verticesList[i];
          q = verticesList[i +1];
          r = verticesList[i + 2];

          covariance[j][k] = p[j] * p[k] + q[j] * q[k] + r[j] + r[k];
        }
        covariance[j][k] /= (3.0f * length);
      }
    }
    PRINTF(3)("-- Calculated Covariance\n");
  }
  else
  {
    for( int i = 0; i < length; ++i)          /* FIX-ME-QUICK: hops of 3, array indiscontinuity*/
    {
      center += verticesList[i];
    }
    center /= length;

    for( int j = 0; j < 3; ++j)
    {
      for( int k = 0; k < 3; ++k)
      {
        for( int i = 0; i < length; i+=3)
        {
          p = verticesList[i];
          q = verticesList[i +1];
          r = verticesList[i + 2];

          covariance[j][k] = p[j] * p[k] + q[j] * q[k] + r[j] + r[k];
        }
        covariance[j][k] /= (3.0f * length);
      }
    }
    PRINTF(3)("-- Calculated Covariance\n");
  }

  PRINTF(3)("\nVertex Data:\n");
  for(int i = 0; i < length; i++)
  {
    //PRINTF(0)("vertex %i: %f, %f, %f\n", i, verticesList[i][0], verticesList[i][1], verticesList[i][2]);
    PRINTF(3)("vertex %i: %f, %f, %f\n", i, box->vertices[i][0], box->vertices[i][1], box->vertices[i][2]);
  }


//   PRINTF(3)("\nCovariance Matrix:\n");
//   for(int j = 0; j < 3; ++j)
//   {
//     PRINTF(3)(" |");
//     for(int k = 0; k < 3; ++k)
//     {
//       PRINTF(3)(" \b%f ", covariance[j][k]);
//     }
//     PRINTF(3)(" |\n");
//   }
  PRINTF(3)("center: %f, %f, %f\n", center.x, center.y, center.z);


//   for(int i = 0; i < 3; ++i)
//   {
//     box->covarianceMatrix[i][0] = covariance[i][0];
//     box->covarianceMatrix[i][1] = covariance[i][1];
//     box->covarianceMatrix[i][3] = covariance[i][2];
//   }
  *box->center = center;
  PRINTF(3)("-- Written Result to obb\n");
}



void OBBTreeNode::calculateBoxEigenvectors(OBB* box, sVec3D* verticesList, int length)
{

  /* now getting spanning vectors of the sub-space:
  the eigenvectors of a symmertric matrix, such as the
  covarience matrix are mutually orthogonal.
  after normalizing them, they can be used as a the basis
  vectors
  */
  Vector**              axis = new Vector*[3];                //!< the references to the obb axis

  coMat[1][1] = box->covarianceMatrix[0][0]; coMat[1][2] = box->covarianceMatrix[0][1]; coMat[1][3] = box->covarianceMatrix[0][2];
  coMat[2][1] = box->covarianceMatrix[1][0]; coMat[2][2] = box->covarianceMatrix[1][1]; coMat[2][3] = box->covarianceMatrix[1][2];
  coMat[3][1] = box->covarianceMatrix[2][0]; coMat[3][2] = box->covarianceMatrix[2][1]; coMat[3][3] = box->covarianceMatrix[2][2];

  /* new jacobi tests */
  JacobI(coMat, 3, eigvlMat, eigvMat, rotCount);
  PRINTF(3)("-- Done Jacobi Decomposition\n");


//   PRINTF(3)("Jacobi\n");
//   for(int j = 1; j < 4; ++j)
//   {
//     PRINTF(3)(" |");
//     for(int k = 1; k < 4; ++k)
//     {
//       PRINTF(3)(" \b%f ", eigvMat[j][k]);
//     }
//     PRINTF(3)(" |\n");
//   }

  axis[0] = new Vector(eigvMat[1][1], eigvMat[2][1], eigvMat[3][1]);
  axis[1] = new Vector(eigvMat[1][2], eigvMat[2][2], eigvMat[3][2]);
  axis[2] = new Vector(eigvMat[1][3], eigvMat[2][3], eigvMat[3][3]);
  box->axis = axis;
  PRINTF(3)("-- Got Axis\n");

  PRINTF(0)("eigenvector: %f, %f, %f\n", box->axis[0]->x, box->axis[0]->y, box->axis[0]->z);
  PRINTF(0)("eigenvector: %f, %f, %f\n", box->axis[1]->x, box->axis[1]->y, box->axis[1]->z);
  PRINTF(0)("eigenvector: %f, %f, %f\n", box->axis[2]->x, box->axis[2]->y, box->axis[2]->z);
}


void OBBTreeNode::calculateBoxAxis(OBB* box, sVec3D* verticesList, int length)
{

  /* now get the axis length */
  Line                ax[3];                                 //!< the axis
  float*              halfLength = new float[3];             //!< half length of the axis
  float               tmpLength;                             //!< tmp save point for the length
  Plane               p0(*box->axis[0], *box->center);       //!< the axis planes
  Plane               p1(*box->axis[1], *box->center);
  Plane               p2(*box->axis[2], *box->center);
  float               maxLength[3];
  float               minLength[3];


  /* get a bad bounding box */
  halfLength[0] = -1.0f;
  for(int j = 0; j < length; ++j)
    {
      tmpLength = fabs(p1.distancePoint(vertices[j]));
      if( tmpLength > halfLength[0])
        halfLength[0] = tmpLength;
    }

  halfLength[1] = -1.0f;
  for(int j = 0; j < length; ++j)
    {
      tmpLength = fabs(p1.distancePoint(vertices[j]));
      if( tmpLength > halfLength[1])
        halfLength[1] = tmpLength;
    }

  halfLength[2] = -1.0f;
  for(int j = 0; j < length; ++j)
    {
      tmpLength = fabs(p1.distancePoint(vertices[j]));
      if( tmpLength > halfLength[2])
        halfLength[2] = tmpLength;
    }



  /* get the maximal dimensions of the body in all directions */
//   maxLength[0] = 0.0f;
//   minLength[0] = 0.0f;
//   for(int j = 0; j < length; ++j)
//   {
//     tmpLength = p0.distancePoint(vertices[j]);
//     if( tmpLength > maxLength[0])
//       maxLength[0] = tmpLength;
//     else if( tmpLength < minLength[0])
//       minLength[0] = tmpLength;
//   }
//
//   maxLength[1] = 0.0f;
//   minLength[1] = 0.0f;
//   for(int j = 0; j < length; ++j)
//   {
//     tmpLength = p0.distancePoint(vertices[j]);
//     if( tmpLength > maxLength[1])
//       maxLength[1] = tmpLength;
//     else if( tmpLength < minLength[1])
//       minLength[1] = tmpLength;
//   }
//
//   maxLength[2] = 0.0f;
//   minLength[2] = 0.0f;
//   for(int j = 0; j < length; ++j)
//   {
//     tmpLength = p0.distancePoint(vertices[j]);
//     if( tmpLength > maxLength[2])
//       maxLength[2] = tmpLength;
//     else if( tmpLength < minLength[2])
//       minLength[2] = tmpLength;
//   }
//
//
//   /* calculate the real centre of the body by using the axis length */
//   float center[3];
//   float offset[3];
//   for(int i = 0; i < 3; ++i)
//     {
//       center[i] = (maxLength[i] - minLength[i]) / 2.0f; // min length is negative
//       offset[i] = halfLength[i] - center[i];
//       box->center[i] +=  *box->axis[i] * offset[i];            // update the new center vector
//       PRINTF(0)("Center Translation Operation: halfLength old: %f, new: %f\n", halfLength[i], center[i]);
//       halfLength[i] = center[i];
//     }



  box->halfLength = halfLength;
  PRINTF(3)("-- Written Axis to obb\n");
  PRINTF(3)("-- Finished Calculating Attributes\n");



//   PRINTF(3)("\nwe got length: \n");
  for(int i = 0; i < 3; ++i)
  {
    //if( box->halfLength[i] == 0.0)
      PRINTF(0)("length[%i] = %f\n", i, box->halfLength[i]);
  }
}



/**
  \brief this separates an ob-box in the middle
  \param box: the box to separate

  this will separate the box into to smaller boxes. the separation is done along the middle of the longest axis
 */
void OBBTreeNode::forkBox(OBB* box)
{
  /* get the longest axis of the box */
  float               aLength = -1.0f;                     //!< the length of the longest axis
  int                 axisIndex = 0;                       //!< this is the nr of the longest axis

  for(int i = 0; i < 3; ++i)
  {
    if( aLength < box->halfLength[i])
    {
      aLength = box->halfLength[i];
      axisIndex = i;
    }
  }

   PRINTF(0)("longest axis is: nr %i with a half-length of: %f\n", axisIndex, aLength);


  /* get the closest vertex near the center */
  float               dist = 999999.0f;                    //!< the smallest distance to each vertex
  float               tmpDist;                             //!< temporary distance
  int                 vertexIndex;
  Plane               middlePlane(*box->axis[axisIndex], *box->center); //!< the middle plane

  for(int i = 0; i < box->numOfVertices; ++i)
  {
    tmpDist = fabs(middlePlane.distancePoint(box->vertices[i]));
    if( tmpDist < dist)
    {
      dist = tmpDist;
      vertexIndex = i;
    }
  }

//   PRINTF(3)("\nthe clostest vertex is nr: %i, with a dist of: %f\n", vertexIndex ,dist);


  /* now definin the separation plane through this specified nearest point and partition
  the points depending on which side they are located
  */
  tList<sVec3D>      partition1;                           //!< the vertex partition 1
  tList<sVec3D>      partition2;                           //!< the vertex partition 2


  this->separationPlane = new Plane(*box->axis[axisIndex], box->vertices[vertexIndex]);  //!< separation plane
  this->sepPlaneCenter = &box->vertices[vertexIndex];
  this->longestAxisIndex = axisIndex;

  for(int i = 0; i < box->numOfVertices; ++i)
  {
    if( this->separationPlane->distancePoint(box->vertices[i]) > 0.0f)
      partition1.add(&box->vertices[i]);
    else
      partition2.add(&box->vertices[i]);
  }
  partition1.add(&box->vertices[vertexIndex]);

//   PRINTF(3)("\npartition1: got %i vertices/ partition 2: got %i vertices\n", partition1.getSize(), partition2.getSize());


  /* now comes the separation into two different sVec3D arrays */
  tIterator<sVec3D>* iterator;                             //!< the iterator to go through the lists
  sVec3D*            element;                              //!< the elements
  int                index;                                //!< index storage place
  sVec3D*            vertList1;                            //!< the vertex list 1
  sVec3D*            vertList2;                            //!< the vertex list 2

  vertList1 = new sVec3D[partition1.getSize()];
  vertList2 = new sVec3D[partition2.getSize()];

  iterator = partition1.getIterator();
  element = iterator->nextElement();
  index = 0;
  while( element != NULL)
  {
    vertList1[index][0] = element[0][0];
    vertList1[index][1] = element[0][1];
    vertList1[index][2] = element[0][2];
    ++index;
    element = iterator->nextElement();
  }

//   PRINTF(0)("\npartition 1:\n");
//   for(int i = 0; i < partition1.getSize(); ++i)
//   {
//     PRINTF(0)("v[%i][0] = %f,\tv[%i][1] = %f,\tv[%i][1] = %f\n", i, vertList1[i][0], i, vertList1[i][1], i, vertList1[i][2]);
//   }

  iterator = partition2.getIterator();
  element = iterator->nextElement();
  index = 0;
  while( element != NULL)
  {
    vertList2[index][0] = element[0][0];
    vertList2[index][1] = element[0][1];
    vertList2[index][2] = element[0][2];
    ++index;
    element = iterator->nextElement();
  }

  this->tmpVert1 = vertList1;
  this->tmpVert2 = vertList2;
  this->tmpLen1 = partition1.getSize();
  this->tmpLen2 = partition2.getSize();

  delete iterator;

//   PRINTF(0)("\npartition 2:\n");
//   for(int i = 0; i < partition2.getSize(); ++i)
//   {
//     PRINTF(0)("v[%i][0] = %f,\tv[%i][1] = %f,\tv[%i][1] = %f\n", i, vertList2[i][0], i,  vertList2[i][1], i, vertList2[i][2]);
//   }
}




void OBBTreeNode::collideWith(const BVTree &tree)
{}




void OBBTreeNode::drawBV(int depth, int drawMode) const
{
  this->obbTree->getMaterial(treeIndex)->select();

  /* draw the model itself, there is some problem concerning this: the vertices are drawn multiple times */
  if( drawMode & DRAW_MODEL || drawMode & DRAW_ALL)
  {
    if( !(drawMode & DRAW_SINGLE && depth != 0))
    {
      //glBegin(GL_LINE_STRIP);
      for(int i = 0; i < this->bvElement->numOfVertices; ++i)
      {
        glPushMatrix();
        //glMatrixMode(GL_MODELVIEW);
        //glVertex3f(this->bvElement->vertices[i][0], this->bvElement->vertices[i][1], this->bvElement->vertices[i][2]);
        glTranslatef(this->bvElement->vertices[i][0], this->bvElement->vertices[i][1], this->bvElement->vertices[i][2]);
        gluSphere(this->sphereObj, 1, 10, 10);
        //PRINTF(0)("v(%f, %f, %f)\n", this->bvElement->vertices[i][0], this->bvElement->vertices[i][1], this->bvElement->vertices[i][2]);
        glPopMatrix();
      }
      //glEnd();
    }
  }


  /* draw world axes */
//   glBegin(GL_LINES);
//   glColor3f(0.0, 0.4, 0.3);
//   glVertex3f(0.0, 0.0, 0.0);
//   glVertex3f(3.0, 0.0, 0.0);
//
//   glVertex3f(0.0, 0.0, 0.0);
//   glVertex3f(0.0, 3.0, 0.0);
//
//   glVertex3f(0.0, 0.0, 0.0);
//   glVertex3f(0.0, 0.0, 3.0);
//   glEnd();


  if( drawMode & DRAW_BV_AXIS || drawMode & DRAW_ALL)
  {
    if( !(drawMode & DRAW_SINGLE && depth != 0))
    {
      /* draw the obb axes */
      glBegin(GL_LINES);
      glColor3f(0.0, 0.4, 0.3);
      glVertex3f(this->bvElement->center->x, this->bvElement->center->y, this->bvElement->center->z);
      glVertex3f(this->bvElement->center->x + this->bvElement->axis[0]->x * this->bvElement->halfLength[0],
                 this->bvElement->center->y + this->bvElement->axis[0]->y * this->bvElement->halfLength[0],
                 this->bvElement->center->z + this->bvElement->axis[0]->z * this->bvElement->halfLength[0]);

      glVertex3f(this->bvElement->center->x, this->bvElement->center->y, this->bvElement->center->z);
      glVertex3f(this->bvElement->center->x + this->bvElement->axis[1]->x * this->bvElement->halfLength[1],
                 this->bvElement->center->y + this->bvElement->axis[1]->y * this->bvElement->halfLength[1],
                 this->bvElement->center->z + this->bvElement->axis[1]->z * this->bvElement->halfLength[1]);

      glVertex3f(this->bvElement->center->x, this->bvElement->center->y, this->bvElement->center->z);
      glVertex3f(this->bvElement->center->x + this->bvElement->axis[2]->x * this->bvElement->halfLength[2],
                 this->bvElement->center->y + this->bvElement->axis[2]->y * this->bvElement->halfLength[2],
                 this->bvElement->center->z + this->bvElement->axis[2]->z * this->bvElement->halfLength[2]);
      glEnd();
    }
  }


  if( drawMode & DRAW_BV_POLYGON || drawMode & DRAW_ALL)
  {
    if( !(drawMode & DRAW_SINGLE && depth != 0))
    {
    Vector cen = *this->bvElement->center;
    Vector** axis = this->bvElement->axis;
    float* len = this->bvElement->halfLength;

    /* draw bounding box */
    glBegin(GL_LINE_LOOP);
    glVertex3f(cen.x + axis[0]->x * len[0] + axis[1]->x * len[1] + axis[2]->x * len[2],
               cen.y + axis[0]->y * len[0] + axis[1]->y * len[1] + axis[2]->y * len[2],
               cen.z + axis[0]->z * len[0] + axis[1]->z * len[1] + axis[2]->z * len[2]);
    glVertex3f(cen.x + axis[0]->x * len[0] + axis[1]->x * len[1] - axis[2]->x * len[2],
               cen.y + axis[0]->y * len[0] + axis[1]->y * len[1] - axis[2]->y * len[2],
               cen.z + axis[0]->z * len[0] + axis[1]->z * len[1] - axis[2]->z * len[2]);
    glVertex3f(cen.x + axis[0]->x * len[0] - axis[1]->x * len[1] - axis[2]->x * len[2],
               cen.y + axis[0]->y * len[0] - axis[1]->y * len[1] - axis[2]->y * len[2],
               cen.z + axis[0]->z * len[0] - axis[1]->z * len[1] - axis[2]->z * len[2]);
    glVertex3f(cen.x + axis[0]->x * len[0] - axis[1]->x * len[1] + axis[2]->x * len[2],
               cen.y + axis[0]->y * len[0] - axis[1]->y * len[1] + axis[2]->y * len[2],
               cen.z + axis[0]->z * len[0] - axis[1]->z * len[1] + axis[2]->z * len[2]);
    glEnd();

    glBegin(GL_LINE_LOOP);
    glVertex3f(cen.x + axis[0]->x * len[0] - axis[1]->x * len[1] + axis[2]->x * len[2],
               cen.y + axis[0]->y * len[0] - axis[1]->y * len[1] + axis[2]->y * len[2],
               cen.z + axis[0]->z * len[0] - axis[1]->z * len[1] + axis[2]->z * len[2]);
    glVertex3f(cen.x + axis[0]->x * len[0] - axis[1]->x * len[1] - axis[2]->x * len[2],
               cen.y + axis[0]->y * len[0] - axis[1]->y * len[1] - axis[2]->y * len[2],
               cen.z + axis[0]->z * len[0] - axis[1]->z * len[1] - axis[2]->z * len[2]);
    glVertex3f(cen.x - axis[0]->x * len[0] - axis[1]->x * len[1] - axis[2]->x * len[2],
               cen.y - axis[0]->y * len[0] - axis[1]->y * len[1] - axis[2]->y * len[2],
               cen.z - axis[0]->z * len[0] - axis[1]->z * len[1] - axis[2]->z * len[2]);
    glVertex3f(cen.x - axis[0]->x * len[0] - axis[1]->x * len[1] + axis[2]->x * len[2],
               cen.y - axis[0]->y * len[0] - axis[1]->y * len[1] + axis[2]->y * len[2],
               cen.z - axis[0]->z * len[0] - axis[1]->z * len[1] + axis[2]->z * len[2]);
    glEnd();

    glBegin(GL_LINE_LOOP);
    glVertex3f(cen.x - axis[0]->x * len[0] - axis[1]->x * len[1] + axis[2]->x * len[2],
               cen.y - axis[0]->y * len[0] - axis[1]->y * len[1] + axis[2]->y * len[2],
               cen.z - axis[0]->z * len[0] - axis[1]->z * len[1] + axis[2]->z * len[2]);
    glVertex3f(cen.x - axis[0]->x * len[0] - axis[1]->x * len[1] - axis[2]->x * len[2],
               cen.y - axis[0]->y * len[0] - axis[1]->y * len[1] - axis[2]->y * len[2],
               cen.z - axis[0]->z * len[0] - axis[1]->z * len[1] - axis[2]->z * len[2]);
    glVertex3f(cen.x - axis[0]->x * len[0] + axis[1]->x * len[1] - axis[2]->x * len[2],
               cen.y - axis[0]->y * len[0] + axis[1]->y * len[1] - axis[2]->y * len[2],
               cen.z - axis[0]->z * len[0] + axis[1]->z * len[1] - axis[2]->z * len[2]);
    glVertex3f(cen.x - axis[0]->x * len[0] + axis[1]->x * len[1] + axis[2]->x * len[2],
               cen.y - axis[0]->y * len[0] + axis[1]->y * len[1] + axis[2]->y * len[2],
               cen.z - axis[0]->z * len[0] + axis[1]->z * len[1] + axis[2]->z * len[2]);
    glEnd();

    glBegin(GL_LINE_LOOP);
    glVertex3f(cen.x - axis[0]->x * len[0] + axis[1]->x * len[1] - axis[2]->x * len[2],
               cen.y - axis[0]->y * len[0] + axis[1]->y * len[1] - axis[2]->y * len[2],
               cen.z - axis[0]->z * len[0] + axis[1]->z * len[1] - axis[2]->z * len[2]);
    glVertex3f(cen.x - axis[0]->x * len[0] + axis[1]->x * len[1] + axis[2]->x * len[2],
               cen.y - axis[0]->y * len[0] + axis[1]->y * len[1] + axis[2]->y * len[2],
               cen.z - axis[0]->z * len[0] + axis[1]->z * len[1] + axis[2]->z * len[2]);
    glVertex3f(cen.x + axis[0]->x * len[0] + axis[1]->x * len[1] + axis[2]->x * len[2],
               cen.y + axis[0]->y * len[0] + axis[1]->y * len[1] + axis[2]->y * len[2],
               cen.z + axis[0]->z * len[0] + axis[1]->z * len[1] + axis[2]->z * len[2]);
    glVertex3f(cen.x + axis[0]->x * len[0] + axis[1]->x * len[1] - axis[2]->x * len[2],
               cen.y + axis[0]->y * len[0] + axis[1]->y * len[1] - axis[2]->y * len[2],
               cen.z + axis[0]->z * len[0] + axis[1]->z * len[1] - axis[2]->z * len[2]);
    glEnd();
    }

  }

  if( drawMode & DRAW_SEPARATING_PLANE || drawMode & DRAW_ALL)
  {
    if( !(drawMode & DRAW_SINGLE && depth != 0))
    {
    /* now draw the separation plane */
    Vector a1 = *this->bvElement->axis[(this->longestAxisIndex + 1)%3];
    Vector a2 = *this->bvElement->axis[(this->longestAxisIndex + 2)%3];
    Vector c = *this->bvElement->center;
    float l1 = this->bvElement->halfLength[(this->longestAxisIndex + 1)%3];
    float l2 = this->bvElement->halfLength[(this->longestAxisIndex + 2)%3];
    glBegin(GL_QUADS);
    glVertex3f(c.x + a1.x * l1 + a2.x * l2, c.y + a1.y * l1+ a2.y * l2, c.z + a1.z * l1 + a2.z * l2);
    glVertex3f(c.x - a1.x * l1 + a2.x * l2, c.y - a1.y * l1+ a2.y * l2, c.z - a1.z * l1 + a2.z * l2);
    glVertex3f(c.x - a1.x * l1 - a2.x * l2, c.y - a1.y * l1- a2.y * l2, c.z - a1.z * l1 - a2.z * l2);
    glVertex3f(c.x + a1.x * l1 - a2.x * l2, c.y + a1.y * l1- a2.y * l2, c.z + a1.z * l1 - a2.z * l2);
    glEnd();
    }
  }

  if( this->nodeLeft != NULL && depth != 0 )
    this->nodeLeft->drawBV(depth - 1, drawMode);
  if( this->nodeRight != NULL && depth != 0)
    this->nodeRight->drawBV(depth - 1, drawMode);

}



void OBBTreeNode::debug()
{

  /*
  for(int i = 0; i < length; i++)
  {
  PRINTF(3)("vertex %i: %f, %f, %f\n", i, verticesList[i][0], verticesList[i][1], verticesList[i][2]);
}
  */
}