Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: orxonox.OLD/branches/terrain/src/lib/collision_detection/aabb_tree_node.cc @ 10210

Last change on this file since 10210 was 10013, checked in by patrick, 18 years ago

merged the collision reaction branche back to trunk

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