Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

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

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

orxonox/trunk: now bounding box gets displayed very well

File size: 18.7 KB
Line 
1/*
2   orxonox - the future of 3D-vertical-scrollers
3
4   Copyright (C) 2004 orx
5
6   This program is free software; you can redistribute it and/or modify
7   it under the terms of the GNU General Public License as published by
8   the Free Software Foundation; either version 2, or (at your option)
9   any later version.
10
11   ### File Specific:
12   main-programmer: Patrick Boenzli
13   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 "vector.h"
23#include "abstract_model.h"
24
25#include <math.h>
26
27
28#define WANT_STREAM
29#define WANT_MATH
30#define WANT_FSTREAM
31
32
33#include "include.h"
34#include "newmat.h"
35#include "newmatap.h"
36#include "newmatio.h"
37
38
39
40
41using namespace std;
42
43
44/**
45   \brief standard constructor
46*/
47OBBTreeNode::OBBTreeNode ()
48{
49   this->setClassID(CL_OBB_TREE_NODE, "OBBTreeNode");
50
51}
52
53
54/**
55   \brief standard deconstructor
56
57*/
58OBBTreeNode::~OBBTreeNode ()
59{
60  // delete what has to be deleted here
61}
62
63
64
65/**
66   \brief creates a new BVTree or BVTree partition
67   \param depth: how much more depth-steps to go: if == 1 don't go any deeper!
68   \param verticesList: the list of vertices of the object - each vertices triple is interpreted as a triangle
69*/
70void OBBTreeNode::spawnBVTree(const int depth, sVec3D *verticesList, const int length)
71{
72  this->depth = depth;
73
74  if( likely( this->depth > 0))
75  {
76    this->bvElement = this->createBox();
77    this->calculateBoxAttributes(this->bvElement, verticesList, length);
78    this->forkBox(this->bvElement);
79  }
80}
81
82
83OBB* OBBTreeNode::createBox()
84{
85  return new OBB();
86}
87
88
89void OBBTreeNode::calculateBoxAttributes(OBB* box, sVec3D* verticesList, int length)
90{
91  float     facelet[length];                         //!< surface area of the i'th triangle of the convex hull
92  float     face;                                    //!< surface area of the entire convex hull
93  Vector    centroid[length];                        //!< centroid of the i'th convex hull
94  Vector    center;                                  //!< the center of the entire hull
95  Vector    p, q, r;                                 //!< holder of the polygon data, much more conveniant to work with Vector than sVec3d
96  Vector    t1, t2;                                  //!< temporary values
97  float     covariance[3][3];                        //!< the covariance matrix
98
99  this->numOfVertices = length;
100  this->vertices = verticesList;
101  box->vertices = verticesList;
102  box->numOfVertices = length;
103
104
105  /* fist compute all the convex hull face/facelets and centroids */
106  for(int i = 0; i < length; i+=3)          /* FIX-ME-QUICK: hops of 3, array indiscontinuity*/
107    {
108      p = verticesList[i];
109      q = verticesList[i +1];
110      r = verticesList[i + 2];
111
112      t1 = p - q; t2 = p - r;
113
114      /* finding the facelet surface via cross-product */
115      facelet[i] = 0.5f * fabs( t1.cross(t2).len() );
116      /* update the entire convex hull surface */
117      face += facelet[i];
118
119      /* calculate the cetroid of the hull triangles */
120      centroid[i] = (p + q + r) * 1/3;
121      /* now calculate the centroid of the entire convex hull, weighted average of triangle centroids */
122      center += centroid[i] * facelet[i];
123    }
124  /* take the average of the centroid sum */
125  center /= face;
126
127
128
129  /* now calculate the covariance matrix - if not written in three for-loops, it would compute faster: minor */
130  for(int j = 0; j < 3; ++j)
131    {
132      for(int k = 0; k < 3; ++k)
133        {
134          for(int i = 0; i < length; i+=3)
135            {
136              p = verticesList[i];
137              q = verticesList[i +1];
138              r = verticesList[i + 2];
139
140              covariance[j][k] = facelet[i] / (12.0f * face) * (9.0f * centroid[i][j] * centroid[i][k] + p[j]* p[k] +
141                                                                q[j] * q[k] + r[j]*r[k]) - center[j] * center[k];
142            }
143        }
144    }
145
146    printf("\nVertex Data:\n");
147    for(int i = 0; i < length; i++)
148    {
149      printf("vertex %i: %f, %f, %f\n", i, verticesList[i][0], verticesList[i][1], verticesList[i][2]);
150    }
151
152  printf("\nCovariance Matrix:\n");
153  for(int j = 0; j < 3; ++j)
154    {
155      printf(" |");
156      for(int k = 0; k < 3; ++k)
157        {
158          printf(" \b%f ", covariance[j][k]);
159        }
160      printf(" |\n");
161    }
162  printf("center: %f, %f, %f\n\n", center.x, center.y, center.z);
163
164
165  for(int i = 0; i < 3; ++i)
166    {
167
168      box->covarianceMatrix[i][0] = covariance[i][0];
169      box->covarianceMatrix[i][1] = covariance[i][1];
170      box->covarianceMatrix[i][3] = covariance[i][2];
171    }
172  *box->center = center;
173
174
175  /* now getting spanning vectors of the sub-space:
176     the eigenvectors of a symmertric matrix, such as the
177     covarience matrix are mutually orthogonal.
178     after normalizing them, they can be used as a the basis
179     vectors
180  */
181  Matrix                V(3,3);                               //!< for eigenvectors
182  DiagonalMatrix        D(3);                                 //!< for eigenvalues
183  SymmetricMatrix       C(3);                                 //!< for the covariance symmetrical matrix
184  Vector**              axis = new Vector*[3];                //!< the references to the obb axis
185
186  C(1,1) = covariance[0][0];
187  C(1,2) = covariance[0][1];
188  C(1,3) = covariance[0][2];
189  C(2,1) = covariance[1][0];
190  C(2,2) = covariance[1][1];
191  C(2,3) = covariance[1][2];
192  C(3,1) = covariance[2][0];
193  C(3,2) = covariance[2][1];
194  C(3,3) = covariance[2][2];
195
196  Jacobi(C, D, V);                                            /* do the jacobi decomposition */
197
198  printf("\nwe got a result! YES: \n");
199
200  for(int j = 1; j < 4; ++j)
201  {
202    printf(" |");
203    for(int k = 1; k < 4; ++k)
204    {
205      printf(" \b%f ", V(j, k));
206    }
207    printf(" |\n");
208  }
209
210  axis[0] = new Vector(V(1, 1), V(2, 1), V(3, 1));
211  axis[1] = new Vector(V(1, 2), V(2, 2), V(3, 2));
212  axis[2] = new Vector(V(1, 3), V(2, 3), V(3, 3));
213  box->axis = axis;
214
215  printf("\neigenvector: %f, %f, %f\n", box->axis[0]->x, box->axis[0]->y, box->axis[0]->z);
216  printf("eigenvector: %f, %f, %f\n", box->axis[1]->x, box->axis[1]->y, box->axis[1]->z);
217  printf("eigenvector: %f, %f, %f\n", box->axis[2]->x, box->axis[2]->y, box->axis[2]->z);
218
219
220  /* now get the axis length */
221  Line                ax[3];                                 //!< the axis
222  float*              halfLength = new float[3];             //!< half length of the axis
223  float               tmpLength;                             //!< tmp save point for the length
224  Plane               p0(*box->axis[0], *box->center);       //!< the axis planes
225  Plane               p1(*box->axis[1], *box->center);
226  Plane               p2(*box->axis[2], *box->center);
227
228  halfLength[0] = -1.0f;
229  for(int j = 0; j < length; ++j)
230  {
231    tmpLength = fabs(p0.distancePoint(vertices[j]));
232    if( tmpLength > halfLength[0])
233      halfLength[0] = tmpLength;
234  }
235
236  halfLength[1] = -1.0f;
237  for(int j = 0; j < length; ++j)
238  {
239    tmpLength = fabs(p1.distancePoint(vertices[j]));
240    if( tmpLength > halfLength[1])
241      halfLength[1] = tmpLength;
242  }
243
244  halfLength[2] = -1.0f;
245  for(int j = 0; j < length; ++j)
246  {
247    tmpLength = fabs(p2.distancePoint(vertices[j]));
248    if( tmpLength > halfLength[2])
249      halfLength[2] = tmpLength;
250  }
251
252  box->halfLength = halfLength;
253
254
255  printf("\nwe got length: \n");
256  for(int i = 0; i < 3; ++i)
257    printf("length[%i] = %f\n", i, box->halfLength[i]);
258}
259
260
261
262/**
263  \brief this separates an ob-box in the middle
264  \param box: the box to separate
265
266  this will separate the box into to smaller boxes. the separation is done along the middle of the longest axis
267 */
268void OBBTreeNode::forkBox(OBB* box)
269{
270  /* get the longest axis of the box */
271  float               aLength = -1.0f;                     //!< the length of the longest axis
272  int                 axisIndex = 0;                       //!< this is the nr of the longest axis
273
274  for(int i = 0; i < 3; ++i)
275  {
276    if( aLength < box->halfLength[i])
277    {
278      aLength = box->halfLength[i];
279      axisIndex = i;
280    }
281  }
282
283  printf("\nlongest axis is: nr %i with a half-length of: %f\n", axisIndex, aLength);
284
285
286  /* get the closest vertex near the center */
287  float               dist = 999999.0f;                    //!< the smallest distance to each vertex
288  float               tmpDist;                             //!< temporary distance
289  int                 vertexIndex;
290  Plane               middlePlane(*box->axis[axisIndex], *box->center); //!< the middle plane
291
292  for(int i = 0; i < box->numOfVertices; ++i)
293  {
294    tmpDist = fabs(middlePlane.distancePoint(box->vertices[i]));
295    if( tmpDist < dist)
296    {
297      dist = tmpDist;
298      vertexIndex = i;
299    }
300  }
301
302  printf("\nthe clostest vertex is nr: %i, with a dist of: %f\n", vertexIndex ,dist);
303
304
305  /* now definin the separation plane through this specified nearest point and partition
306    the points depending on which side they are located
307  */
308  Plane              separationPlane(*box->axis[axisIndex], box->vertices[vertexIndex]);  //!< separation plane
309  tList<sVec3D>      partition1;                           //!< the vertex partition 1
310  tList<sVec3D>      partition2;                           //!< the vertex partition 2
311
312  for(int i = 0; i < box->numOfVertices; ++i)
313  {
314    if( separationPlane.distancePoint(box->vertices[i]) > 0.0f)
315      partition1.add(&box->vertices[i]);
316    else
317      partition2.add(&box->vertices[i]);
318  }
319  partition1.add(&box->vertices[vertexIndex]);
320
321  printf("\npartition1: got %i vertices/ partition 2: got %i vertices\n", partition1.getSize(), partition2.getSize());
322
323
324  /* now comes the separation into two different sVec3D arrays */
325  tIterator<sVec3D>* iterator;                             //!< the iterator to go through the lists
326  sVec3D*            element;                              //!< the elements
327  int                index;                                //!< index storage place
328  sVec3D*            vertList1;                            //!< the vertex list 1
329  sVec3D*            vertList2;                            //!< the vertex list 2
330
331  vertList1 = new sVec3D[partition1.getSize()];
332  vertList2 = new sVec3D[partition2.getSize()];
333
334  iterator = partition1.getIterator();
335  element = iterator->nextElement();
336  index = 0;
337  while( element != NULL)
338  {
339    vertList1[index][0] = element[0][0];
340    vertList1[index][1] = element[0][1];
341    vertList1[index][2] = element[0][2];
342    ++index;
343    element = iterator->nextElement();
344  }
345
346  printf("\npartition 1:\n");
347  for(int i = 0; i < partition1.getSize(); ++i)
348  {
349    printf("v[%i][0] = %f\n", i, vertList1[i][0]);
350    printf("v[%i][1] = %f\n", i, vertList1[i][1]);
351    printf("v[%i][2] = %f\n", i, vertList1[i][2]);
352  }
353
354  iterator = partition2.getIterator();
355  element = iterator->nextElement();
356  index = 0;
357  while( element != NULL)
358  {
359    vertList2[index][0] = element[0][0];
360    vertList2[index][1] = element[0][1];
361    vertList2[index][2] = element[0][2];
362    ++index;
363    element = iterator->nextElement();
364  }
365
366  printf("\npartition 2:\n");
367  for(int i = 0; i < partition2.getSize(); ++i)
368  {
369    printf("v[%i][0] = %f\n", i, vertList2[i][0]);
370    printf("v[%i][1] = %f\n", i, vertList2[i][1]);
371    printf("v[%i][2] = %f\n", i, vertList2[i][2]);
372  }
373
374  /* now spawn the obb tree: create the nodes and descent */
375  OBBTreeNode*       node1 = new OBBTreeNode();
376  OBBTreeNode*       node2 = new OBBTreeNode();
377
378  this->nodeLeft = node1;
379  this->nodeRight = node2;
380
381  this->nodeLeft->spawnBVTree(this->depth - 1, vertList1, partition1.getSize());
382  this->nodeRight->spawnBVTree(this->depth - 1, vertList2, partition2.getSize());
383}
384
385
386void OBBTreeNode::collideWith(const BVTree &tree)
387{}
388
389
390void OBBTreeNode::drawBV(int currentDepth, const int depth) const
391{
392//   glBegin(GL_LINE_LOOP);
393//   glColor3f(1.0, 1.0, 1.0);
394//   for(int i = 0; i < this->bvElement->numOfVertices; ++i)
395//     {
396//       glVertex3f(this->bvElement->vertices[i][0], this->bvElement->vertices[i][1], this->bvElement->vertices[i][2]);
397//       //printf("v(%f, %f, %f)\n", this->vertices[i][0], this->vertices[i][1], this->vertices[i][2]);
398//     }
399//   glEnd();
400}
401
402
403void OBBTreeNode::drawBVPolygon(int currentDepth, const int depth) const
404{
405
406  OBBTree::material->select();
407
408  /* draw world axes */
409  glBegin(GL_LINES);
410  glColor3f(0.0, 0.4, 0.3);
411  glVertex3f(0.0, 0.0, 0.0);
412  glVertex3f(3.0, 0.0, 0.0);
413
414  glVertex3f(0.0, 0.0, 0.0);
415  glVertex3f(0.0, 3.0, 0.0);
416
417  glVertex3f(0.0, 0.0, 0.0);
418  glVertex3f(0.0, 0.0, 3.0);
419  glEnd();
420
421
422
423  /* draw the obb axes */
424  glBegin(GL_LINES);
425  glColor3f(0.0, 0.4, 0.3);
426  glVertex3f(this->bvElement->center->x, this->bvElement->center->y, this->bvElement->center->z);
427  glVertex3f(this->bvElement->center->x + this->bvElement->axis[0]->x * this->bvElement->halfLength[0],
428             this->bvElement->center->y + this->bvElement->axis[0]->y * this->bvElement->halfLength[0],
429             this->bvElement->center->z + this->bvElement->axis[0]->z * this->bvElement->halfLength[0]);
430
431  glVertex3f(this->bvElement->center->x, this->bvElement->center->y, this->bvElement->center->z);
432  glVertex3f(this->bvElement->center->x + this->bvElement->axis[1]->x * this->bvElement->halfLength[1],
433             this->bvElement->center->y + this->bvElement->axis[1]->y * this->bvElement->halfLength[1],
434             this->bvElement->center->z + this->bvElement->axis[1]->z * this->bvElement->halfLength[1]);
435
436  glVertex3f(this->bvElement->center->x, this->bvElement->center->y, this->bvElement->center->z);
437  glVertex3f(this->bvElement->center->x + this->bvElement->axis[2]->x * this->bvElement->halfLength[2],
438             this->bvElement->center->y + this->bvElement->axis[2]->y * this->bvElement->halfLength[2],
439             this->bvElement->center->z + this->bvElement->axis[2]->z * this->bvElement->halfLength[2]);
440  glEnd();
441
442
443  Vector cen = *this->bvElement->center;
444  Vector** axis = this->bvElement->axis;
445  float* len = this->bvElement->halfLength;
446
447  /* draw bounding box */
448  glBegin(GL_LINE_LOOP);
449  glColor3f(0.3, 0.4, 0.7);
450  glVertex3f(cen.x + axis[0]->x * len[0] + axis[1]->x * len[1] + axis[2]->x * len[2],
451             cen.y + axis[0]->y * len[0] + axis[1]->y * len[1] + axis[2]->y * len[2],
452             cen.z + axis[0]->z * len[0] + axis[1]->z * len[1] + axis[2]->z * len[2]);
453  glVertex3f(cen.x + axis[0]->x * len[0] + axis[1]->x * len[1] - axis[2]->x * len[2],
454             cen.y + axis[0]->y * len[0] + axis[1]->y * len[1] - axis[2]->y * len[2],
455             cen.z + axis[0]->z * len[0] + axis[1]->z * len[1] - axis[2]->z * len[2]);
456  glVertex3f(cen.x + axis[0]->x * len[0] - axis[1]->x * len[1] - axis[2]->x * len[2],
457             cen.y + axis[0]->y * len[0] - axis[1]->y * len[1] - axis[2]->y * len[2],
458             cen.z + axis[0]->z * len[0] - axis[1]->z * len[1] - axis[2]->z * len[2]);
459  glVertex3f(cen.x + axis[0]->x * len[0] - axis[1]->x * len[1] + axis[2]->x * len[2],
460             cen.y + axis[0]->y * len[0] - axis[1]->y * len[1] + axis[2]->y * len[2],
461             cen.z + axis[0]->z * len[0] - axis[1]->z * len[1] + axis[2]->z * len[2]);
462  glEnd();
463
464  glBegin(GL_LINE_LOOP);
465  glVertex3f(cen.x + axis[0]->x * len[0] - axis[1]->x * len[1] + axis[2]->x * len[2],
466             cen.y + axis[0]->y * len[0] - axis[1]->y * len[1] + axis[2]->y * len[2],
467             cen.z + axis[0]->z * len[0] - axis[1]->z * len[1] + axis[2]->z * len[2]);
468  glVertex3f(cen.x + axis[0]->x * len[0] - axis[1]->x * len[1] - axis[2]->x * len[2],
469             cen.y + axis[0]->y * len[0] - axis[1]->y * len[1] - axis[2]->y * len[2],
470             cen.z + axis[0]->z * len[0] - axis[1]->z * len[1] - axis[2]->z * len[2]);
471  glVertex3f(cen.x - axis[0]->x * len[0] - axis[1]->x * len[1] - axis[2]->x * len[2],
472             cen.y - axis[0]->y * len[0] - axis[1]->y * len[1] - axis[2]->y * len[2],
473             cen.z - axis[0]->z * len[0] - axis[1]->z * len[1] - axis[2]->z * len[2]);
474  glVertex3f(cen.x - axis[0]->x * len[0] - axis[1]->x * len[1] + axis[2]->x * len[2],
475             cen.y - axis[0]->y * len[0] - axis[1]->y * len[1] + axis[2]->y * len[2],
476             cen.z - axis[0]->z * len[0] - axis[1]->z * len[1] + axis[2]->z * len[2]);
477  glEnd();
478
479  glBegin(GL_LINE_LOOP);
480  glVertex3f(cen.x - axis[0]->x * len[0] - axis[1]->x * len[1] + axis[2]->x * len[2],
481             cen.y - axis[0]->y * len[0] - axis[1]->y * len[1] + axis[2]->y * len[2],
482             cen.z - axis[0]->z * len[0] - axis[1]->z * len[1] + axis[2]->z * len[2]);
483  glVertex3f(cen.x - axis[0]->x * len[0] - axis[1]->x * len[1] - axis[2]->x * len[2],
484             cen.y - axis[0]->y * len[0] - axis[1]->y * len[1] - axis[2]->y * len[2],
485             cen.z - axis[0]->z * len[0] - axis[1]->z * len[1] - axis[2]->z * len[2]);
486  glVertex3f(cen.x - axis[0]->x * len[0] + axis[1]->x * len[1] - axis[2]->x * len[2],
487             cen.y - axis[0]->y * len[0] + axis[1]->y * len[1] - axis[2]->y * len[2],
488             cen.z - axis[0]->z * len[0] + axis[1]->z * len[1] - axis[2]->z * len[2]);
489  glVertex3f(cen.x - axis[0]->x * len[0] + axis[1]->x * len[1] + axis[2]->x * len[2],
490             cen.y - axis[0]->y * len[0] + axis[1]->y * len[1] + axis[2]->y * len[2],
491             cen.z - axis[0]->z * len[0] + axis[1]->z * len[1] + axis[2]->z * len[2]);
492  glEnd();
493
494  glBegin(GL_LINE_LOOP);
495  glVertex3f(cen.x - axis[0]->x * len[0] + axis[1]->x * len[1] - axis[2]->x * len[2],
496             cen.y - axis[0]->y * len[0] + axis[1]->y * len[1] - axis[2]->y * len[2],
497             cen.z - axis[0]->z * len[0] + axis[1]->z * len[1] - axis[2]->z * len[2]);
498  glVertex3f(cen.x - axis[0]->x * len[0] + axis[1]->x * len[1] + axis[2]->x * len[2],
499             cen.y - axis[0]->y * len[0] + axis[1]->y * len[1] + axis[2]->y * len[2],
500             cen.z - axis[0]->z * len[0] + axis[1]->z * len[1] + axis[2]->z * len[2]);
501  glVertex3f(cen.x + axis[0]->x * len[0] + axis[1]->x * len[1] + axis[2]->x * len[2],
502             cen.y + axis[0]->y * len[0] + axis[1]->y * len[1] + axis[2]->y * len[2],
503             cen.z + axis[0]->z * len[0] + axis[1]->z * len[1] + axis[2]->z * len[2]);
504  glVertex3f(cen.x + axis[0]->x * len[0] + axis[1]->x * len[1] - axis[2]->x * len[2],
505             cen.y + axis[0]->y * len[0] + axis[1]->y * len[1] - axis[2]->y * len[2],
506             cen.z + axis[0]->z * len[0] + axis[1]->z * len[1] - axis[2]->z * len[2]);
507  glEnd();
508
509/*
510  glVertex3f(cen.x - axis[0]->x * len[0] + axis[1]->x * len[1] - axis[2]->x * len[2],
511             cen.y - axis[0]->y * len[0] + axis[1]->y * len[1] - axis[2]->y * len[2],
512             cen.z - axis[0]->z * len[0] + axis[1]->z * len[1] - axis[2]->z * len[2]);
513  glVertex3f(cen.x - axis[0]->x * len[0] + axis[1]->x * len[1] + axis[2]->x * len[2],
514             cen.y - axis[0]->y * len[0] + axis[1]->y * len[1] + axis[2]->y * len[2],
515             cen.z - axis[0]->z * len[0] + axis[1]->z * len[1] + axis[2]->z * len[2]);*/
516
517
518  glEnd();
519
520
521}
522
523
524void OBBTreeNode::drawBVBlended(int currentDepth, const int depth) const
525{}
526
527
528void OBBTreeNode::debug()
529{
530
531  /*
532  for(int i = 0; i < length; i++)
533    {
534      printf("vertex %i: %f, %f, %f\n", i, verticesList[i][0], verticesList[i][1], verticesList[i][2]);
535    }
536  */
537}
Note: See TracBrowser for help on using the repository browser.