Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

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

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

orxonox/trunk: merged the network bak to the trunk
merged with command:
svn merge -r8804:HEAD https://svn.orxonox.net/orxonox/branches/multi_player_map .

conflicts all resolved in favour of the branche

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