Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

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

Last change on this file since 7485 was 6617, checked in by bensch, 19 years ago

trunk: split Rotation/Line/Quaternion/Plane(Rectangle) into seperate files

File size: 38.3 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   co-programmer: ...
14*/
15
16#define DEBUG_SPECIAL_MODULE DEBUG_MODULE_COLLISION
17
18#include "obb_tree_node.h"
19#include "list.h"
20#include "obb.h"
21#include "obb_tree.h"
22#include "matrix.h"
23#include "model.h"
24#include "world_entity.h"
25#include "plane.h"
26
27#include "color.h"
28
29#include "debug.h"
30#include "glincl.h"
31
32
33
34using namespace std;
35
36OBBTree*  OBBTreeNode::obbTree = NULL;
37
38float**  OBBTreeNode::coMat = NULL;
39float**  OBBTreeNode::eigvMat = NULL;
40float*   OBBTreeNode::eigvlMat = NULL;
41int*     OBBTreeNode::rotCount = NULL;
42GLUquadricObj* OBBTreeNode_sphereObj = NULL;
43
44/**
45 *  standard constructor
46 */
47OBBTreeNode::OBBTreeNode ()
48{
49  this->setClassID(CL_OBB_TREE_NODE, "OBBTreeNode");
50  this->nodeLeft = NULL;
51  this->nodeRight = NULL;
52  this->bvElement = NULL;
53
54  if( OBBTreeNode::coMat == NULL)
55  {
56    OBBTreeNode::coMat = new float*[4];
57    for(int i = 0; i < 4; i++)
58      OBBTreeNode::coMat[i] = new float[4];
59  }
60  if( OBBTreeNode::eigvMat == NULL)
61  {
62    OBBTreeNode::eigvMat = new float*[4];
63    for( int i = 0; i < 4; i++)
64      OBBTreeNode::eigvMat[i] = new float[4];
65  }
66  if( OBBTreeNode::eigvlMat == NULL)
67  {
68    OBBTreeNode::eigvlMat = new float[4];
69  }
70  if( OBBTreeNode::rotCount == NULL)
71    OBBTreeNode::rotCount = new int;
72
73  if( OBBTreeNode_sphereObj == NULL)
74    OBBTreeNode_sphereObj = gluNewQuadric();
75}
76
77
78/**
79 *  standard deconstructor
80 */
81OBBTreeNode::~OBBTreeNode ()
82{
83  if( this->nodeLeft)
84  {
85    delete this->nodeLeft;
86    this->nodeLeft = NULL;
87  }
88  if( this->nodeRight)
89  {
90    delete this->nodeRight;
91    this->nodeRight = NULL;
92  }
93  if( this->bvElement)
94    delete this->bvElement;
95  this->bvElement = NULL;
96}
97
98
99/**
100 *  creates a new BVTree or BVTree partition
101 * @param depth: how much more depth-steps to go: if == 1 don't go any deeper!
102 * @param modInfo: model informations from the abstrac model
103 *
104 * this function creates the Bounding Volume tree from a modelInfo struct and bases its calculations
105 * on the triangle informations (triangle soup not polygon soup)
106 */
107void OBBTreeNode::spawnBVTree(const int depth, const modelInfo& modInfo)
108{
109  int length = 0;
110  sVec3D* verticesList;
111
112  PRINT(3)("\n");
113  this->treeIndex = this->obbTree->getID();
114  PRINTF(3)("OBB Depth: %i, tree index: %i, numVertices: %i\n", depth, treeIndex, length);
115  this->depth = depth;
116
117
118  this->bvElement = new OBB();
119  this->bvElement->vertices = verticesList;
120  this->bvElement->numOfVertices = length;
121  PRINTF(3)("Created OBBox\n");
122  this->calculateBoxCovariance(this->bvElement, modInfo);
123  PRINTF(3)("Calculated attributes1\n");
124  this->calculateBoxEigenvectors(this->bvElement, modInfo);
125  PRINTF(3)("Calculated attributes2\n");
126  this->calculateBoxAxis(this->bvElement,modInfo);
127  PRINTF(3)("Calculated attributes3\n");
128
129  /* if this is the first node, the vertices data are the original ones of the model itself, so dont delete them in cleanup */
130  if( this->treeIndex == 1)
131    this->bvElement->bOrigVertices = true;
132
133  if( likely( this->depth > 0))
134  {
135    this->forkBox(this->bvElement);
136
137
138//     if(this->tmpLen1 > 2)
139//     {
140//       OBBTreeNode* node1 = new OBBTreeNode();
141//       this->nodeLeft = node1;
142//       this->nodeLeft->spawnBVTree(depth - 1, this->tmpVert1, this->tmpLen1);
143//     }
144//     else
145//     {
146//       PRINTF(3)("Aboarding tree walk: less than 3 vertices left\n");
147//     }
148//
149//     if( this->tmpLen2 > 2)
150//     {
151//       OBBTreeNode* node2 = new OBBTreeNode();
152//       this->nodeRight = node2;
153//       this->nodeRight->spawnBVTree(depth - 1, this->tmpVert2, this->tmpLen2);
154//     }
155//     else
156//     {
157//       PRINTF(3)("Abording tree walk: less than 3 vertices left\n");
158//     }
159
160  }
161}
162
163
164/**
165 *  creates a new BVTree or BVTree partition
166 * @param depth: how much more depth-steps to go: if == 1 don't go any deeper!
167 * @param verticesList: the list of vertices of the object - each vertices triple is interpreted as a triangle
168 *
169 * this function creates an Bounding Volume tree from a vertices soup (no triangle data)
170 */
171void OBBTreeNode::spawnBVTree(const int depth, const sVec3D *verticesList, unsigned int length)
172{
173  PRINT(3)("\n");
174  this->treeIndex = this->obbTree->getID();
175  PRINTF(3)("OBB Depth: %i, tree index: %i, numVertices: %i\n", depth, treeIndex, length);
176  this->depth = depth;
177
178
179  this->bvElement = new OBB();
180  this->bvElement->vertices = verticesList;
181  this->bvElement->numOfVertices = length;
182  PRINTF(3)("Created OBBox\n");
183  this->calculateBoxCovariance(this->bvElement, verticesList, length);
184  PRINTF(3)("Calculated attributes1\n");
185  this->calculateBoxEigenvectors(this->bvElement, verticesList, length);
186  PRINTF(3)("Calculated attributes2\n");
187  this->calculateBoxAxis(this->bvElement, verticesList, length);
188  PRINTF(3)("Calculated attributes3\n");
189
190  /* if this is the first node, the vertices data are the original ones of the model itself, so dont delete them in cleanup */
191  if( this->treeIndex == 1)
192    this->bvElement->bOrigVertices = true;
193
194  if( likely( this->depth > 0))
195  {
196    this->forkBox(this->bvElement);
197
198
199    if(this->tmpLen1 > 2)
200    {
201      OBBTreeNode* node1 = new OBBTreeNode();
202      this->nodeLeft = node1;
203      this->nodeLeft->spawnBVTree(depth - 1, this->tmpVert1, this->tmpLen1);
204    }
205    else
206    {
207      PRINTF(3)("Aboarding tree walk: less than 3 vertices left\n");
208    }
209
210    if( this->tmpLen2 > 2)
211    {
212      OBBTreeNode* node2 = new OBBTreeNode();
213      this->nodeRight = node2;
214      this->nodeRight->spawnBVTree(depth - 1, this->tmpVert2, this->tmpLen2);
215    }
216    else
217    {
218      PRINTF(3)("Abording tree walk: less than 3 vertices left\n");
219    }
220  }
221}
222
223
224void OBBTreeNode::calculateBoxCovariance(OBB* box, const modelInfo& modInfo)
225{}
226
227
228void OBBTreeNode::calculateBoxCovariance(OBB* box, const sVec3D* verticesList, unsigned int length)
229{
230  float     facelet[length];                         //!< surface area of the i'th triangle of the convex hull
231  float     face = 0.0f;                             //!< surface area of the entire convex hull
232  Vector    centroid[length];                        //!< centroid of the i'th convex hull
233  Vector    center;                                  //!< the center of the entire hull
234  Vector    p, q, r;                                 //!< holder of the polygon data, much more conveniant to work with Vector than sVec3d
235  Vector    t1, t2;                                  //!< temporary values
236  float     covariance[3][3] = {0,0,0, 0,0,0, 0,0,0};//!< the covariance matrix
237  int       mode = 0;                                //!< mode = 0: vertex soup, no connections, mode = 1: 3 following verteces build a triangle
238
239  this->numOfVertices = length;
240  this->vertices = verticesList;
241
242
243  if( likely(mode == 0))
244  {
245    /* fist compute all the convex hull face/facelets and centroids */
246    for( int i = 0; i+3 < length ; i+=3)          /* FIX-ME-QUICK: hops of 3, array indiscontinuity*/
247    {
248      p = verticesList[i];
249      q = verticesList[i + 1];
250      r = verticesList[i + 2];
251
252      t1 = p - q; t2 = p - r;
253
254      /* finding the facelet surface via cross-product */
255      facelet[i] = 0.5f * fabs( t1.cross(t2).len() );
256      /* update the entire convex hull surface */
257      face += facelet[i];
258
259      /* calculate the cetroid of the hull triangles */
260      centroid[i] = (p + q + r) * 1/3;
261      /* now calculate the centroid of the entire convex hull, weighted average of triangle centroids */
262      center += centroid[i] * facelet[i];
263    }
264    /* take the average of the centroid sum */
265    center /= face;
266    PRINTF(3)("-- Calculated Center\n");
267
268
269    /* now calculate the covariance matrix - if not written in three for-loops, it would compute faster: minor */
270    for( int j = 0; j < 3; ++j)
271    {
272      for( int k = 0; k < 3; ++k)
273      {
274        for( int i = 0; i + 3 < length; i+=3)
275        {
276          p = verticesList[i];
277          q = verticesList[i + 1];
278          r = verticesList[i + 2];
279
280          covariance[j][k] = facelet[i] / (12.0f * face) * (9.0f * centroid[i][j] * centroid[i][k] + p[j] * p[k] +
281              q[j] * q[k] + r[j] * r[k]) - center[j] * center[k];
282        }
283      }
284    }
285    PRINTF(3)("-- Calculated Covariance\n");
286  }
287  else if( mode == 1)
288  {
289    for( int i = 0; i + 3 < length; i+=3)          /* FIX-ME-QUICK: hops of 3, array indiscontinuity*/
290    {
291      p = verticesList[i];
292      q = verticesList[i + 1];
293      r = verticesList[i + 2];
294
295      centroid[i] = (p + q + r) / 3.0f;
296      center += centroid[i];
297    }
298    center /= length;
299
300    for( int j = 0; j < 3; ++j)
301    {
302      for( int k = 0; k < 3; ++k)
303      {
304        for( int i = 0; i + 3 < length; i+=3)
305        {
306          p = verticesList[i];
307          q = verticesList[i +1];
308          r = verticesList[i + 2];
309
310          covariance[j][k] = p[j] * p[k] + q[j] * q[k] + r[j] + r[k];
311        }
312        covariance[j][k] /= (3.0f * length);
313      }
314    }
315    PRINTF(3)("-- Calculated Covariance\n");
316  }
317  else if( mode == 2)
318  {
319    /* fist compute all the convex hull face/facelets and centroids */
320    for(int i = 0; i + 3 < length; i+=3)          /* FIX-ME-QUICK: hops of 3, array indiscontinuity*/
321    {
322      p = verticesList[i];
323      q = verticesList[i + 1];
324      r = verticesList[i + 2];
325
326      t1 = p - q; t2 = p - r;
327
328      /* finding the facelet surface via cross-product */
329      facelet[i] = 0.5f * fabs( t1.cross(t2).len() );
330      /* update the entire convex hull surface */
331      face += facelet[i];
332
333      /* calculate the cetroid of the hull triangles */
334      centroid[i] = (p + q + r) * 1/3;
335      /* now calculate the centroid of the entire convex hull, weighted average of triangle centroids */
336      center += centroid[i] * facelet[i];
337    }
338    /* take the average of the centroid sum */
339    center /= face;
340    PRINTF(3)("-- Calculated Center\n");
341
342    for( int j = 0; j < 3; ++j)
343    {
344      for( int k = 0; k < 3; ++k)
345      {
346        for( int i = 0; i + 3 < length; i+=3)
347        {
348          p = verticesList[i];
349          q = verticesList[i +1];
350          r = verticesList[i + 2];
351
352          covariance[j][k] = p[j] * p[k] + q[j] * q[k] + r[j] + r[k];
353        }
354        covariance[j][k] /= (3.0f * length);
355      }
356    }
357    PRINTF(3)("-- Calculated Covariance\n");
358  }
359  else
360  {
361    for( int i = 0; i < length; ++i)          /* FIX-ME-QUICK: hops of 3, array indiscontinuity*/
362    {
363      center += verticesList[i];
364    }
365    center /= length;
366
367    for( int j = 0; j < 3; ++j)
368    {
369      for( int k = 0; k < 3; ++k)
370      {
371        for( int i = 0; i + 3 < length; i+=3)
372        {
373          p = verticesList[i];
374          q = verticesList[i +1];
375          r = verticesList[i + 2];
376
377          covariance[j][k] = p[j] * p[k] + q[j] * q[k] + r[j] + r[k];
378        }
379        covariance[j][k] /= (3.0f * length);
380      }
381    }
382    PRINTF(3)("-- Calculated Covariance\n");
383  }
384
385  PRINTF(3)("\nVertex Data:\n");
386  for(int i = 0; i < length; i++)
387  {
388    PRINTF(3)("vertex %i: %f, %f, %f\n", i, box->vertices[i][0], box->vertices[i][1], box->vertices[i][2]);
389  }
390
391
392  PRINTF(3)("\nCovariance Matrix:\n");
393  for(int j = 0; j < 3; ++j)
394  {
395    PRINT(3)(" |");
396    for(int k = 0; k < 3; ++k)
397    {
398      PRINT(3)(" \b%f ", covariance[j][k]);
399    }
400    PRINT(3)(" |\n");
401  }
402
403  PRINTF(3)("center: %f, %f, %f\n", center.x, center.y, center.z);
404
405
406  for(int i = 0; i < 3; ++i)
407  {
408    box->covarianceMatrix[i][0] = covariance[i][0];
409    box->covarianceMatrix[i][1] = covariance[i][1];
410    box->covarianceMatrix[i][2] = covariance[i][2];
411  }
412  *box->center = center;
413  PRINTF(3)("-- Written Result to obb\n");
414}
415
416
417void OBBTreeNode::calculateBoxEigenvectors(OBB* box, const modelInfo& modInfo)
418{}
419
420void OBBTreeNode::calculateBoxEigenvectors(OBB* box, const sVec3D* verticesList, unsigned int length)
421{
422
423  /* now getting spanning vectors of the sub-space:
424  the eigenvectors of a symmertric matrix, such as the
425  covarience matrix are mutually orthogonal.
426  after normalizing them, they can be used as a the basis
427  vectors
428  */
429  Vector*              axis = new Vector[3];                //!< the references to the obb axis
430
431  Matrix covMat(  box->covarianceMatrix  );
432  covMat.getEigenVectors(axis[0], axis[1], axis[2] );
433
434
435  /* new jacobi tests */
436//  JacobI(OBBTreeNode::coMat, OBBTreeNode::eigvlMat, OBBTreeNode::eigvMat, OBBTreeNode::rotCount);
437//  PRINTF(3)("-- Done Jacobi Decomposition\n");
438
439
440//   PRINTF(0)("Jacobi\n");
441//   for(int j = 0; j < 3; ++j)
442//   {
443//     printf(" |");
444//     for(int k = 0; k < 3; ++k)
445//     {
446//       printf(" \t%f ", OBBTreeNode::OBBTreeNode::eigvMat[j][k]);
447//     }
448//     printf(" |\n");
449//   }
450
451/*  axis[0].x = OBBTreeNode::eigvMat[0][0]; axis[0].y = OBBTreeNode::eigvMat[1][0]; axis[0].z = OBBTreeNode::eigvMat[2][0];
452  axis[1].x = OBBTreeNode::eigvMat[0][1]; axis[1].y = OBBTreeNode::eigvMat[1][1]; axis[1].z = OBBTreeNode::eigvMat[2][1];
453  axis[2].x = OBBTreeNode::eigvMat[0][2]; axis[2].y = OBBTreeNode::eigvMat[1][2]; axis[2].z = OBBTreeNode::eigvMat[2][2];
454  axis[0].normalize();
455  axis[1].normalize();
456  axis[2].normalize();*/
457  box->axis = axis;
458
459//   PRINTF(0)("-- Got Axis\n");
460//
461//   PRINTF(0)("eigenvector: %f, %f, %f\n", box->axis[0].x, box->axis[0].y, box->axis[0].z);
462//   PRINTF(0)("eigenvector: %f, %f, %f\n", box->axis[1].x, box->axis[1].y, box->axis[1].z);
463//   PRINTF(0)("eigenvector: %f, %f, %f\n", box->axis[2].x, box->axis[2].y, box->axis[2].z);
464}
465
466
467void OBBTreeNode::calculateBoxAxis(OBB* box, const modelInfo& modInfo)
468{
469  this->calculateBoxAxis(box, (const sVec3D*)modInfo.pVertices, modInfo.numVertices);
470}
471
472
473
474void OBBTreeNode::calculateBoxAxis(OBB* box, const sVec3D* verticesList, unsigned int length)
475{
476
477  /* now get the axis length */
478  Line                ax[3];                                 //!< the axis
479  float*              halfLength = new float[3];             //!< half length of the axis
480  float               tmpLength;                             //!< tmp save point for the length
481  Plane               p0(box->axis[0], *box->center);       //!< the axis planes
482  Plane               p1(box->axis[1], *box->center);
483  Plane               p2(box->axis[2], *box->center);
484  float               maxLength[3];
485  float               minLength[3];
486
487
488  /* get a bad bounding box */
489  halfLength[0] = -1.0f;
490  for(int j = 0; j < length; ++j)
491    {
492      tmpLength = fabs(p0.distancePoint(vertices[j]));
493      if( tmpLength > halfLength[0])
494        halfLength[0] = tmpLength;
495    }
496
497  halfLength[1] = -1.0f;
498  for(int j = 0; j < length; ++j)
499    {
500      tmpLength = fabs(p1.distancePoint(vertices[j]));
501      if( tmpLength > halfLength[1])
502        halfLength[1] = tmpLength;
503    }
504
505  halfLength[2] = -1.0f;
506  for(int j = 0; j < length; ++j)
507    {
508      tmpLength = fabs(p2.distancePoint(vertices[j]));
509      if( tmpLength > halfLength[2])
510        halfLength[2] = tmpLength;
511    }
512
513
514
515  /* get the maximal dimensions of the body in all directions */
516    maxLength[0] = p0.distancePoint(vertices[0]);
517    minLength[0] = p0.distancePoint(vertices[0]);
518   for(int j = 0; j < length; ++j)
519   {
520     tmpLength = p0.distancePoint(vertices[j]);
521     if( tmpLength > maxLength[0])
522       maxLength[0] = tmpLength;
523     else if( tmpLength < minLength[0])
524       minLength[0] = tmpLength;
525   }
526
527   maxLength[1] = p1.distancePoint(vertices[0]);
528   minLength[1] = p1.distancePoint(vertices[0]);
529   for(int j = 0; j < length; ++j)
530   {
531     tmpLength = p1.distancePoint(vertices[j]);
532     if( tmpLength > maxLength[1])
533       maxLength[1] = tmpLength;
534     else if( tmpLength < minLength[1])
535       minLength[1] = tmpLength;
536   }
537
538   maxLength[2] = p2.distancePoint(vertices[0]);
539   minLength[2] = p2.distancePoint(vertices[0]);
540   for(int j = 0; j < length; ++j)
541   {
542     tmpLength = p2.distancePoint(vertices[j]);
543     if( tmpLength > maxLength[2])
544       maxLength[2] = tmpLength;
545     else if( tmpLength < minLength[2])
546       minLength[2] = tmpLength;
547   }
548
549
550   /* calculate the real centre of the body by using the axis length */
551   float centerOffset[3];
552   float newHalfLength[3];
553   for(int i = 0; i < 3; ++i)
554     {
555       PRINTF(3)("max: %f, min: %f \n", maxLength[i], minLength[i]);
556       centerOffset[i] = (maxLength[i] + minLength[i]) / 2.0f;       // min length is negatie
557       newHalfLength[i] = (maxLength[i] - minLength[i]) / 2.0f;      // min length is negative
558       *box->center +=  (box->axis[i] * centerOffset[i]);            // update the new center vector
559       halfLength[i] = newHalfLength[i];
560     }
561
562
563
564  box->halfLength = halfLength;
565  PRINTF(3)("-- Written Axis to obb\n");
566  PRINTF(3)("-- Finished Calculating Attributes\n");
567}
568
569
570
571/**
572  \brief this separates an ob-box in the middle
573* @param box: the box to separate
574
575  this will separate the box into to smaller boxes. the separation is done along the middle of the longest axis
576 */
577void OBBTreeNode::forkBox(OBB* box)
578{
579  /* get the longest axis of the box */
580  float               aLength = -1.0f;                     //!< the length of the longest axis
581  int                 axisIndex = 0;                       //!< this is the nr of the longest axis
582
583  for(int i = 0; i < 3; ++i)
584  {
585    if( aLength < box->halfLength[i])
586    {
587      aLength = box->halfLength[i];
588      axisIndex = i;
589    }
590  }
591
592   PRINTF(3)("longest axis is: nr %i with a half-length of: %f\n", axisIndex, aLength);
593
594
595  /* get the closest vertex near the center */
596  float               dist = 999999.0f;                    //!< the smallest distance to each vertex
597  float               tmpDist;                             //!< temporary distance
598  int                 vertexIndex;
599  Plane               middlePlane(box->axis[axisIndex], *box->center); //!< the middle plane
600
601  vertexIndex = 0;
602  for(int i = 0; i < box->numOfVertices; ++i)
603  {
604    tmpDist = fabs(middlePlane.distancePoint(box->vertices[i]));
605    if( tmpDist < dist)
606    {
607      dist = tmpDist;
608      vertexIndex = i;
609    }
610  }
611
612  PRINTF(3)("\nthe clostest vertex is nr: %i, with a dist of: %f\n", vertexIndex ,dist);
613
614
615  /* now definin the separation plane through this specified nearest point and partition
616  the points depending on which side they are located
617  */
618  tList<const sVec3D>      partition1;                           //!< the vertex partition 1
619  tList<const sVec3D>      partition2;                           //!< the vertex partition 2
620
621
622  PRINTF(3)("vertex index: %i, of %i\n", vertexIndex, box->numOfVertices);
623  this->separationPlane = new Plane(box->axis[axisIndex], box->vertices[vertexIndex]);  //!< separation plane
624  this->sepPlaneCenter = &box->vertices[vertexIndex];
625  this->longestAxisIndex = axisIndex;
626
627  for(int i = 0; i < box->numOfVertices; ++i)
628  {
629    if( i == vertexIndex) continue;
630    tmpDist = this->separationPlane->distancePoint(box->vertices[i]);
631    if( tmpDist > 0.0)
632      partition1.add(&box->vertices[i]); /* positive numbers plus zero */
633    else
634      partition2.add(&box->vertices[i]); /* negatice numbers */
635  }
636  partition1.add(&box->vertices[vertexIndex]);
637  partition2.add(&box->vertices[vertexIndex]);
638
639  PRINTF(3)("\npartition1: got %i vertices/ partition 2: got %i vertices\n", partition1.getSize(), partition2.getSize());
640
641
642  /* now comes the separation into two different sVec3D arrays */
643  tIterator<const sVec3D>* iterator;                       //!< the iterator to go through the lists
644  const sVec3D*      element;                              //!< the elements
645  int                index;                                //!< index storage place
646  sVec3D*            vertList1;                            //!< the vertex list 1
647  sVec3D*            vertList2;                            //!< the vertex list 2
648
649  vertList1 = new sVec3D[partition1.getSize()];
650  vertList2 = new sVec3D[partition2.getSize()];
651
652  iterator = partition1.getIterator();
653  element = iterator->firstElement();
654  index = 0;
655  while( element != NULL)
656  {
657    vertList1[index][0] = element[0][0];
658    vertList1[index][1] = element[0][1];
659    vertList1[index][2] = element[0][2];
660    ++index;
661    element = iterator->nextElement();
662  }
663
664//   PRINTF(0)("\npartition 1:\n");
665//   for(int i = 0; i < partition1.getSize(); ++i)
666//   {
667//     PRINTF(0)("v[%i][0] = %f,\tv[%i][1] = %f,\tv[%i][1] = %f\n", i, vertList1[i][0], i, vertList1[i][1], i, vertList1[i][2]);
668//   }
669
670  iterator = partition2.getIterator();
671  element = iterator->firstElement();
672  index = 0;
673  while( element != NULL)
674  {
675    vertList2[index][0] = element[0][0];
676    vertList2[index][1] = element[0][1];
677    vertList2[index][2] = element[0][2];
678    ++index;
679    element = iterator->nextElement();
680  }
681
682  this->tmpVert1 = vertList1;
683  this->tmpVert2 = vertList2;
684  this->tmpLen1 = partition1.getSize();
685  this->tmpLen2 = partition2.getSize();
686
687  delete iterator;
688
689//   PRINTF(0)("\npartition 2:\n");
690//   for(int i = 0; i < partition2.getSize(); ++i)
691//   {
692//     PRINTF(0)("v[%i][0] = %f,\tv[%i][1] = %f,\tv[%i][1] = %f\n", i, vertList2[i][0], i,  vertList2[i][1], i, vertList2[i][2]);
693//   }
694}
695
696
697
698
699void OBBTreeNode::collideWith(BVTreeNode* treeNode, WorldEntity* nodeA, WorldEntity* nodeB)
700{
701  PRINTF(3)("collideWith\n");
702  /* if the obb overlap, make subtests: check which node is realy overlaping  */
703  PRINT(3)("Checking OBB %i vs %i: ", this->getIndex(), treeNode->getIndex());
704  if( unlikely(treeNode == NULL)) return;
705
706  if( this->overlapTest(this->bvElement, ((OBBTreeNode*)treeNode)->bvElement, nodeA, nodeB))
707  {
708    PRINTF(3)("collision @ lvl %i, object %s vs. %s, (%p, %p)\n", this->depth, nodeA->getClassName(), nodeB->getClassName(), this->nodeLeft, this->nodeRight);
709
710    /* check if left node overlaps */
711    if( likely( this->nodeLeft != NULL))
712    {
713      PRINT(3)("Checking OBB %i vs %i: ", this->nodeLeft->getIndex(), treeNode->getIndex());
714      if( this->overlapTest(this->nodeLeft->bvElement, ((OBBTreeNode*)treeNode)->bvElement, nodeA, nodeB))
715      {
716        this->nodeLeft->collideWith(((OBBTreeNode*)treeNode)->nodeLeft, nodeA, nodeB);
717        this->nodeLeft->collideWith(((OBBTreeNode*)treeNode)->nodeRight, nodeA, nodeB);
718      }
719    }
720    /* check if right node overlaps */
721    if( likely( this->nodeRight != NULL))
722    {
723      PRINT(3)("Checking OBB %i vs %i: ", this->nodeRight->getIndex(), treeNode->getIndex());
724      if(this->overlapTest(this->nodeRight->bvElement, ((OBBTreeNode*)treeNode)->bvElement, nodeA, nodeB))
725      {
726       this->nodeRight->collideWith(((OBBTreeNode*)treeNode)->nodeLeft, nodeA, nodeB);
727       this->nodeRight->collideWith(((OBBTreeNode*)treeNode)->nodeRight, nodeA, nodeB);
728      }
729    }
730
731    /* so there is a collision and this is the last box in the tree (i.e. leaf) */
732    if( unlikely(this->nodeRight == NULL && this->nodeLeft == NULL))
733    {
734      nodeA->collidesWith(nodeB, *((OBBTreeNode*)treeNode)->bvElement->center);
735
736      nodeB->collidesWith(nodeA, *this->bvElement->center);
737    }
738
739  }
740}
741
742
743
744bool OBBTreeNode::overlapTest(OBB* boxA, OBB* boxB, WorldEntity* nodeA, WorldEntity* nodeB)
745{
746  /* first check all axis */
747  Vector t;
748  float rA = 0.0f;
749  float rB = 0.0f;
750  Vector l;
751  Vector rotAxisA[3];
752  Vector rotAxisB[3];
753
754  rotAxisA[0] =  nodeA->getAbsDir().apply(boxA->axis[0]);
755  rotAxisA[1] =  nodeA->getAbsDir().apply(boxA->axis[1]);
756  rotAxisA[2] =  nodeA->getAbsDir().apply(boxA->axis[2]);
757
758  rotAxisB[0] =  nodeB->getAbsDir().apply(boxB->axis[0]);
759  rotAxisB[1] =  nodeB->getAbsDir().apply(boxB->axis[1]);
760  rotAxisB[2] =  nodeB->getAbsDir().apply(boxB->axis[2]);
761
762  t = nodeA->getAbsCoor() + nodeA->getAbsDir().apply(*boxA->center) - ( nodeB->getAbsCoor() + nodeB->getAbsDir().apply(*boxB->center));
763
764//   printf("\n");
765//   printf("(%f, %f, %f) -> (%f, %f, %f)\n", boxA->axis[0].x, boxA->axis[0].y, boxA->axis[0].z, rotAxisA[0].x, rotAxisA[0].y, rotAxisA[0].z);
766//   printf("(%f, %f, %f) -> (%f, %f, %f)\n", boxA->axis[1].x, boxA->axis[1].y, boxA->axis[1].z, rotAxisA[1].x, rotAxisA[1].y, rotAxisA[1].z);
767//   printf("(%f, %f, %f) -> (%f, %f, %f)\n", boxA->axis[2].x, boxA->axis[2].y, boxA->axis[2].z, rotAxisA[2].x, rotAxisA[2].y, rotAxisA[2].z);
768//
769//   printf("(%f, %f, %f) -> (%f, %f, %f)\n", boxB->axis[0].x, boxB->axis[0].y, boxB->axis[0].z, rotAxisB[0].x, rotAxisB[0].y, rotAxisB[0].z);
770//   printf("(%f, %f, %f) -> (%f, %f, %f)\n", boxB->axis[1].x, boxB->axis[1].y, boxB->axis[1].z, rotAxisB[1].x, rotAxisB[1].y, rotAxisB[1].z);
771//   printf("(%f, %f, %f) -> (%f, %f, %f)\n", boxB->axis[2].x, boxB->axis[2].y, boxB->axis[2].z, rotAxisB[2].x, rotAxisB[2].y, rotAxisB[2].z);
772
773
774  /* All 3 axis of the object A */
775  for( int j = 0; j < 3; ++j)
776  {
777    rA = 0.0f;
778    rB = 0.0f;
779    l = rotAxisA[j];
780
781    rA += fabs(boxA->halfLength[0] * rotAxisA[0].dot(l));
782    rA += fabs(boxA->halfLength[1] * rotAxisA[1].dot(l));
783    rA += fabs(boxA->halfLength[2] * rotAxisA[2].dot(l));
784
785    rB += fabs(boxB->halfLength[0] * rotAxisB[0].dot(l));
786    rB += fabs(boxB->halfLength[1] * rotAxisB[1].dot(l));
787    rB += fabs(boxB->halfLength[2] * rotAxisB[2].dot(l));
788
789    PRINTF(3)("s = %f, rA+rB = %f\n", fabs(t.dot(l)), rA+rB);
790
791    if( (rA + rB) < fabs(t.dot(l)))
792    {
793      PRINT(3)("no Collision\n");
794      return false;
795    }
796  }
797
798  /* All 3 axis of the object B */
799  for( int j = 0; j < 3; ++j)
800  {
801    rA = 0.0f;
802    rB = 0.0f;
803    l = rotAxisB[j];
804
805    rA += fabs(boxA->halfLength[0] * rotAxisA[0].dot(l));
806    rA += fabs(boxA->halfLength[1] * rotAxisA[1].dot(l));
807    rA += fabs(boxA->halfLength[2] * rotAxisA[2].dot(l));
808
809    rB += fabs(boxB->halfLength[0] * rotAxisB[0].dot(l));
810    rB += fabs(boxB->halfLength[1] * rotAxisB[1].dot(l));
811    rB += fabs(boxB->halfLength[2] * rotAxisB[2].dot(l));
812
813    PRINTF(3)("s = %f, rA+rB = %f\n", fabs(t.dot(l)), rA+rB);
814
815    if( (rA + rB) < fabs(t.dot(l)))
816    {
817      PRINT(3)("no Collision\n");
818      return false;
819    }
820  }
821
822
823  /* Now check for all face cross products */
824
825  for( int j = 0; j < 3; ++j)
826  {
827    for(int k = 0; k < 3; ++k )
828    {
829      rA = 0.0f;
830      rB = 0.0f;
831      l = rotAxisA[j].cross(rotAxisB[k]);
832
833      rA += fabs(boxA->halfLength[0] * rotAxisA[0].dot(l));
834      rA += fabs(boxA->halfLength[1] * rotAxisA[1].dot(l));
835      rA += fabs(boxA->halfLength[2] * rotAxisA[2].dot(l));
836
837      rB += fabs(boxB->halfLength[0] * rotAxisB[0].dot(l));
838      rB += fabs(boxB->halfLength[1] * rotAxisB[1].dot(l));
839      rB += fabs(boxB->halfLength[2] * rotAxisB[2].dot(l));
840
841      PRINTF(3)("s = %f, rA+rB = %f\n", fabs(t.dot(l)), rA+rB);
842
843      if( (rA + rB) < fabs(t.dot(l)))
844      {
845        PRINT(3)("keine Kollision\n");
846        return false;
847      }
848    }
849  }
850
851
852  boxA->bCollided = true; /* use this ONLY(!!!!) for drawing operations */
853  boxB->bCollided = true;
854  PRINT(3)("Kollision!\n");
855  return true;
856}
857
858
859
860
861
862void OBBTreeNode::drawBV(int depth, int drawMode, const Vector& color,  bool top) const
863{
864
865  /* draw the model itself, there is some problem concerning this: the vertices are drawn multiple times */
866  if( drawMode & DRAW_MODEL || drawMode & DRAW_ALL)
867  {
868    if( !(drawMode & DRAW_SINGLE && depth != 0))
869    {
870      if( drawMode & DRAW_POINTS)
871        glBegin(GL_POINTS);
872      for(int i = 0; i < this->bvElement->numOfVertices; ++i)
873      {
874        if( drawMode & DRAW_POINTS)
875          glVertex3f(this->bvElement->vertices[i][0], this->bvElement->vertices[i][1], this->bvElement->vertices[i][2]);
876        else
877        {
878          glPushMatrix();
879          glTranslatef(this->bvElement->vertices[i][0], this->bvElement->vertices[i][1], this->bvElement->vertices[i][2]);
880          gluSphere(OBBTreeNode_sphereObj, 0.1, 10, 10);
881          glPopMatrix();
882        }
883      }
884      if( drawMode & DRAW_POINTS)
885        glEnd();
886    }
887  }
888
889  if (top)
890  {
891    glPushAttrib(GL_ENABLE_BIT);
892    glDisable(GL_LIGHTING);
893    glDisable(GL_TEXTURE_2D);
894  }
895  glColor3f(color.x, color.y, color.z);
896
897
898  /* draw world axes */
899  if( drawMode & DRAW_BV_AXIS)
900  {
901    glBegin(GL_LINES);
902    glColor3f(1.0, 0.0, 0.0);
903    glVertex3f(0.0, 0.0, 0.0);
904    glVertex3f(3.0, 0.0, 0.0);
905
906    glColor3f(0.0, 1.0, 0.0);
907    glVertex3f(0.0, 0.0, 0.0);
908    glVertex3f(0.0, 3.0, 0.0);
909
910    glColor3f(0.0, 0.0, 1.0);
911    glVertex3f(0.0, 0.0, 0.0);
912    glVertex3f(0.0, 0.0, 3.0);
913    glEnd();
914  }
915
916
917  if( drawMode & DRAW_BV_AXIS || drawMode & DRAW_ALL)
918  {
919    if( !(drawMode & DRAW_SINGLE && depth != 0))
920    {
921      /* draw the obb axes */
922      glBegin(GL_LINES);
923      glColor3f(0.0, 0.4, 0.3);
924      glVertex3f(this->bvElement->center->x, this->bvElement->center->y, this->bvElement->center->z);
925      glVertex3f(this->bvElement->center->x + this->bvElement->axis[0].x * this->bvElement->halfLength[0],
926                 this->bvElement->center->y + this->bvElement->axis[0].y * this->bvElement->halfLength[0],
927                 this->bvElement->center->z + this->bvElement->axis[0].z * this->bvElement->halfLength[0]);
928
929      glVertex3f(this->bvElement->center->x, this->bvElement->center->y, this->bvElement->center->z);
930      glVertex3f(this->bvElement->center->x + this->bvElement->axis[1].x * this->bvElement->halfLength[1],
931                 this->bvElement->center->y + this->bvElement->axis[1].y * this->bvElement->halfLength[1],
932                 this->bvElement->center->z + this->bvElement->axis[1].z * this->bvElement->halfLength[1]);
933
934      glVertex3f(this->bvElement->center->x, this->bvElement->center->y, this->bvElement->center->z);
935      glVertex3f(this->bvElement->center->x + this->bvElement->axis[2].x * this->bvElement->halfLength[2],
936                 this->bvElement->center->y + this->bvElement->axis[2].y * this->bvElement->halfLength[2],
937                 this->bvElement->center->z + this->bvElement->axis[2].z * this->bvElement->halfLength[2]);
938      glEnd();
939    }
940  }
941
942
943  /* DRAW POLYGONS */
944  if( drawMode & DRAW_BV_POLYGON || drawMode & DRAW_ALL || drawMode & DRAW_BV_BLENDED)
945  {
946    if (top)
947    {
948      glEnable(GL_BLEND);
949      glBlendFunc(GL_SRC_ALPHA, GL_ONE);
950    }
951
952    if(this->nodeLeft == NULL || this->nodeRight == NULL)
953      depth = 0;
954    if( !(drawMode & DRAW_SINGLE && depth != 0))
955    {
956    Vector cen = *this->bvElement->center;
957    Vector* axis = this->bvElement->axis;
958    float* len = this->bvElement->halfLength;
959
960    if( this->bvElement->bCollided)
961    {
962      glColor4f(1.0, 1.0, 1.0, .5); // COLLISION COLOR
963    }
964    else if( drawMode & DRAW_BV_BLENDED)
965    {
966      glColor4f(color.x, color.y, color.z, .5);
967    }
968
969    /* draw bounding box */
970    if( drawMode & DRAW_BV_BLENDED)
971      glBegin(GL_QUADS);
972    else
973      glBegin(GL_LINE_LOOP);
974    glVertex3f(cen.x + axis[0].x * len[0] + axis[1].x * len[1] + axis[2].x * len[2],
975               cen.y + axis[0].y * len[0] + axis[1].y * len[1] + axis[2].y * len[2],
976               cen.z + axis[0].z * len[0] + axis[1].z * len[1] + axis[2].z * len[2]);
977    glVertex3f(cen.x + axis[0].x * len[0] + axis[1].x * len[1] - axis[2].x * len[2],
978               cen.y + axis[0].y * len[0] + axis[1].y * len[1] - axis[2].y * len[2],
979               cen.z + axis[0].z * len[0] + axis[1].z * len[1] - axis[2].z * len[2]);
980    glVertex3f(cen.x + axis[0].x * len[0] - axis[1].x * len[1] - axis[2].x * len[2],
981               cen.y + axis[0].y * len[0] - axis[1].y * len[1] - axis[2].y * len[2],
982               cen.z + axis[0].z * len[0] - axis[1].z * len[1] - axis[2].z * len[2]);
983    glVertex3f(cen.x + axis[0].x * len[0] - axis[1].x * len[1] + axis[2].x * len[2],
984               cen.y + axis[0].y * len[0] - axis[1].y * len[1] + axis[2].y * len[2],
985               cen.z + axis[0].z * len[0] - axis[1].z * len[1] + axis[2].z * len[2]);
986    glEnd();
987
988    if( drawMode & DRAW_BV_BLENDED)
989      glBegin(GL_QUADS);
990    else
991      glBegin(GL_LINE_LOOP);
992    glVertex3f(cen.x + axis[0].x * len[0] - axis[1].x * len[1] + axis[2].x * len[2],
993               cen.y + axis[0].y * len[0] - axis[1].y * len[1] + axis[2].y * len[2],
994               cen.z + axis[0].z * len[0] - axis[1].z * len[1] + axis[2].z * len[2]);
995    glVertex3f(cen.x + axis[0].x * len[0] - axis[1].x * len[1] - axis[2].x * len[2],
996               cen.y + axis[0].y * len[0] - axis[1].y * len[1] - axis[2].y * len[2],
997               cen.z + axis[0].z * len[0] - axis[1].z * len[1] - axis[2].z * len[2]);
998    glVertex3f(cen.x - axis[0].x * len[0] - axis[1].x * len[1] - axis[2].x * len[2],
999               cen.y - axis[0].y * len[0] - axis[1].y * len[1] - axis[2].y * len[2],
1000               cen.z - axis[0].z * len[0] - axis[1].z * len[1] - axis[2].z * len[2]);
1001    glVertex3f(cen.x - axis[0].x * len[0] - axis[1].x * len[1] + axis[2].x * len[2],
1002               cen.y - axis[0].y * len[0] - axis[1].y * len[1] + axis[2].y * len[2],
1003               cen.z - axis[0].z * len[0] - axis[1].z * len[1] + axis[2].z * len[2]);
1004    glEnd();
1005
1006    if( drawMode & DRAW_BV_BLENDED)
1007      glBegin(GL_QUADS);
1008    else
1009      glBegin(GL_LINE_LOOP);
1010    glVertex3f(cen.x - axis[0].x * len[0] - axis[1].x * len[1] + axis[2].x * len[2],
1011               cen.y - axis[0].y * len[0] - axis[1].y * len[1] + axis[2].y * len[2],
1012               cen.z - axis[0].z * len[0] - axis[1].z * len[1] + axis[2].z * len[2]);
1013    glVertex3f(cen.x - axis[0].x * len[0] - axis[1].x * len[1] - axis[2].x * len[2],
1014               cen.y - axis[0].y * len[0] - axis[1].y * len[1] - axis[2].y * len[2],
1015               cen.z - axis[0].z * len[0] - axis[1].z * len[1] - axis[2].z * len[2]);
1016    glVertex3f(cen.x - axis[0].x * len[0] + axis[1].x * len[1] - axis[2].x * len[2],
1017               cen.y - axis[0].y * len[0] + axis[1].y * len[1] - axis[2].y * len[2],
1018               cen.z - axis[0].z * len[0] + axis[1].z * len[1] - axis[2].z * len[2]);
1019    glVertex3f(cen.x - axis[0].x * len[0] + axis[1].x * len[1] + axis[2].x * len[2],
1020               cen.y - axis[0].y * len[0] + axis[1].y * len[1] + axis[2].y * len[2],
1021               cen.z - axis[0].z * len[0] + axis[1].z * len[1] + axis[2].z * len[2]);
1022    glEnd();
1023
1024    if( drawMode & DRAW_BV_BLENDED)
1025      glBegin(GL_QUADS);
1026    else
1027      glBegin(GL_LINE_LOOP);
1028    glVertex3f(cen.x - axis[0].x * len[0] + axis[1].x * len[1] - axis[2].x * len[2],
1029               cen.y - axis[0].y * len[0] + axis[1].y * len[1] - axis[2].y * len[2],
1030               cen.z - axis[0].z * len[0] + axis[1].z * len[1] - axis[2].z * len[2]);
1031    glVertex3f(cen.x - axis[0].x * len[0] + axis[1].x * len[1] + axis[2].x * len[2],
1032               cen.y - axis[0].y * len[0] + axis[1].y * len[1] + axis[2].y * len[2],
1033               cen.z - axis[0].z * len[0] + axis[1].z * len[1] + axis[2].z * len[2]);
1034    glVertex3f(cen.x + axis[0].x * len[0] + axis[1].x * len[1] + axis[2].x * len[2],
1035               cen.y + axis[0].y * len[0] + axis[1].y * len[1] + axis[2].y * len[2],
1036               cen.z + axis[0].z * len[0] + axis[1].z * len[1] + axis[2].z * len[2]);
1037    glVertex3f(cen.x + axis[0].x * len[0] + axis[1].x * len[1] - axis[2].x * len[2],
1038               cen.y + axis[0].y * len[0] + axis[1].y * len[1] - axis[2].y * len[2],
1039               cen.z + axis[0].z * len[0] + axis[1].z * len[1] - axis[2].z * len[2]);
1040    glEnd();
1041
1042
1043    if( drawMode & DRAW_BV_BLENDED)
1044    {
1045      glBegin(GL_QUADS);
1046      glVertex3f(cen.x - axis[0].x * len[0] + axis[1].x * len[1] - axis[2].x * len[2],
1047                 cen.y - axis[0].y * len[0] + axis[1].y * len[1] - axis[2].y * len[2],
1048                 cen.z - axis[0].z * len[0] + axis[1].z * len[1] - axis[2].z * len[2]);
1049      glVertex3f(cen.x + axis[0].x * len[0] + axis[1].x * len[1] - axis[2].x * len[2],
1050                 cen.y + axis[0].y * len[0] + axis[1].y * len[1] - axis[2].y * len[2],
1051                 cen.z + axis[0].z * len[0] + axis[1].z * len[1] - axis[2].z * len[2]);
1052      glVertex3f(cen.x + axis[0].x * len[0] - axis[1].x * len[1] - axis[2].x * len[2],
1053                 cen.y + axis[0].y * len[0] - axis[1].y * len[1] - axis[2].y * len[2],
1054                 cen.z + axis[0].z * len[0] - axis[1].z * len[1] - axis[2].z * len[2]);
1055      glVertex3f(cen.x - axis[0].x * len[0] - axis[1].x * len[1] - axis[2].x * len[2],
1056                 cen.y - axis[0].y * len[0] - axis[1].y * len[1] - axis[2].y * len[2],
1057                 cen.z - axis[0].z * len[0] - axis[1].z * len[1] - axis[2].z * len[2]);
1058      glEnd();
1059
1060      glBegin(GL_QUADS);
1061      glVertex3f(cen.x - axis[0].x * len[0] + axis[1].x * len[1] + axis[2].x * len[2],
1062                 cen.y - axis[0].y * len[0] + axis[1].y * len[1] + axis[2].y * len[2],
1063                 cen.z - axis[0].z * len[0] + axis[1].z * len[1] + axis[2].z * len[2]);
1064      glVertex3f(cen.x + axis[0].x * len[0] + axis[1].x * len[1] + axis[2].x * len[2],
1065                 cen.y + axis[0].y * len[0] + axis[1].y * len[1] + axis[2].y * len[2],
1066                 cen.z + axis[0].z * len[0] + axis[1].z * len[1] + axis[2].z * len[2]);
1067      glVertex3f(cen.x + axis[0].x * len[0] - axis[1].x * len[1] + axis[2].x * len[2],
1068                 cen.y + axis[0].y * len[0] - axis[1].y * len[1] + axis[2].y * len[2],
1069                 cen.z + axis[0].z * len[0] - axis[1].z * len[1] + axis[2].z * len[2]);
1070      glVertex3f(cen.x - axis[0].x * len[0] - axis[1].x * len[1] + axis[2].x * len[2],
1071                 cen.y - axis[0].y * len[0] - axis[1].y * len[1] + axis[2].y * len[2],
1072                 cen.z - axis[0].z * len[0] - axis[1].z * len[1] + axis[2].z * len[2]);
1073      glEnd();
1074    }
1075
1076
1077    if( drawMode & DRAW_BV_BLENDED)
1078      glColor3f(color.x, color.y, color.z);
1079    }
1080
1081  }
1082
1083  /* DRAW SEPARATING PLANE */
1084  if( drawMode & DRAW_SEPARATING_PLANE || drawMode & DRAW_ALL)
1085  {
1086    if( !(drawMode & DRAW_SINGLE && depth != 0))
1087    {
1088      if( drawMode & DRAW_BV_BLENDED)
1089        glColor4f(color.x, color.y, color.z, .6);
1090
1091    /* now draw the separation plane */
1092    Vector a1 = this->bvElement->axis[(this->longestAxisIndex + 1)%3];
1093    Vector a2 = this->bvElement->axis[(this->longestAxisIndex + 2)%3];
1094    Vector c = *this->bvElement->center;
1095    float l1 = this->bvElement->halfLength[(this->longestAxisIndex + 1)%3];
1096    float l2 = this->bvElement->halfLength[(this->longestAxisIndex + 2)%3];
1097    glBegin(GL_QUADS);
1098    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);
1099    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);
1100    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);
1101    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);
1102    glEnd();
1103
1104    if( drawMode & DRAW_BV_BLENDED)
1105      glColor4f(color.x, color.y, color.z, 1.0);
1106
1107    }
1108  }
1109
1110
1111
1112  if (depth > 0)
1113  {
1114    if( this->nodeLeft != NULL)
1115      this->nodeLeft->drawBV(depth - 1, drawMode, Color::HSVtoRGB(Color::RGBtoHSV(color)+Vector(15.0,0.0,0.0)), false);
1116    if( this->nodeRight != NULL)
1117      this->nodeRight->drawBV(depth - 1, drawMode, Color::HSVtoRGB(Color::RGBtoHSV(color)+Vector(30.0,0.0,0.0)), false);
1118  }
1119  this->bvElement->bCollided = false;
1120
1121  if (top)
1122    glPopAttrib();
1123}
1124
1125
1126
1127void OBBTreeNode::debug() const
1128{
1129
1130  /*
1131  for(int i = 0; i < length; i++)
1132  {
1133  PRINTF(3)("vertex %i: %f, %f, %f\n", i, verticesList[i][0], verticesList[i][1], verticesList[i][2]);
1134}
1135  */
1136}
Note: See TracBrowser for help on using the repository browser.