Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: orxonox.OLD/trunk/src/lib/collision_detection/aabb_tree_node.cc @ 9122

Last change on this file since 9122 was 8724, checked in by bensch, 18 years ago

merged the bsp-model-stuff back here

File size: 34.1 KB
Line 
1/*
2   orxonox - the future of 3D-vertical-scrollers
3
4   Copyright (C) 2004 orx
5
6   This program is free software; you can redistribute it and/or modify
7   it under the terms of the GNU General Public License as published by
8   the Free Software Foundation; either version 2, or (at your option)
9   any later version.
10
11### File Specific:
12   main-programmer: Patrick Boenzli
13*/
14
15#define DEBUG_SPECIAL_MODULE 3/* DEBUG_MODULE_COLLISION_DETECTION*/
16
17#include "aabb_tree_node.h"
18#include "aabb.h"
19
20#include "bv_tree.h"
21
22#include "matrix.h"
23#include "model.h"
24#include "world_entity.h"
25#include "plane.h"
26
27#include "color.h"
28#include "glincl.h"
29
30#include <list>
31#include <vector>
32#include "debug.h"
33
34
35
36using namespace std;
37
38
39GLUquadricObj* AABBTreeNode_sphereObj = NULL;
40
41
42/**
43 *  standard constructor
44 * @param tree: reference to the obb tree
45 * @param depth: the depth of the obb tree to generate
46 */
47AABBTreeNode::AABBTreeNode (const OBBTree& tree, AABBTreeNode* prev, int depth)
48    : BVTreeNode()
49{
50  this->setClassID(CL_OBB_TREE_NODE, "AABBTreeNode");
51
52  this->obbTree = &tree;
53  this->nodePrev = prev;
54  this->depth = depth;
55  this->nextID = 0;
56
57  this->init();
58}
59
60
61/**
62 *  standard constructor
63 * @param depth: the depth of the obb tree to generate
64 */
65AABBTreeNode::AABBTreeNode(int depth)
66{
67  this->depth = depth;
68}
69
70/**
71 *  init funciton
72 */
73void AABBTreeNode::init()
74{
75  this->nodeLeft = NULL;
76  this->nodeRight = NULL;
77  this->bvElement = NULL;
78
79  this->triangleIndexList1 = NULL;
80  this->triangleIndexList2 = NULL;
81
82  this->modelInf = NULL;
83  this->triangleIndexes = NULL;
84
85  if( AABBTreeNode_sphereObj == NULL)
86    AABBTreeNode_sphereObj = gluNewQuadric();
87
88  this->owner = NULL;
89
90  /* debug ids */
91  if( this->nodePrev)
92    this->treeIndex = 100 * this->depth + this->nodePrev->getID();
93  else
94    this->treeIndex = 0;
95}
96
97/**
98 *  standard deconstructor
99 */
100AABBTreeNode::~AABBTreeNode ()
101{
102  if( this->nodeLeft)
103    delete this->nodeLeft;
104  if( this->nodeRight)
105    delete this->nodeRight;
106
107  if( this->bvElement)
108    delete this->bvElement;
109}
110
111
112
113void AABBTreeNode::spawnBVTree(Model* model)
114{
115
116  const modelInfo* modelInf = model->getModelInfo();
117
118  int* triangleIndexes = new int[modelInf->numTriangles];
119  for(unsigned int i = 0; i < modelInf->numTriangles; ++i)
120    triangleIndexes[i] = i;
121
122  this->spawnBVTree(*modelInf, triangleIndexes, modelInf->numTriangles);
123}
124
125
126/**
127 *  creates a new BVTree or BVTree partition
128 * @param depth: how much more depth-steps to go: if == 1 don't go any deeper!
129 * @param modInfo: model informations from the abstrac model
130 *
131 * this function creates the Bounding Volume tree from a modelInfo struct and bases its calculations
132 * on the triangle informations (triangle soup not polygon soup)
133 */
134void AABBTreeNode::spawnBVTree(const modelInfo& modelInf, const int* triangleIndexes, int length)
135{
136  PRINTF(4)("\n==============================Creating AABB Tree Node==================\n");
137  PRINT(4)(" AABB Tree Infos: \n");
138  PRINT(4)("\tDepth: %i \n\tTree Index: %i \n\tNumber of Triangles: %i\n", depth, this->treeIndex, length);
139  this->depth = depth;
140
141  this->bvElement = new AABB();
142  this->bvElement->modelInf = &modelInf;
143  this->bvElement->triangleIndexes = triangleIndexes;
144  this->bvElement->triangleIndexesLength = length;
145
146  /* create the bounding boxes in three steps */
147  this->calculateBoxCovariance(*this->bvElement, modelInf, triangleIndexes, length);
148  this->calculateBoxEigenvectors(*this->bvElement, modelInf, triangleIndexes, length);
149  this->calculateBoxAxis(*this->bvElement, modelInf, triangleIndexes, length);
150
151  /* do we need to descent further in the obb tree?*/
152  if( likely( this->depth > 0))
153  {
154    this->forkBox(*this->bvElement);
155
156    if( this->triangleIndexLength1 >= 3)
157    {
158      this->nodeLeft = new AABBTreeNode(*this->obbTree, this, depth - 1);
159      this->nodeLeft->spawnBVTree(modelInf, this->triangleIndexList1, this->triangleIndexLength1);
160    }
161    if( this->triangleIndexLength2 >= 3)
162    {
163      this->nodeRight = new AABBTreeNode(*this->obbTree, this, depth - 1);
164      this->nodeRight->spawnBVTree(modelInf, this->triangleIndexList2, this->triangleIndexLength2);
165    }
166  }
167}
168
169
170
171/**
172 *  calculate the box covariance matrix
173 * @param box: reference to the box
174 * @param modelInf: the model info structure of the model
175 * @param tirangleIndexes: an array with the indexes of the triangles inside this
176 * @param length: the length of the indexes array
177 */
178void AABBTreeNode::calculateBoxCovariance(AABB& box, const modelInfo& modelInf, const int* triangleIndexes, int length)
179{
180  float     facelet[length];                         //!< surface area of the i'th triangle of the convex hull
181  float     face = 0.0f;                             //!< surface area of the entire convex hull
182  Vector    centroid[length];                        //!< centroid of the i'th convex hull
183  Vector    center;                                  //!< the center of the entire hull
184  Vector    p, q, r;                                 //!< holder of the polygon data, much more conveniant to work with Vector than sVec3d
185  Vector    t1, t2;                                  //!< temporary values
186  float     covariance[3][3] = {{0,0,0}, {0,0,0}, {0,0,0}};//!< the covariance matrix
187
188  /* fist compute all the convex hull face/facelets and centroids */
189  for( int i = 0; i < length ; ++i)
190  {
191    p = &modelInf.pVertices[modelInf.pTriangles[triangleIndexes[i]].indexToVertices[0]];
192    q = &modelInf.pVertices[modelInf.pTriangles[triangleIndexes[i]].indexToVertices[1]];
193    r = &modelInf.pVertices[modelInf.pTriangles[triangleIndexes[i]].indexToVertices[2]];
194
195    /* finding the facelet surface via cross-product */
196    t1 = p - q;
197    t2 = p - r;
198    facelet[i] = 0.5f * /*fabs*/( t1.cross(t2).len() );
199    /* update the entire convex hull surface */
200    face += facelet[i];
201
202    /* calculate the cetroid of the hull triangles */
203    centroid[i] = (p + q + r) / 3.0f;
204    /* now calculate the centroid of the entire convex hull, weighted average of triangle centroids */
205    center += centroid[i] * facelet[i];
206    /* the arithmetical center */
207  }
208  /* take the average of the centroid sum */
209  center /= face;
210
211
212  /* now calculate the covariance matrix - if not written in three for-loops,
213     it would compute faster: minor */
214  for( int j = 0; j < 3; ++j)
215  {
216    for( int k = 0; k < 3; ++k)
217    {
218      for( int i = 0; i < length; ++i)
219      {
220        p = (&modelInf.pVertices[modelInf.pTriangles[triangleIndexes[i]].indexToVertices[0]]);
221        q = (&modelInf.pVertices[modelInf.pTriangles[triangleIndexes[i]].indexToVertices[1]]);
222        r = (&modelInf.pVertices[modelInf.pTriangles[triangleIndexes[i]].indexToVertices[2]]);
223
224        covariance[j][k] = facelet[i] * (9.0f * centroid[i][j] * centroid[i][k] + p[j] * p[k] +
225                           q[j] * q[k] + r[j] * r[k]);
226      }
227      covariance[j][k] = covariance[j][k] / (12.0f * face) - center[j] * center[k];
228    }
229  }
230  for( int i = 0; i < 3; ++i)
231  {
232    box.covarianceMatrix[i][0] = covariance[i][0];
233    box.covarianceMatrix[i][1] = covariance[i][1];
234    box.covarianceMatrix[i][2] = covariance[i][2];
235  }
236  box.center = center;
237
238  /* debug output section*/
239  PRINTF(4)("\nOBB Covariance Matrix:\n");
240  for(int j = 0; j < 3; ++j)
241  {
242    PRINT(4)("\t\t");
243    for(int k = 0; k < 3; ++k)
244    {
245      PRINT(4)("%11.4f\t", covariance[j][k]);
246    }
247    PRINT(4)("\n");
248  }
249  PRINTF(4)("\nWeighteed AABB Center:\n\t\t%11.4f\t %11.4f\t %11.4f\n", center.x, center.y, center.z);
250}
251
252
253
254/**
255 *  calculate the eigenvectors for the object oriented box
256 * @param box: reference to the box
257 * @param modelInf: the model info structure of the model
258 * @param tirangleIndexes: an array with the indexes of the triangles inside this
259 * @param length: the length of the indexes array
260 */
261void AABBTreeNode::calculateBoxEigenvectors(AABB& box, const modelInfo& modelInf,
262    const int* triangleIndexes, int length)
263{
264
265  Vector         axis[3];                            //!< the references to the obb axis
266  Matrix         covMat(  box.covarianceMatrix  );   //!< covariance matrix (in the matrix dataform)
267
268  /*
269  now getting spanning vectors of the sub-space:
270  the eigenvectors of a symmertric matrix, such as the
271  covarience matrix are mutually orthogonal.
272  after normalizing them, they can be used as a the basis
273  vectors
274  */
275
276
277  // this is for axis aligned bouning boxes only
278   box.axis[0] = Vector(1,0,0);
279   box.axis[1] = Vector(0,1,0);
280   box.axis[2] = Vector(0,0,1);
281
282   PRINTF(4)("Eigenvectors:\n");
283   PRINT(4)("\t\t%11.2f \t%11.2f \t%11.2f\n", box.axis[0].x, box.axis[0].y, box.axis[0].z);
284   PRINT(4)("\t\t%11.2f \t%11.2f \t%11.2f\n", box.axis[1].x, box.axis[1].y, box.axis[1].z);
285   PRINT(4)("\t\t%11.2f \t%11.2f \t%11.2f\n", box.axis[2].x, box.axis[2].y, box.axis[2].z);
286}
287
288
289
290
291/**
292 *  calculate the eigenvectors for the object oriented box
293 * @param box: reference to the box
294 * @param modelInf: the model info structure of the model
295 * @param tirangleIndexes: an array with the indexes of the triangles inside this
296 * @param length: the length of the indexes array
297 */
298void AABBTreeNode::calculateBoxAxis(AABB& box, const modelInfo& modelInf, const int* triangleIndexes, int length)
299{
300
301  PRINTF(4)("Calculate Box Axis\n");
302  /* now get the axis length */
303  float               tmpLength;                             //!< tmp save point for the length
304  Plane               p0(box.axis[0], box.center);           //!< the axis planes
305  Plane               p1(box.axis[1], box.center);           //!< the axis planes
306  Plane               p2(box.axis[2], box.center);           //!< the axis planes
307  float               maxLength[3];                          //!< maximal lenth of the axis
308  float               minLength[3];                          //!< minimal length of the axis
309  const float*        tmpVec;                                //!< variable taking tmp vectors
310  float               centerOffset[3];
311
312  /* get the maximal dimensions of the body in all directions */
313  /* for the initialisation the value just has to be inside of the polygon soup -> first vertices (rand) */
314  for( int k = 0; k  < 3; k++)
315  {
316    tmpVec = (&modelInf.pVertices[modelInf.pTriangles[triangleIndexes[0]].indexToVertices[0]]);
317    Plane* p;
318    if( k == 0)
319      p = &p0;
320    else if( k == 1)
321      p = &p1;
322    else
323      p = &p2;
324    maxLength[k] = p->distancePoint(tmpVec);
325    minLength[k] = p->distancePoint(tmpVec);
326
327    for( int j = 0; j < length; ++j) {
328      for( int i = 0; i < 3; ++i) {
329        tmpVec = &modelInf.pVertices[modelInf.pTriangles[triangleIndexes[j]].indexToVertices[i]];
330        tmpLength = p->distancePoint(tmpVec);
331
332        if( tmpLength > maxLength[k])
333          maxLength[k] = tmpLength;
334        else if( tmpLength < minLength[k])
335          minLength[k] = tmpLength;
336      }
337    }
338  }
339
340
341
342  /* calculate the real centre of the body by using the axis length */
343  for( int i = 0; i < 3; ++i)
344  {
345    if( maxLength[i] > 0.0f && minLength[i] > 0.0f)  // both axis positiv
346      centerOffset[i] = minLength[i] + (maxLength[i] - minLength[i]) / 2.0f;
347    else if( maxLength[i] > 0.0f && maxLength[i] < 0.0f) // positiv and negativ
348      centerOffset[i] = (maxLength[i] + minLength[i]) / 2.0f;
349    else //both negativ
350     centerOffset[i] = minLength[i] + (maxLength[i] - minLength[i]) / 2.0f;
351
352    box.halfLength[i] = (maxLength[i] - minLength[i]) / 2.0f;
353  }
354
355  box.center += (box.axis[0] * centerOffset[0]);
356  box.center += (box.axis[1] * centerOffset[1]);
357  box.center += (box.axis[2] * centerOffset[2]);
358
359
360  PRINTF(4)("\n");
361  PRINT(4)("\tAxis halflength x: %11.2f (max: %11.2f, \tmin: %11.2f), offset: %11.2f\n", box.halfLength[0], maxLength[0], minLength[0], centerOffset[0]);
362  PRINT(4)("\tAxis halflength y: %11.2f (max: %11.2f, \tmin: %11.2f), offset: %11.2f\n", box.halfLength[1], maxLength[1], minLength[1], centerOffset[1] );
363  PRINT(4)("\tAxis halflength z: %11.2f (max: %11.2f, \tmin: %11.2f), offset: %11.2f\n", box.halfLength[2], maxLength[2], minLength[2], centerOffset[2]);
364}
365
366
367
368/**
369 *  this separates an ob-box in the middle
370 * @param box: the box to separate
371 *
372 * this will separate the box into to smaller boxes. the separation is done along the middle of the longest axis
373 */
374void AABBTreeNode::forkBox(AABB& box)
375{
376
377  PRINTF(4)("Fork Box\n");
378  PRINTF(4)("Calculating the longest Axis\n");
379  /* get the longest axis of the box */
380  float               longestAxis = -1.0f;                 //!< the length of the longest axis
381  int                 longestAxisIndex = 0;                //!< this is the nr of the longest axis
382
383
384  /* now get the longest axis of the three exiting */
385  for( int i = 0; i < 3; ++i)
386  {
387    if( longestAxis < box.halfLength[i])
388    {
389      longestAxis = box.halfLength[i];
390      longestAxisIndex = i;
391    }
392  }
393  PRINTF(4)("\nLongest Axis is: Nr %i with a half-length of:%11.2f\n", longestAxisIndex, longestAxis);
394
395
396  PRINTF(4)("Separating along the longest axis\n");
397  /* get the closest vertex near the center */
398  float               tmpDist;                             //!< variable to save diverse distances temporarily
399  Plane               middlePlane(box.axis[longestAxisIndex], box.center); //!< the middle plane
400
401
402  /* now definin the separation plane through this specified nearest point and partition
403  the points depending on which side they are located
404  */
405  std::list<int>           partition1;                           //!< the vertex partition 1
406  std::list<int>           partition2;                           //!< the vertex partition 2
407  float*                   triangleCenter = new float[3];        //!< the center of the triangle
408  const float*             a;                                    //!< triangle  edge a
409  const float*             b;                                    //!< triangle  edge b
410  const float*             c;                                    //!< triangle  edge c
411
412
413  /* find the center of the box */
414  this->separationPlane = Plane(box.axis[longestAxisIndex], box.center);
415  this->sepPlaneCenter[0] = box.center.x;
416  this->sepPlaneCenter[1] = box.center.y;
417  this->sepPlaneCenter[2] = box.center.z;
418  this->longestAxisIndex = longestAxisIndex;
419
420  for( int i = 0; i < box.triangleIndexesLength; ++i)
421  {
422    /* first calculate the middle of the triangle */
423    a = &box.modelInf->pVertices[box.modelInf->pTriangles[box.triangleIndexes[i]].indexToVertices[0]];
424    b = &box.modelInf->pVertices[box.modelInf->pTriangles[box.triangleIndexes[i]].indexToVertices[1]];
425    c = &box.modelInf->pVertices[box.modelInf->pTriangles[box.triangleIndexes[i]].indexToVertices[2]];
426
427    triangleCenter[0] = (a[0] + b[0] + c[0]) / 3.0f;
428    triangleCenter[1] = (a[1] + b[1] + c[1]) / 3.0f;
429    triangleCenter[2] = (a[2] + b[2] + c[2]) / 3.0f;
430    tmpDist = this->separationPlane.distancePoint(*((sVec3D*)triangleCenter));
431
432    if( tmpDist > 0.0f)
433      partition1.push_back(box.triangleIndexes[i]); /* positive numbers plus zero */
434    else if( tmpDist < 0.0f)
435      partition2.push_back(box.triangleIndexes[i]); /* negatice numbers */
436    else {
437      partition1.push_back(box.triangleIndexes[i]); /* 0.0f? unprobable... */
438      partition2.push_back(box.triangleIndexes[i]);
439    }
440  }
441  PRINTF(4)("\nPartition1: got \t%i Vertices \nPartition2: got \t%i Vertices\n", partition1.size(), partition2.size());
442
443
444  /* now comes the separation into two different sVec3D arrays */
445  int                index;                                //!< index storage place
446  int*               triangleIndexList1;                   //!< the vertex list 1
447  int*               triangleIndexList2;                   //!< the vertex list 2
448  std::list<int>::iterator element;                        //!< the list iterator
449
450  triangleIndexList1 = new int[partition1.size()];
451  triangleIndexList2 = new int[partition2.size()];
452
453  for( element = partition1.begin(), index = 0; element != partition1.end(); element++, index++)
454    triangleIndexList1[index] = (*element);
455
456  for( element = partition2.begin(), index = 0; element != partition2.end(); element++, index++)
457    triangleIndexList2[index] = (*element);
458
459  if( this->triangleIndexList1!= NULL)
460    delete[] this->triangleIndexList1;
461  this->triangleIndexList1 = triangleIndexList1;
462  this->triangleIndexLength1 = partition1.size();
463
464  if( this->triangleIndexList2 != NULL)
465    delete[] this->triangleIndexList2;
466  this->triangleIndexList2 = triangleIndexList2;
467  this->triangleIndexLength2 = partition2.size();
468}
469
470
471
472/**
473 * collides one tree with an other
474 *  @param treeNode the other bv tree node
475 *  @param nodeA  the worldentity belonging to this bv
476 *  @param nodeB the worldentity belonging to treeNode
477 */
478void AABBTreeNode::collideWith(BVTreeNode* treeNode, WorldEntity* nodeA, WorldEntity* nodeB)
479{
480  if( unlikely(treeNode == NULL || nodeA == NULL || nodeB == NULL))
481    return;
482
483  PRINTF(4)("collideWith\n");
484  PRINTF(5)("Checking AABB %i vs %i: ", this->getIndex(), treeNode->getIndex());
485
486  // for now only collide with AABBTreeNodes
487  this->collideWithOBB((AABBTreeNode*)treeNode, nodeA, nodeB);
488}
489
490
491
492/**
493 * collides one obb tree with an other
494 *  @param treeNode the other bv tree node
495 *  @param nodeA  the worldentity belonging to this bv
496 *  @param nodeB the worldentity belonging to treeNode
497 */
498void AABBTreeNode::collideWithOBB(AABBTreeNode* treeNode, WorldEntity* nodeA, WorldEntity* nodeB)
499{
500
501  if( this->overlapTest(this->bvElement, treeNode->bvElement, nodeA, nodeB))
502  {
503    PRINTF(5)("collision @ lvl %i, object %s vs. %s, (%p, %p)\n", this->depth, nodeA->getClassName(), nodeB->getClassName(), this->nodeLeft, this->nodeRight);
504
505
506    // left node
507    if( this->nodeLeft != NULL )
508    {
509      if( this->overlapTest(this->nodeLeft->bvElement, treeNode->bvElement, nodeA, nodeB))
510      {
511        if( treeNode->nodeLeft != NULL)
512          this->nodeLeft->collideWith(treeNode->nodeLeft, nodeA, nodeB);
513        if( treeNode->nodeRight != NULL)
514          this->nodeLeft->collideWith(treeNode->nodeRight, nodeA, nodeB);
515      }
516    }
517
518    // right node
519    if( this->nodeRight != NULL )
520    {
521      if( this->overlapTest(this->nodeRight->bvElement, treeNode->bvElement, nodeA, nodeB))
522      {
523        if( treeNode->nodeLeft != NULL)
524          this->nodeRight->collideWith(treeNode->nodeLeft, nodeA, nodeB);
525        if( treeNode->nodeRight != NULL)
526          this->nodeRight->collideWith(treeNode->nodeRight, nodeA, nodeB);
527      }
528    }
529
530
531    // hybrid mode: we reached the end of this obbtree, now reach the end of the other tree
532    if( this->nodeLeft == NULL && this->nodeRight == NULL)
533    {
534      if( treeNode->nodeLeft != NULL)
535        this->collideWith(treeNode->nodeLeft, nodeA, nodeB);
536      if( treeNode->nodeRight != NULL)
537        this->collideWith(treeNode->nodeRight, nodeA, nodeB);
538    }
539
540
541    // now check if we reached the end of both trees
542    if( unlikely((this->nodeRight == NULL && this->nodeLeft == NULL) &&
543        (treeNode->nodeRight == NULL && treeNode->nodeLeft == NULL)) )
544    {
545      nodeA->registerCollision(nodeA, nodeB, (BoundingVolume*)this->bvElement, (BoundingVolume*)treeNode->bvElement);
546    }
547
548  }
549}
550
551
552/**
553 * this actualy checks if one obb box touches the other
554 * @param boxA the box from nodeA
555 * @param boxB the box from nodeB
556 * @param nodeA the node itself
557 * @param nodeB the node itself
558 */
559bool AABBTreeNode::overlapTest(AABB* boxA, AABB* boxB, WorldEntity* nodeA, WorldEntity* nodeB)
560{
561  //HACK remove this again
562  this->owner = nodeA;
563  //   if( boxB == NULL || boxA == NULL)
564  //     return false;
565
566  /* first check all axis */
567  Vector      t;
568  float       rA = 0.0f;
569  float       rB = 0.0f;
570  Vector      l;
571  Vector      rotAxisA[3];
572  Vector      rotAxisB[3];
573
574  rotAxisA[0] =  nodeA->getAbsDir().apply(boxA->axis[0]);
575  rotAxisA[1] =  nodeA->getAbsDir().apply(boxA->axis[1]);
576  rotAxisA[2] =  nodeA->getAbsDir().apply(boxA->axis[2]);
577
578  rotAxisB[0] =  nodeB->getAbsDir().apply(boxB->axis[0]);
579  rotAxisB[1] =  nodeB->getAbsDir().apply(boxB->axis[1]);
580  rotAxisB[2] =  nodeB->getAbsDir().apply(boxB->axis[2]);
581
582  t = nodeA->getAbsCoor() + nodeA->getAbsDir().apply(boxA->center) - ( nodeB->getAbsCoor() + nodeB->getAbsDir().apply(boxB->center));
583
584  /* All 3 axis of the object A */
585  for( int j = 0; j < 3; ++j)
586  {
587    rA = 0.0f;
588    rB = 0.0f;
589    l = rotAxisA[j];
590
591    rA += fabs(boxA->halfLength[0] * rotAxisA[0].dot(l));
592    rA += fabs(boxA->halfLength[1] * rotAxisA[1].dot(l));
593    rA += fabs(boxA->halfLength[2] * rotAxisA[2].dot(l));
594
595    rB += fabs(boxB->halfLength[0] * rotAxisB[0].dot(l));
596    rB += fabs(boxB->halfLength[1] * rotAxisB[1].dot(l));
597    rB += fabs(boxB->halfLength[2] * rotAxisB[2].dot(l));
598
599    PRINTF(5)("s = %f, rA+rB = %f\n", fabs(t.dot(l)), rA+rB);
600
601    if( (rA + rB) < fabs(t.dot(l)))
602    {
603      PRINTF(4)("no Collision\n");
604      return false;
605    }
606  }
607
608  /* All 3 axis of the object B */
609  for( int j = 0; j < 3; ++j)
610  {
611    rA = 0.0f;
612    rB = 0.0f;
613    l = rotAxisB[j];
614
615    rA += fabs(boxA->halfLength[0] * rotAxisA[0].dot(l));
616    rA += fabs(boxA->halfLength[1] * rotAxisA[1].dot(l));
617    rA += fabs(boxA->halfLength[2] * rotAxisA[2].dot(l));
618
619    rB += fabs(boxB->halfLength[0] * rotAxisB[0].dot(l));
620    rB += fabs(boxB->halfLength[1] * rotAxisB[1].dot(l));
621    rB += fabs(boxB->halfLength[2] * rotAxisB[2].dot(l));
622
623    PRINTF(5)("s = %f, rA+rB = %f\n", fabs(t.dot(l)), rA+rB);
624
625    if( (rA + rB) < fabs(t.dot(l)))
626    {
627      PRINTF(4)("no Collision\n");
628      return false;
629    }
630  }
631
632
633  /* Now check for all face cross products */
634
635  for( int j = 0; j < 3; ++j)
636  {
637    for(int k = 0; k < 3; ++k )
638    {
639      rA = 0.0f;
640      rB = 0.0f;
641      l = rotAxisA[j].cross(rotAxisB[k]);
642
643      rA += fabs(boxA->halfLength[0] * rotAxisA[0].dot(l));
644      rA += fabs(boxA->halfLength[1] * rotAxisA[1].dot(l));
645      rA += fabs(boxA->halfLength[2] * rotAxisA[2].dot(l));
646
647      rB += fabs(boxB->halfLength[0] * rotAxisB[0].dot(l));
648      rB += fabs(boxB->halfLength[1] * rotAxisB[1].dot(l));
649      rB += fabs(boxB->halfLength[2] * rotAxisB[2].dot(l));
650
651      PRINTF(5)("s = %f, rA+rB = %f\n", fabs(t.dot(l)), rA+rB);
652
653      if( (rA + rB) < fabs(t.dot(l)))
654      {
655        PRINTF(4)("keine Kollision\n");
656        return false;
657      }
658    }
659  }
660
661  /* FIXME: there is no collision mark set now */
662     boxA->bCollided = true; /* use this ONLY(!!!!) for drawing operations */
663     boxB->bCollided = true;
664
665
666  PRINTF(4)("Kollision!\n");
667  return true;
668}
669
670
671/**
672 *
673 * draw the BV tree - debug mode
674 */
675void AABBTreeNode::drawBV(int depth, int drawMode, const Vector& color,  bool top) const
676{
677  /* this function can be used to draw the triangles and/or the points only  */
678  if( 1 /*drawMode & DRAW_MODEL || drawMode & DRAW_ALL*/)
679  {
680    if( depth == 0/*!(drawMode & DRAW_SINGLE && depth != 0)*/)
681    {
682      if( 1 /*drawMode & DRAW_POINTS*/)
683      {
684        glBegin(GL_POINTS);
685        glColor3f(0.3, 0.8, 0.54);
686        for(unsigned int i = 0; i < this->bvElement->modelInf->numVertices*3; i+=3)
687          glVertex3f(this->bvElement->modelInf->pVertices[i],
688                     this->bvElement->modelInf->pVertices[i+1],
689                     this->bvElement->modelInf->pVertices[i+2]);
690        glEnd();
691      }
692    }
693  }
694
695  if (top)
696  {
697    glPushAttrib(GL_ENABLE_BIT);
698    glDisable(GL_LIGHTING);
699    glDisable(GL_TEXTURE_2D);
700  }
701  glColor3f(color.x, color.y, color.z);
702
703
704  /* draw world axes */
705  if( 1 /*drawMode & DRAW_BV_AXIS*/)
706  {
707    glBegin(GL_LINES);
708    glColor3f(1.0, 0.0, 0.0);
709    glVertex3f(0.0, 0.0, 0.0);
710    glVertex3f(3.0, 0.0, 0.0);
711
712    glColor3f(0.0, 1.0, 0.0);
713    glVertex3f(0.0, 0.0, 0.0);
714    glVertex3f(0.0, 3.0, 0.0);
715
716    glColor3f(0.0, 0.0, 1.0);
717    glVertex3f(0.0, 0.0, 0.0);
718    glVertex3f(0.0, 0.0, 3.0);
719    glEnd();
720  }
721
722
723  if( 1/*drawMode & DRAW_BV_AXIS || drawMode & DRAW_ALL*/)
724  {
725    if( 1/*drawMode & DRAW_SINGLE && depth != 0*/)
726    {
727      /* draw the obb axes */
728      glBegin(GL_LINES);
729      glColor3f(1.0, 0.0, 0.0);
730      glVertex3f(this->bvElement->center.x, this->bvElement->center.y, this->bvElement->center.z);
731      glVertex3f(this->bvElement->center.x + this->bvElement->axis[0].x * this->bvElement->halfLength[0],
732                 this->bvElement->center.y + this->bvElement->axis[0].y * this->bvElement->halfLength[0],
733                 this->bvElement->center.z + this->bvElement->axis[0].z * this->bvElement->halfLength[0]);
734
735      glColor3f(0.0, 1.0, 0.0);
736      glVertex3f(this->bvElement->center.x, this->bvElement->center.y, this->bvElement->center.z);
737      glVertex3f(this->bvElement->center.x + this->bvElement->axis[1].x * this->bvElement->halfLength[1],
738                 this->bvElement->center.y + this->bvElement->axis[1].y * this->bvElement->halfLength[1],
739                 this->bvElement->center.z + this->bvElement->axis[1].z * this->bvElement->halfLength[1]);
740
741      glColor3f(0.0, 0.0, 1.0);
742      glVertex3f(this->bvElement->center.x, this->bvElement->center.y, this->bvElement->center.z);
743      glVertex3f(this->bvElement->center.x + this->bvElement->axis[2].x * this->bvElement->halfLength[2],
744                 this->bvElement->center.y + this->bvElement->axis[2].y * this->bvElement->halfLength[2],
745                 this->bvElement->center.z + this->bvElement->axis[2].z * this->bvElement->halfLength[2]);
746      glEnd();
747    }
748  }
749
750
751  /* DRAW POLYGONS */
752  if( drawMode & DRAW_BV_POLYGON || drawMode & DRAW_ALL || drawMode & DRAW_BV_BLENDED)
753  {
754    if (top)
755    {
756      glEnable(GL_BLEND);
757      glBlendFunc(GL_SRC_ALPHA, GL_ONE);
758    }
759
760    if( this->nodeLeft == NULL && this->nodeRight == NULL)
761      depth = 0;
762
763    if( depth == 0 /*!(drawMode & DRAW_SINGLE && depth != 0)*/)
764    {
765
766
767      Vector cen = this->bvElement->center;
768      Vector* axis = this->bvElement->axis;
769      float* len = this->bvElement->halfLength;
770
771      if( this->bvElement->bCollided)
772      {
773        glColor4f(1.0, 1.0, 1.0, .5); // COLLISION COLOR
774      }
775      else if( drawMode & DRAW_BV_BLENDED)
776      {
777        glColor4f(color.x, color.y, color.z, .5);
778      }
779
780
781      /* draw bounding box */
782      if( drawMode & DRAW_BV_BLENDED)
783        glBegin(GL_QUADS);
784      else
785        glBegin(GL_LINE_LOOP);
786      glVertex3f(cen.x + axis[0].x * len[0] + axis[1].x * len[1] + axis[2].x * len[2],
787                 cen.y + axis[0].y * len[0] + axis[1].y * len[1] + axis[2].y * len[2],
788                 cen.z + axis[0].z * len[0] + axis[1].z * len[1] + axis[2].z * len[2]);
789      glVertex3f(cen.x + axis[0].x * len[0] + axis[1].x * len[1] - axis[2].x * len[2],
790                 cen.y + axis[0].y * len[0] + axis[1].y * len[1] - axis[2].y * len[2],
791                 cen.z + axis[0].z * len[0] + axis[1].z * len[1] - axis[2].z * len[2]);
792      glVertex3f(cen.x + axis[0].x * len[0] - axis[1].x * len[1] - axis[2].x * len[2],
793                 cen.y + axis[0].y * len[0] - axis[1].y * len[1] - axis[2].y * len[2],
794                 cen.z + axis[0].z * len[0] - axis[1].z * len[1] - axis[2].z * len[2]);
795      glVertex3f(cen.x + axis[0].x * len[0] - axis[1].x * len[1] + axis[2].x * len[2],
796                 cen.y + axis[0].y * len[0] - axis[1].y * len[1] + axis[2].y * len[2],
797                 cen.z + axis[0].z * len[0] - axis[1].z * len[1] + axis[2].z * len[2]);
798      glEnd();
799
800      if( drawMode & DRAW_BV_BLENDED)
801        glBegin(GL_QUADS);
802      else
803        glBegin(GL_LINE_LOOP);
804      glVertex3f(cen.x + axis[0].x * len[0] - axis[1].x * len[1] + axis[2].x * len[2],
805                 cen.y + axis[0].y * len[0] - axis[1].y * len[1] + axis[2].y * len[2],
806                 cen.z + axis[0].z * len[0] - axis[1].z * len[1] + axis[2].z * len[2]);
807      glVertex3f(cen.x + axis[0].x * len[0] - axis[1].x * len[1] - axis[2].x * len[2],
808                 cen.y + axis[0].y * len[0] - axis[1].y * len[1] - axis[2].y * len[2],
809                 cen.z + axis[0].z * len[0] - axis[1].z * len[1] - axis[2].z * len[2]);
810      glVertex3f(cen.x - axis[0].x * len[0] - axis[1].x * len[1] - axis[2].x * len[2],
811                 cen.y - axis[0].y * len[0] - axis[1].y * len[1] - axis[2].y * len[2],
812                 cen.z - axis[0].z * len[0] - axis[1].z * len[1] - axis[2].z * len[2]);
813      glVertex3f(cen.x - axis[0].x * len[0] - axis[1].x * len[1] + axis[2].x * len[2],
814                 cen.y - axis[0].y * len[0] - axis[1].y * len[1] + axis[2].y * len[2],
815                 cen.z - axis[0].z * len[0] - axis[1].z * len[1] + axis[2].z * len[2]);
816      glEnd();
817
818      if( drawMode & DRAW_BV_BLENDED)
819        glBegin(GL_QUADS);
820      else
821        glBegin(GL_LINE_LOOP);
822      glVertex3f(cen.x - axis[0].x * len[0] - axis[1].x * len[1] + axis[2].x * len[2],
823                 cen.y - axis[0].y * len[0] - axis[1].y * len[1] + axis[2].y * len[2],
824                 cen.z - axis[0].z * len[0] - axis[1].z * len[1] + axis[2].z * len[2]);
825      glVertex3f(cen.x - axis[0].x * len[0] - axis[1].x * len[1] - axis[2].x * len[2],
826                 cen.y - axis[0].y * len[0] - axis[1].y * len[1] - axis[2].y * len[2],
827                 cen.z - axis[0].z * len[0] - axis[1].z * len[1] - axis[2].z * len[2]);
828      glVertex3f(cen.x - axis[0].x * len[0] + axis[1].x * len[1] - axis[2].x * len[2],
829                 cen.y - axis[0].y * len[0] + axis[1].y * len[1] - axis[2].y * len[2],
830                 cen.z - axis[0].z * len[0] + axis[1].z * len[1] - axis[2].z * len[2]);
831      glVertex3f(cen.x - axis[0].x * len[0] + axis[1].x * len[1] + axis[2].x * len[2],
832                 cen.y - axis[0].y * len[0] + axis[1].y * len[1] + axis[2].y * len[2],
833                 cen.z - axis[0].z * len[0] + axis[1].z * len[1] + axis[2].z * len[2]);
834      glEnd();
835
836      if( drawMode & DRAW_BV_BLENDED)
837        glBegin(GL_QUADS);
838      else
839        glBegin(GL_LINE_LOOP);
840      glVertex3f(cen.x - axis[0].x * len[0] + axis[1].x * len[1] - axis[2].x * len[2],
841                 cen.y - axis[0].y * len[0] + axis[1].y * len[1] - axis[2].y * len[2],
842                 cen.z - axis[0].z * len[0] + axis[1].z * len[1] - axis[2].z * len[2]);
843      glVertex3f(cen.x - axis[0].x * len[0] + axis[1].x * len[1] + axis[2].x * len[2],
844                 cen.y - axis[0].y * len[0] + axis[1].y * len[1] + axis[2].y * len[2],
845                 cen.z - axis[0].z * len[0] + axis[1].z * len[1] + axis[2].z * len[2]);
846      glVertex3f(cen.x + axis[0].x * len[0] + axis[1].x * len[1] + axis[2].x * len[2],
847                 cen.y + axis[0].y * len[0] + axis[1].y * len[1] + axis[2].y * len[2],
848                 cen.z + axis[0].z * len[0] + axis[1].z * len[1] + axis[2].z * len[2]);
849      glVertex3f(cen.x + axis[0].x * len[0] + axis[1].x * len[1] - axis[2].x * len[2],
850                 cen.y + axis[0].y * len[0] + axis[1].y * len[1] - axis[2].y * len[2],
851                 cen.z + axis[0].z * len[0] + axis[1].z * len[1] - axis[2].z * len[2]);
852      glEnd();
853
854
855      if( drawMode & DRAW_BV_BLENDED)
856      {
857        glBegin(GL_QUADS);
858        glVertex3f(cen.x - axis[0].x * len[0] + axis[1].x * len[1] - axis[2].x * len[2],
859                   cen.y - axis[0].y * len[0] + axis[1].y * len[1] - axis[2].y * len[2],
860                   cen.z - axis[0].z * len[0] + axis[1].z * len[1] - axis[2].z * len[2]);
861        glVertex3f(cen.x + axis[0].x * len[0] + axis[1].x * len[1] - axis[2].x * len[2],
862                   cen.y + axis[0].y * len[0] + axis[1].y * len[1] - axis[2].y * len[2],
863                   cen.z + axis[0].z * len[0] + axis[1].z * len[1] - axis[2].z * len[2]);
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]);
867        glVertex3f(cen.x - axis[0].x * len[0] - axis[1].x * len[1] - axis[2].x * len[2],
868                   cen.y - axis[0].y * len[0] - axis[1].y * len[1] - axis[2].y * len[2],
869                   cen.z - axis[0].z * len[0] - axis[1].z * len[1] - axis[2].z * len[2]);
870        glEnd();
871
872        glBegin(GL_QUADS);
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        glVertex3f(cen.x + axis[0].x * len[0] + axis[1].x * len[1] + axis[2].x * len[2],
877                   cen.y + axis[0].y * len[0] + axis[1].y * len[1] + axis[2].y * len[2],
878                   cen.z + axis[0].z * len[0] + axis[1].z * len[1] + axis[2].z * len[2]);
879        glVertex3f(cen.x + axis[0].x * len[0] - axis[1].x * len[1] + axis[2].x * len[2],
880                   cen.y + axis[0].y * len[0] - axis[1].y * len[1] + axis[2].y * len[2],
881                   cen.z + axis[0].z * len[0] - axis[1].z * len[1] + axis[2].z * len[2]);
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        glEnd();
886      }
887
888      if( drawMode & DRAW_BV_BLENDED)
889        glColor3f(color.x, color.y, color.z);
890    }
891  }
892
893  /* DRAW SEPARATING PLANE */
894  if( drawMode & DRAW_SEPARATING_PLANE || drawMode & DRAW_ALL)
895  {
896    if( !(drawMode & DRAW_SINGLE && depth != 0))
897    {
898      if( drawMode & DRAW_BV_BLENDED)
899        glColor4f(color.x, color.y, color.z, .6);
900
901      /* now draw the separation plane */
902      Vector a1 = this->bvElement->axis[(this->longestAxisIndex + 1)%3];
903      Vector a2 = this->bvElement->axis[(this->longestAxisIndex + 2)%3];
904      Vector c = this->bvElement->center;
905      float l1 = this->bvElement->halfLength[(this->longestAxisIndex + 1)%3];
906      float l2 = this->bvElement->halfLength[(this->longestAxisIndex + 2)%3];
907      glBegin(GL_QUADS);
908      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);
909      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);
910      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);
911      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);
912      glEnd();
913
914      if( drawMode & DRAW_BV_BLENDED)
915        glColor4f(color.x, color.y, color.z, 1.0);
916
917    }
918  }
919
920
921
922  if (depth > 0)
923  {
924    if( this->nodeLeft != NULL)
925      this->nodeLeft->drawBV(depth - 1, drawMode, Color::HSVtoRGB(Color::RGBtoHSV(color)+Vector(15.0,0.0,0.0)), false);
926    if( this->nodeRight != NULL)
927      this->nodeRight->drawBV(depth - 1, drawMode, Color::HSVtoRGB(Color::RGBtoHSV(color)+Vector(30.0,0.0,0.0)), false);
928  }
929  this->bvElement->bCollided = false;
930
931  if (top)
932    glPopAttrib();
933}
934
935
936
937void AABBTreeNode::debug() const
938{
939  PRINT(0)("========AABBTreeNode::debug()=====\n");
940  PRINT(0)(" Current depth: %i", this->depth);
941  PRINT(0)(" ");
942  PRINT(0)("=================================\n");
943}
Note: See TracBrowser for help on using the repository browser.