/* 
   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 <math.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)
{
  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    centre;                                  //!< the centre of the entire hull
  OBB*      obb = new OBB();                         //!< the new obb to add
  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
    
  /* 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 */
      centre += centroid[i] * facelet[i];
    }
  /* take the average of the centroid sum */
  centre /= 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]) - centre[j] * centre[k];
	    }
	}
    }
}


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


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


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


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