Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: orxonox.OLD/trunk/src/lib/collision_detection/obb_tree_node.cc @ 9416

Last change on this file since 9416 was 9406, checked in by bensch, 18 years ago

orxonox/trunk: merged the proxy back

merged with commandsvn merge -r9346:HEAD https://svn.orxonox.net/orxonox/branches/proxy .

no conflicts

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