Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: orxonox.OLD/branches/terrain/src/lib/collision_detection/obb_tree_node.cc @ 8465

Last change on this file since 8465 was 8190, checked in by patrick, 19 years ago

trunk: merged the cr branche to trunk

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