Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

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

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

merged the collision reaction branche back to trunk

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