Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

Ignore:
Timestamp:
May 18, 2006, 11:44:21 PM (18 years ago)
Author:
patrick
Message:

trunk: merged the cd branche back to trunk

File:
1 edited

Legend:

Unmodified
Added
Removed

  • trunk/src/lib/collision_detection/obb_tree_node.cc

    r6617 r7711  
    1111### File Specific:
    1212   main-programmer: Patrick Boenzli
    13    co-programmer: ...
    1413*/
    1514
    16 #define DEBUG_SPECIAL_MODULE DEBUG_MODULE_COLLISION
     15#define DEBUG_SPECIAL_MODULE 3/* DEBUG_MODULE_COLLISION_DETECTION*/
    1716
    1817#include "obb_tree_node.h"
    19 #include "list.h"
     18#include "obb_tree.h"
    2019#include "obb.h"
    21 #include "obb_tree.h"
     20
    2221#include "matrix.h"
    2322#include "model.h"
    … …  
    2625
    2726#include "color.h"
    28 
     27#include "glincl.h"
     28
     29#include <list>
     30#include <vector>
    2931#include "debug.h"
    30 #include "glincl.h"
    3132
    3233
    … …  
    3435using namespace std;
    3536
    36 OBBTree*  OBBTreeNode::obbTree = NULL;
    37 
    38 float**  OBBTreeNode::coMat = NULL;
    39 float**  OBBTreeNode::eigvMat = NULL;
    40 float*   OBBTreeNode::eigvlMat = NULL;
    41 int*     OBBTreeNode::rotCount = NULL;
     37
    4238GLUquadricObj* OBBTreeNode_sphereObj = NULL;
     39
    4340
    4441/**
    4542 *  standard constructor
     43 * @param tree: reference to the obb tree
     44 * @param depth: the depth of the obb tree to generate
    4645 */
    47 OBBTreeNode::OBBTreeNode ()
     46OBBTreeNode::OBBTreeNode (const OBBTree& tree, OBBTreeNode* prev, int depth)
     47    : BVTreeNode()
    4848{
    4949  this->setClassID(CL_OBB_TREE_NODE, "OBBTreeNode");
     50
     51  this->obbTree = &tree;
     52  this->nodePrev = prev;
     53  this->depth = depth;
     54  this->nextID = 0;
     55
    5056  this->nodeLeft = NULL;
    5157  this->nodeRight = NULL;
    5258  this->bvElement = NULL;
    5359
    54   if( OBBTreeNode::coMat == NULL)
    55   {
    56     OBBTreeNode::coMat = new float*[4];
    57     for(int i = 0; i < 4; i++)
    58       OBBTreeNode::coMat[i] = new float[4];
    59   }
    60   if( OBBTreeNode::eigvMat == NULL)
    61   {
    62     OBBTreeNode::eigvMat = new float*[4];
    63     for( int i = 0; i < 4; i++)
    64       OBBTreeNode::eigvMat[i] = new float[4];
    65   }
    66   if( OBBTreeNode::eigvlMat == NULL)
    67   {
    68     OBBTreeNode::eigvlMat = new float[4];
    69   }
    70   if( OBBTreeNode::rotCount == NULL)
    71     OBBTreeNode::rotCount = new int;
     60  this->triangleIndexList1 = NULL;
     61  this->triangleIndexList2 = NULL;
     62
     63  this->modelInf = NULL;
     64  this->triangleIndexes = NULL;
    7265
    7366  if( OBBTreeNode_sphereObj == NULL)
    7467    OBBTreeNode_sphereObj = gluNewQuadric();
     68
     69  this->owner = NULL;
     70
     71  /* debug ids */
     72  if( this->nodePrev)
     73    this->treeIndex = 100 * this->depth + this->nodePrev->getID();
     74  else
     75    this->treeIndex = 0;
    7576}
    7677
    … …  
    8283{
    8384  if( this->nodeLeft)
    84   {
    8585    delete this->nodeLeft;
    86     this->nodeLeft = NULL;
    87   }
    8886  if( this->nodeRight)
    89   {
    9087    delete this->nodeRight;
    91     this->nodeRight = NULL;
    92   }
     88
    9389  if( this->bvElement)
    9490    delete this->bvElement;
    95   this->bvElement = NULL;
     91
     92//   if( this->triangleIndexList1 != NULL)
     93//     delete [] this->triangleIndexList1;
     94//   if( this->triangleIndexList2 != NULL)
     95//     delete [] this->triangleIndexList2;
    9696}
    9797
    … …  
    105105 * on the triangle informations (triangle soup not polygon soup)
    106106 */
    107 void OBBTreeNode::spawnBVTree(const int depth, const modelInfo& modInfo)
    108 {
    109   int length = 0;
    110   sVec3D* verticesList;
    111 
    112   PRINT(3)("\n");
    113   this->treeIndex = this->obbTree->getID();
    114   PRINTF(3)("OBB Depth: %i, tree index: %i, numVertices: %i\n", depth, treeIndex, length);
     107void OBBTreeNode::spawnBVTree(const modelInfo& modelInf, const int* triangleIndexes, int length)
     108{
     109  PRINTF(3)("\n==============================Creating OBB Tree Node==================\n");
     110  PRINT(3)(" OBB Tree Infos: \n");
     111  PRINT(3)("\tDepth: %i \n\tTree Index: %i \n\tNumber of Triangles: %i\n", depth, this->treeIndex, length);
    115112  this->depth = depth;
    116113
    117 
    118114  this->bvElement = new OBB();
    119   this->bvElement->vertices = verticesList;
    120   this->bvElement->numOfVertices = length;
    121   PRINTF(3)("Created OBBox\n");
    122   this->calculateBoxCovariance(this->bvElement, modInfo);
    123   PRINTF(3)("Calculated attributes1\n");
    124   this->calculateBoxEigenvectors(this->bvElement, modInfo);
    125   PRINTF(3)("Calculated attributes2\n");
    126   this->calculateBoxAxis(this->bvElement,modInfo);
    127   PRINTF(3)("Calculated attributes3\n");
    128 
    129   /* if this is the first node, the vertices data are the original ones of the model itself, so dont delete them in cleanup */
    130   if( this->treeIndex == 1)
    131     this->bvElement->bOrigVertices = true;
    132 
     115  this->bvElement->modelInf = &modelInf;
     116  this->bvElement->triangleIndexes = triangleIndexes;
     117  this->bvElement->triangleIndexesLength = length;
     118
     119  // full debug output
     120
     121  //indexes debug
     122//   for( int i = 0; i < length; ++i)
     123//   {
     124//     PRINTF(2)("triangle[%i], index: %i\n", i, triangleIndexes[i]);
     125//   }
     126//   for( int i = 0; i < length; ++i)
     127//   {
     128//     PRINTF(2)("triangle[%i]\n", i);
     129//     for(int j = 0; j < 3; ++j)
     130//     {
     131//       PRINTF(2)("  vertex[%i]: %f, %f, %f\n", j,
     132//       (&modelInf.pVertices[modelInf.pTriangles[triangleIndexes[i]].indexToVertices[j]])[0],
     133//       (&modelInf.pVertices[modelInf.pTriangles[triangleIndexes[i]].indexToVertices[j]])[1],
     134//       (&modelInf.pVertices[modelInf.pTriangles[triangleIndexes[i]].indexToVertices[j]])[2]);
     135//     }
     136//   }
     137//   PRINT(0)("\n\n\n");
     138//   for( int i = 0; i < this->bvElement->modelInf->numVertices*3; i+=3)
     139//     PRINTF(0)(  "vertex[%i]: %f, %f, %f\n", i, this->bvElement->modelInf->pVertices[i],
     140//                this->bvElement->modelInf->pVertices[i+1],
     141//                this->bvElement->modelInf->pVertices[i+2]);
     142
     143
     144
     145  /* create the bounding boxes in three steps */
     146  this->calculateBoxCovariance(*this->bvElement, modelInf, triangleIndexes, length);
     147  this->calculateBoxEigenvectors(*this->bvElement, modelInf, triangleIndexes, length);
     148//   this->calculateBoxAxis(*this->bvElement, modelInf, triangleIndexes, length);
     149//   this->calculateBoxAxis(*this->bvElement, modelInf, triangleIndexes, length);
     150  this->calculateBoxAxis(*this->bvElement, modelInf, triangleIndexes, length);
     151
     152  /* do we need to descent further in the obb tree?*/
    133153  if( likely( this->depth > 0))
    134154  {
    135     this->forkBox(this->bvElement);
    136 
    137 
    138 //     if(this->tmpLen1 > 2)
    139 //     {
    140 //       OBBTreeNode* node1 = new OBBTreeNode();
    141 //       this->nodeLeft = node1;
    142 //       this->nodeLeft->spawnBVTree(depth - 1, this->tmpVert1, this->tmpLen1);
    143 //     }
    144 //     else
    145 //     {
    146 //       PRINTF(3)("Aboarding tree walk: less than 3 vertices left\n");
    147 //     }
    148 //
    149 //     if( this->tmpLen2 > 2)
    150 //     {
    151 //       OBBTreeNode* node2 = new OBBTreeNode();
    152 //       this->nodeRight = node2;
    153 //       this->nodeRight->spawnBVTree(depth - 1, this->tmpVert2, this->tmpLen2);
    154 //     }
    155 //     else
    156 //     {
    157 //       PRINTF(3)("Abording tree walk: less than 3 vertices left\n");
    158 //     }
    159 
    160   }
    161 }
     155    this->forkBox(*this->bvElement);
     156
     157    if( this->triangleIndexLength1 >= 3)
     158    {
     159      this->nodeLeft = new OBBTreeNode(*this->obbTree, this, depth - 1);
     160      this->nodeLeft->spawnBVTree(modelInf, this->triangleIndexList1, this->triangleIndexLength1);
     161    }
     162    if( this->triangleIndexLength2 >= 3)
     163    {
     164      this->nodeRight = new OBBTreeNode(*this->obbTree, this, depth - 1);
     165      this->nodeRight->spawnBVTree(modelInf, this->triangleIndexList2, this->triangleIndexLength2);
     166    }
     167  }
     168}
     169
    162170
    163171
    164172/**
    165  *  creates a new BVTree or BVTree partition
    166  * @param depth: how much more depth-steps to go: if == 1 don't go any deeper!
    167  * @param verticesList: the list of vertices of the object - each vertices triple is interpreted as a triangle
    168  *
    169  * this function creates an Bounding Volume tree from a vertices soup (no triangle data)
     173 *  calculate the box covariance matrix
     174 * @param box: reference to the box
     175 * @param modelInf: the model info structure of the model
     176 * @param tirangleIndexes: an array with the indexes of the triangles inside this
     177 * @param length: the length of the indexes array
    170178 */
    171 void OBBTreeNode::spawnBVTree(const int depth, const sVec3D *verticesList, unsigned int length)
    172 {
    173   PRINT(3)("\n");
    174   this->treeIndex = this->obbTree->getID();
    175   PRINTF(3)("OBB Depth: %i, tree index: %i, numVertices: %i\n", depth, treeIndex, length);
    176   this->depth = depth;
    177 
    178 
    179   this->bvElement = new OBB();
    180   this->bvElement->vertices = verticesList;
    181   this->bvElement->numOfVertices = length;
    182   PRINTF(3)("Created OBBox\n");
    183   this->calculateBoxCovariance(this->bvElement, verticesList, length);
    184   PRINTF(3)("Calculated attributes1\n");
    185   this->calculateBoxEigenvectors(this->bvElement, verticesList, length);
    186   PRINTF(3)("Calculated attributes2\n");
    187   this->calculateBoxAxis(this->bvElement, verticesList, length);
    188   PRINTF(3)("Calculated attributes3\n");
    189 
    190   /* if this is the first node, the vertices data are the original ones of the model itself, so dont delete them in cleanup */
    191   if( this->treeIndex == 1)
    192     this->bvElement->bOrigVertices = true;
    193 
    194   if( likely( this->depth > 0))
    195   {
    196     this->forkBox(this->bvElement);
    197 
    198 
    199     if(this->tmpLen1 > 2)
    200     {
    201       OBBTreeNode* node1 = new OBBTreeNode();
    202       this->nodeLeft = node1;
    203       this->nodeLeft->spawnBVTree(depth - 1, this->tmpVert1, this->tmpLen1);
    204     }
    205     else
    206     {
    207       PRINTF(3)("Aboarding tree walk: less than 3 vertices left\n");
    208     }
    209 
    210     if( this->tmpLen2 > 2)
    211     {
    212       OBBTreeNode* node2 = new OBBTreeNode();
    213       this->nodeRight = node2;
    214       this->nodeRight->spawnBVTree(depth - 1, this->tmpVert2, this->tmpLen2);
    215     }
    216     else
    217     {
    218       PRINTF(3)("Abording tree walk: less than 3 vertices left\n");
    219     }
    220   }
    221 }
    222 
    223 
    224 void OBBTreeNode::calculateBoxCovariance(OBB* box, const modelInfo& modInfo)
    225 {}
    226 
    227 
    228 void OBBTreeNode::calculateBoxCovariance(OBB* box, const sVec3D* verticesList, unsigned int length)
     179void OBBTreeNode::calculateBoxCovariance(OBB& box, const modelInfo& modelInf, const int* triangleIndexes, int length)
    229180{
    230181  float     facelet[length];                         //!< surface area of the i'th triangle of the convex hull
    … …  
    235186  Vector    t1, t2;                                  //!< temporary values
    236187  float     covariance[3][3] = {0,0,0, 0,0,0, 0,0,0};//!< the covariance matrix
    237   int       mode = 0;                                //!< mode = 0: vertex soup, no connections, mode = 1: 3 following verteces build a triangle
    238 
    239   this->numOfVertices = length;
    240   this->vertices = verticesList;
    241 
    242 
    243   if( likely(mode == 0))
    244   {
    245     /* fist compute all the convex hull face/facelets and centroids */
    246     for( int i = 0; i+3 < length ; i+=3)          /* FIX-ME-QUICK: hops of 3, array indiscontinuity*/
    247     {
    248       p = verticesList[i];
    249       q = verticesList[i + 1];
    250       r = verticesList[i + 2];
    251 
    252       t1 = p - q; t2 = p - r;
    253 
    254       /* finding the facelet surface via cross-product */
    255       facelet[i] = 0.5f * fabs( t1.cross(t2).len() );
    256       /* update the entire convex hull surface */
    257       face += facelet[i];
    258 
    259       /* calculate the cetroid of the hull triangles */
    260       centroid[i] = (p + q + r) * 1/3;
    261       /* now calculate the centroid of the entire convex hull, weighted average of triangle centroids */
    262       center += centroid[i] * facelet[i];
    263     }
    264     /* take the average of the centroid sum */
    265     center /= face;
    266     PRINTF(3)("-- Calculated Center\n");
    267 
    268 
    269     /* now calculate the covariance matrix - if not written in three for-loops, it would compute faster: minor */
    270     for( int j = 0; j < 3; ++j)
    271     {
    272       for( int k = 0; k < 3; ++k)
     188  const float*   tmpVec = NULL;                           //!< a temp saving place for sVec3Ds
     189
     190  /* fist compute all the convex hull face/facelets and centroids */
     191  for( int i = 0; i < length ; ++i)
     192  {
     193    p = &modelInf.pVertices[modelInf.pTriangles[triangleIndexes[i]].indexToVertices[0]];
     194    q = &modelInf.pVertices[modelInf.pTriangles[triangleIndexes[i]].indexToVertices[1]];
     195    r = &modelInf.pVertices[modelInf.pTriangles[triangleIndexes[i]].indexToVertices[2]];
     196
     197    /* finding the facelet surface via cross-product */
     198    t1 = p - q;
     199    t2 = p - r;
     200    facelet[i] = 0.5f * /*fabs*/( t1.cross(t2).len() );
     201    /* update the entire convex hull surface */
     202    face += facelet[i];
     203
     204    /* calculate the cetroid of the hull triangles */
     205    centroid[i] = (p + q + r) / 3.0f;
     206    /* now calculate the centroid of the entire convex hull, weighted average of triangle centroids */
     207    center += centroid[i] * facelet[i];
     208    /* the arithmetical center */
     209  }
     210  /* take the average of the centroid sum */
     211  center /= face;
     212
     213
     214  /* now calculate the covariance matrix - if not written in three for-loops,
     215     it would compute faster: minor */
     216  for( int j = 0; j < 3; ++j)
     217  {
     218    for( int k = 0; k < 3; ++k)
     219    {
     220      for( int i = 0; i < length; ++i)
    273221      {
    274         for( int i = 0; i + 3 < length; i+=3)
    275         {
    276           p = verticesList[i];
    277           q = verticesList[i + 1];
    278           r = verticesList[i + 2];
    279 
    280           covariance[j][k] = facelet[i] / (12.0f * face) * (9.0f * centroid[i][j] * centroid[i][k] + p[j] * p[k] +
    281               q[j] * q[k] + r[j] * r[k]) - center[j] * center[k];
    282         }
     222        p = (&modelInf.pVertices[modelInf.pTriangles[triangleIndexes[i]].indexToVertices[0]]);
     223        q = (&modelInf.pVertices[modelInf.pTriangles[triangleIndexes[i]].indexToVertices[1]]);
     224        r = (&modelInf.pVertices[modelInf.pTriangles[triangleIndexes[i]].indexToVertices[2]]);
     225
     226        covariance[j][k] = facelet[i] * (9.0f * centroid[i][j] * centroid[i][k] + p[j] * p[k] +
     227                           q[j] * q[k] + r[j] * r[k]);
    283228      }
    284     }
    285     PRINTF(3)("-- Calculated Covariance\n");
    286   }
    287   else if( mode == 1)
    288   {
    289     for( int i = 0; i + 3 < length; i+=3)          /* FIX-ME-QUICK: hops of 3, array indiscontinuity*/
    290     {
    291       p = verticesList[i];
    292       q = verticesList[i + 1];
    293       r = verticesList[i + 2];
    294 
    295       centroid[i] = (p + q + r) / 3.0f;
    296       center += centroid[i];
    297     }
    298     center /= length;
    299 
    300     for( int j = 0; j < 3; ++j)
    301     {
    302       for( int k = 0; k < 3; ++k)
    303       {
    304         for( int i = 0; i + 3 < length; i+=3)
    305         {
    306           p = verticesList[i];
    307           q = verticesList[i +1];
    308           r = verticesList[i + 2];
    309 
    310           covariance[j][k] = p[j] * p[k] + q[j] * q[k] + r[j] + r[k];
    311         }
    312         covariance[j][k] /= (3.0f * length);
    313       }
    314     }
    315     PRINTF(3)("-- Calculated Covariance\n");
    316   }
    317   else if( mode == 2)
    318   {
    319     /* fist compute all the convex hull face/facelets and centroids */
    320     for(int i = 0; i + 3 < length; i+=3)          /* FIX-ME-QUICK: hops of 3, array indiscontinuity*/
    321     {
    322       p = verticesList[i];
    323       q = verticesList[i + 1];
    324       r = verticesList[i + 2];
    325 
    326       t1 = p - q; t2 = p - r;
    327 
    328       /* finding the facelet surface via cross-product */
    329       facelet[i] = 0.5f * fabs( t1.cross(t2).len() );
    330       /* update the entire convex hull surface */
    331       face += facelet[i];
    332 
    333       /* calculate the cetroid of the hull triangles */
    334       centroid[i] = (p + q + r) * 1/3;
    335       /* now calculate the centroid of the entire convex hull, weighted average of triangle centroids */
    336       center += centroid[i] * facelet[i];
    337     }
    338     /* take the average of the centroid sum */
    339     center /= face;
    340     PRINTF(3)("-- Calculated Center\n");
    341 
    342     for( int j = 0; j < 3; ++j)
    343     {
    344       for( int k = 0; k < 3; ++k)
    345       {
    346         for( int i = 0; i + 3 < length; i+=3)
    347         {
    348           p = verticesList[i];
    349           q = verticesList[i +1];
    350           r = verticesList[i + 2];
    351 
    352           covariance[j][k] = p[j] * p[k] + q[j] * q[k] + r[j] + r[k];
    353         }
    354         covariance[j][k] /= (3.0f * length);
    355       }
    356     }
    357     PRINTF(3)("-- Calculated Covariance\n");
    358   }
    359   else
    360   {
    361     for( int i = 0; i < length; ++i)          /* FIX-ME-QUICK: hops of 3, array indiscontinuity*/
    362     {
    363       center += verticesList[i];
    364     }
    365     center /= length;
    366 
    367     for( int j = 0; j < 3; ++j)
    368     {
    369       for( int k = 0; k < 3; ++k)
    370       {
    371         for( int i = 0; i + 3 < length; i+=3)
    372         {
    373           p = verticesList[i];
    374           q = verticesList[i +1];
    375           r = verticesList[i + 2];
    376 
    377           covariance[j][k] = p[j] * p[k] + q[j] * q[k] + r[j] + r[k];
    378         }
    379         covariance[j][k] /= (3.0f * length);
    380       }
    381     }
    382     PRINTF(3)("-- Calculated Covariance\n");
    383   }
    384 
    385   PRINTF(3)("\nVertex Data:\n");
    386   for(int i = 0; i < length; i++)
    387   {
    388     PRINTF(3)("vertex %i: %f, %f, %f\n", i, box->vertices[i][0], box->vertices[i][1], box->vertices[i][2]);
    389   }
    390 
    391 
    392   PRINTF(3)("\nCovariance Matrix:\n");
     229      covariance[j][k] = covariance[j][k] / (12.0f * face) - center[j] * center[k];
     230    }
     231  }
     232  for( int i = 0; i < 3; ++i)
     233  {
     234    box.covarianceMatrix[i][0] = covariance[i][0];
     235    box.covarianceMatrix[i][1] = covariance[i][1];
     236    box.covarianceMatrix[i][2] = covariance[i][2];
     237  }
     238  box.center = center;
     239
     240  /* debug output section*/
     241  PRINTF(3)("\nOBB Covariance Matrix:\n");
    393242  for(int j = 0; j < 3; ++j)
    394243  {
    395     PRINT(3)(" |");
     244    PRINT(3)("\t\t");
    396245    for(int k = 0; k < 3; ++k)
    397246    {
    398       PRINT(3)(" \b%f ", covariance[j][k]);
    399     }
    400     PRINT(3)(" |\n");
    401   }
    402 
    403   PRINTF(3)("center: %f, %f, %f\n", center.x, center.y, center.z);
    404 
    405 
    406   for(int i = 0; i < 3; ++i)
    407   {
    408     box->covarianceMatrix[i][0] = covariance[i][0];
    409     box->covarianceMatrix[i][1] = covariance[i][1];
    410     box->covarianceMatrix[i][2] = covariance[i][2];
    411   }
    412   *box->center = center;
    413   PRINTF(3)("-- Written Result to obb\n");
    414 }
    415 
    416 
    417 void OBBTreeNode::calculateBoxEigenvectors(OBB* box, const modelInfo& modInfo)
    418 {}
    419 
    420 void OBBTreeNode::calculateBoxEigenvectors(OBB* box, const sVec3D* verticesList, unsigned int length)
    421 {
    422 
    423   /* now getting spanning vectors of the sub-space:
     247      PRINT(3)("%11.4f\t", covariance[j][k]);
     248    }
     249    PRINT(3)("\n");
     250  }
     251  PRINTF(3)("\nWeighteed OBB Center:\n\t\t%11.4f\t %11.4f\t %11.4f\n", center.x, center.y, center.z);
     252//   PRINTF(3)("\nArithmetical OBB Center:\n\t\t%11.4f\t %11.4f\t %11.4f\n", box.arithCenter.x, box.arithCenter.y, box.arithCenter.z);
     253
     254  /* write back the covariance matrix data to the object oriented bouning box */
     255}
     256
     257
     258
     259/**
     260 *  calculate the eigenvectors for the object oriented box
     261 * @param box: reference to the box
     262 * @param modelInf: the model info structure of the model
     263 * @param tirangleIndexes: an array with the indexes of the triangles inside this
     264 * @param length: the length of the indexes array
     265 */
     266void OBBTreeNode::calculateBoxEigenvectors(OBB& box, const modelInfo& modelInf,
     267    const int* triangleIndexes, int length)
     268{
     269
     270  Vector         axis[3];                            //!< the references to the obb axis
     271  Matrix         covMat(  box.covarianceMatrix  );   //!< covariance matrix (in the matrix dataform)
     272
     273  /*
     274  now getting spanning vectors of the sub-space:
    424275  the eigenvectors of a symmertric matrix, such as the
    425276  covarience matrix are mutually orthogonal.
    … …  
    427278  vectors
    428279  */
    429   Vector*              axis = new Vector[3];                //!< the references to the obb axis
    430 
    431   Matrix covMat(  box->covarianceMatrix  );
     280
     281  /* calculate the axis */
    432282  covMat.getEigenVectors(axis[0], axis[1], axis[2] );
    433 
    434 
    435   /* new jacobi tests */
    436 //  JacobI(OBBTreeNode::coMat, OBBTreeNode::eigvlMat, OBBTreeNode::eigvMat, OBBTreeNode::rotCount);
    437 //  PRINTF(3)("-- Done Jacobi Decomposition\n");
    438 
    439 
    440 //   PRINTF(0)("Jacobi\n");
    441 //   for(int j = 0; j < 3; ++j)
    442 //   {
    443 //     printf(" |");
    444 //     for(int k = 0; k < 3; ++k)
    445 //     {
    446 //       printf(" \t%f ", OBBTreeNode::OBBTreeNode::eigvMat[j][k]);
    447 //     }
    448 //     printf(" |\n");
    449 //   }
    450 
    451 /*  axis[0].x = OBBTreeNode::eigvMat[0][0]; axis[0].y = OBBTreeNode::eigvMat[1][0]; axis[0].z = OBBTreeNode::eigvMat[2][0];
    452   axis[1].x = OBBTreeNode::eigvMat[0][1]; axis[1].y = OBBTreeNode::eigvMat[1][1]; axis[1].z = OBBTreeNode::eigvMat[2][1];
    453   axis[2].x = OBBTreeNode::eigvMat[0][2]; axis[2].y = OBBTreeNode::eigvMat[1][2]; axis[2].z = OBBTreeNode::eigvMat[2][2];
    454   axis[0].normalize();
    455   axis[1].normalize();
    456   axis[2].normalize();*/
    457   box->axis = axis;
    458 
    459 //   PRINTF(0)("-- Got Axis\n");
    460 //
    461 //   PRINTF(0)("eigenvector: %f, %f, %f\n", box->axis[0].x, box->axis[0].y, box->axis[0].z);
    462 //   PRINTF(0)("eigenvector: %f, %f, %f\n", box->axis[1].x, box->axis[1].y, box->axis[1].z);
    463 //   PRINTF(0)("eigenvector: %f, %f, %f\n", box->axis[2].x, box->axis[2].y, box->axis[2].z);
    464 }
    465 
    466 
    467 void OBBTreeNode::calculateBoxAxis(OBB* box, const modelInfo& modInfo)
    468 {
    469   this->calculateBoxAxis(box, (const sVec3D*)modInfo.pVertices, modInfo.numVertices);
    470 }
    471 
    472 
    473 
    474 void OBBTreeNode::calculateBoxAxis(OBB* box, const sVec3D* verticesList, unsigned int length)
    475 {
    476 
     283  box.axis[0] = axis[0];
     284  box.axis[1] = axis[1];
     285  box.axis[2] = axis[2];
     286
     287//   box.axis[0] = Vector(1,0,0);
     288//   box.axis[1] = Vector(0,1,0);
     289//   box.axis[2] = Vector(0,0,1);
     290
     291  PRINTF(3)("Eigenvectors:\n");
     292  PRINT(3)("\t\t%11.2f \t%11.2f \t%11.2f\n", box.axis[0].x, box.axis[0].y, box.axis[0].z);
     293  PRINT(3)("\t\t%11.2f \t%11.2f \t%11.2f\n", box.axis[1].x, box.axis[1].y, box.axis[1].z);
     294  PRINT(3)("\t\t%11.2f \t%11.2f \t%11.2f\n", box.axis[2].x, box.axis[2].y, box.axis[2].z);
     295}
     296
     297
     298
     299
     300/**
     301 *  calculate the eigenvectors for the object oriented box
     302 * @param box: reference to the box
     303 * @param modelInf: the model info structure of the model
     304 * @param tirangleIndexes: an array with the indexes of the triangles inside this
     305 * @param length: the length of the indexes array
     306 */
     307void OBBTreeNode::calculateBoxAxis(OBB& box, const modelInfo& modelInf, const int* triangleIndexes, int length)
     308{
     309
     310  PRINTF(3)("Calculate Box Axis\n");
    477311  /* now get the axis length */
    478   Line                ax[3];                                 //!< the axis
    479   float*              halfLength = new float[3];             //!< half length of the axis
     312  float               halfLength[3];                         //!< half length of the axis
    480313  float               tmpLength;                             //!< tmp save point for the length
    481   Plane               p0(box->axis[0], *box->center);       //!< the axis planes
    482   Plane               p1(box->axis[1], *box->center);
    483   Plane               p2(box->axis[2], *box->center);
    484   float               maxLength[3];
    485   float               minLength[3];
    486 
    487 
    488   /* get a bad bounding box */
    489   halfLength[0] = -1.0f;
    490   for(int j = 0; j < length; ++j)
    491     {
    492       tmpLength = fabs(p0.distancePoint(vertices[j]));
    493       if( tmpLength > halfLength[0])
    494         halfLength[0] = tmpLength;
    495     }
    496 
    497   halfLength[1] = -1.0f;
    498   for(int j = 0; j < length; ++j)
    499     {
    500       tmpLength = fabs(p1.distancePoint(vertices[j]));
    501       if( tmpLength > halfLength[1])
    502         halfLength[1] = tmpLength;
    503     }
    504 
    505   halfLength[2] = -1.0f;
    506   for(int j = 0; j < length; ++j)
    507     {
    508       tmpLength = fabs(p2.distancePoint(vertices[j]));
    509       if( tmpLength > halfLength[2])
    510         halfLength[2] = tmpLength;
    511     }
    512 
    513 
     314  Plane               p0(box.axis[0], box.center);           //!< the axis planes
     315  Plane               p1(box.axis[1], box.center);           //!< the axis planes
     316  Plane               p2(box.axis[2], box.center);           //!< the axis planes
     317  float               maxLength[3];                          //!< maximal lenth of the axis
     318  float               minLength[3];                          //!< minimal length of the axis
     319  const float*        tmpVec;                                //!< variable taking tmp vectors
     320  float               centerOffset[3];
    514321
    515322  /* get the maximal dimensions of the body in all directions */
    516     maxLength[0] = p0.distancePoint(vertices[0]);
    517     minLength[0] = p0.distancePoint(vertices[0]);
    518    for(int j = 0; j < length; ++j)
    519    {
    520      tmpLength = p0.distancePoint(vertices[j]);
    521      if( tmpLength > maxLength[0])
    522        maxLength[0] = tmpLength;
    523      else if( tmpLength < minLength[0])
    524        minLength[0] = tmpLength;
    525    }
    526 
    527    maxLength[1] = p1.distancePoint(vertices[0]);
    528    minLength[1] = p1.distancePoint(vertices[0]);
    529    for(int j = 0; j < length; ++j)
    530    {
    531      tmpLength = p1.distancePoint(vertices[j]);
    532      if( tmpLength > maxLength[1])
    533        maxLength[1] = tmpLength;
    534      else if( tmpLength < minLength[1])
    535        minLength[1] = tmpLength;
    536    }
    537 
    538    maxLength[2] = p2.distancePoint(vertices[0]);
    539    minLength[2] = p2.distancePoint(vertices[0]);
    540    for(int j = 0; j < length; ++j)
    541    {
    542      tmpLength = p2.distancePoint(vertices[j]);
    543      if( tmpLength > maxLength[2])
    544        maxLength[2] = tmpLength;
    545      else if( tmpLength < minLength[2])
    546        minLength[2] = tmpLength;
    547    }
    548 
    549 
    550    /* calculate the real centre of the body by using the axis length */
    551    float centerOffset[3];
    552    float newHalfLength[3];
    553    for(int i = 0; i < 3; ++i)
    554      {
    555        PRINTF(3)("max: %f, min: %f \n", maxLength[i], minLength[i]);
    556        centerOffset[i] = (maxLength[i] + minLength[i]) / 2.0f;       // min length is negatie
    557        newHalfLength[i] = (maxLength[i] - minLength[i]) / 2.0f;      // min length is negative
    558        *box->center +=  (box->axis[i] * centerOffset[i]);            // update the new center vector
    559        halfLength[i] = newHalfLength[i];
    560      }
    561 
    562 
    563 
    564   box->halfLength = halfLength;
    565   PRINTF(3)("-- Written Axis to obb\n");
    566   PRINTF(3)("-- Finished Calculating Attributes\n");
     323  /* for the initialisation the value just has to be inside of the polygon soup -> first vertices (rand) */
     324  for( int k = 0; k  < 3; k++)
     325  {
     326    tmpVec = (&modelInf.pVertices[modelInf.pTriangles[triangleIndexes[0]].indexToVertices[0]]);
     327    Plane* p;
     328    if( k == 0)
     329      p = &p0;
     330    else if( k == 1)
     331      p = &p1;
     332    else
     333      p = &p2;
     334    maxLength[k] = p->distancePoint(tmpVec);
     335    minLength[k] = p->distancePoint(tmpVec);
     336
     337    for( int j = 0; j < length; ++j) {
     338      for( int i = 0; i < 3; ++i) {
     339        tmpVec = &modelInf.pVertices[modelInf.pTriangles[triangleIndexes[j]].indexToVertices[i]];
     340        tmpLength = p->distancePoint(tmpVec);
     341
     342        if( tmpLength > maxLength[k])
     343          maxLength[k] = tmpLength;
     344        else if( tmpLength < minLength[k])
     345          minLength[k] = tmpLength;
     346      }
     347    }
     348  }
     349
     350
     351
     352  /* calculate the real centre of the body by using the axis length */
     353  for( int i = 0; i < 3; ++i)
     354  {
     355    if( maxLength[i] > 0.0f && minLength[i] > 0.0f)  // both axis positiv
     356      centerOffset[i] = minLength[i] + (maxLength[i] - minLength[i]) / 2.0f;
     357    else if( maxLength[i] > 0.0f && maxLength[i] < 0.0f) // positiv and negativ
     358      centerOffset[i] = (maxLength[i] + minLength[i]) / 2.0f;
     359    else //both negativ
     360     centerOffset[i] = minLength[i] + (maxLength[i] - minLength[i]) / 2.0f;
     361
     362    box.halfLength[i] = (maxLength[i] - minLength[i]) / 2.0f;
     363  }
     364
     365  box.center += (box.axis[0] * centerOffset[0]);
     366  box.center += (box.axis[1] * centerOffset[1]);
     367  box.center += (box.axis[2] * centerOffset[2]);
     368
     369
     370  PRINTF(3)("\n");
     371  PRINT(3)("\tAxis halflength x: %11.2f (max: %11.2f, \tmin: %11.2f), offset: %11.2f\n", box.halfLength[0], maxLength[0], minLength[0], centerOffset[0]);
     372  PRINT(3)("\tAxis halflength y: %11.2f (max: %11.2f, \tmin: %11.2f), offset: %11.2f\n", box.halfLength[1], maxLength[1], minLength[1], centerOffset[1] );
     373  PRINT(3)("\tAxis halflength z: %11.2f (max: %11.2f, \tmin: %11.2f), offset: %11.2f\n", box.halfLength[2], maxLength[2], minLength[2], centerOffset[2]);
    567374}
    568375
    … …  
    570377
    571378/**
    572   \brief this separates an ob-box in the middle
    573 * @param box: the box to separate
    574 
    575   this will separate the box into to smaller boxes. the separation is done along the middle of the longest axis
     379 * this separates an ob-box in the middle
     380 * @param box: the box to separate
     381 *
     382 * this will separate the box into to smaller boxes. the separation is done along the middle of the longest axis
    576383 */
    577 void OBBTreeNode::forkBox(OBB* box)
    578 {
     384void OBBTreeNode::forkBox(OBB& box)
     385{
     386
     387  PRINTF(3)("Fork Box\n");
     388  PRINTF(4)("Calculating the longest Axis\n");
    579389  /* get the longest axis of the box */
    580   float               aLength = -1.0f;                     //!< the length of the longest axis
    581   int                 axisIndex = 0;                       //!< this is the nr of the longest axis
    582 
    583   for(int i = 0; i < 3; ++i)
    584   {
    585     if( aLength < box->halfLength[i])
    586     {
    587       aLength = box->halfLength[i];
    588       axisIndex = i;
    589     }
    590   }
    591 
    592    PRINTF(3)("longest axis is: nr %i with a half-length of: %f\n", axisIndex, aLength);
    593 
    594 
     390  float               longestAxis = -1.0f;                 //!< the length of the longest axis
     391  int                 longestAxisIndex = 0;                //!< this is the nr of the longest axis
     392
     393
     394  /* now get the longest axis of the three exiting */
     395  for( int i = 0; i < 3; ++i)
     396  {
     397    if( longestAxis < box.halfLength[i])
     398    {
     399      longestAxis = box.halfLength[i];
     400      longestAxisIndex = i;
     401    }
     402  }
     403  PRINTF(3)("\nLongest Axis is: Nr %i with a half-length of:%11.2f\n", longestAxisIndex, longestAxis);
     404
     405
     406  PRINTF(4)("Separating along the longest axis\n");
    595407  /* get the closest vertex near the center */
    596408  float               dist = 999999.0f;                    //!< the smallest distance to each vertex
    597   float               tmpDist;                             //!< temporary distance
    598   int                 vertexIndex;
    599   Plane               middlePlane(box->axis[axisIndex], *box->center); //!< the middle plane
    600 
    601   vertexIndex = 0;
    602   for(int i = 0; i < box->numOfVertices; ++i)
    603   {
    604     tmpDist = fabs(middlePlane.distancePoint(box->vertices[i]));
    605     if( tmpDist < dist)
    606     {
    607       dist = tmpDist;
    608       vertexIndex = i;
    609     }
    610   }
    611 
    612   PRINTF(3)("\nthe clostest vertex is nr: %i, with a dist of: %f\n", vertexIndex ,dist);
     409  float               tmpDist;                             //!< variable to save diverse distances temporarily
     410  int                 vertexIndex;                         //!< index of the vertex near the center
     411  Plane               middlePlane(box.axis[longestAxisIndex], box.center); //!< the middle plane
     412  const sVec3D*       tmpVec;                              //!< temp simple 3D vector
    613413
    614414
    … …  
    616416  the points depending on which side they are located
    617417  */
    618   tList<const sVec3D>      partition1;                           //!< the vertex partition 1
    619   tList<const sVec3D>      partition2;                           //!< the vertex partition 2
    620 
    621 
    622   PRINTF(3)("vertex index: %i, of %i\n", vertexIndex, box->numOfVertices);
    623   this->separationPlane = new Plane(box->axis[axisIndex], box->vertices[vertexIndex]);  //!< separation plane
    624   this->sepPlaneCenter = &box->vertices[vertexIndex];
    625   this->longestAxisIndex = axisIndex;
    626 
    627   for(int i = 0; i < box->numOfVertices; ++i)
    628   {
    629     if( i == vertexIndex) continue;
    630     tmpDist = this->separationPlane->distancePoint(box->vertices[i]);
    631     if( tmpDist > 0.0)
    632       partition1.add(&box->vertices[i]); /* positive numbers plus zero */
    633     else
    634       partition2.add(&box->vertices[i]); /* negatice numbers */
    635   }
    636   partition1.add(&box->vertices[vertexIndex]);
    637   partition2.add(&box->vertices[vertexIndex]);
    638 
    639   PRINTF(3)("\npartition1: got %i vertices/ partition 2: got %i vertices\n", partition1.getSize(), partition2.getSize());
     418  std::list<int>           partition1;                           //!< the vertex partition 1
     419  std::list<int>           partition2;                           //!< the vertex partition 2
     420  float*                   triangleCenter = new float[3];        //!< the center of the triangle
     421  const float*             a;                                    //!< triangle  edge a
     422  const float*             b;                                    //!< triangle  edge b
     423  const float*             c;                                    //!< triangle  edge c
     424
     425
     426  /* find the center of the box */
     427  this->separationPlane = Plane(box.axis[longestAxisIndex], box.center);
     428  this->sepPlaneCenter[0] = box.center.x;
     429  this->sepPlaneCenter[1] = box.center.y;
     430  this->sepPlaneCenter[2] = box.center.z;
     431  this->longestAxisIndex = longestAxisIndex;
     432
     433  for( int i = 0; i < box.triangleIndexesLength; ++i)
     434  {
     435    /* first calculate the middle of the triangle */
     436    a = &box.modelInf->pVertices[box.modelInf->pTriangles[box.triangleIndexes[i]].indexToVertices[0]];
     437    b = &box.modelInf->pVertices[box.modelInf->pTriangles[box.triangleIndexes[i]].indexToVertices[1]];
     438    c = &box.modelInf->pVertices[box.modelInf->pTriangles[box.triangleIndexes[i]].indexToVertices[2]];
     439
     440    triangleCenter[0] = (a[0] + b[0] + c[0]) / 3.0f;
     441    triangleCenter[1] = (a[1] + b[1] + c[1]) / 3.0f;
     442    triangleCenter[2] = (a[2] + b[2] + c[2]) / 3.0f;
     443    tmpDist = this->separationPlane.distancePoint(*((sVec3D*)triangleCenter));
     444
     445    if( tmpDist > 0.0f)
     446      partition1.push_back(box.triangleIndexes[i]); /* positive numbers plus zero */
     447    else if( tmpDist < 0.0f)
     448      partition2.push_back(box.triangleIndexes[i]); /* negatice numbers */
     449    else {
     450      partition1.push_back(box.triangleIndexes[i]); /* 0.0f? unprobable... */
     451      partition2.push_back(box.triangleIndexes[i]);
     452    }
     453  }
     454  PRINTF(3)("\nPartition1: got \t%i Vertices \nPartition2: got \t%i Vertices\n", partition1.size(), partition2.size());
    640455
    641456
    642457  /* now comes the separation into two different sVec3D arrays */
    643   tIterator<const sVec3D>* iterator;                       //!< the iterator to go through the lists
    644   const sVec3D*      element;                              //!< the elements
    645458  int                index;                                //!< index storage place
    646   sVec3D*            vertList1;                            //!< the vertex list 1
    647   sVec3D*            vertList2;                            //!< the vertex list 2
    648 
    649   vertList1 = new sVec3D[partition1.getSize()];
    650   vertList2 = new sVec3D[partition2.getSize()];
    651 
    652   iterator = partition1.getIterator();
    653   element = iterator->firstElement();
    654   index = 0;
    655   while( element != NULL)
    656   {
    657     vertList1[index][0] = element[0][0];
    658     vertList1[index][1] = element[0][1];
    659     vertList1[index][2] = element[0][2];
    660     ++index;
    661     element = iterator->nextElement();
    662   }
    663 
    664 //   PRINTF(0)("\npartition 1:\n");
    665 //   for(int i = 0; i < partition1.getSize(); ++i)
    666 //   {
    667 //     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]);
    668 //   }
    669 
    670   iterator = partition2.getIterator();
    671   element = iterator->firstElement();
    672   index = 0;
    673   while( element != NULL)
    674   {
    675     vertList2[index][0] = element[0][0];
    676     vertList2[index][1] = element[0][1];
    677     vertList2[index][2] = element[0][2];
    678     ++index;
    679     element = iterator->nextElement();
    680   }
    681 
    682   this->tmpVert1 = vertList1;
    683   this->tmpVert2 = vertList2;
    684   this->tmpLen1 = partition1.getSize();
    685   this->tmpLen2 = partition2.getSize();
    686 
    687   delete iterator;
    688 
    689 //   PRINTF(0)("\npartition 2:\n");
    690 //   for(int i = 0; i < partition2.getSize(); ++i)
    691 //   {
    692 //     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]);
    693 //   }
     459  int*               triangleIndexList1;                   //!< the vertex list 1
     460  int*               triangleIndexList2;                   //!< the vertex list 2
     461  std::list<int>::iterator element;                        //!< the list iterator
     462
     463  triangleIndexList1 = new int[partition1.size()];
     464  triangleIndexList2 = new int[partition2.size()];
     465
     466  for( element = partition1.begin(), index = 0; element != partition1.end(); element++, index++)
     467    triangleIndexList1[index] = (*element);
     468
     469  for( element = partition2.begin(), index = 0; element != partition2.end(); element++, index++)
     470    triangleIndexList2[index] = (*element);
     471
     472  if( this->triangleIndexList1!= NULL)
     473    delete[] this->triangleIndexList1;
     474  this->triangleIndexList1 = triangleIndexList1;
     475  this->triangleIndexLength1 = partition1.size();
     476
     477  if( this->triangleIndexList2 != NULL)
     478    delete[] this->triangleIndexList2;
     479  this->triangleIndexList2 = triangleIndexList2;
     480  this->triangleIndexLength2 = partition2.size();
    694481}
    695482
    … …  
    699486void OBBTreeNode::collideWith(BVTreeNode* treeNode, WorldEntity* nodeA, WorldEntity* nodeB)
    700487{
    701   PRINTF(3)("collideWith\n");
     488  if( unlikely(treeNode == NULL))
     489    return;
     490
     491  PRINTF(4)("collideWith\n");
    702492  /* if the obb overlap, make subtests: check which node is realy overlaping  */
    703   PRINT(3)("Checking OBB %i vs %i: ", this->getIndex(), treeNode->getIndex());
    704   if( unlikely(treeNode == NULL)) return;
    705 
    706   if( this->overlapTest(this->bvElement, ((OBBTreeNode*)treeNode)->bvElement, nodeA, nodeB))
    707   {
    708     PRINTF(3)("collision @ lvl %i, object %s vs. %s, (%p, %p)\n", this->depth, nodeA->getClassName(), nodeB->getClassName(), this->nodeLeft, this->nodeRight);
     493  PRINTF(4)("Checking OBB %i vs %i: ", this->getIndex(), treeNode->getIndex());
     494  //   if( unlikely(treeNode == NULL)) return;
     495
     496
     497  if( this->overlapTest(*this->bvElement, *(((const OBBTreeNode*)&treeNode)->bvElement), nodeA, nodeB))
     498  {
     499    PRINTF(4)("collision @ lvl %i, object %s vs. %s, (%p, %p)\n", this->depth, nodeA->getClassName(), nodeB->getClassName(), this->nodeLeft, this->nodeRight);
    709500
    710501    /* check if left node overlaps */
    711502    if( likely( this->nodeLeft != NULL))
    712503    {
    713       PRINT(3)("Checking OBB %i vs %i: ", this->nodeLeft->getIndex(), treeNode->getIndex());
    714       if( this->overlapTest(this->nodeLeft->bvElement, ((OBBTreeNode*)treeNode)->bvElement, nodeA, nodeB))
     504      PRINTF(4)("Checking OBB %i vs %i: ", this->nodeLeft->getIndex(), treeNode->getIndex());
     505      if( this->overlapTest(*this->nodeLeft->bvElement, *(((const OBBTreeNode*)&treeNode)->bvElement), nodeA, nodeB))
    715506      {
    716         this->nodeLeft->collideWith(((OBBTreeNode*)treeNode)->nodeLeft, nodeA, nodeB);
    717         this->nodeLeft->collideWith(((OBBTreeNode*)treeNode)->nodeRight, nodeA, nodeB);
     507        this->nodeLeft->collideWith((((const OBBTreeNode*)treeNode)->nodeLeft), nodeA, nodeB);
     508        this->nodeLeft->collideWith((((const OBBTreeNode*)treeNode)->nodeRight), nodeA, nodeB);
    718509      }
    719510    }
    … …  
    721512    if( likely( this->nodeRight != NULL))
    722513    {
    723       PRINT(3)("Checking OBB %i vs %i: ", this->nodeRight->getIndex(), treeNode->getIndex());
    724       if(this->overlapTest(this->nodeRight->bvElement, ((OBBTreeNode*)treeNode)->bvElement, nodeA, nodeB))
     514      PRINTF(4)("Checking OBB %i vs %i: ", this->nodeRight->getIndex(), treeNode->getIndex());
     515      if(this->overlapTest(*this->nodeRight->bvElement, *(((const OBBTreeNode*)&treeNode)->bvElement), nodeA, nodeB))
    725516      {
    726        this->nodeRight->collideWith(((OBBTreeNode*)treeNode)->nodeLeft, nodeA, nodeB);
    727        this->nodeRight->collideWith(((OBBTreeNode*)treeNode)->nodeRight, nodeA, nodeB);
     517        this->nodeRight->collideWith((((const OBBTreeNode*)treeNode)->nodeLeft), nodeA, nodeB);
     518        this->nodeRight->collideWith((((const OBBTreeNode*)treeNode)->nodeRight), nodeA, nodeB);
    728519      }
    729520    }
    730521
    731522    /* so there is a collision and this is the last box in the tree (i.e. leaf) */
     523    /* FIXME: If we would choose || insead of && there would also be asymmetrical cases supported */
    732524    if( unlikely(this->nodeRight == NULL && this->nodeLeft == NULL))
    733525    {
    734       nodeA->collidesWith(nodeB, *((OBBTreeNode*)treeNode)->bvElement->center);
    735 
    736       nodeB->collidesWith(nodeA, *this->bvElement->center);
    737     }
    738 
    739   }
    740 }
    741 
    742 
    743 
    744 bool OBBTreeNode::overlapTest(OBB* boxA, OBB* boxB, WorldEntity* nodeA, WorldEntity* nodeB)
    745 {
     526      nodeA->collidesWith(nodeB, (((const OBBTreeNode*)&treeNode)->bvElement->center));
     527
     528      nodeB->collidesWith(nodeA, this->bvElement->center);
     529    }
     530
     531  }
     532}
     533
     534
     535
     536bool OBBTreeNode::overlapTest(OBB& boxA, OBB& boxB, WorldEntity* nodeA, WorldEntity* nodeB)
     537{
     538  //HACK remove this again
     539  this->owner = nodeA;
     540  //   if( boxB == NULL || boxA == NULL)
     541  //     return false;
     542
    746543  /* first check all axis */
    747544  Vector t;
    … …  
    752549  Vector rotAxisB[3];
    753550
    754   rotAxisA[0] =  nodeA->getAbsDir().apply(boxA->axis[0]);
    755   rotAxisA[1] =  nodeA->getAbsDir().apply(boxA->axis[1]);
    756   rotAxisA[2] =  nodeA->getAbsDir().apply(boxA->axis[2]);
    757 
    758   rotAxisB[0] =  nodeB->getAbsDir().apply(boxB->axis[0]);
    759   rotAxisB[1] =  nodeB->getAbsDir().apply(boxB->axis[1]);
    760   rotAxisB[2] =  nodeB->getAbsDir().apply(boxB->axis[2]);
    761 
    762   t = nodeA->getAbsCoor() + nodeA->getAbsDir().apply(*boxA->center) - ( nodeB->getAbsCoor() + nodeB->getAbsDir().apply(*boxB->center));
    763 
    764 //   printf("\n");
    765 //   printf("(%f, %f, %f) -> (%f, %f, %f)\n", boxA->axis[0].x, boxA->axis[0].y, boxA->axis[0].z, rotAxisA[0].x, rotAxisA[0].y, rotAxisA[0].z);
    766 //   printf("(%f, %f, %f) -> (%f, %f, %f)\n", boxA->axis[1].x, boxA->axis[1].y, boxA->axis[1].z, rotAxisA[1].x, rotAxisA[1].y, rotAxisA[1].z);
    767 //   printf("(%f, %f, %f) -> (%f, %f, %f)\n", boxA->axis[2].x, boxA->axis[2].y, boxA->axis[2].z, rotAxisA[2].x, rotAxisA[2].y, rotAxisA[2].z);
    768 //
    769 //   printf("(%f, %f, %f) -> (%f, %f, %f)\n", boxB->axis[0].x, boxB->axis[0].y, boxB->axis[0].z, rotAxisB[0].x, rotAxisB[0].y, rotAxisB[0].z);
    770 //   printf("(%f, %f, %f) -> (%f, %f, %f)\n", boxB->axis[1].x, boxB->axis[1].y, boxB->axis[1].z, rotAxisB[1].x, rotAxisB[1].y, rotAxisB[1].z);
    771 //   printf("(%f, %f, %f) -> (%f, %f, %f)\n", boxB->axis[2].x, boxB->axis[2].y, boxB->axis[2].z, rotAxisB[2].x, rotAxisB[2].y, rotAxisB[2].z);
     551  rotAxisA[0] =  nodeA->getAbsDir().apply(boxA.axis[0]);
     552  rotAxisA[1] =  nodeA->getAbsDir().apply(boxA.axis[1]);
     553  rotAxisA[2] =  nodeA->getAbsDir().apply(boxA.axis[2]);
     554
     555  rotAxisB[0] =  nodeB->getAbsDir().apply(boxB.axis[0]);
     556  rotAxisB[1] =  nodeB->getAbsDir().apply(boxB.axis[1]);
     557  rotAxisB[2] =  nodeB->getAbsDir().apply(boxB.axis[2]);
     558
     559
     560  t = nodeA->getAbsCoor() + nodeA->getAbsDir().apply(boxA.center) - ( nodeB->getAbsCoor() + nodeB->getAbsDir().apply(boxB.center));
     561
     562  //   printf("\n");
     563  //   printf("(%f, %f, %f) -> (%f, %f, %f)\n", boxA->axis[0].x, boxA->axis[0].y, boxA->axis[0].z, rotAxisA[0].x, rotAxisA[0].y, rotAxisA[0].z);
     564  //   printf("(%f, %f, %f) -> (%f, %f, %f)\n", boxA->axis[1].x, boxA->axis[1].y, boxA->axis[1].z, rotAxisA[1].x, rotAxisA[1].y, rotAxisA[1].z);
     565  //   printf("(%f, %f, %f) -> (%f, %f, %f)\n", boxA->axis[2].x, boxA->axis[2].y, boxA->axis[2].z, rotAxisA[2].x, rotAxisA[2].y, rotAxisA[2].z);
     566  //
     567  //   printf("(%f, %f, %f) -> (%f, %f, %f)\n", boxB->axis[0].x, boxB->axis[0].y, boxB->axis[0].z, rotAxisB[0].x, rotAxisB[0].y, rotAxisB[0].z);
     568  //   printf("(%f, %f, %f) -> (%f, %f, %f)\n", boxB->axis[1].x, boxB->axis[1].y, boxB->axis[1].z, rotAxisB[1].x, rotAxisB[1].y, rotAxisB[1].z);
     569  //   printf("(%f, %f, %f) -> (%f, %f, %f)\n", boxB->axis[2].x, boxB->axis[2].y, boxB->axis[2].z, rotAxisB[2].x, rotAxisB[2].y, rotAxisB[2].z);
    772570
    773571
    … …  
    779577    l = rotAxisA[j];
    780578
    781     rA += fabs(boxA->halfLength[0] * rotAxisA[0].dot(l));
    782     rA += fabs(boxA->halfLength[1] * rotAxisA[1].dot(l));
    783     rA += fabs(boxA->halfLength[2] * rotAxisA[2].dot(l));
    784 
    785     rB += fabs(boxB->halfLength[0] * rotAxisB[0].dot(l));
    786     rB += fabs(boxB->halfLength[1] * rotAxisB[1].dot(l));
    787     rB += fabs(boxB->halfLength[2] * rotAxisB[2].dot(l));
    788 
    789     PRINTF(3)("s = %f, rA+rB = %f\n", fabs(t.dot(l)), rA+rB);
     579    rA += fabs(boxA.halfLength[0] * rotAxisA[0].dot(l));
     580    rA += fabs(boxA.halfLength[1] * rotAxisA[1].dot(l));
     581    rA += fabs(boxA.halfLength[2] * rotAxisA[2].dot(l));
     582
     583    rB += fabs(boxB.halfLength[0] * rotAxisB[0].dot(l));
     584    rB += fabs(boxB.halfLength[1] * rotAxisB[1].dot(l));
     585    rB += fabs(boxB.halfLength[2] * rotAxisB[2].dot(l));
     586
     587    PRINTF(5)("s = %f, rA+rB = %f\n", fabs(t.dot(l)), rA+rB);
    790588
    791589    if( (rA + rB) < fabs(t.dot(l)))
    792590    {
    793       PRINT(3)("no Collision\n");
     591      PRINTF(4)("no Collision\n");
    794592      return false;
    795593    }
    … …  
    803601    l = rotAxisB[j];
    804602
    805     rA += fabs(boxA->halfLength[0] * rotAxisA[0].dot(l));
    806     rA += fabs(boxA->halfLength[1] * rotAxisA[1].dot(l));
    807     rA += fabs(boxA->halfLength[2] * rotAxisA[2].dot(l));
    808 
    809     rB += fabs(boxB->halfLength[0] * rotAxisB[0].dot(l));
    810     rB += fabs(boxB->halfLength[1] * rotAxisB[1].dot(l));
    811     rB += fabs(boxB->halfLength[2] * rotAxisB[2].dot(l));
    812 
    813     PRINTF(3)("s = %f, rA+rB = %f\n", fabs(t.dot(l)), rA+rB);
     603    rA += fabs(boxA.halfLength[0] * rotAxisA[0].dot(l));
     604    rA += fabs(boxA.halfLength[1] * rotAxisA[1].dot(l));
     605    rA += fabs(boxA.halfLength[2] * rotAxisA[2].dot(l));
     606
     607    rB += fabs(boxB.halfLength[0] * rotAxisB[0].dot(l));
     608    rB += fabs(boxB.halfLength[1] * rotAxisB[1].dot(l));
     609    rB += fabs(boxB.halfLength[2] * rotAxisB[2].dot(l));
     610
     611    PRINTF(5)("s = %f, rA+rB = %f\n", fabs(t.dot(l)), rA+rB);
    814612
    815613    if( (rA + rB) < fabs(t.dot(l)))
    816614    {
    817       PRINT(3)("no Collision\n");
     615      PRINTF(4)("no Collision\n");
    818616      return false;
    819617    }
    … …  
    831629      l = rotAxisA[j].cross(rotAxisB[k]);
    832630
    833       rA += fabs(boxA->halfLength[0] * rotAxisA[0].dot(l));
    834       rA += fabs(boxA->halfLength[1] * rotAxisA[1].dot(l));
    835       rA += fabs(boxA->halfLength[2] * rotAxisA[2].dot(l));
    836 
    837       rB += fabs(boxB->halfLength[0] * rotAxisB[0].dot(l));
    838       rB += fabs(boxB->halfLength[1] * rotAxisB[1].dot(l));
    839       rB += fabs(boxB->halfLength[2] * rotAxisB[2].dot(l));
    840 
    841       PRINTF(3)("s = %f, rA+rB = %f\n", fabs(t.dot(l)), rA+rB);
     631      rA += fabs(boxA.halfLength[0] * rotAxisA[0].dot(l));
     632      rA += fabs(boxA.halfLength[1] * rotAxisA[1].dot(l));
     633      rA += fabs(boxA.halfLength[2] * rotAxisA[2].dot(l));
     634
     635      rB += fabs(boxB.halfLength[0] * rotAxisB[0].dot(l));
     636      rB += fabs(boxB.halfLength[1] * rotAxisB[1].dot(l));
     637      rB += fabs(boxB.halfLength[2] * rotAxisB[2].dot(l));
     638
     639      PRINTF(5)("s = %f, rA+rB = %f\n", fabs(t.dot(l)), rA+rB);
    842640
    843641      if( (rA + rB) < fabs(t.dot(l)))
    844642      {
    845         PRINT(3)("keine Kollision\n");
     643        PRINTF(4)("keine Kollision\n");
    846644        return false;
    847645      }
    … …  
    849647  }
    850648
    851 
    852   boxA->bCollided = true; /* use this ONLY(!!!!) for drawing operations */
    853   boxB->bCollided = true;
    854   PRINT(3)("Kollision!\n");
     649  /* FIXME: there is no collision mark set now */
     650     boxA.bCollided = true; /* use this ONLY(!!!!) for drawing operations */
     651     boxB.bCollided = true;
     652
     653
     654  PRINTF(4)("Kollision!\n");
    855655  return true;
    856656}
    … …  
    860660
    861661
     662
     663
     664
     665
     666
     667/**
     668 *
     669 * draw the BV tree - debug mode
     670 */
    862671void OBBTreeNode::drawBV(int depth, int drawMode, const Vector& color,  bool top) const
    863672{
    864 
    865   /* draw the model itself, there is some problem concerning this: the vertices are drawn multiple times */
    866   if( drawMode & DRAW_MODEL || drawMode & DRAW_ALL)
    867   {
    868     if( !(drawMode & DRAW_SINGLE && depth != 0))
    869     {
    870       if( drawMode & DRAW_POINTS)
     673  /* this function can be used to draw the triangles and/or the points only  */
     674  if( 1 /*drawMode & DRAW_MODEL || drawMode & DRAW_ALL*/)
     675  {
     676    if( depth == 0/*!(drawMode & DRAW_SINGLE && depth != 0)*/)
     677    {
     678      if( 1 /*drawMode & DRAW_POINTS*/)
     679      {
    871680        glBegin(GL_POINTS);
    872       for(int i = 0; i < this->bvElement->numOfVertices; ++i)
    873       {
    874         if( drawMode & DRAW_POINTS)
    875           glVertex3f(this->bvElement->vertices[i][0], this->bvElement->vertices[i][1], this->bvElement->vertices[i][2]);
    876         else
    877         {
    878           glPushMatrix();
    879           glTranslatef(this->bvElement->vertices[i][0], this->bvElement->vertices[i][1], this->bvElement->vertices[i][2]);
    880           gluSphere(OBBTreeNode_sphereObj, 0.1, 10, 10);
    881           glPopMatrix();
    882         }
     681        glColor3f(0.3, 0.8, 0.54);
     682        for( int i = 0; i < this->bvElement->modelInf->numVertices*3; i+=3)
     683          glVertex3f(this->bvElement->modelInf->pVertices[i],
     684                     this->bvElement->modelInf->pVertices[i+1],
     685                     this->bvElement->modelInf->pVertices[i+2]);
     686        glEnd();
    883687      }
    884       if( drawMode & DRAW_POINTS)
    885         glEnd();
    886688    }
    887689  }
    … …  
    897699
    898700  /* draw world axes */
    899   if( drawMode & DRAW_BV_AXIS)
     701  if( 1 /*drawMode & DRAW_BV_AXIS*/)
    900702  {
    901703    glBegin(GL_LINES);
    … …  
    915717
    916718
    917   if( drawMode & DRAW_BV_AXIS || drawMode & DRAW_ALL)
    918   {
    919     if( !(drawMode & DRAW_SINGLE && depth != 0))
     719  if( 1/*drawMode & DRAW_BV_AXIS || drawMode & DRAW_ALL*/)
     720  {
     721    if( 1/*drawMode & DRAW_SINGLE && depth != 0*/)
    920722    {
    921723      /* draw the obb axes */
    922724      glBegin(GL_LINES);
    923       glColor3f(0.0, 0.4, 0.3);
    924       glVertex3f(this->bvElement->center->x, this->bvElement->center->y, this->bvElement->center->z);
    925       glVertex3f(this->bvElement->center->x + this->bvElement->axis[0].x * this->bvElement->halfLength[0],
    926                  this->bvElement->center->y + this->bvElement->axis[0].y * this->bvElement->halfLength[0],
    927                  this->bvElement->center->z + this->bvElement->axis[0].z * this->bvElement->halfLength[0]);
    928 
    929       glVertex3f(this->bvElement->center->x, this->bvElement->center->y, this->bvElement->center->z);
    930       glVertex3f(this->bvElement->center->x + this->bvElement->axis[1].x * this->bvElement->halfLength[1],
    931                  this->bvElement->center->y + this->bvElement->axis[1].y * this->bvElement->halfLength[1],
    932                  this->bvElement->center->z + this->bvElement->axis[1].z * this->bvElement->halfLength[1]);
    933 
    934       glVertex3f(this->bvElement->center->x, this->bvElement->center->y, this->bvElement->center->z);
    935       glVertex3f(this->bvElement->center->x + this->bvElement->axis[2].x * this->bvElement->halfLength[2],
    936                  this->bvElement->center->y + this->bvElement->axis[2].y * this->bvElement->halfLength[2],
    937                  this->bvElement->center->z + this->bvElement->axis[2].z * this->bvElement->halfLength[2]);
     725      glColor3f(1.0, 0.0, 0.0);
     726      glVertex3f(this->bvElement->center.x, this->bvElement->center.y, this->bvElement->center.z);
     727      glVertex3f(this->bvElement->center.x + this->bvElement->axis[0].x * this->bvElement->halfLength[0],
     728                 this->bvElement->center.y + this->bvElement->axis[0].y * this->bvElement->halfLength[0],
     729                 this->bvElement->center.z + this->bvElement->axis[0].z * this->bvElement->halfLength[0]);
     730
     731      glColor3f(0.0, 1.0, 0.0);
     732      glVertex3f(this->bvElement->center.x, this->bvElement->center.y, this->bvElement->center.z);
     733      glVertex3f(this->bvElement->center.x + this->bvElement->axis[1].x * this->bvElement->halfLength[1],
     734                 this->bvElement->center.y + this->bvElement->axis[1].y * this->bvElement->halfLength[1],
     735                 this->bvElement->center.z + this->bvElement->axis[1].z * this->bvElement->halfLength[1]);
     736
     737      glColor3f(0.0, 0.0, 1.0);
     738      glVertex3f(this->bvElement->center.x, this->bvElement->center.y, this->bvElement->center.z);
     739      glVertex3f(this->bvElement->center.x + this->bvElement->axis[2].x * this->bvElement->halfLength[2],
     740                 this->bvElement->center.y + this->bvElement->axis[2].y * this->bvElement->halfLength[2],
     741                 this->bvElement->center.z + this->bvElement->axis[2].z * this->bvElement->halfLength[2]);
    938742      glEnd();
    939743    }
    … …  
    950754    }
    951755
    952     if(this->nodeLeft == NULL || this->nodeRight == NULL)
     756    if( this->nodeLeft == NULL && this->nodeRight == NULL)
    953757      depth = 0;
    954     if( !(drawMode & DRAW_SINGLE && depth != 0))
    955     {
    956     Vector cen = *this->bvElement->center;
    957     Vector* axis = this->bvElement->axis;
    958     float* len = this->bvElement->halfLength;
    959 
    960     if( this->bvElement->bCollided)
    961     {
    962       glColor4f(1.0, 1.0, 1.0, .5); // COLLISION COLOR
    963     }
    964     else if( drawMode & DRAW_BV_BLENDED)
    965     {
    966       glColor4f(color.x, color.y, color.z, .5);
    967     }
    968 
    969     /* draw bounding box */
    970     if( drawMode & DRAW_BV_BLENDED)
    971       glBegin(GL_QUADS);
    972     else
    973       glBegin(GL_LINE_LOOP);
    974     glVertex3f(cen.x + axis[0].x * len[0] + axis[1].x * len[1] + axis[2].x * len[2],
    975                cen.y + axis[0].y * len[0] + axis[1].y * len[1] + axis[2].y * len[2],
    976                cen.z + axis[0].z * len[0] + axis[1].z * len[1] + axis[2].z * len[2]);
    977     glVertex3f(cen.x + axis[0].x * len[0] + axis[1].x * len[1] - axis[2].x * len[2],
    978                cen.y + axis[0].y * len[0] + axis[1].y * len[1] - axis[2].y * len[2],
    979                cen.z + axis[0].z * len[0] + axis[1].z * len[1] - axis[2].z * len[2]);
    980     glVertex3f(cen.x + axis[0].x * len[0] - axis[1].x * len[1] - axis[2].x * len[2],
    981                cen.y + axis[0].y * len[0] - axis[1].y * len[1] - axis[2].y * len[2],
    982                cen.z + axis[0].z * len[0] - axis[1].z * len[1] - axis[2].z * len[2]);
    983     glVertex3f(cen.x + axis[0].x * len[0] - axis[1].x * len[1] + axis[2].x * len[2],
    984                cen.y + axis[0].y * len[0] - axis[1].y * len[1] + axis[2].y * len[2],
    985                cen.z + axis[0].z * len[0] - axis[1].z * len[1] + axis[2].z * len[2]);
    986     glEnd();
    987 
    988     if( drawMode & DRAW_BV_BLENDED)
    989       glBegin(GL_QUADS);
    990     else
    991       glBegin(GL_LINE_LOOP);
    992     glVertex3f(cen.x + axis[0].x * len[0] - axis[1].x * len[1] + axis[2].x * len[2],
    993                cen.y + axis[0].y * len[0] - axis[1].y * len[1] + axis[2].y * len[2],
    994                cen.z + axis[0].z * len[0] - axis[1].z * len[1] + axis[2].z * len[2]);
    995     glVertex3f(cen.x + axis[0].x * len[0] - axis[1].x * len[1] - axis[2].x * len[2],
    996                cen.y + axis[0].y * len[0] - axis[1].y * len[1] - axis[2].y * len[2],
    997                cen.z + axis[0].z * len[0] - axis[1].z * len[1] - axis[2].z * len[2]);
    998     glVertex3f(cen.x - axis[0].x * len[0] - axis[1].x * len[1] - axis[2].x * len[2],
    999                cen.y - axis[0].y * len[0] - axis[1].y * len[1] - axis[2].y * len[2],
    1000                cen.z - axis[0].z * len[0] - axis[1].z * len[1] - axis[2].z * len[2]);
    1001     glVertex3f(cen.x - axis[0].x * len[0] - axis[1].x * len[1] + axis[2].x * len[2],
    1002                cen.y - axis[0].y * len[0] - axis[1].y * len[1] + axis[2].y * len[2],
    1003                cen.z - axis[0].z * len[0] - axis[1].z * len[1] + axis[2].z * len[2]);
    1004     glEnd();
    1005 
    1006     if( drawMode & DRAW_BV_BLENDED)
    1007       glBegin(GL_QUADS);
    1008     else
    1009       glBegin(GL_LINE_LOOP);
    1010     glVertex3f(cen.x - axis[0].x * len[0] - axis[1].x * len[1] + axis[2].x * len[2],
    1011                cen.y - axis[0].y * len[0] - axis[1].y * len[1] + axis[2].y * len[2],
    1012                cen.z - axis[0].z * len[0] - axis[1].z * len[1] + axis[2].z * len[2]);
    1013     glVertex3f(cen.x - axis[0].x * len[0] - axis[1].x * len[1] - axis[2].x * len[2],
    1014                cen.y - axis[0].y * len[0] - axis[1].y * len[1] - axis[2].y * len[2],
    1015                cen.z - axis[0].z * len[0] - axis[1].z * len[1] - axis[2].z * len[2]);
    1016     glVertex3f(cen.x - axis[0].x * len[0] + axis[1].x * len[1] - axis[2].x * len[2],
    1017                cen.y - axis[0].y * len[0] + axis[1].y * len[1] - axis[2].y * len[2],
    1018                cen.z - axis[0].z * len[0] + axis[1].z * len[1] - axis[2].z * len[2]);
    1019     glVertex3f(cen.x - axis[0].x * len[0] + axis[1].x * len[1] + axis[2].x * len[2],
    1020                cen.y - axis[0].y * len[0] + axis[1].y * len[1] + axis[2].y * len[2],
    1021                cen.z - axis[0].z * len[0] + axis[1].z * len[1] + axis[2].z * len[2]);
    1022     glEnd();
    1023 
    1024     if( drawMode & DRAW_BV_BLENDED)
    1025       glBegin(GL_QUADS);
    1026     else
    1027       glBegin(GL_LINE_LOOP);
    1028     glVertex3f(cen.x - axis[0].x * len[0] + axis[1].x * len[1] - axis[2].x * len[2],
    1029                cen.y - axis[0].y * len[0] + axis[1].y * len[1] - axis[2].y * len[2],
    1030                cen.z - axis[0].z * len[0] + axis[1].z * len[1] - axis[2].z * len[2]);
    1031     glVertex3f(cen.x - axis[0].x * len[0] + axis[1].x * len[1] + axis[2].x * len[2],
    1032                cen.y - axis[0].y * len[0] + axis[1].y * len[1] + axis[2].y * len[2],
    1033                cen.z - axis[0].z * len[0] + axis[1].z * len[1] + axis[2].z * len[2]);
    1034     glVertex3f(cen.x + axis[0].x * len[0] + axis[1].x * len[1] + axis[2].x * len[2],
    1035                cen.y + axis[0].y * len[0] + axis[1].y * len[1] + axis[2].y * len[2],
    1036                cen.z + axis[0].z * len[0] + axis[1].z * len[1] + axis[2].z * len[2]);
    1037     glVertex3f(cen.x + axis[0].x * len[0] + axis[1].x * len[1] - axis[2].x * len[2],
    1038                cen.y + axis[0].y * len[0] + axis[1].y * len[1] - axis[2].y * len[2],
    1039                cen.z + axis[0].z * len[0] + axis[1].z * len[1] - axis[2].z * len[2]);
    1040     glEnd();
    1041 
    1042 
    1043     if( drawMode & DRAW_BV_BLENDED)
    1044     {
    1045       glBegin(GL_QUADS);
    1046       glVertex3f(cen.x - axis[0].x * len[0] + axis[1].x * len[1] - axis[2].x * len[2],
    1047                  cen.y - axis[0].y * len[0] + axis[1].y * len[1] - axis[2].y * len[2],
    1048                  cen.z - axis[0].z * len[0] + axis[1].z * len[1] - axis[2].z * len[2]);
     758
     759    if( depth == 0 /*!(drawMode & DRAW_SINGLE && depth != 0)*/)
     760    {
     761
     762
     763      Vector cen = this->bvElement->center;
     764      Vector* axis = this->bvElement->axis;
     765      float* len = this->bvElement->halfLength;
     766
     767      if( this->bvElement->bCollided)
     768      {
     769        glColor4f(1.0, 1.0, 1.0, .5); // COLLISION COLOR
     770      }
     771      else if( drawMode & DRAW_BV_BLENDED)
     772      {
     773        glColor4f(color.x, color.y, color.z, .5);
     774      }
     775
     776      // debug out
     777      if( this->obbTree->getOwner() != NULL)
     778      {
     779        PRINTF(4)("debug poly draw: depth: %i, mode: %i, entity-name: %s, class: %s\n", depth, drawMode, this->obbTree->getOwner()->getName(), this->obbTree->getOwner()->getClassName());
     780      }
     781      else
     782        PRINTF(4)("debug poly draw: depth: %i, mode: %i\n", depth, drawMode);
     783
     784
     785      /* draw bounding box */
     786      if( drawMode & DRAW_BV_BLENDED)
     787        glBegin(GL_QUADS);
     788      else
     789        glBegin(GL_LINE_LOOP);
     790      glVertex3f(cen.x + axis[0].x * len[0] + axis[1].x * len[1] + axis[2].x * len[2],
     791                 cen.y + axis[0].y * len[0] + axis[1].y * len[1] + axis[2].y * len[2],
     792                 cen.z + axis[0].z * len[0] + axis[1].z * len[1] + axis[2].z * len[2]);
    1049793      glVertex3f(cen.x + axis[0].x * len[0] + axis[1].x * len[1] - axis[2].x * len[2],
    1050794                 cen.y + axis[0].y * len[0] + axis[1].y * len[1] - axis[2].y * len[2],
    … …  
    1053797                 cen.y + axis[0].y * len[0] - axis[1].y * len[1] - axis[2].y * len[2],
    1054798                 cen.z + axis[0].z * len[0] - axis[1].z * len[1] - axis[2].z * len[2]);
     799      glVertex3f(cen.x + axis[0].x * len[0] - axis[1].x * len[1] + axis[2].x * len[2],
     800                 cen.y + axis[0].y * len[0] - axis[1].y * len[1] + axis[2].y * len[2],
     801                 cen.z + axis[0].z * len[0] - axis[1].z * len[1] + axis[2].z * len[2]);
     802      glEnd();
     803
     804      if( drawMode & DRAW_BV_BLENDED)
     805        glBegin(GL_QUADS);
     806      else
     807        glBegin(GL_LINE_LOOP);
     808      glVertex3f(cen.x + axis[0].x * len[0] - axis[1].x * len[1] + axis[2].x * len[2],
     809                 cen.y + axis[0].y * len[0] - axis[1].y * len[1] + axis[2].y * len[2],
     810                 cen.z + axis[0].z * len[0] - axis[1].z * len[1] + axis[2].z * len[2]);
     811      glVertex3f(cen.x + axis[0].x * len[0] - axis[1].x * len[1] - axis[2].x * len[2],
     812                 cen.y + axis[0].y * len[0] - axis[1].y * len[1] - axis[2].y * len[2],
     813                 cen.z + axis[0].z * len[0] - axis[1].z * len[1] - axis[2].z * len[2]);
    1055814      glVertex3f(cen.x - axis[0].x * len[0] - axis[1].x * len[1] - axis[2].x * len[2],
    1056815                 cen.y - axis[0].y * len[0] - axis[1].y * len[1] - axis[2].y * len[2],
    1057816                 cen.z - axis[0].z * len[0] - axis[1].z * len[1] - axis[2].z * len[2]);
     817      glVertex3f(cen.x - axis[0].x * len[0] - axis[1].x * len[1] + axis[2].x * len[2],
     818                 cen.y - axis[0].y * len[0] - axis[1].y * len[1] + axis[2].y * len[2],
     819                 cen.z - axis[0].z * len[0] - axis[1].z * len[1] + axis[2].z * len[2]);
    1058820      glEnd();
    1059821
    1060       glBegin(GL_QUADS);
     822      if( drawMode & DRAW_BV_BLENDED)
     823        glBegin(GL_QUADS);
     824      else
     825        glBegin(GL_LINE_LOOP);
     826      glVertex3f(cen.x - axis[0].x * len[0] - axis[1].x * len[1] + axis[2].x * len[2],
     827                 cen.y - axis[0].y * len[0] - axis[1].y * len[1] + axis[2].y * len[2],
     828                 cen.z - axis[0].z * len[0] - axis[1].z * len[1] + axis[2].z * len[2]);
     829      glVertex3f(cen.x - axis[0].x * len[0] - axis[1].x * len[1] - axis[2].x * len[2],
     830                 cen.y - axis[0].y * len[0] - axis[1].y * len[1] - axis[2].y * len[2],
     831                 cen.z - axis[0].z * len[0] - axis[1].z * len[1] - axis[2].z * len[2]);
     832      glVertex3f(cen.x - axis[0].x * len[0] + axis[1].x * len[1] - axis[2].x * len[2],
     833                 cen.y - axis[0].y * len[0] + axis[1].y * len[1] - axis[2].y * len[2],
     834                 cen.z - axis[0].z * len[0] + axis[1].z * len[1] - axis[2].z * len[2]);
     835      glVertex3f(cen.x - axis[0].x * len[0] + axis[1].x * len[1] + axis[2].x * len[2],
     836                 cen.y - axis[0].y * len[0] + axis[1].y * len[1] + axis[2].y * len[2],
     837                 cen.z - axis[0].z * len[0] + axis[1].z * len[1] + axis[2].z * len[2]);
     838      glEnd();
     839
     840      if( drawMode & DRAW_BV_BLENDED)
     841        glBegin(GL_QUADS);
     842      else
     843        glBegin(GL_LINE_LOOP);
     844      glVertex3f(cen.x - axis[0].x * len[0] + axis[1].x * len[1] - axis[2].x * len[2],
     845                 cen.y - axis[0].y * len[0] + axis[1].y * len[1] - axis[2].y * len[2],
     846                 cen.z - axis[0].z * len[0] + axis[1].z * len[1] - axis[2].z * len[2]);
    1061847      glVertex3f(cen.x - axis[0].x * len[0] + axis[1].x * len[1] + axis[2].x * len[2],
    1062848                 cen.y - axis[0].y * len[0] + axis[1].y * len[1] + axis[2].y * len[2],
    … …  
    1065851                 cen.y + axis[0].y * len[0] + axis[1].y * len[1] + axis[2].y * len[2],
    1066852                 cen.z + axis[0].z * len[0] + axis[1].z * len[1] + axis[2].z * len[2]);
    1067       glVertex3f(cen.x + axis[0].x * len[0] - axis[1].x * len[1] + axis[2].x * len[2],
    1068                  cen.y + axis[0].y * len[0] - axis[1].y * len[1] + axis[2].y * len[2],
    1069                  cen.z + axis[0].z * len[0] - axis[1].z * len[1] + axis[2].z * len[2]);
    1070       glVertex3f(cen.x - axis[0].x * len[0] - axis[1].x * len[1] + axis[2].x * len[2],
    1071                  cen.y - axis[0].y * len[0] - axis[1].y * len[1] + axis[2].y * len[2],
    1072                  cen.z - axis[0].z * len[0] - axis[1].z * len[1] + axis[2].z * len[2]);
     853      glVertex3f(cen.x + axis[0].x * len[0] + axis[1].x * len[1] - axis[2].x * len[2],
     854                 cen.y + axis[0].y * len[0] + axis[1].y * len[1] - axis[2].y * len[2],
     855                 cen.z + axis[0].z * len[0] + axis[1].z * len[1] - axis[2].z * len[2]);
    1073856      glEnd();
    1074     }
    1075 
    1076 
    1077     if( drawMode & DRAW_BV_BLENDED)
    1078       glColor3f(color.x, color.y, color.z);
    1079     }
    1080 
     857
     858
     859      if( drawMode & DRAW_BV_BLENDED)
     860      {
     861        glBegin(GL_QUADS);
     862        glVertex3f(cen.x - axis[0].x * len[0] + axis[1].x * len[1] - axis[2].x * len[2],
     863                   cen.y - axis[0].y * len[0] + axis[1].y * len[1] - axis[2].y * len[2],
     864                   cen.z - axis[0].z * len[0] + axis[1].z * len[1] - axis[2].z * len[2]);
     865        glVertex3f(cen.x + axis[0].x * len[0] + axis[1].x * len[1] - axis[2].x * len[2],
     866                   cen.y + axis[0].y * len[0] + axis[1].y * len[1] - axis[2].y * len[2],
     867                   cen.z + axis[0].z * len[0] + axis[1].z * len[1] - axis[2].z * len[2]);
     868        glVertex3f(cen.x + axis[0].x * len[0] - axis[1].x * len[1] - axis[2].x * len[2],
     869                   cen.y + axis[0].y * len[0] - axis[1].y * len[1] - axis[2].y * len[2],
     870                   cen.z + axis[0].z * len[0] - axis[1].z * len[1] - axis[2].z * len[2]);
     871        glVertex3f(cen.x - axis[0].x * len[0] - axis[1].x * len[1] - axis[2].x * len[2],
     872                   cen.y - axis[0].y * len[0] - axis[1].y * len[1] - axis[2].y * len[2],
     873                   cen.z - axis[0].z * len[0] - axis[1].z * len[1] - axis[2].z * len[2]);
     874        glEnd();
     875
     876        glBegin(GL_QUADS);
     877        glVertex3f(cen.x - axis[0].x * len[0] + axis[1].x * len[1] + axis[2].x * len[2],
     878                   cen.y - axis[0].y * len[0] + axis[1].y * len[1] + axis[2].y * len[2],
     879                   cen.z - axis[0].z * len[0] + axis[1].z * len[1] + axis[2].z * len[2]);
     880        glVertex3f(cen.x + axis[0].x * len[0] + axis[1].x * len[1] + axis[2].x * len[2],
     881                   cen.y + axis[0].y * len[0] + axis[1].y * len[1] + axis[2].y * len[2],
     882                   cen.z + axis[0].z * len[0] + axis[1].z * len[1] + axis[2].z * len[2]);
     883        glVertex3f(cen.x + axis[0].x * len[0] - axis[1].x * len[1] + axis[2].x * len[2],
     884                   cen.y + axis[0].y * len[0] - axis[1].y * len[1] + axis[2].y * len[2],
     885                   cen.z + axis[0].z * len[0] - axis[1].z * len[1] + axis[2].z * len[2]);
     886        glVertex3f(cen.x - axis[0].x * len[0] - axis[1].x * len[1] + axis[2].x * len[2],
     887                   cen.y - axis[0].y * len[0] - axis[1].y * len[1] + axis[2].y * len[2],
     888                   cen.z - axis[0].z * len[0] - axis[1].z * len[1] + axis[2].z * len[2]);
     889        glEnd();
     890      }
     891
     892      if( drawMode & DRAW_BV_BLENDED)
     893        glColor3f(color.x, color.y, color.z);
     894    }
    1081895  }
    1082896
    … …  
    1089903        glColor4f(color.x, color.y, color.z, .6);
    1090904
    1091     /* now draw the separation plane */
    1092     Vector a1 = this->bvElement->axis[(this->longestAxisIndex + 1)%3];
    1093     Vector a2 = this->bvElement->axis[(this->longestAxisIndex + 2)%3];
    1094     Vector c = *this->bvElement->center;
    1095     float l1 = this->bvElement->halfLength[(this->longestAxisIndex + 1)%3];
    1096     float l2 = this->bvElement->halfLength[(this->longestAxisIndex + 2)%3];
    1097     glBegin(GL_QUADS);
    1098     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);
    1099     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);
    1100     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);
    1101     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);
    1102     glEnd();
    1103 
    1104     if( drawMode & DRAW_BV_BLENDED)
    1105       glColor4f(color.x, color.y, color.z, 1.0);
     905      /* now draw the separation plane */
     906      Vector a1 = this->bvElement->axis[(this->longestAxisIndex + 1)%3];
     907      Vector a2 = this->bvElement->axis[(this->longestAxisIndex + 2)%3];
     908      Vector c = this->bvElement->center;
     909      float l1 = this->bvElement->halfLength[(this->longestAxisIndex + 1)%3];
     910      float l2 = this->bvElement->halfLength[(this->longestAxisIndex + 2)%3];
     911      glBegin(GL_QUADS);
     912      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);
     913      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);
     914      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);
     915      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);
     916      glEnd();
     917
     918      if( drawMode & DRAW_BV_BLENDED)
     919        glColor4f(color.x, color.y, color.z, 1.0);
    1106920
    1107921    }
    … …  
    1127941void OBBTreeNode::debug() const
    1128942{
    1129 
    1130   /*
    1131   for(int i = 0; i < length; i++)
    1132   {
    1133   PRINTF(3)("vertex %i: %f, %f, %f\n", i, verticesList[i][0], verticesList[i][1], verticesList[i][2]);
    1134 }
    1135   */
    1136 }
     943  PRINT(0)("========OBBTreeNode::debug()=====\n");
     944  PRINT(0)(" Current depth: %i", this->depth);
     945  PRINT(0)(" ");
     946  PRINT(0)("=================================\n");
     947}
Note: See TracChangeset for help on using the changeset viewer.