source: orxonox.OLD/orxonox/trunk/src/lib/collision_detection/obb_tree_node.cc @ 4581

/* 
   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 "vector.h"
#include "abstract_model.h"

#include <math.h>


#define WANT_STREAM
#define WANT_MATH
#define WANT_FSTREAM


#include "include.h"
#include "newmat.h"
#include "newmatap.h"
#include "newmatio.h"




using namespace std;


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

}


/**
   \brief standard deconstructor

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



/**
   \brief creates a new BVTree or BVTree partition
   \param depth: the depth of the tree
   \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)
{
  this->bvElement = this->createBox();
  this->calculateBoxAttributes(this->bvElement, verticesList, length);
  this->forkBox(this->bvElement);
}


OBB* OBBTreeNode::createBox()
{
  return new OBB();
}


void OBBTreeNode::calculateBoxAttributes(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
    
  this->numOfVertices = length;
  this->vertices = verticesList;


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


  
  /* 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("\nVertex Data:\n");
    for(int i = 0; i < length; i++)
    {
      printf("vertex %i: %f, %f, %f\n", i, verticesList[i][0], verticesList[i][1], verticesList[i][2]);
    }
  
  printf("\nCovariance Matrix:\n");
  for(int j = 0; j < 3; ++j)
    {
      printf(" |");
      for(int k = 0; k < 3; ++k)
        {
          printf(" \b%f ", covariance[j][k]);
        }
      printf(" |\n");
    }
  printf("center: %f, %f, %f\n\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;


  /* 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
  */
  Matrix                V(3,3);                               //!< for eigenvectors
  DiagonalMatrix        D(3);                                 //!< for eigenvalues    
  SymmetricMatrix       C(3);                                 //!< for the covariance symmetrical matrix
  Vector**              axis = new Vector*[3];                //!< the references to the obb axis
  
  C(1,1) = covariance[0][0];
  C(1,2) = covariance[0][1];
  C(1,3) = covariance[0][2];
  C(2,1) = covariance[1][0];    
  C(2,2) = covariance[1][1];
  C(2,3) = covariance[1][2]; 
  C(3,1) = covariance[2][0];
  C(3,2) = covariance[2][1];
  C(3,3) = covariance[2][2];

  Jacobi(C, D, V);                                            /* do the jacobi decomposition */

  printf("we got a result! YES: \n");

  for(int j = 1; j < 4; ++j)
  {
    printf(" |");
    for(int k = 1; k < 4; ++k)
    {
      printf(" \b%f ", V(j, k));
    }
    printf(" |\n");
  }

  axis[0] = new Vector(V(1, 1), V(2, 1), V(3, 1));
  axis[1] = new Vector(V(1, 2), V(2, 2), V(3, 2));
  axis[2] = new Vector(V(1, 3), V(2, 3), V(3, 3));
  box->axis = axis;
  
  printf("eigenvector: %f, %f, %f\n", V(1, 1), V(2, 1), V(3, 1));
  printf("eigenvector: %f, %f, %f\n", V(1, 2), V(2, 2), V(3, 2));
  printf("eigenvector: %f, %f, %f\n", V(1, 3), V(2, 3), V(3, 3));

  box->axis = axis;

  /* 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
  
  ax[0].r = *box->center; ax[0].a = *box->axis[0];
  ax[1].r = *box->center; ax[1].a = *box->axis[1];
  ax[2].r = *box->center; ax[2].a = *box->axis[2];

  for(int i = 0; i < 3; ++i)
  {
    for(int j = 0; j < length; ++j)
    {
      tmpLength = ax[i].distancePoint(vertices[j]);
      if( tmpLength > halfLength[i])
        halfLength[i] = tmpLength; 
    }
  }

  box->halfLength = halfLength;
  
  printf("we got length: \n");
  for(int i = 0; i < 3; ++i)
    printf("length[%i] = %f\n", i, halfLength[i]);
}


void OBBTreeNode::forkBox(OBB* box)
{
  /* get the longest axis of the box */
  float aLength = -1.0f;
  int axisNr = 0;
  for(int i = 0; i < 3; ++i)
    {
      if( aLength < box->axis[i]->len())
        {
          aLength = box->axis[i]->len();
          axisNr = i;
        }
    }
  
  /* get the closest vertex near the center */
  
}


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


void OBBTreeNode::drawBV(int currentDepth, const int depth) const
{
  glBegin(GL_LINE_LOOP);
  glColor3f(1.0, 1.0, 1.0);
  for(int i = 0; i < this->numOfVertices; ++i)
    {
      glVertex3f(this->vertices[i][0], this->vertices[i][1], this->vertices[i][2]);
      //printf("v(%f, %f, %f)\n", this->vertices[i][0], this->vertices[i][1], this->vertices[i][2]);
    }
  glEnd();
}


void OBBTreeNode::drawBVPolygon(int currentDepth, const int depth) const
{
  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->axis[0]->x * this->bvElement->halfLength[0], this->bvElement->axis[0]->y * this->bvElement->halfLength[0], 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->axis[1]->x * this->bvElement->halfLength[1], this->bvElement->axis[1]->y * this->bvElement->halfLength[1], 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->axis[2]->x * this->bvElement->halfLength[2], this->bvElement->axis[2]->y * this->bvElement->halfLength[2], this->bvElement->axis[2]->z * this->bvElement->halfLength[2]);
  
  glEnd();
}


void OBBTreeNode::drawBVBlended(int currentDepth, const int depth) const
{}


void OBBTreeNode::debug()
{

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