Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

Ignore:
Timestamp:
Dec 3, 2005, 1:31:23 AM (18 years ago)
Author:
patrick
Message:

collision_detection: very much work on the cd engine, now the obbs do assemble again, but not yet correctly. Much more debug work to come..

File:
1 edited

Legend:

Unmodified
Added
Removed

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

    r5870 r5882  
    1717
    1818#include "obb_tree_node.h"
    19 #include "list.h"
    20 #include <list>
     19#include "obb_tree.h"
    2120#include "obb.h"
    22 #include "obb_tree.h"
     21
    2322#include "matrix.h"
    2423#include "abstract_model.h"
    … …  
    2625
    2726#include "color.h"
    28 
     27#include "glincl.h"
     28
     29#include <list>
    2930#include "debug.h"
    30 #include "glincl.h"
    3131
    3232
    … …  
    3434using namespace std;
    3535
    36 float**  OBBTreeNode::coMat = NULL;
    37 float**  OBBTreeNode::eigvMat = NULL;
    38 float*   OBBTreeNode::eigvlMat = NULL;
    39 int*     OBBTreeNode::rotCount = NULL;
     36
    4037GLUquadricObj* OBBTreeNode_sphereObj = NULL;
     38
    4139
    4240/**
    4341 *  standard constructor
     42 * @param tree: reference to the obb tree
     43 * @param depth: the depth of the obb tree to generate
    4444 */
    4545OBBTreeNode::OBBTreeNode (const OBBTree& tree, unsigned int depth)
    … …  
    5555  this->bvElement = NULL;
    5656
    57   this->tmpVert1 = NULL;
    58   this->tmpVert2 = NULL;
    59 
    60   if( OBBTreeNode::coMat == NULL)
    61   {
    62     OBBTreeNode::coMat = new float*[4];
    63     for(int i = 0; i < 4; i++)
    64       OBBTreeNode::coMat[i] = new float[4];
    65   }
    66   if( OBBTreeNode::eigvMat == NULL)
    67   {
    68     OBBTreeNode::eigvMat = new float*[4];
    69     for( int i = 0; i < 4; i++)
    70       OBBTreeNode::eigvMat[i] = new float[4];
    71   }
    72   if( OBBTreeNode::eigvlMat == NULL)
    73   {
    74     OBBTreeNode::eigvlMat = new float[4];
    75   }
    76   if( OBBTreeNode::rotCount == NULL)
    77     OBBTreeNode::rotCount = new int;
     57  this->triangleIndexList1 = NULL;
     58  this->triangleIndexList2 = NULL;
     59
     60  this->modelInf = NULL;
     61  this->triangleIndexes = NULL;
    7862
    7963  if( OBBTreeNode_sphereObj == NULL)
    … …  
    8872{
    8973  if( this->nodeLeft)
    90   {
    9174    delete this->nodeLeft;
    92     this->nodeLeft = NULL;
    93   }
    9475  if( this->nodeRight)
    95   {
    9676    delete this->nodeRight;
    97     this->nodeRight = NULL;
    98   }
     77
    9978  if( this->bvElement)
    10079    delete this->bvElement;
    101   this->bvElement = NULL;
    102 
    103   if (this->tmpVert1 != NULL)
    104     delete this->tmpVert1;
    105   if (this->tmpVert2 != NULL)
    106     delete this->tmpVert2;
     80
     81  if( this->triangleIndexList1 != NULL)
     82    delete [] this->triangleIndexList1;
     83  if( this->triangleIndexList2 != NULL)
     84    delete [] this->triangleIndexList2;
    10785}
    10886
    … …  
    11896void OBBTreeNode::spawnBVTree(const modelInfo& modelInf, const int* triangleIndexes, unsigned int length)
    11997{
    120   sVec3D* verticesList;
    121 
    122   PRINTF(3)("OBB Depth: %i, tree index: %i, numVertices: %i\n", depth, treeIndex, length);
    123   printf("OBBTreeNode::spawnBVTree()\n");
     98  PRINTF(3)("\n==============================Creating OBB Tree Node==================\n");
     99  PRINT(3)(" OBB Tree Infos: \n");
     100  PRINT(3)("\tDepth: %i \n\tTree Index: %i \n\tNumber of Vertices: %i\n", depth, treeIndex, length);
    124101  this->depth = depth;
    125102
    … …  
    137114  if( likely( this->depth > 0))
    138115  {
    139     //this->forkBox(*this->bvElement);
    140 
    141 
    142     //     if(this->tmpLen1 > 2)
    143     //     {
    144     //       OBBTreeNode* node1 = new OBBTreeNode();
    145     //       this->nodeLeft = node1;
    146     //       this->nodeLeft->spawnBVTree(depth - 1, this->tmpVert1, this->tmpLen1);
    147     //     }
    148     //     else
    149     //     {
    150     //       PRINTF(3)("Aboarding tree walk: less than 3 vertices left\n");
    151     //     }
    152     //
    153     //     if( this->tmpLen2 > 2)
    154     //     {
    155     //       OBBTreeNode* node2 = new OBBTreeNode();
    156     //       this->nodeRight = node2;
    157     //       this->nodeRight->spawnBVTree(depth - 1, this->tmpVert2, this->tmpLen2);
    158     //     }
    159     //     else
    160     //     {
    161     //       PRINTF(3)("Abording tree walk: less than 3 vertices left\n");
    162     //     }
    163 
    164   }
    165 }
     116    this->forkBox(*this->bvElement);
     117
     118    if( this->triangleIndexLength1 >= 3)
     119    {
     120      this->nodeLeft = new OBBTreeNode(*this->obbTree, depth - 1);
     121      this->nodeLeft->spawnBVTree(modelInf, this->triangleIndexList1, this->triangleIndexLength1);
     122    }
     123    if( this->triangleIndexLength2 >= 3)
     124    {
     125      this->nodeRight = new OBBTreeNode(*this->obbTree, depth - 1);
     126      this->nodeRight->spawnBVTree(modelInf, this->triangleIndexList2, this->triangleIndexLength2);
     127    }
     128  }
     129}
     130
    166131
    167132
    168133/**
    169  *  creates a new BVTree or BVTree partition
    170  * @param depth: how much more depth-steps to go: if == 1 don't go any deeper!
    171  * @param verticesList: the list of vertices of the object - each vertices triple is interpreted as a triangle
    172  *
    173  * this function creates an Bounding Volume tree from a vertices soup (no triangle data)
     134 *  calculate the box covariance matrix
     135 * @param box: reference to the box
     136 * @param modelInf: the model info structure of the model
     137 * @param tirangleIndexes: an array with the indexes of the triangles inside this
     138 * @param length: the length of the indexes array
    174139 */
    175 void OBBTreeNode::spawnBVTree(const sVec3D *verticesList, unsigned int length)
    176 {
    177   //   PRINTF(3)("\n");
    178   //   PRINTF(3)("OBB Depth: %i, tree index: %i, numVertices: %i\n", depth, treeIndex, length);
    179   //   this->depth = depth;
    180   //
    181   //
    182   //   this->bvElement = new OBB();
    183   //   this->bvElement->vertices = verticesList;
    184   //   this->bvElement->numOfVertices = length;
    185   //   PRINTF(3)("Created OBBox\n");
    186   //   this->calculateBoxCovariance(this->bvElement, verticesList, length);
    187   //   PRINTF(3)("Calculated attributes1\n");
    188   //   this->calculateBoxEigenvectors(this->bvElement, verticesList, length);
    189   //   PRINTF(3)("Calculated attributes2\n");
    190   //   this->calculateBoxAxis(this->bvElement, verticesList, length);
    191   //   PRINTF(3)("Calculated attributes3\n");
    192   //
    193   //
    194   //
    195   //   if( likely( this->depth > 0))
    196   //   {
    197   //     this->forkBox(this->bvElement);
    198   //
    199   //
    200   //     if(this->tmpLen1 > 2)
    201   //     {
    202   //       OBBTreeNode* node1 = new OBBTreeNode(this->obbTree);
    203   //       this->nodeLeft = node1;
    204   //       this->nodeLeft->spawnBVTree(depth - 1, this->tmpVert1, this->tmpLen1);
    205   //     }
    206   //     else
    207   //     {
    208   //       PRINTF(3)("Aboarding tree walk: less than 3 vertices left\n");
    209   //     }
    210   //
    211   //     if( this->tmpLen2 > 2)
    212   //     {
    213   //       OBBTreeNode* node2 = new OBBTreeNode(this->obbTree);
    214   //       this->nodeRight = node2;
    215   //       this->nodeRight->spawnBVTree(depth - 1, this->tmpVert2, this->tmpLen2);
    216   //     }
    217   //     else
    218   //     {
    219   //       PRINTF(3)("Abording tree walk: less than 3 vertices left\n");
    220   //     }
    221   //   }
    222 }
    223 
    224 
    225140void OBBTreeNode::calculateBoxCovariance(OBB& box, const modelInfo& modelInf, const int* triangleIndexes, unsigned int length)
    226141{
    227 
    228   PRINTF(3)("Created OBBox\n");
    229 
    230142  float     facelet[length];                         //!< surface area of the i'th triangle of the convex hull
    231143  float     face = 0.0f;                             //!< surface area of the entire convex hull
    … …  
    288200  for(int j = 0; j < 3; ++j)
    289201  {
    290     PRINTF(3)(" |");
     202    PRINT(3)("\t\t");
    291203    for(int k = 0; k < 3; ++k)
    292204    {
    293       PRINTF(3)(" \b%f ", covariance[j][k]);
    294     }
    295     PRINTF(3)(" |\n");
    296   }
    297   PRINTF(3)("OBB Center: %f, %f, %f\n", center.x, center.y, center.z);
     205      PRINT(3)("%11.2f\t", covariance[j][k]);
     206    }
     207    PRINT(3)("\n");
     208  }
     209  PRINTF(3)("\nOBB Center:\n\t\t%11.2f\t %11.2f\t %11.2f\n", center.x, center.y, center.z);
    298210
    299211  /* write back the covariance matrix data to the object oriented bouning box */
    … …  
    320232{
    321233
    322   PRINTF(3)("Calculate the Box Eigenvectors\n");
    323 
    324234  Vector         axis[3];                            //!< the references to the obb axis
    325235  Matrix         covMat(  box.covarianceMatrix  );   //!< covariance matrix (in the matrix dataform)
    … …  
    339249  box.axis[2] = axis[2];
    340250
    341   PRINTF(0)("-- Got Axis\n");
    342   PRINTF(0)("Eigenvector: %f, %f, %f\n", box.axis[0].x, box.axis[0].y, box.axis[0].z);
    343   PRINTF(0)("Eigenvector: %f, %f, %f\n", box.axis[1].x, box.axis[1].y, box.axis[1].z);
    344   PRINTF(0)("Eigenvector: %f, %f, %f\n", box.axis[2].x, box.axis[2].y, box.axis[2].z);
     251  PRINTF(3)("Eigenvectors:\n");
     252  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);
     253  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);
     254  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);
    345255}
    346256
    … …  
    361271
    362272
    363   PRINTF(3)("Calculated attributes3\n");
     273  PRINTF(3)("Calculate Box Axis\n");
    364274  /* now get the axis length */
    365275  Line                ax[3];                                 //!< the axis
    366276  float               halfLength[3];                         //!< half length of the axis
    367277  float               tmpLength;                             //!< tmp save point for the length
    368   Plane               p0(box.axis[0], box.center);        //!< the axis planes
    369   Plane               p1(box.axis[1], box.center);
    370   Plane               p2(box.axis[2], box.center);
    371   float               maxLength[3];
    372   float               minLength[3];
    373   const sVec3D*       tmpVec;
     278  Plane               p0(box.axis[0], box.center);           //!< the axis planes
     279  Plane               p1(box.axis[1], box.center);           //!< the axis planes
     280  Plane               p2(box.axis[2], box.center);           //!< the axis planes
     281  float               maxLength[3];                          //!< maximal lenth of the axis
     282  float               minLength[3];                          //!< minimal length of the axis
     283  const sVec3D*       tmpVec;                                //!< variable taking tmp vectors
    374284
    375285  /*
    … …  
    476386  for(int i = 0; i < 3; ++i)
    477387  {
    478     PRINTF(3)("max: %f, min: %f \n", maxLength[i], minLength[i]);
    479388    centerOffset[i] = (maxLength[i] + minLength[i]) / 2.0f;       // min length is negatie
    480389    newHalfLength[i] = (maxLength[i] - minLength[i]) / 2.0f;      // min length is negative
    481     box.center +=  (box.axis[i] * centerOffset[i]);            // update the new center vector
     390    box.center +=  (box.axis[i] * centerOffset[i]);               // update the new center vector
    482391    halfLength[i] = newHalfLength[i];
    483392  }
    484 
     393  PRINTF(3)("\n");
     394  PRINT(3)("\tmax: %11.2f, \tmin: %11.2f\n", maxLength[0], minLength[0]);
     395  PRINT(3)("\tmax: %11.2f, \tmin: %11.2f\n", maxLength[1], minLength[1]);
     396  PRINT(3)("\tmax: %11.2f, \tmin: %11.2f\n", maxLength[2], minLength[2]);
    485397
    486398
    … …  
    488400  box.halfLength[1] = halfLength[1];
    489401  box.halfLength[2] = halfLength[2];
    490   PRINTF(3)("-- Written Axis to obb\n");
    491   PRINTF(3)("-- Finished Calculating Attributes\n");
    492402}
    493403
    … …  
    503413{
    504414
     415  PRINTF(3)("Fork Box\n");
     416  PRINTF(4)("Calculating the longest Axis\n");
    505417  /* get the longest axis of the box */
    506   float               aLength = -1.0f;                     //!< the length of the longest axis
    507   int                 axisIndex = 0;                       //!< this is the nr of the longest axis
    508 
    509   for(int i = 0; i < 3; ++i)
    510   {
    511     if( aLength < box.halfLength[i])
    512     {
    513       aLength = box.halfLength[i];
    514       axisIndex = i;
    515     }
    516   }
    517 
    518   PRINTF(3)("longest axis is: nr %i with a half-length of: %f\n", axisIndex, aLength);
    519 
    520 
     418  float               longestAxis = -1.0f;                 //!< the length of the longest axis
     419  int                 longestAxisIndex = 0;                //!< this is the nr of the longest axis
     420
     421
     422  /* now get the longest axis of the three exiting */
     423  for( int i = 0; i < 3; ++i)
     424  {
     425    if( longestAxis < box.halfLength[i])
     426    {
     427      longestAxis = box.halfLength[i];
     428      longestAxisIndex = i;
     429    }
     430  }
     431  PRINTF(3)("\nLongest Axis is: Nr %i with a half-length of:%11.2f\n", longestAxisIndex, longestAxis);
     432
     433
     434  PRINTF(4)("Separating along the longest axis\n");
    521435  /* get the closest vertex near the center */
    522436  float               dist = 999999.0f;                    //!< the smallest distance to each vertex
    523   float               tmpDist;                             //!< temporary distance
    524   int                 vertexIndex;
    525   Plane               middlePlane(box.axis[axisIndex], box.center); //!< the middle plane
    526 
    527   vertexIndex = 0;
    528   for(int i = 0; i < box.numOfVertices; ++i)
    529   {
    530     tmpDist = fabs(middlePlane.distancePoint(box.vertices[i]));
    531     if( tmpDist < dist)
    532     {
    533       dist = tmpDist;
    534       vertexIndex = i;
    535     }
    536   }
    537 
    538   PRINTF(3)("\nthe clostest vertex is nr: %i, with a dist of: %f\n", vertexIndex ,dist);
     437  float               tmpDist;                             //!< variable to save diverse distances temporarily
     438  int                 vertexIndex;                         //!< index of the vertex near the center
     439  Plane               middlePlane(box.axis[longestAxisIndex], box.center); //!< the middle plane
     440  const sVec3D*       tmpVec;                              //!< temp simple 3D vector
    539441
    540442
    … …  
    542444  the points depending on which side they are located
    543445  */
    544   std::list<const sVec3D*> partition1;                           //!< the vertex partition 1
    545   std::list<const sVec3D*> partition2;                           //!< the vertex partition 2
    546 
    547 
    548   //nameList.push_back("Pumba");
    549   //nameList.push_back("Mogli");
    550   //nameList.push_back("Timon");
    551 
    552 //   std::list<char*>::iterator element;
    553 //   for (element = nameList.begin(); element != nameList.end(); element++)
    554 //   {
    555 //     PRINTF(3)("found name: %s in list\n", (*name));
    556 //   }
    557 
    558 
    559 
    560   PRINTF(3)("vertex index: %i, of %i\n", vertexIndex, box.numOfVertices);
    561   this->separationPlane = Plane(box.axis[axisIndex], box.vertices[vertexIndex]);  //!< separation plane
    562   this->sepPlaneCenter = &box.vertices[vertexIndex];
    563   this->longestAxisIndex = axisIndex;
    564 
    565   for(int i = 0; i < box.numOfVertices; ++i)
    566   {
    567     if( i == vertexIndex)
    568       continue;
    569     tmpDist = this->separationPlane.distancePoint(box.vertices[i]);
    570     if( tmpDist > 0.0)
    571       partition1.push_back(&box.vertices[i]); /* positive numbers plus zero */
     446  std::list<int>           partition1;                           //!< the vertex partition 1
     447  std::list<int>           partition2;                           //!< the vertex partition 2
     448  float*                   triangleCenter;                       //!< the center of the triangle
     449  const float*             a;                                    //!< triangle  edge a
     450  const float*             b;                                    //!< triangle  edge b
     451  const float*             c;                                    //!< triangle  edge c
     452
     453  this->separationPlane = Plane(box.axis[longestAxisIndex], box.center);
     454  this->sepPlaneCenter[0] = box.center.x;
     455  this->sepPlaneCenter[1] = box.center.y;
     456  this->sepPlaneCenter[2] = box.center.z;
     457  this->longestAxisIndex = longestAxisIndex;
     458
     459  for( int i = 0; i < box.triangleIndexesLength; ++i)
     460  {
     461    /* first calculate the middle of the triangle */
     462    a = &box.modelInf->pVertices[box.modelInf->pTriangles[box.triangleIndexes[i]].indexToVertices[0]];
     463    b = &box.modelInf->pVertices[box.modelInf->pTriangles[box.triangleIndexes[i]].indexToVertices[1]];
     464    c = &box.modelInf->pVertices[box.modelInf->pTriangles[box.triangleIndexes[i]].indexToVertices[2]];
     465
     466    triangleCenter[0] = (a[0] + b[0] + c[0])/3.0f;
     467    triangleCenter[1] = (a[1] + b[1] + c[1])/3.0f;
     468    triangleCenter[2] = (a[2] + b[2] + c[2])/3.0f;
     469
     470    tmpDist = this->separationPlane.distancePoint(*((sVec3D*)triangleCenter));
     471    if( tmpDist > 0.0f)
     472      partition1.push_back(i); /* positive numbers plus zero */
    572473    else
    573       partition2.push_back(&box.vertices[i]); /* negatice numbers */
    574   }
    575   partition1.push_back(&box.vertices[vertexIndex]);
    576   partition2.push_back(&box.vertices[vertexIndex]);
    577 
    578   PRINTF(3)("\npartition1: got %i vertices/ partition 2: got %i vertices\n", partition1.size(), partition2.size());
     474      partition2.push_back(i); /* negatice numbers */
     475  }
     476  PRINTF(3)("\nPartition1: got \t%i Vertices \nPartition2: got \t%i Vertices\n", partition1.size(), partition2.size());
    579477
    580478
    581479  /* now comes the separation into two different sVec3D arrays */
    582   tIterator<const sVec3D>* iterator;                       //!< the iterator to go through the lists
    583480  int                index;                                //!< index storage place
    584   sVec3D*            vertList1;                            //!< the vertex list 1
    585   sVec3D*            vertList2;                            //!< the vertex list 2
    586   std::list<const sVec3D*>::iterator element;              //!< the list iterator
    587 
    588   vertList1 = new sVec3D[partition1.size()];
    589   vertList2 = new sVec3D[partition2.size()];
    590 
    591 
    592   for(element = partition1.begin(), index = 0; element != partition1.end(); element++, index++)
    593   {
    594     vertList1[index][0] = (*element)[0][0];
    595     vertList1[index][1] = (*element)[0][1];
    596     vertList1[index][2] = (*element)[0][2];
    597     ++index;
    598   }
    599 
    600   //   PRINTF(0)("\npartition 1:\n");
    601   //   for(int i = 0; i < partition1.getSize(); ++i)
    602   //   {
    603   //     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]);
    604   //   }
    605 
    606 
    607 
    608   for(element = partition2.begin(), index = 0; element != partition2.end(); element++, index++)
    609   {
    610     vertList2[index][0] = (*element)[0][0];
    611     vertList2[index][1] = (*element)[0][1];
    612     vertList2[index][2] = (*element)[0][2];
    613   }
    614 
    615   if (this->tmpVert1 != NULL)
    616     delete[] this->tmpVert1;
    617   this->tmpVert1 = vertList1;
    618   if (this->tmpVert2 != NULL)
    619     delete[] this->tmpVert2;
    620   this->tmpVert2 = vertList2;
    621   this->tmpLen1 = partition1.size();
    622   this->tmpLen2 = partition2.size();
    623 
    624 
    625   //   PRINTF(0)("\npartition 2:\n");
    626   //   for(int i = 0; i < partition2.getSize(); ++i)
    627   //   {
    628   //     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]);
    629   //   }
     481  int*               triangleIndexList1;                   //!< the vertex list 1
     482  int*               triangleIndexList2;                   //!< the vertex list 2
     483  std::list<int>::iterator element;                        //!< the list iterator
     484
     485  triangleIndexList1 = new int[partition1.size()];
     486  triangleIndexList2 = new int[partition2.size()];
     487
     488  for( element = partition1.begin(), index = 0; element != partition1.end(); element++, index++)
     489    triangleIndexList1[index] = (*element);
     490
     491  for( element = partition2.begin(), index = 0; element != partition2.end(); element++, index++)
     492    triangleIndexList2[index] = (*element);
     493
     494  if( this->triangleIndexList1!= NULL)
     495    delete[] this->triangleIndexList1;
     496  this->triangleIndexList1 = triangleIndexList1;
     497  this->triangleIndexLength1 = partition1.size();
     498
     499  if( this->triangleIndexList2 != NULL)
     500    delete[] this->triangleIndexList2;
     501  this->triangleIndexList2 = triangleIndexList2;
     502  this->triangleIndexLength2 = partition2.size();
    630503}
    631504
    … …  
    814687      if( drawMode & DRAW_POINTS)
    815688        glBegin(GL_POINTS);
    816       for(int i = 0; i < this->bvElement->numOfVertices; ++i)
     689      for( int i = 0; i < this->bvElement->modelInf->numVertices; i+=3)
    817690      {
    818691        if( drawMode & DRAW_POINTS)
    819           glVertex3f(this->bvElement->vertices[i][0], this->bvElement->vertices[i][1], this->bvElement->vertices[i][2]);
     692          glVertex3f(this->bvElement->modelInf->pVertices[i], this->bvElement->modelInf->pVertices[i+1], this->bvElement->modelInf->pVertices[i+2]);
    820693        else
    821694        {
    822695          glPushMatrix();
    823           glTranslatef(this->bvElement->vertices[i][0], this->bvElement->vertices[i][1], this->bvElement->vertices[i][2]);
     696          glVertex3f(this->bvElement->modelInf->pVertices[i], this->bvElement->modelInf->pVertices[i+1], this->bvElement->modelInf->pVertices[i+2]);
    824697          gluSphere(OBBTreeNode_sphereObj, 0.1, 10, 10);
    825698          glPopMatrix();
Note: See TracChangeset for help on using the changeset viewer.