aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/libraries/ode-0.9/ode/src/collision_quadtreespace.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'libraries/ode-0.9/ode/src/collision_quadtreespace.cpp')
-rw-r--r--libraries/ode-0.9/ode/src/collision_quadtreespace.cpp584
1 files changed, 0 insertions, 584 deletions
diff --git a/libraries/ode-0.9/ode/src/collision_quadtreespace.cpp b/libraries/ode-0.9/ode/src/collision_quadtreespace.cpp
deleted file mode 100644
index 86a1833..0000000
--- a/libraries/ode-0.9/ode/src/collision_quadtreespace.cpp
+++ /dev/null
@@ -1,584 +0,0 @@
1/*************************************************************************
2 * *
3 * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. *
4 * All rights reserved. Email: russ@q12.org Web: www.q12.org *
5 * *
6 * This library is free software; you can redistribute it and/or *
7 * modify it under the terms of EITHER: *
8 * (1) The GNU Lesser General Public License as published by the Free *
9 * Software Foundation; either version 2.1 of the License, or (at *
10 * your option) any later version. The text of the GNU Lesser *
11 * General Public License is included with this library in the *
12 * file LICENSE.TXT. *
13 * (2) The BSD-style license that is included with this library in *
14 * the file LICENSE-BSD.TXT. *
15 * *
16 * This library is distributed in the hope that it will be useful, *
17 * but WITHOUT ANY WARRANTY; without even the implied warranty of *
18 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files *
19 * LICENSE.TXT and LICENSE-BSD.TXT for more details. *
20 * *
21 *************************************************************************/
22
23// QuadTreeSpace by Erwin de Vries.
24
25#include <ode/common.h>
26#include <ode/matrix.h>
27#include <ode/collision_space.h>
28#include <ode/collision.h>
29#include "collision_kernel.h"
30
31#include "collision_space_internal.h"
32
33
34#define AXIS0 0
35#define AXIS1 1
36#define UP 2
37
38//#define DRAWBLOCKS
39
40const int SPLITAXIS = 2;
41const int SPLITS = SPLITAXIS * SPLITAXIS;
42
43#define GEOM_ENABLED(g) (g)->gflags & GEOM_ENABLED
44
45class Block{
46public:
47 dReal MinX, MaxX;
48 dReal MinZ, MaxZ;
49
50 dGeomID First;
51 int GeomCount;
52
53 Block* Parent;
54 Block* Children;
55
56 void Create(const dVector3 Center, const dVector3 Extents, Block* Parent, int Depth, Block*& Blocks);
57
58 void Collide(void* UserData, dNearCallback* Callback);
59 void Collide(dGeomID Object, dGeomID g, void* UserData, dNearCallback* Callback);
60
61 void CollideLocal(dGeomID Object, void* UserData, dNearCallback* Callback);
62
63 void AddObject(dGeomID Object);
64 void DelObject(dGeomID Object);
65 void Traverse(dGeomID Object);
66
67 bool Inside(const dReal* AABB);
68
69 Block* GetBlock(const dReal* AABB);
70 Block* GetBlockChild(const dReal* AABB);
71};
72
73
74#ifdef DRAWBLOCKS
75#include "..\..\Include\drawstuff\\drawstuff.h"
76
77static void DrawBlock(Block* Block){
78 dVector3 v[8];
79 v[0][AXIS0] = Block->MinX;
80 v[0][UP] = REAL(-1.0);
81 v[0][AXIS1] = Block->MinZ;
82
83 v[1][AXIS0] = Block->MinX;
84 v[1][UP] = REAL(-1.0);
85 v[1][AXIS1] = Block->MaxZ;
86
87 v[2][AXIS0] = Block->MaxX;
88 v[2][UP] = REAL(-1.0);
89 v[2][AXIS1] = Block->MinZ;
90
91 v[3][AXIS0] = Block->MaxX;
92 v[3][UP] = REAL(-1.0);
93 v[3][AXIS1] = Block->MaxZ;
94
95 v[4][AXIS0] = Block->MinX;
96 v[4][UP] = REAL(1.0);
97 v[4][AXIS1] = Block->MinZ;
98
99 v[5][AXIS0] = Block->MinX;
100 v[5][UP] = REAL(1.0);
101 v[5][AXIS1] = Block->MaxZ;
102
103 v[6][AXIS0] = Block->MaxX;
104 v[6][UP] = REAL(1.0);
105 v[6][AXIS1] = Block->MinZ;
106
107 v[7][AXIS0] = Block->MaxX;
108 v[7][UP] = REAL(1.0);
109 v[7][AXIS1] = Block->MaxZ;
110
111 // Bottom
112 dsDrawLine(v[0], v[1]);
113 dsDrawLine(v[1], v[3]);
114 dsDrawLine(v[3], v[2]);
115 dsDrawLine(v[2], v[0]);
116
117 // Top
118 dsDrawLine(v[4], v[5]);
119 dsDrawLine(v[5], v[7]);
120 dsDrawLine(v[7], v[6]);
121 dsDrawLine(v[6], v[4]);
122
123 // Sides
124 dsDrawLine(v[0], v[4]);
125 dsDrawLine(v[1], v[5]);
126 dsDrawLine(v[2], v[6]);
127 dsDrawLine(v[3], v[7]);
128}
129#endif //DRAWBLOCKS
130
131
132void Block::Create(const dVector3 Center, const dVector3 Extents, Block* Parent, int Depth, Block*& Blocks){
133 GeomCount = 0;
134 First = 0;
135
136 MinX = Center[AXIS0] - Extents[AXIS0];
137 MaxX = Center[AXIS0] + Extents[AXIS0];
138
139 MinZ = Center[AXIS1] - Extents[AXIS1];
140 MaxZ = Center[AXIS1] + Extents[AXIS1];
141
142 this->Parent = Parent;
143 if (Depth > 0){
144 Children = Blocks;
145 Blocks += SPLITS;
146
147 dVector3 ChildExtents;
148 ChildExtents[AXIS0] = Extents[AXIS0] / SPLITAXIS;
149 ChildExtents[AXIS1] = Extents[AXIS1] / SPLITAXIS;
150 ChildExtents[UP] = Extents[UP];
151
152 for (int i = 0; i < SPLITAXIS; i++){
153 for (int j = 0; j < SPLITAXIS; j++){
154 int Index = i * SPLITAXIS + j;
155
156 dVector3 ChildCenter;
157 ChildCenter[AXIS0] = Center[AXIS0] - Extents[AXIS0] + ChildExtents[AXIS0] + i * (ChildExtents[AXIS0] * 2);
158 ChildCenter[AXIS1] = Center[AXIS1] - Extents[AXIS1] + ChildExtents[AXIS1] + j * (ChildExtents[AXIS1] * 2);
159 ChildCenter[UP] = Center[UP];
160
161 Children[Index].Create(ChildCenter, ChildExtents, this, Depth - 1, Blocks);
162 }
163 }
164 }
165 else Children = 0;
166}
167
168void Block::Collide(void* UserData, dNearCallback* Callback){
169#ifdef DRAWBLOCKS
170 DrawBlock(this);
171#endif
172 // Collide the local list
173 dxGeom* g = First;
174 while (g){
175 if (GEOM_ENABLED(g)){
176 Collide(g, g->next, UserData, Callback);
177 }
178 g = g->next;
179 }
180
181 // Recurse for children
182 if (Children){
183 for (int i = 0; i < SPLITS; i++){
184 if (Children[i].GeomCount <= 1){ // Early out
185 continue;
186 }
187 Children[i].Collide(UserData, Callback);
188 }
189 }
190}
191
192void Block::Collide(dxGeom* g1, dxGeom* g2, void* UserData, dNearCallback* Callback){
193#ifdef DRAWBLOCKS
194 DrawBlock(this);
195#endif
196 // Collide against local list
197 while (g2){
198 if (GEOM_ENABLED(g2)){
199 collideAABBs (g1, g2, UserData, Callback);
200 }
201 g2 = g2->next;
202 }
203
204 // Collide against children
205 if (Children){
206 for (int i = 0; i < SPLITS; i++){
207 // Early out for empty blocks
208 if (Children[i].GeomCount == 0){
209 continue;
210 }
211
212 // Does the geom's AABB collide with the block?
213 // Dont do AABB tests for single geom blocks.
214 if (Children[i].GeomCount == 1 && Children[i].First){
215 //
216 }
217 else if (true){
218 if (g1->aabb[AXIS0 * 2 + 0] > Children[i].MaxX ||
219 g1->aabb[AXIS0 * 2 + 1] < Children[i].MinX ||
220 g1->aabb[AXIS1 * 2 + 0] > Children[i].MaxZ ||
221 g1->aabb[AXIS1 * 2 + 1] < Children[i].MinZ) continue;
222 }
223 Children[i].Collide(g1, Children[i].First, UserData, Callback);
224 }
225 }
226}
227
228void Block::CollideLocal(dxGeom* g1, void* UserData, dNearCallback* Callback){
229 // Collide against local list
230 dxGeom* g2 = First;
231 while (g2){
232 if (GEOM_ENABLED(g2)){
233 collideAABBs (g1, g2, UserData, Callback);
234 }
235 g2 = g2->next;
236 }
237}
238
239void Block::AddObject(dGeomID Object){
240 // Add the geom
241 Object->next = First;
242 First = Object;
243 Object->tome = (dxGeom**)this;
244
245 // Now traverse upwards to tell that we have a geom
246 Block* Block = this;
247 do{
248 Block->GeomCount++;
249 Block = Block->Parent;
250 }
251 while (Block);
252}
253
254void Block::DelObject(dGeomID Object){
255 // Del the geom
256 dxGeom* g = First;
257 dxGeom* Last = 0;
258 while (g){
259 if (g == Object){
260 if (Last){
261 Last->next = g->next;
262 }
263 else First = g->next;
264
265 break;
266 }
267 Last = g;
268 g = g->next;
269 }
270
271 Object->tome = 0;
272
273 // Now traverse upwards to tell that we have lost a geom
274 Block* Block = this;
275 do{
276 Block->GeomCount--;
277 Block = Block->Parent;
278 }
279 while (Block);
280}
281
282void Block::Traverse(dGeomID Object){
283 Block* NewBlock = GetBlock(Object->aabb);
284
285 if (NewBlock != this){
286 // Remove the geom from the old block and add it to the new block.
287 // This could be more optimal, but the loss should be very small.
288 DelObject(Object);
289 NewBlock->AddObject(Object);
290 }
291}
292
293bool Block::Inside(const dReal* AABB){
294 return AABB[AXIS0 * 2 + 0] >= MinX && AABB[AXIS0 * 2 + 1] <= MaxX && AABB[AXIS1 * 2 + 0] >= MinZ && AABB[AXIS1 * 2 + 1] <= MaxZ;
295}
296
297Block* Block::GetBlock(const dReal* AABB){
298 if (Inside(AABB)){
299 return GetBlockChild(AABB); // Child or this will have a good block
300 }
301 else if (Parent){
302 return Parent->GetBlock(AABB); // Parent has a good block
303 }
304 else return this; // We are at the root, so we have little choice
305}
306
307Block* Block::GetBlockChild(const dReal* AABB){
308 if (Children){
309 for (int i = 0; i < SPLITS; i++){
310 if (Children[i].Inside(AABB)){
311 return Children[i].GetBlockChild(AABB); // Child will have good block
312 }
313 }
314 }
315 return this; // This is the best block
316}
317
318//****************************************************************************
319// quadtree space
320
321struct dxQuadTreeSpace : public dxSpace{
322 Block* Blocks; // Blocks[0] is the root
323
324 dArray<dxGeom*> DirtyList;
325
326 dxQuadTreeSpace(dSpaceID _space, dVector3 Center, dVector3 Extents, int Depth);
327 ~dxQuadTreeSpace();
328
329 dxGeom* getGeom(int i);
330
331 void add(dxGeom* g);
332 void remove(dxGeom* g);
333 void dirty(dxGeom* g);
334
335 void computeAABB();
336
337 void cleanGeoms();
338 void collide(void* UserData, dNearCallback* Callback);
339 void collide2(void* UserData, dxGeom* g1, dNearCallback* Callback);
340
341 // Temp data
342 Block* CurrentBlock; // Only used while enumerating
343 int* CurrentChild; // Only used while enumerating
344 int CurrentLevel; // Only used while enumerating
345 dxGeom* CurrentObject; // Only used while enumerating
346 int CurrentIndex;
347};
348
349dxQuadTreeSpace::dxQuadTreeSpace(dSpaceID _space, dVector3 Center, dVector3 Extents, int Depth) : dxSpace(_space){
350 type = dQuadTreeSpaceClass;
351
352 int BlockCount = 0;
353 for (int i = 0; i <= Depth; i++){
354 BlockCount += (int)pow((dReal)SPLITS, i);
355 }
356
357 Blocks = (Block*)dAlloc(BlockCount * sizeof(Block));
358 Block* Blocks = this->Blocks + 1; // This pointer gets modified!
359
360 this->Blocks[0].Create(Center, Extents, 0, Depth, Blocks);
361
362 CurrentBlock = 0;
363 CurrentChild = (int*)dAlloc((Depth + 1) * sizeof(int));
364 CurrentLevel = 0;
365 CurrentObject = 0;
366 CurrentIndex = -1;
367
368 // Init AABB. We initialize to infinity because it is not illegal for an object to be outside of the tree. Its simply inserted in the root block
369 aabb[0] = -dInfinity;
370 aabb[1] = dInfinity;
371 aabb[2] = -dInfinity;
372 aabb[3] = dInfinity;
373 aabb[4] = -dInfinity;
374 aabb[5] = dInfinity;
375}
376
377dxQuadTreeSpace::~dxQuadTreeSpace(){
378 int Depth = 0;
379 Block* Current = &Blocks[0];
380 while (Current){
381 Depth++;
382 Current = Current->Children;
383 }
384
385 int BlockCount = 0;
386 for (int i = 0; i < Depth; i++){
387 BlockCount += (int)pow((dReal)SPLITS, i);
388 }
389
390 dFree(Blocks, BlockCount * sizeof(Block));
391 dFree(CurrentChild, (Depth + 1) * sizeof(int));
392}
393
394dxGeom* dxQuadTreeSpace::getGeom(int Index){
395 dUASSERT(Index >= 0 && Index < count, "index out of range");
396
397 //@@@
398 dDebug (0,"dxQuadTreeSpace::getGeom() not yet implemented");
399
400 return 0;
401
402 // This doesnt work
403
404 /*if (CurrentIndex == Index){
405 // Loop through all objects in the local list
406CHILDRECURSE:
407 if (CurrentObject){
408 dGeomID g = CurrentObject;
409 CurrentObject = CurrentObject->next;
410 CurrentIndex++;
411
412#ifdef DRAWBLOCKS
413 DrawBlock(CurrentBlock);
414#endif //DRAWBLOCKS
415 return g;
416 }
417 else{
418 // Now lets loop through our children. Starting at index 0.
419 if (CurrentBlock->Children){
420 CurrentChild[CurrentLevel] = 0;
421PARENTRECURSE:
422 for (int& i = CurrentChild[CurrentLevel]; i < SPLITS; i++){
423 if (CurrentBlock->Children[i].GeomCount == 0){
424 continue;
425 }
426 CurrentBlock = &CurrentBlock->Children[i];
427 CurrentObject = CurrentBlock->First;
428
429 i++;
430
431 CurrentLevel++;
432 goto CHILDRECURSE;
433 }
434 }
435 }
436
437 // Now lets go back to the parent so it can continue processing its other children.
438 if (CurrentBlock->Parent){
439 CurrentBlock = CurrentBlock->Parent;
440 CurrentLevel--;
441 goto PARENTRECURSE;
442 }
443 }
444 else{
445 CurrentBlock = &Blocks[0];
446 CurrentLevel = 0;
447 CurrentObject = CurrentObject;
448 CurrentIndex = 0;
449
450 // Other states are already set
451 CurrentObject = CurrentBlock->First;
452 }
453
454
455 if (current_geom && current_index == Index - 1){
456 //current_geom = current_geom->next; // next
457 current_index = Index;
458 return current_geom;
459 }
460 else for (int i = 0; i < Index; i++){ // this will be verrrrrrry slow
461 getGeom(i);
462 }*/
463
464 return 0;
465}
466
467void dxQuadTreeSpace::add(dxGeom* g){
468 CHECK_NOT_LOCKED (this);
469 dAASSERT(g);
470 dUASSERT(g->parent_space == 0 && g->next == 0, "geom is already in a space");
471
472 g->gflags |= GEOM_DIRTY | GEOM_AABB_BAD;
473 DirtyList.push(g);
474
475 // add
476 g->parent_space = this;
477 Blocks[0].GetBlock(g->aabb)->AddObject(g); // Add to best block
478 count++;
479
480 // enumerator has been invalidated
481 current_geom = 0;
482
483 dGeomMoved(this);
484}
485
486void dxQuadTreeSpace::remove(dxGeom* g){
487 CHECK_NOT_LOCKED(this);
488 dAASSERT(g);
489 dUASSERT(g->parent_space == this,"object is not in this space");
490
491 // remove
492 ((Block*)g->tome)->DelObject(g);
493 count--;
494
495 for (int i = 0; i < DirtyList.size(); i++){
496 if (DirtyList[i] == g){
497 DirtyList.remove(i);
498 // (mg) there can be multiple instances of a dirty object on stack be sure to remove ALL and not just first, for this we decrement i
499 --i;
500 }
501 }
502
503 // safeguard
504 g->next = 0;
505 g->tome = 0;
506 g->parent_space = 0;
507
508 // enumerator has been invalidated
509 current_geom = 0;
510
511 // the bounding box of this space (and that of all the parents) may have
512 // changed as a consequence of the removal.
513 dGeomMoved(this);
514}
515
516void dxQuadTreeSpace::dirty(dxGeom* g){
517 DirtyList.push(g);
518}
519
520void dxQuadTreeSpace::computeAABB(){
521 //
522}
523
524void dxQuadTreeSpace::cleanGeoms(){
525 // compute the AABBs of all dirty geoms, and clear the dirty flags
526 lock_count++;
527
528 for (int i = 0; i < DirtyList.size(); i++){
529 dxGeom* g = DirtyList[i];
530 if (IS_SPACE(g)){
531 ((dxSpace*)g)->cleanGeoms();
532 }
533 g->recomputeAABB();
534 g->gflags &= (~(GEOM_DIRTY|GEOM_AABB_BAD));
535
536 ((Block*)g->tome)->Traverse(g);
537 }
538 DirtyList.setSize(0);
539
540 lock_count--;
541}
542
543void dxQuadTreeSpace::collide(void* UserData, dNearCallback* Callback){
544 dAASSERT(Callback);
545
546 lock_count++;
547 cleanGeoms();
548
549 Blocks[0].Collide(UserData, Callback);
550
551 lock_count--;
552}
553
554void dxQuadTreeSpace::collide2(void* UserData, dxGeom* g1, dNearCallback* Callback){
555 dAASSERT(g1 && Callback);
556
557 lock_count++;
558 cleanGeoms();
559 g1->recomputeAABB();
560
561 if (g1->parent_space == this){
562 // The block the geom is in
563 Block* CurrentBlock = (Block*)g1->tome;
564
565 // Collide against block and its children
566 CurrentBlock->Collide(g1, CurrentBlock->First, UserData, Callback);
567
568 // Collide against parents
569 while (true){
570 CurrentBlock = CurrentBlock->Parent;
571 if (!CurrentBlock){
572 break;
573 }
574 CurrentBlock->CollideLocal(g1, UserData, Callback);
575 }
576 }
577 else Blocks[0].Collide(g1, Blocks[0].First, UserData, Callback);
578
579 lock_count--;
580}
581
582dSpaceID dQuadTreeSpaceCreate(dxSpace* space, dVector3 Center, dVector3 Extents, int Depth){
583 return new dxQuadTreeSpace(space, Center, Extents, Depth);
584}
generated by cgit v1.1 at 2024-10-26 10:25:27 +0000