Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

Changeset 5825 in orxonox.OLD for branches


Ignore:
Timestamp:
Nov 29, 2005, 6:01:26 PM (20 years ago)
Author:
patrick
Message:

collision_detection: some source reformat and function reorg

Location:
branches/collision_detection/src/lib/collision_detection
Files:
2 edited

Legend:

Unmodified
Added
Removed

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

    r5718 r5825  
    4343 */
    4444OBBTreeNode::OBBTreeNode (const OBBTree& tree, unsigned int depth)
    45   : BVTreeNode()
     45    : BVTreeNode()
    4646{
    4747  this->setClassID(CL_OBB_TREE_NODE, "OBBTreeNode");
    … …  
    139139
    140140
    141 //     if(this->tmpLen1 > 2)
    142 //     {
    143 //       OBBTreeNode* node1 = new OBBTreeNode();
    144 //       this->nodeLeft = node1;
    145 //       this->nodeLeft->spawnBVTree(depth - 1, this->tmpVert1, this->tmpLen1);
    146 //     }
    147 //     else
    148 //     {
    149 //       PRINTF(3)("Aboarding tree walk: less than 3 vertices left\n");
    150 //     }
    151 //
    152 //     if( this->tmpLen2 > 2)
    153 //     {
    154 //       OBBTreeNode* node2 = new OBBTreeNode();
    155 //       this->nodeRight = node2;
    156 //       this->nodeRight->spawnBVTree(depth - 1, this->tmpVert2, this->tmpLen2);
    157 //     }
    158 //     else
    159 //     {
    160 //       PRINTF(3)("Abording tree walk: less than 3 vertices left\n");
    161 //     }
     141    //     if(this->tmpLen1 > 2)
     142    //     {
     143    //       OBBTreeNode* node1 = new OBBTreeNode();
     144    //       this->nodeLeft = node1;
     145    //       this->nodeLeft->spawnBVTree(depth - 1, this->tmpVert1, this->tmpLen1);
     146    //     }
     147    //     else
     148    //     {
     149    //       PRINTF(3)("Aboarding tree walk: less than 3 vertices left\n");
     150    //     }
     151    //
     152    //     if( this->tmpLen2 > 2)
     153    //     {
     154    //       OBBTreeNode* node2 = new OBBTreeNode();
     155    //       this->nodeRight = node2;
     156    //       this->nodeRight->spawnBVTree(depth - 1, this->tmpVert2, this->tmpLen2);
     157    //     }
     158    //     else
     159    //     {
     160    //       PRINTF(3)("Abording tree walk: less than 3 vertices left\n");
     161    //     }
    162162
    163163  }
    … …  
    174174void OBBTreeNode::spawnBVTree(const sVec3D *verticesList, unsigned int length)
    175175{
    176 //   PRINTF(3)("\n");
    177 //   PRINTF(3)("OBB Depth: %i, tree index: %i, numVertices: %i\n", depth, treeIndex, length);
    178 //   this->depth = depth;
    179 //
    180 //
    181 //   this->bvElement = new OBB();
    182 //   this->bvElement->vertices = verticesList;
    183 //   this->bvElement->numOfVertices = length;
    184 //   PRINTF(3)("Created OBBox\n");
    185 //   this->calculateBoxCovariance(this->bvElement, verticesList, length);
    186 //   PRINTF(3)("Calculated attributes1\n");
    187 //   this->calculateBoxEigenvectors(this->bvElement, verticesList, length);
    188 //   PRINTF(3)("Calculated attributes2\n");
    189 //   this->calculateBoxAxis(this->bvElement, verticesList, length);
    190 //   PRINTF(3)("Calculated attributes3\n");
    191 //
    192 //
    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(this->obbTree);
    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(this->obbTree);
    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 //   }
     176  //   PRINTF(3)("\n");
     177  //   PRINTF(3)("OBB Depth: %i, tree index: %i, numVertices: %i\n", depth, treeIndex, length);
     178  //   this->depth = depth;
     179  //
     180  //
     181  //   this->bvElement = new OBB();
     182  //   this->bvElement->vertices = verticesList;
     183  //   this->bvElement->numOfVertices = length;
     184  //   PRINTF(3)("Created OBBox\n");
     185  //   this->calculateBoxCovariance(this->bvElement, verticesList, length);
     186  //   PRINTF(3)("Calculated attributes1\n");
     187  //   this->calculateBoxEigenvectors(this->bvElement, verticesList, length);
     188  //   PRINTF(3)("Calculated attributes2\n");
     189  //   this->calculateBoxAxis(this->bvElement, verticesList, length);
     190  //   PRINTF(3)("Calculated attributes3\n");
     191  //
     192  //
     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(this->obbTree);
     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(this->obbTree);
     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  //   }
    221221}
    222222
    223223
    224224void OBBTreeNode::calculateBoxCovariance(OBB& box, const modelInfo& modelInf,
    225                                          const int* triangleIndexes, unsigned int length)
    226 {
    227   const sVec3D* verticesList;
     225    const int* triangleIndexes, unsigned int length)
     226{
    228227
    229228  PRINTF(3)("Created OBBox\n");
    … …  
    251250
    252251    /* finding the facelet surface via cross-product */
    253     t1 = p - q; t2 = p - r;
     252    t1 = p - q;
     253    t2 = p - r;
    254254    facelet[i] = 0.5f * fabs( t1.cross(t2).len() );
    255255    /* update the entire convex hull surface */
    … …  
    263263  /* take the average of the centroid sum */
    264264  center /= face;
    265   PRINTF(3)("-- Calculated Center\n");
    266265
    267266
    … …  
    282281
    283282        covariance[j][k] = facelet[i] / (12.0f * face) * (9.0f * centroid[i][j] * centroid[i][k] + p[j] * p[k] +
    284             q[j] * q[k] + r[j] * r[k]) - center[j] * center[k];
     283                           q[j] * q[k] + r[j] * r[k]) - center[j] * center[k];
    285284      }
    286285    }
    287286  }
    288   PRINTF(3)("-- Calculated Covariance\n");
    289 
    290 
    291   PRINTF(3)("\nVertex Data:\n");
    292   for(int i = 0; i < length; i++)
    293     PRINTF(3)(" Vertex[%i]: %f, %f, %f\n", i, box.vertices[i][0], box.vertices[i][1], box.vertices[i][2]);
     287
    294288  PRINTF(3)("\nOBB Covariance Matrix:\n");
    295   for(int j = 0; j < 3; ++j) { PRINTF(3)(" |"); for(int k = 0; k < 3; ++k) { PRINTF(3)(" \b%f ", covariance[j][k]); } PRINTF(3)(" |\n"); }
     289  for(int j = 0; j < 3; ++j)
     290  {
     291    PRINTF(3)(" |");
     292    for(int k = 0; k < 3; ++k)
     293    {
     294      PRINTF(3)(" \b%f ", covariance[j][k]);
     295    }
     296    PRINTF(3)(" |\n");
     297  }
    296298  PRINTF(3)("OBB Center: %f, %f, %f\n", center.x, center.y, center.z);
    297299
    298 
     300  /* write back the covariance matrix data to the object oriented bouning box */
    299301  for(int i = 0; i < 3; ++i)
    300302  {
    … …  
    304306  }
    305307  box.center = center;
    306 
    307   PRINTF(3)("-- Written Result to OBB\n");
    308308}
    309309
    310310
    311311void OBBTreeNode::calculateBoxEigenvectors(OBB& box, const modelInfo& modInfo,
    312                                            const int* triangleIndexes, unsigned int length)
    313 {}
    314 
    315 void OBBTreeNode::calculateBoxEigenvectors(OBB& box, const sVec3D* verticesList, unsigned int length)
    316 {
    317   PRINTF(3)("Calculated attributes1\n");
    318   /* now getting spanning vectors of the sub-space:
     312    const int* triangleIndexes, unsigned int length)
     313{
     314
     315  PRINTF(3)("Calculate the Box Eigenvectors\n");
     316
     317  const sVec3D*  verticesList;
     318  Vector         axis[3];                            //!< the references to the obb axis
     319  Matrix         covMat(  box.covarianceMatrix  );   //!< covariance matrix (in the matrix dataform)
     320
     321  /*
     322  now getting spanning vectors of the sub-space:
    319323  the eigenvectors of a symmertric matrix, such as the
    320324  covarience matrix are mutually orthogonal.
    … …  
    322326  vectors
    323327  */
    324   Vector axis[3];                //!< the references to the obb axis
    325 
    326   Matrix covMat(  box.covarianceMatrix  );
     328
     329  /* calculate the axis */
    327330  covMat.getEigenVectors(axis[0], axis[1], axis[2] );
    328 
    329 
    330   /* new jacobi tests */
    331 //  JacobI(OBBTreeNode::coMat, OBBTreeNode::eigvlMat, OBBTreeNode::eigvMat, OBBTreeNode::rotCount);
    332 //  PRINTF(3)("-- Done Jacobi Decomposition\n");
    333 
    334 
    335 //   PRINTF(0)("Jacobi\n");
    336 //   for(int j = 0; j < 3; ++j)
    337 //   {
    338 //     printf(" |");
    339 //     for(int k = 0; k < 3; ++k)
    340 //     {
    341 //       printf(" \t%f ", OBBTreeNode::OBBTreeNode::eigvMat[j][k]);
    342 //     }
    343 //     printf(" |\n");
    344 //   }
    345 
    346 /*  axis[0].x = OBBTreeNode::eigvMat[0][0]; axis[0].y = OBBTreeNode::eigvMat[1][0]; axis[0].z = OBBTreeNode::eigvMat[2][0];
    347   axis[1].x = OBBTreeNode::eigvMat[0][1]; axis[1].y = OBBTreeNode::eigvMat[1][1]; axis[1].z = OBBTreeNode::eigvMat[2][1];
    348   axis[2].x = OBBTreeNode::eigvMat[0][2]; axis[2].y = OBBTreeNode::eigvMat[1][2]; axis[2].z = OBBTreeNode::eigvMat[2][2];
    349   axis[0].normalize();
    350   axis[1].normalize();
    351   axis[2].normalize();*/
    352331  box.axis[0] = axis[0];
    353332  box.axis[1] = axis[1];
    354333  box.axis[2] = axis[2];
    355334
    356 //   PRINTF(0)("-- Got Axis\n");
    357 //
    358 //   PRINTF(0)("eigenvector: %f, %f, %f\n", box.axis[0].x, box.axis[0].y, box.axis[0].z);
    359 //   PRINTF(0)("eigenvector: %f, %f, %f\n", box.axis[1].x, box.axis[1].y, box.axis[1].z);
    360 //   PRINTF(0)("eigenvector: %f, %f, %f\n", box.axis[2].x, box.axis[2].y, box.axis[2].z);
     335  PRINTF(0)("-- Got Axis\n");
     336  PRINTF(0)("Eigenvector: %f, %f, %f\n", box.axis[0].x, box.axis[0].y, box.axis[0].z);
     337  PRINTF(0)("Eigenvector: %f, %f, %f\n", box.axis[1].x, box.axis[1].y, box.axis[1].z);
     338  PRINTF(0)("Eigenvector: %f, %f, %f\n", box.axis[2].x, box.axis[2].y, box.axis[2].z);
    361339}
    362340
    … …  
    386364  halfLength[0] = -1.0f;
    387365  for(int j = 0; j < length; ++j)
    388     {
    389       tmpLength = fabs(p0.distancePoint(vertices[j]));
    390       if( tmpLength > halfLength[0])
    391         halfLength[0] = tmpLength;
    392     }
     366  {
     367    tmpLength = fabs(p0.distancePoint(vertices[j]));
     368    if( tmpLength > halfLength[0])
     369      halfLength[0] = tmpLength;
     370  }
    393371
    394372  halfLength[1] = -1.0f;
    395373  for(int j = 0; j < length; ++j)
    396     {
    397       tmpLength = fabs(p1.distancePoint(vertices[j]));
    398       if( tmpLength > halfLength[1])
    399         halfLength[1] = tmpLength;
    400     }
     374  {
     375    tmpLength = fabs(p1.distancePoint(vertices[j]));
     376    if( tmpLength > halfLength[1])
     377      halfLength[1] = tmpLength;
     378  }
    401379
    402380  halfLength[2] = -1.0f;
    403381  for(int j = 0; j < length; ++j)
    404     {
    405       tmpLength = fabs(p2.distancePoint(vertices[j]));
    406       if( tmpLength > halfLength[2])
    407         halfLength[2] = tmpLength;
    408     }
     382  {
     383    tmpLength = fabs(p2.distancePoint(vertices[j]));
     384    if( tmpLength > halfLength[2])
     385      halfLength[2] = tmpLength;
     386  }
    409387
    410388
    411389
    412390  /* get the maximal dimensions of the body in all directions */
    413     maxLength[0] = p0.distancePoint(vertices[0]);
    414     minLength[0] = p0.distancePoint(vertices[0]);
    415    for(int j = 0; j < length; ++j)
    416    {
    417      tmpLength = p0.distancePoint(vertices[j]);
    418      if( tmpLength > maxLength[0])
    419        maxLength[0] = tmpLength;
    420      else if( tmpLength < minLength[0])
    421        minLength[0] = tmpLength;
    422    }
    423 
    424    maxLength[1] = p1.distancePoint(vertices[0]);
    425    minLength[1] = p1.distancePoint(vertices[0]);
    426    for(int j = 0; j < length; ++j)
    427    {
    428      tmpLength = p1.distancePoint(vertices[j]);
    429      if( tmpLength > maxLength[1])
    430        maxLength[1] = tmpLength;
    431      else if( tmpLength < minLength[1])
    432        minLength[1] = tmpLength;
    433    }
    434 
    435    maxLength[2] = p2.distancePoint(vertices[0]);
    436    minLength[2] = p2.distancePoint(vertices[0]);
    437    for(int j = 0; j < length; ++j)
    438    {
    439      tmpLength = p2.distancePoint(vertices[j]);
    440      if( tmpLength > maxLength[2])
    441        maxLength[2] = tmpLength;
    442      else if( tmpLength < minLength[2])
    443        minLength[2] = tmpLength;
    444    }
    445 
    446 
    447    /* calculate the real centre of the body by using the axis length */
    448    float centerOffset[3];
    449    float newHalfLength[3];
    450    for(int i = 0; i < 3; ++i)
    451      {
    452        PRINTF(3)("max: %f, min: %f \n", maxLength[i], minLength[i]);
    453        centerOffset[i] = (maxLength[i] + minLength[i]) / 2.0f;       // min length is negatie
    454        newHalfLength[i] = (maxLength[i] - minLength[i]) / 2.0f;      // min length is negative
    455        box.center +=  (box.axis[i] * centerOffset[i]);            // update the new center vector
    456        halfLength[i] = newHalfLength[i];
    457      }
     391  maxLength[0] = p0.distancePoint(vertices[0]);
     392  minLength[0] = p0.distancePoint(vertices[0]);
     393  for(int j = 0; j < length; ++j)
     394  {
     395    tmpLength = p0.distancePoint(vertices[j]);
     396    if( tmpLength > maxLength[0])
     397      maxLength[0] = tmpLength;
     398    else if( tmpLength < minLength[0])
     399      minLength[0] = tmpLength;
     400  }
     401
     402  maxLength[1] = p1.distancePoint(vertices[0]);
     403  minLength[1] = p1.distancePoint(vertices[0]);
     404  for(int j = 0; j < length; ++j)
     405  {
     406    tmpLength = p1.distancePoint(vertices[j]);
     407    if( tmpLength > maxLength[1])
     408      maxLength[1] = tmpLength;
     409    else if( tmpLength < minLength[1])
     410      minLength[1] = tmpLength;
     411  }
     412
     413  maxLength[2] = p2.distancePoint(vertices[0]);
     414  minLength[2] = p2.distancePoint(vertices[0]);
     415  for(int j = 0; j < length; ++j)
     416  {
     417    tmpLength = p2.distancePoint(vertices[j]);
     418    if( tmpLength > maxLength[2])
     419      maxLength[2] = tmpLength;
     420    else if( tmpLength < minLength[2])
     421      minLength[2] = tmpLength;
     422  }
     423
     424
     425  /* calculate the real centre of the body by using the axis length */
     426  float centerOffset[3];
     427  float newHalfLength[3];
     428  for(int i = 0; i < 3; ++i)
     429  {
     430    PRINTF(3)("max: %f, min: %f \n", maxLength[i], minLength[i]);
     431    centerOffset[i] = (maxLength[i] + minLength[i]) / 2.0f;       // min length is negatie
     432    newHalfLength[i] = (maxLength[i] - minLength[i]) / 2.0f;      // min length is negative
     433    box.center +=  (box.axis[i] * centerOffset[i]);            // update the new center vector
     434    halfLength[i] = newHalfLength[i];
     435  }
    458436
    459437
    … …  
    489467  }
    490468
    491    PRINTF(3)("longest axis is: nr %i with a half-length of: %f\n", axisIndex, aLength);
     469  PRINTF(3)("longest axis is: nr %i with a half-length of: %f\n", axisIndex, aLength);
    492470
    493471
    … …  
    526504  for(int i = 0; i < box.numOfVertices; ++i)
    527505  {
    528     if( i == vertexIndex) continue;
     506    if( i == vertexIndex)
     507      continue;
    529508    tmpDist = this->separationPlane.distancePoint(box.vertices[i]);
    530509    if( tmpDist > 0.0)
    … …  
    561540  }
    562541  delete iterator;
    563 //   PRINTF(0)("\npartition 1:\n");
    564 //   for(int i = 0; i < partition1.getSize(); ++i)
    565 //   {
    566 //     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]);
    567 //   }
     542  //   PRINTF(0)("\npartition 1:\n");
     543  //   for(int i = 0; i < partition1.getSize(); ++i)
     544  //   {
     545  //     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]);
     546  //   }
    568547
    569548  iterator = partition2.getIterator();
    … …  
    590569  delete iterator;
    591570
    592 //   PRINTF(0)("\npartition 2:\n");
    593 //   for(int i = 0; i < partition2.getSize(); ++i)
    594 //   {
    595 //     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]);
    596 //   }
     571  //   PRINTF(0)("\npartition 2:\n");
     572  //   for(int i = 0; i < partition2.getSize(); ++i)
     573  //   {
     574  //     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]);
     575  //   }
    597576}
    598577
    … …  
    605584  /* if the obb overlap, make subtests: check which node is realy overlaping  */
    606585  PRINTF(3)("Checking OBB %i vs %i: ", this->getIndex(), treeNode.getIndex());
    607 //   if( unlikely(treeNode == NULL)) return;
     586  //   if( unlikely(treeNode == NULL)) return;
    608587
    609588
    … …  
    649628bool OBBTreeNode::overlapTest(const OBB& boxA, const OBB& boxB, const WorldEntity& nodeA, const WorldEntity& nodeB) const
    650629{
    651 //   if( boxB == NULL || boxA == NULL)
    652 //     return false;
     630  //   if( boxB == NULL || boxA == NULL)
     631  //     return false;
    653632
    654633  /* first check all axis */
    … …  
    671650  t = nodeA.getAbsCoor() + nodeA.getAbsDir().apply(boxA.center) - ( nodeB.getAbsCoor() + nodeB.getAbsDir().apply(boxB.center));
    672651
    673 //   printf("\n");
    674 //   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);
    675 //   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);
    676 //   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);
    677 //
    678 //   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);
    679 //   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);
    680 //   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);
     652  //   printf("\n");
     653  //   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);
     654  //   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);
     655  //   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);
     656  //
     657  //   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);
     658  //   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);
     659  //   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);
    681660
    682661
    … …  
    759738
    760739  /* FIXME: there is no collision mark set now */
    761 //   boxA.bCollided = true; /* use this ONLY(!!!!) for drawing operations */
    762 //   boxB.bCollided = true;
     740  //   boxA.bCollided = true; /* use this ONLY(!!!!) for drawing operations */
     741  //   boxB.bCollided = true;
    763742
    764743
    … …  
    865844    if( !(drawMode & DRAW_SINGLE && depth != 0))
    866845    {
    867     Vector cen = this->bvElement->center;
    868     Vector* axis = this->bvElement->axis;
    869     float* len = this->bvElement->halfLength;
    870 
    871     if( this->bvElement->bCollided)
    872     {
    873       glColor4f(1.0, 1.0, 1.0, .5); // COLLISION COLOR
    874     }
    875     else if( drawMode & DRAW_BV_BLENDED)
    876     {
    877       glColor4f(color.x, color.y, color.z, .5);
    878     }
    879 
    880     /* draw bounding box */
    881     if( drawMode & DRAW_BV_BLENDED)
    882       glBegin(GL_QUADS);
    883     else
    884       glBegin(GL_LINE_LOOP);
    885     glVertex3f(cen.x + axis[0].x * len[0] + axis[1].x * len[1] + axis[2].x * len[2],
    886                cen.y + axis[0].y * len[0] + axis[1].y * len[1] + axis[2].y * len[2],
    887                cen.z + axis[0].z * len[0] + axis[1].z * len[1] + axis[2].z * len[2]);
    888     glVertex3f(cen.x + axis[0].x * len[0] + axis[1].x * len[1] - axis[2].x * len[2],
    889                cen.y + axis[0].y * len[0] + axis[1].y * len[1] - axis[2].y * len[2],
    890                cen.z + axis[0].z * len[0] + axis[1].z * len[1] - axis[2].z * len[2]);
    891     glVertex3f(cen.x + axis[0].x * len[0] - axis[1].x * len[1] - axis[2].x * len[2],
    892                cen.y + axis[0].y * len[0] - axis[1].y * len[1] - axis[2].y * len[2],
    893                cen.z + axis[0].z * len[0] - axis[1].z * len[1] - axis[2].z * len[2]);
    894     glVertex3f(cen.x + axis[0].x * len[0] - axis[1].x * len[1] + axis[2].x * len[2],
    895                cen.y + axis[0].y * len[0] - axis[1].y * len[1] + axis[2].y * len[2],
    896                cen.z + axis[0].z * len[0] - axis[1].z * len[1] + axis[2].z * len[2]);
    897     glEnd();
    898 
    899     if( drawMode & DRAW_BV_BLENDED)
    900       glBegin(GL_QUADS);
    901     else
    902       glBegin(GL_LINE_LOOP);
    903     glVertex3f(cen.x + axis[0].x * len[0] - axis[1].x * len[1] + axis[2].x * len[2],
    904                cen.y + axis[0].y * len[0] - axis[1].y * len[1] + axis[2].y * len[2],
    905                cen.z + axis[0].z * len[0] - axis[1].z * len[1] + axis[2].z * len[2]);
    906     glVertex3f(cen.x + axis[0].x * len[0] - axis[1].x * len[1] - axis[2].x * len[2],
    907                cen.y + axis[0].y * len[0] - axis[1].y * len[1] - axis[2].y * len[2],
    908                cen.z + axis[0].z * len[0] - axis[1].z * len[1] - axis[2].z * len[2]);
    909     glVertex3f(cen.x - axis[0].x * len[0] - axis[1].x * len[1] - axis[2].x * len[2],
    910                cen.y - axis[0].y * len[0] - axis[1].y * len[1] - axis[2].y * len[2],
    911                cen.z - axis[0].z * len[0] - axis[1].z * len[1] - axis[2].z * len[2]);
    912     glVertex3f(cen.x - axis[0].x * len[0] - axis[1].x * len[1] + axis[2].x * len[2],
    913                cen.y - axis[0].y * len[0] - axis[1].y * len[1] + axis[2].y * len[2],
    914                cen.z - axis[0].z * len[0] - axis[1].z * len[1] + axis[2].z * len[2]);
    915     glEnd();
    916 
    917     if( drawMode & DRAW_BV_BLENDED)
    918       glBegin(GL_QUADS);
    919     else
    920       glBegin(GL_LINE_LOOP);
    921     glVertex3f(cen.x - axis[0].x * len[0] - axis[1].x * len[1] + axis[2].x * len[2],
    922                cen.y - axis[0].y * len[0] - axis[1].y * len[1] + axis[2].y * len[2],
    923                cen.z - axis[0].z * len[0] - axis[1].z * len[1] + axis[2].z * len[2]);
    924     glVertex3f(cen.x - axis[0].x * len[0] - axis[1].x * len[1] - axis[2].x * len[2],
    925                cen.y - axis[0].y * len[0] - axis[1].y * len[1] - axis[2].y * len[2],
    926                cen.z - axis[0].z * len[0] - axis[1].z * len[1] - axis[2].z * len[2]);
    927     glVertex3f(cen.x - axis[0].x * len[0] + axis[1].x * len[1] - axis[2].x * len[2],
    928                cen.y - axis[0].y * len[0] + axis[1].y * len[1] - axis[2].y * len[2],
    929                cen.z - axis[0].z * len[0] + axis[1].z * len[1] - axis[2].z * len[2]);
    930     glVertex3f(cen.x - axis[0].x * len[0] + axis[1].x * len[1] + axis[2].x * len[2],
    931                cen.y - axis[0].y * len[0] + axis[1].y * len[1] + axis[2].y * len[2],
    932                cen.z - axis[0].z * len[0] + axis[1].z * len[1] + axis[2].z * len[2]);
    933     glEnd();
    934 
    935     if( drawMode & DRAW_BV_BLENDED)
    936       glBegin(GL_QUADS);
    937     else
    938       glBegin(GL_LINE_LOOP);
    939     glVertex3f(cen.x - axis[0].x * len[0] + axis[1].x * len[1] - axis[2].x * len[2],
    940                cen.y - axis[0].y * len[0] + axis[1].y * len[1] - axis[2].y * len[2],
    941                cen.z - axis[0].z * len[0] + axis[1].z * len[1] - axis[2].z * len[2]);
    942     glVertex3f(cen.x - axis[0].x * len[0] + axis[1].x * len[1] + axis[2].x * len[2],
    943                cen.y - axis[0].y * len[0] + axis[1].y * len[1] + axis[2].y * len[2],
    944                cen.z - axis[0].z * len[0] + axis[1].z * len[1] + axis[2].z * len[2]);
    945     glVertex3f(cen.x + axis[0].x * len[0] + axis[1].x * len[1] + axis[2].x * len[2],
    946                cen.y + axis[0].y * len[0] + axis[1].y * len[1] + axis[2].y * len[2],
    947                cen.z + axis[0].z * len[0] + axis[1].z * len[1] + axis[2].z * len[2]);
    948     glVertex3f(cen.x + axis[0].x * len[0] + axis[1].x * len[1] - axis[2].x * len[2],
    949                cen.y + axis[0].y * len[0] + axis[1].y * len[1] - axis[2].y * len[2],
    950                cen.z + axis[0].z * len[0] + axis[1].z * len[1] - axis[2].z * len[2]);
    951     glEnd();
    952 
    953 
    954     if( drawMode & DRAW_BV_BLENDED)
    955     {
    956       glBegin(GL_QUADS);
    957       glVertex3f(cen.x - axis[0].x * len[0] + axis[1].x * len[1] - axis[2].x * len[2],
    958                  cen.y - axis[0].y * len[0] + axis[1].y * len[1] - axis[2].y * len[2],
    959                  cen.z - axis[0].z * len[0] + axis[1].z * len[1] - axis[2].z * len[2]);
     846      Vector cen = this->bvElement->center;
     847      Vector* axis = this->bvElement->axis;
     848      float* len = this->bvElement->halfLength;
     849
     850      if( this->bvElement->bCollided)
     851      {
     852        glColor4f(1.0, 1.0, 1.0, .5); // COLLISION COLOR
     853      }
     854      else if( drawMode & DRAW_BV_BLENDED)
     855      {
     856        glColor4f(color.x, color.y, color.z, .5);
     857      }
     858
     859      /* draw bounding box */
     860      if( drawMode & DRAW_BV_BLENDED)
     861        glBegin(GL_QUADS);
     862      else
     863        glBegin(GL_LINE_LOOP);
     864      glVertex3f(cen.x + axis[0].x * len[0] + axis[1].x * len[1] + axis[2].x * len[2],
     865                 cen.y + axis[0].y * len[0] + axis[1].y * len[1] + axis[2].y * len[2],
     866                 cen.z + axis[0].z * len[0] + axis[1].z * len[1] + axis[2].z * len[2]);
    960867      glVertex3f(cen.x + axis[0].x * len[0] + axis[1].x * len[1] - axis[2].x * len[2],
    961868                 cen.y + axis[0].y * len[0] + axis[1].y * len[1] - axis[2].y * len[2],
    … …  
    964871                 cen.y + axis[0].y * len[0] - axis[1].y * len[1] - axis[2].y * len[2],
    965872                 cen.z + axis[0].z * len[0] - axis[1].z * len[1] - axis[2].z * len[2]);
     873      glVertex3f(cen.x + axis[0].x * len[0] - axis[1].x * len[1] + axis[2].x * len[2],
     874                 cen.y + axis[0].y * len[0] - axis[1].y * len[1] + axis[2].y * len[2],
     875                 cen.z + axis[0].z * len[0] - axis[1].z * len[1] + axis[2].z * len[2]);
     876      glEnd();
     877
     878      if( drawMode & DRAW_BV_BLENDED)
     879        glBegin(GL_QUADS);
     880      else
     881        glBegin(GL_LINE_LOOP);
     882      glVertex3f(cen.x + axis[0].x * len[0] - axis[1].x * len[1] + axis[2].x * len[2],
     883                 cen.y + axis[0].y * len[0] - axis[1].y * len[1] + axis[2].y * len[2],
     884                 cen.z + axis[0].z * len[0] - axis[1].z * len[1] + axis[2].z * len[2]);
     885      glVertex3f(cen.x + axis[0].x * len[0] - axis[1].x * len[1] - axis[2].x * len[2],
     886                 cen.y + axis[0].y * len[0] - axis[1].y * len[1] - axis[2].y * len[2],
     887                 cen.z + axis[0].z * len[0] - axis[1].z * len[1] - axis[2].z * len[2]);
    966888      glVertex3f(cen.x - axis[0].x * len[0] - axis[1].x * len[1] - axis[2].x * len[2],
    967889                 cen.y - axis[0].y * len[0] - axis[1].y * len[1] - axis[2].y * len[2],
    968890                 cen.z - axis[0].z * len[0] - axis[1].z * len[1] - axis[2].z * len[2]);
     891      glVertex3f(cen.x - axis[0].x * len[0] - axis[1].x * len[1] + axis[2].x * len[2],
     892                 cen.y - axis[0].y * len[0] - axis[1].y * len[1] + axis[2].y * len[2],
     893                 cen.z - axis[0].z * len[0] - axis[1].z * len[1] + axis[2].z * len[2]);
    969894      glEnd();
    970895
    971       glBegin(GL_QUADS);
     896      if( drawMode & DRAW_BV_BLENDED)
     897        glBegin(GL_QUADS);
     898      else
     899        glBegin(GL_LINE_LOOP);
     900      glVertex3f(cen.x - axis[0].x * len[0] - axis[1].x * len[1] + axis[2].x * len[2],
     901                 cen.y - axis[0].y * len[0] - axis[1].y * len[1] + axis[2].y * len[2],
     902                 cen.z - axis[0].z * len[0] - axis[1].z * len[1] + axis[2].z * len[2]);
     903      glVertex3f(cen.x - axis[0].x * len[0] - axis[1].x * len[1] - axis[2].x * len[2],
     904                 cen.y - axis[0].y * len[0] - axis[1].y * len[1] - axis[2].y * len[2],
     905                 cen.z - axis[0].z * len[0] - axis[1].z * len[1] - axis[2].z * len[2]);
     906      glVertex3f(cen.x - axis[0].x * len[0] + axis[1].x * len[1] - axis[2].x * len[2],
     907                 cen.y - axis[0].y * len[0] + axis[1].y * len[1] - axis[2].y * len[2],
     908                 cen.z - axis[0].z * len[0] + axis[1].z * len[1] - axis[2].z * len[2]);
     909      glVertex3f(cen.x - axis[0].x * len[0] + axis[1].x * len[1] + axis[2].x * len[2],
     910                 cen.y - axis[0].y * len[0] + axis[1].y * len[1] + axis[2].y * len[2],
     911                 cen.z - axis[0].z * len[0] + axis[1].z * len[1] + axis[2].z * len[2]);
     912      glEnd();
     913
     914      if( drawMode & DRAW_BV_BLENDED)
     915        glBegin(GL_QUADS);
     916      else
     917        glBegin(GL_LINE_LOOP);
     918      glVertex3f(cen.x - axis[0].x * len[0] + axis[1].x * len[1] - axis[2].x * len[2],
     919                 cen.y - axis[0].y * len[0] + axis[1].y * len[1] - axis[2].y * len[2],
     920                 cen.z - axis[0].z * len[0] + axis[1].z * len[1] - axis[2].z * len[2]);
    972921      glVertex3f(cen.x - axis[0].x * len[0] + axis[1].x * len[1] + axis[2].x * len[2],
    973922                 cen.y - axis[0].y * len[0] + axis[1].y * len[1] + axis[2].y * len[2],
    … …  
    976925                 cen.y + axis[0].y * len[0] + axis[1].y * len[1] + axis[2].y * len[2],
    977926                 cen.z + axis[0].z * len[0] + axis[1].z * len[1] + axis[2].z * len[2]);
    978       glVertex3f(cen.x + axis[0].x * len[0] - axis[1].x * len[1] + axis[2].x * len[2],
    979                  cen.y + axis[0].y * len[0] - axis[1].y * len[1] + axis[2].y * len[2],
    980                  cen.z + axis[0].z * len[0] - axis[1].z * len[1] + axis[2].z * len[2]);
    981       glVertex3f(cen.x - axis[0].x * len[0] - axis[1].x * len[1] + axis[2].x * len[2],
    982                  cen.y - axis[0].y * len[0] - axis[1].y * len[1] + axis[2].y * len[2],
    983                  cen.z - axis[0].z * len[0] - axis[1].z * len[1] + axis[2].z * len[2]);
     927      glVertex3f(cen.x + axis[0].x * len[0] + axis[1].x * len[1] - axis[2].x * len[2],
     928                 cen.y + axis[0].y * len[0] + axis[1].y * len[1] - axis[2].y * len[2],
     929                 cen.z + axis[0].z * len[0] + axis[1].z * len[1] - axis[2].z * len[2]);
    984930      glEnd();
    985     }
    986 
    987 
    988     if( drawMode & DRAW_BV_BLENDED)
    989       glColor3f(color.x, color.y, color.z);
     931
     932
     933      if( drawMode & DRAW_BV_BLENDED)
     934      {
     935        glBegin(GL_QUADS);
     936        glVertex3f(cen.x - axis[0].x * len[0] + axis[1].x * len[1] - axis[2].x * len[2],
     937                   cen.y - axis[0].y * len[0] + axis[1].y * len[1] - axis[2].y * len[2],
     938                   cen.z - axis[0].z * len[0] + axis[1].z * len[1] - axis[2].z * len[2]);
     939        glVertex3f(cen.x + axis[0].x * len[0] + axis[1].x * len[1] - axis[2].x * len[2],
     940                   cen.y + axis[0].y * len[0] + axis[1].y * len[1] - axis[2].y * len[2],
     941                   cen.z + axis[0].z * len[0] + axis[1].z * len[1] - axis[2].z * len[2]);
     942        glVertex3f(cen.x + axis[0].x * len[0] - axis[1].x * len[1] - axis[2].x * len[2],
     943                   cen.y + axis[0].y * len[0] - axis[1].y * len[1] - axis[2].y * len[2],
     944                   cen.z + axis[0].z * len[0] - axis[1].z * len[1] - axis[2].z * len[2]);
     945        glVertex3f(cen.x - axis[0].x * len[0] - axis[1].x * len[1] - axis[2].x * len[2],
     946                   cen.y - axis[0].y * len[0] - axis[1].y * len[1] - axis[2].y * len[2],
     947                   cen.z - axis[0].z * len[0] - axis[1].z * len[1] - axis[2].z * len[2]);
     948        glEnd();
     949
     950        glBegin(GL_QUADS);
     951        glVertex3f(cen.x - axis[0].x * len[0] + axis[1].x * len[1] + axis[2].x * len[2],
     952                   cen.y - axis[0].y * len[0] + axis[1].y * len[1] + axis[2].y * len[2],
     953                   cen.z - axis[0].z * len[0] + axis[1].z * len[1] + axis[2].z * len[2]);
     954        glVertex3f(cen.x + axis[0].x * len[0] + axis[1].x * len[1] + axis[2].x * len[2],
     955                   cen.y + axis[0].y * len[0] + axis[1].y * len[1] + axis[2].y * len[2],
     956                   cen.z + axis[0].z * len[0] + axis[1].z * len[1] + axis[2].z * len[2]);
     957        glVertex3f(cen.x + axis[0].x * len[0] - axis[1].x * len[1] + axis[2].x * len[2],
     958                   cen.y + axis[0].y * len[0] - axis[1].y * len[1] + axis[2].y * len[2],
     959                   cen.z + axis[0].z * len[0] - axis[1].z * len[1] + axis[2].z * len[2]);
     960        glVertex3f(cen.x - axis[0].x * len[0] - axis[1].x * len[1] + axis[2].x * len[2],
     961                   cen.y - axis[0].y * len[0] - axis[1].y * len[1] + axis[2].y * len[2],
     962                   cen.z - axis[0].z * len[0] - axis[1].z * len[1] + axis[2].z * len[2]);
     963        glEnd();
     964      }
     965
     966
     967      if( drawMode & DRAW_BV_BLENDED)
     968        glColor3f(color.x, color.y, color.z);
    990969    }
    991970
    … …  
    1000979        glColor4f(color.x, color.y, color.z, .6);
    1001980
    1002     /* now draw the separation plane */
    1003     Vector a1 = this->bvElement->axis[(this->longestAxisIndex + 1)%3];
    1004     Vector a2 = this->bvElement->axis[(this->longestAxisIndex + 2)%3];
    1005     Vector c = this->bvElement->center;
    1006     float l1 = this->bvElement->halfLength[(this->longestAxisIndex + 1)%3];
    1007     float l2 = this->bvElement->halfLength[(this->longestAxisIndex + 2)%3];
    1008     glBegin(GL_QUADS);
    1009     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);
    1010     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);
    1011     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);
    1012     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);
    1013     glEnd();
    1014 
    1015     if( drawMode & DRAW_BV_BLENDED)
    1016       glColor4f(color.x, color.y, color.z, 1.0);
     981      /* now draw the separation plane */
     982      Vector a1 = this->bvElement->axis[(this->longestAxisIndex + 1)%3];
     983      Vector a2 = this->bvElement->axis[(this->longestAxisIndex + 2)%3];
     984      Vector c = this->bvElement->center;
     985      float l1 = this->bvElement->halfLength[(this->longestAxisIndex + 1)%3];
     986      float l2 = this->bvElement->halfLength[(this->longestAxisIndex + 2)%3];
     987      glBegin(GL_QUADS);
     988      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);
     989      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);
     990      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);
     991      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);
     992      glEnd();
     993
     994      if( drawMode & DRAW_BV_BLENDED)
     995        glColor4f(color.x, color.y, color.z, 1.0);
    1017996
    1018997    }
    … …  
    10381017void OBBTreeNode::debug() const
    10391018{
    1040 
    1041   /*
    1042   for(int i = 0; i < length; i++)
    1043   {
    1044   PRINTF(3)("vertex %i: %f, %f, %f\n", i, verticesList[i][0], verticesList[i][1], verticesList[i][2]);
    1045 }
    1046   */
    1047 }
     1019  PRINT(0)("========OBBTreeNode::debug()=====\n");
     1020  PRINT(0)(" Current depth: %i", this->depth);
     1021  PRINT(0)(" ");
     1022  PRINT(0)("=================================\n");
     1023}

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

    r5718 r5825  
    2121
    2222//! A class that represents a bounding volume tree
    23 class OBBTreeNode : public BVTreeNode {
     23class OBBTreeNode : public BVTreeNode
     24{
    2425
    2526
    … …  
    2930
    3031    /*  this function returns the bounding volume of this tree node @return: returns the BV */
    31     virtual inline const BoundingVolume* getBV() const { return (BoundingVolume*)this->bvElement; }
     32    virtual inline const BoundingVolume* getBV() const
     33    {
     34      return (BoundingVolume*)this->bvElement;
     35    }
    3236
    3337    virtual void spawnBVTree(const sVec3D *verticesList, unsigned int length);
    … …  
    4044
    4145  private:
    42     void calculateBoxEigenvectors(OBB& box, const sVec3D* verticesList, unsigned int length);
    4346    void calculateBoxAxis(OBB& box, const sVec3D* verticesList, unsigned int length);
    4447
    … …  
    6265    const OBBTree*      obbTree;                    //!< reference to the obb tree
    6366
     67    const modelInfo*    modelInf;                   //!< pointer to the models modelInfo object
     68    const int*          triangleIndexes;            //!< indexes to the used model triangles
     69
    6470    const sVec3D*       vertices;                   //!< pointer to the vertices data
    6571    int                 numOfVertices;              //!< number of vertices in vertices data
Note: See TracChangeset for help on using the changeset viewer.