|
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 const int SPLITAXIS = 2; |
|
39 const int SPLITS = SPLITAXIS * SPLITAXIS; |
|
40 |
|
41 #define GEOM_ENABLED(g) (g)->gflags & GEOM_ENABLED |
|
42 |
|
43 class Block{ |
|
44 public: |
|
45 dReal MinX, MaxX; |
|
46 dReal MinZ, MaxZ; |
|
47 |
|
48 dGeomID First; |
|
49 int GeomCount; |
|
50 |
|
51 Block* Parent; |
|
52 Block* Children; |
|
53 |
|
54 void Create(const dVector3 Center, const dVector3 Extents, Block* Parent, int Depth, Block*& Blocks); |
|
55 |
|
56 void Collide(void* UserData, dNearCallback* Callback); |
|
57 void Collide(dGeomID Object, dGeomID g, void* UserData, dNearCallback* Callback); |
|
58 |
|
59 void CollideLocal(dGeomID Object, void* UserData, dNearCallback* Callback); |
|
60 |
|
61 void AddObject(dGeomID Object); |
|
62 void DelObject(dGeomID Object); |
|
63 void Traverse(dGeomID Object); |
|
64 |
|
65 bool Inside(const dReal* AABB); |
|
66 |
|
67 Block* GetBlock(const dReal* AABB); |
|
68 Block* GetBlockChild(const dReal* AABB); |
|
69 }; |
|
70 |
|
71 |
|
72 void Block::Create(const dVector3 Center, const dVector3 Extents, Block* Parent, int Depth, Block*& Blocks){ |
|
73 GeomCount = 0; |
|
74 First = 0; |
|
75 |
|
76 MinX = Center[AXIS0] - Extents[AXIS0]; |
|
77 MaxX = Center[AXIS0] + Extents[AXIS0]; |
|
78 |
|
79 MinZ = Center[AXIS1] - Extents[AXIS1]; |
|
80 MaxZ = Center[AXIS1] + Extents[AXIS1]; |
|
81 |
|
82 this->Parent = Parent; |
|
83 if (Depth > 0){ |
|
84 Children = Blocks; |
|
85 Blocks += SPLITS; |
|
86 |
|
87 dVector3 ChildExtents; |
|
88 ChildExtents[AXIS0] = Extents[AXIS0] / SPLITAXIS; |
|
89 ChildExtents[AXIS1] = Extents[AXIS1] / SPLITAXIS; |
|
90 ChildExtents[UP] = Extents[UP]; |
|
91 |
|
92 for (int i = 0; i < SPLITAXIS; i++){ |
|
93 for (int j = 0; j < SPLITAXIS; j++){ |
|
94 int Index = i * SPLITAXIS + j; |
|
95 |
|
96 dVector3 ChildCenter; |
|
97 ChildCenter[AXIS0] = Center[AXIS0] - Extents[AXIS0] + ChildExtents[AXIS0] + i * (ChildExtents[AXIS0] * 2); |
|
98 ChildCenter[AXIS1] = Center[AXIS1] - Extents[AXIS1] + ChildExtents[AXIS1] + j * (ChildExtents[AXIS1] * 2); |
|
99 ChildCenter[UP] = Center[UP]; |
|
100 |
|
101 Children[Index].Create(ChildCenter, ChildExtents, this, Depth - 1, Blocks); |
|
102 } |
|
103 } |
|
104 } |
|
105 else Children = 0; |
|
106 } |
|
107 |
|
108 void Block::Collide(void* UserData, dNearCallback* Callback){ |
|
109 |
|
110 // Collide the local list |
|
111 dxGeom* g = First; |
|
112 while (g){ |
|
113 if (GEOM_ENABLED(g)){ |
|
114 Collide(g, g->next, UserData, Callback); |
|
115 } |
|
116 g = g->next; |
|
117 } |
|
118 |
|
119 // Recurse for children |
|
120 if (Children){ |
|
121 for (int i = 0; i < SPLITS; i++){ |
|
122 if (Children[i].GeomCount <= 1){ // Early out |
|
123 continue; |
|
124 } |
|
125 Children[i].Collide(UserData, Callback); |
|
126 } |
|
127 } |
|
128 } |
|
129 |
|
130 void Block::Collide(dxGeom* g1, dxGeom* g2, void* UserData, dNearCallback* Callback){ |
|
131 |
|
132 // Collide against local list |
|
133 while (g2){ |
|
134 if (GEOM_ENABLED(g2)){ |
|
135 collideAABBs (g1, g2, UserData, Callback); |
|
136 } |
|
137 g2 = g2->next; |
|
138 } |
|
139 |
|
140 // Collide against children |
|
141 if (Children){ |
|
142 for (int i = 0; i < SPLITS; i++){ |
|
143 // Early out for empty blocks |
|
144 if (Children[i].GeomCount == 0){ |
|
145 continue; |
|
146 } |
|
147 |
|
148 // Does the geom's AABB collide with the block? |
|
149 // Dont do AABB tests for single geom blocks. |
|
150 if (Children[i].GeomCount == 1 && Children[i].First){ |
|
151 // |
|
152 } |
|
153 else if (true){ |
|
154 if (g1->aabb[AXIS0 * 2 + 0] > Children[i].MaxX || |
|
155 g1->aabb[AXIS0 * 2 + 1] < Children[i].MinX || |
|
156 g1->aabb[AXIS1 * 2 + 0] > Children[i].MaxZ || |
|
157 g1->aabb[AXIS1 * 2 + 1] < Children[i].MinZ) continue; |
|
158 } |
|
159 Children[i].Collide(g1, Children[i].First, UserData, Callback); |
|
160 } |
|
161 } |
|
162 } |
|
163 |
|
164 void Block::CollideLocal(dxGeom* g1, void* UserData, dNearCallback* Callback){ |
|
165 // Collide against local list |
|
166 dxGeom* g2 = First; |
|
167 while (g2){ |
|
168 if (GEOM_ENABLED(g2)){ |
|
169 collideAABBs (g1, g2, UserData, Callback); |
|
170 } |
|
171 g2 = g2->next; |
|
172 } |
|
173 } |
|
174 |
|
175 void Block::AddObject(dGeomID Object){ |
|
176 // Add the geom |
|
177 Object->next = First; |
|
178 First = Object; |
|
179 Object->tome = (dxGeom**)this; |
|
180 |
|
181 // Now traverse upwards to tell that we have a geom |
|
182 Block* Block = this; |
|
183 do{ |
|
184 Block->GeomCount++; |
|
185 Block = Block->Parent; |
|
186 } |
|
187 while (Block); |
|
188 } |
|
189 |
|
190 void Block::DelObject(dGeomID Object){ |
|
191 // Del the geom |
|
192 dxGeom* g = First; |
|
193 dxGeom* Last = 0; |
|
194 while (g){ |
|
195 if (g == Object){ |
|
196 if (Last){ |
|
197 Last->next = g->next; |
|
198 } |
|
199 else First = g->next; |
|
200 |
|
201 break; |
|
202 } |
|
203 Last = g; |
|
204 g = g->next; |
|
205 } |
|
206 |
|
207 Object->tome = 0; |
|
208 |
|
209 // Now traverse upwards to tell that we have lost a geom |
|
210 Block* Block = this; |
|
211 do{ |
|
212 Block->GeomCount--; |
|
213 Block = Block->Parent; |
|
214 } |
|
215 while (Block); |
|
216 } |
|
217 |
|
218 void Block::Traverse(dGeomID Object){ |
|
219 Block* NewBlock = GetBlock(Object->aabb); |
|
220 |
|
221 if (NewBlock != this){ |
|
222 // Remove the geom from the old block and add it to the new block. |
|
223 // This could be more optimal, but the loss should be very small. |
|
224 DelObject(Object); |
|
225 NewBlock->AddObject(Object); |
|
226 } |
|
227 } |
|
228 |
|
229 bool Block::Inside(const dReal* AABB){ |
|
230 return AABB[AXIS0 * 2 + 0] >= MinX && AABB[AXIS0 * 2 + 1] <= MaxX && AABB[AXIS1 * 2 + 0] >= MinZ && AABB[AXIS1 * 2 + 1] <= MaxZ; |
|
231 } |
|
232 |
|
233 Block* Block::GetBlock(const dReal* AABB){ |
|
234 if (Inside(AABB)){ |
|
235 return GetBlockChild(AABB); // Child or this will have a good block |
|
236 } |
|
237 else if (Parent){ |
|
238 return Parent->GetBlock(AABB); // Parent has a good block |
|
239 } |
|
240 else return this; // We are at the root, so we have little choice |
|
241 } |
|
242 |
|
243 Block* Block::GetBlockChild(const dReal* AABB){ |
|
244 if (Children){ |
|
245 for (int i = 0; i < SPLITS; i++){ |
|
246 if (Children[i].Inside(AABB)){ |
|
247 return Children[i].GetBlockChild(AABB); // Child will have good block |
|
248 } |
|
249 } |
|
250 } |
|
251 return this; // This is the best block |
|
252 } |
|
253 |
|
254 //**************************************************************************** |
|
255 // quadtree space |
|
256 |
|
257 struct dxQuadTreeSpace : public dxSpace{ |
|
258 Block* Blocks; // Blocks[0] is the root |
|
259 |
|
260 dArray<dxGeom*> DirtyList; |
|
261 |
|
262 dxQuadTreeSpace(dSpaceID _space, dVector3 Center, dVector3 Extents, int Depth); |
|
263 ~dxQuadTreeSpace(); |
|
264 |
|
265 dxGeom* getGeom(int i); |
|
266 |
|
267 void add(dxGeom* g); |
|
268 void remove(dxGeom* g); |
|
269 void dirty(dxGeom* g); |
|
270 |
|
271 void computeAABB(); |
|
272 |
|
273 void cleanGeoms(); |
|
274 void collide(void* UserData, dNearCallback* Callback); |
|
275 void collide2(void* UserData, dxGeom* g1, dNearCallback* Callback); |
|
276 |
|
277 // Temp data |
|
278 Block* CurrentBlock; // Only used while enumerating |
|
279 int* CurrentChild; // Only used while enumerating |
|
280 int CurrentLevel; // Only used while enumerating |
|
281 dxGeom* CurrentObject; // Only used while enumerating |
|
282 int CurrentIndex; |
|
283 }; |
|
284 |
|
285 dxQuadTreeSpace::dxQuadTreeSpace(dSpaceID _space, dVector3 Center, dVector3 Extents, int Depth) : dxSpace(_space){ |
|
286 type = dQuadTreeSpaceClass; |
|
287 |
|
288 int BlockCount = 0; |
|
289 for (int i = 0; i <= Depth; i++){ |
|
290 BlockCount += (int)pow(SPLITS, i); |
|
291 } |
|
292 |
|
293 Blocks = (Block*)dAlloc(BlockCount * sizeof(Block)); |
|
294 Block* Blocks = this->Blocks + 1; // This pointer gets modified! |
|
295 |
|
296 this->Blocks[0].Create(Center, Extents, 0, Depth, Blocks); |
|
297 |
|
298 CurrentBlock = 0; |
|
299 CurrentChild = (int*)dAlloc((Depth + 1) * sizeof(int)); |
|
300 CurrentLevel = 0; |
|
301 CurrentObject = 0; |
|
302 CurrentIndex = -1; |
|
303 |
|
304 // 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 |
|
305 aabb[0] = -dInfinity; |
|
306 aabb[1] = dInfinity; |
|
307 aabb[2] = -dInfinity; |
|
308 aabb[3] = dInfinity; |
|
309 aabb[4] = -dInfinity; |
|
310 aabb[5] = dInfinity; |
|
311 } |
|
312 |
|
313 dxQuadTreeSpace::~dxQuadTreeSpace(){ |
|
314 int Depth = 0; |
|
315 Block* Current = &Blocks[0]; |
|
316 while (Current){ |
|
317 Depth++; |
|
318 Current = Current->Children; |
|
319 } |
|
320 |
|
321 int BlockCount = 0; |
|
322 for (int i = 0; i < Depth; i++){ |
|
323 BlockCount += (int)pow(SPLITS, i); |
|
324 } |
|
325 |
|
326 dFree(Blocks, BlockCount * sizeof(Block)); |
|
327 dFree(CurrentChild, (Depth + 1) * sizeof(int)); |
|
328 } |
|
329 |
|
330 dxGeom* dxQuadTreeSpace::getGeom(int /*Index*/){ |
|
331 |
|
332 //@@@ |
|
333 |
|
334 return 0; |
|
335 } |
|
336 |
|
337 void dxQuadTreeSpace::add(dxGeom* g){ |
|
338 |
|
339 |
|
340 g->gflags |= GEOM_DIRTY | GEOM_AABB_BAD; |
|
341 DirtyList.push(g); |
|
342 |
|
343 // add |
|
344 g->parent_space = this; |
|
345 Blocks[0].GetBlock(g->aabb)->AddObject(g); // Add to best block |
|
346 count++; |
|
347 |
|
348 // enumerator has been invalidated |
|
349 current_geom = 0; |
|
350 |
|
351 dGeomMoved(this); |
|
352 } |
|
353 |
|
354 void dxQuadTreeSpace::remove(dxGeom* g){ |
|
355 |
|
356 // remove |
|
357 ((Block*)g->tome)->DelObject(g); |
|
358 count--; |
|
359 |
|
360 for (int i = 0; i < DirtyList.size(); i++){ |
|
361 if (DirtyList[i] == g){ |
|
362 DirtyList.remove(i); |
|
363 // (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 |
|
364 --i; |
|
365 } |
|
366 } |
|
367 |
|
368 // safeguard |
|
369 g->next = 0; |
|
370 g->tome = 0; |
|
371 g->parent_space = 0; |
|
372 |
|
373 // enumerator has been invalidated |
|
374 current_geom = 0; |
|
375 |
|
376 // the bounding box of this space (and that of all the parents) may have |
|
377 // changed as a consequence of the removal. |
|
378 dGeomMoved(this); |
|
379 } |
|
380 |
|
381 void dxQuadTreeSpace::dirty(dxGeom* g){ |
|
382 DirtyList.push(g); |
|
383 } |
|
384 |
|
385 void dxQuadTreeSpace::computeAABB(){ |
|
386 // |
|
387 } |
|
388 |
|
389 void dxQuadTreeSpace::cleanGeoms(){ |
|
390 // compute the AABBs of all dirty geoms, and clear the dirty flags |
|
391 lock_count++; |
|
392 |
|
393 for (int i = 0; i < DirtyList.size(); i++){ |
|
394 dxGeom* g = DirtyList[i]; |
|
395 if (IS_SPACE(g)){ |
|
396 ((dxSpace*)g)->cleanGeoms(); |
|
397 } |
|
398 g->recomputeAABB(); |
|
399 g->gflags &= (~(GEOM_DIRTY|GEOM_AABB_BAD)); |
|
400 |
|
401 ((Block*)g->tome)->Traverse(g); |
|
402 } |
|
403 DirtyList.setSize(0); |
|
404 |
|
405 lock_count--; |
|
406 } |
|
407 |
|
408 void dxQuadTreeSpace::collide(void* UserData, dNearCallback* Callback){ |
|
409 |
|
410 |
|
411 lock_count++; |
|
412 cleanGeoms(); |
|
413 |
|
414 Blocks[0].Collide(UserData, Callback); |
|
415 |
|
416 lock_count--; |
|
417 } |
|
418 |
|
419 void dxQuadTreeSpace::collide2(void* UserData, dxGeom* g1, dNearCallback* Callback){ |
|
420 |
|
421 lock_count++; |
|
422 cleanGeoms(); |
|
423 g1->recomputeAABB(); |
|
424 |
|
425 if (g1->parent_space == this){ |
|
426 // The block the geom is in |
|
427 Block* CurrentBlock = (Block*)g1->tome; |
|
428 |
|
429 // Collide against block and its children |
|
430 CurrentBlock->Collide(g1, CurrentBlock->First, UserData, Callback); |
|
431 |
|
432 // Collide against parents |
|
433 while (true){ |
|
434 CurrentBlock = CurrentBlock->Parent; |
|
435 if (!CurrentBlock){ |
|
436 break; |
|
437 } |
|
438 CurrentBlock->CollideLocal(g1, UserData, Callback); |
|
439 } |
|
440 } |
|
441 else Blocks[0].Collide(g1, Blocks[0].First, UserData, Callback); |
|
442 |
|
443 lock_count--; |
|
444 } |
|
445 |
|
446 EXPORT_C dSpaceID dQuadTreeSpaceCreate(dxSpace* space, dVector3 Center, dVector3 Extents, int Depth){ |
|
447 return new dxQuadTreeSpace(space, Center, Extents, Depth); |
|
448 } |