aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/libraries/ode-0.9/ode/src/collision_space.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'libraries/ode-0.9/ode/src/collision_space.cpp')
-rw-r--r--libraries/ode-0.9/ode/src/collision_space.cpp790
1 files changed, 0 insertions, 790 deletions
diff --git a/libraries/ode-0.9/ode/src/collision_space.cpp b/libraries/ode-0.9/ode/src/collision_space.cpp
deleted file mode 100644
index 1a8fc81..0000000
--- a/libraries/ode-0.9/ode/src/collision_space.cpp
+++ /dev/null
@@ -1,790 +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/*
24
25spaces
26
27*/
28
29#include <ode/common.h>
30#include <ode/matrix.h>
31#include <ode/collision_space.h>
32#include <ode/collision.h>
33#include "collision_kernel.h"
34
35#include "collision_space_internal.h"
36
37#ifdef _MSC_VER
38#pragma warning(disable:4291) // for VC++, no complaints about "no matching operator delete found"
39#endif
40
41//****************************************************************************
42// make the geom dirty by setting the GEOM_DIRTY and GEOM_BAD_AABB flags
43// and moving it to the front of the space's list. all the parents of a
44// dirty geom also become dirty.
45
46void dGeomMoved (dxGeom *geom)
47{
48 dAASSERT (geom);
49
50 // if geom is offset, mark it as needing a calculate
51 if (geom->offset_posr) {
52 geom->gflags |= GEOM_POSR_BAD;
53 }
54
55 // from the bottom of the space heirarchy up, process all clean geoms
56 // turning them into dirty geoms.
57 dxSpace *parent = geom->parent_space;
58
59 while (parent && (geom->gflags & GEOM_DIRTY)==0) {
60 CHECK_NOT_LOCKED (parent);
61 geom->gflags |= GEOM_DIRTY | GEOM_AABB_BAD;
62 parent->dirty (geom);
63 geom = parent;
64 parent = parent->parent_space;
65 }
66
67 // all the remaining dirty geoms must have their AABB_BAD flags set, to
68 // ensure that their AABBs get recomputed
69 while (geom) {
70 geom->gflags |= GEOM_DIRTY | GEOM_AABB_BAD;
71 CHECK_NOT_LOCKED (geom->parent_space);
72 geom = geom->parent_space;
73 }
74}
75
76#define GEOM_ENABLED(g) ((g)->gflags & GEOM_ENABLED)
77
78//****************************************************************************
79// dxSpace
80
81dxSpace::dxSpace (dSpaceID _space) : dxGeom (_space,0)
82{
83 count = 0;
84 first = 0;
85 cleanup = 1;
86 current_index = 0;
87 current_geom = 0;
88 lock_count = 0;
89}
90
91
92dxSpace::~dxSpace()
93{
94 CHECK_NOT_LOCKED (this);
95 if (cleanup) {
96 // note that destroying each geom will call remove()
97 dxGeom *g,*n;
98 for (g = first; g; g=n) {
99 n = g->next;
100 dGeomDestroy (g);
101 }
102 }
103 else {
104 dxGeom *g,*n;
105 for (g = first; g; g=n) {
106 n = g->next;
107 remove (g);
108 }
109 }
110}
111
112
113void dxSpace::computeAABB()
114{
115 if (first) {
116 int i;
117 dReal a[6];
118 a[0] = dInfinity;
119 a[1] = -dInfinity;
120 a[2] = dInfinity;
121 a[3] = -dInfinity;
122 a[4] = dInfinity;
123 a[5] = -dInfinity;
124 for (dxGeom *g=first; g; g=g->next) {
125 g->recomputeAABB();
126 for (i=0; i<6; i += 2) if (g->aabb[i] < a[i]) a[i] = g->aabb[i];
127 for (i=1; i<6; i += 2) if (g->aabb[i] > a[i]) a[i] = g->aabb[i];
128 }
129 memcpy(aabb,a,6*sizeof(dReal));
130 }
131 else {
132 dSetZero (aabb,6);
133 }
134}
135
136
137void dxSpace::setCleanup (int mode)
138{
139 cleanup = (mode != 0);
140}
141
142
143int dxSpace::getCleanup()
144{
145 return cleanup;
146}
147
148
149int dxSpace::query (dxGeom *geom)
150{
151 dAASSERT (geom);
152 return (geom->parent_space == this);
153}
154
155
156int dxSpace::getNumGeoms()
157{
158 return count;
159}
160
161
162// the dirty geoms are numbered 0..k, the clean geoms are numbered k+1..count-1
163
164dxGeom *dxSpace::getGeom (int i)
165{
166 dUASSERT (i >= 0 && i < count,"index out of range");
167 if (current_geom && current_index == i-1) {
168 current_geom = current_geom->next;
169 current_index = i;
170 return current_geom;
171 }
172 else {
173 dxGeom *g=first;
174 for (int j=0; j<i; j++) {
175 if (g) g = g->next; else return 0;
176 }
177 current_geom = g;
178 current_index = i;
179 return g;
180 }
181}
182
183
184void dxSpace::add (dxGeom *geom)
185{
186 CHECK_NOT_LOCKED (this);
187 dAASSERT (geom);
188 dUASSERT (geom->parent_space == 0 && geom->next == 0,
189 "geom is already in a space");
190
191 // add
192 geom->parent_space = this;
193 geom->spaceAdd (&first);
194 count++;
195
196 // enumerator has been invalidated
197 current_geom = 0;
198
199 // new geoms are added to the front of the list and are always
200 // considered to be dirty. as a consequence, this space and all its
201 // parents are dirty too.
202 geom->gflags |= GEOM_DIRTY | GEOM_AABB_BAD;
203 dGeomMoved (this);
204}
205
206
207void dxSpace::remove (dxGeom *geom)
208{
209 CHECK_NOT_LOCKED (this);
210 dAASSERT (geom);
211 dUASSERT (geom->parent_space == this,"object is not in this space");
212
213 // remove
214 geom->spaceRemove();
215 count--;
216
217 // safeguard
218 geom->next = 0;
219 geom->tome = 0;
220 geom->parent_space = 0;
221
222 // enumerator has been invalidated
223 current_geom = 0;
224
225 // the bounding box of this space (and that of all the parents) may have
226 // changed as a consequence of the removal.
227 dGeomMoved (this);
228}
229
230
231void dxSpace::dirty (dxGeom *geom)
232{
233 geom->spaceRemove();
234 geom->spaceAdd (&first);
235}
236
237//****************************************************************************
238// simple space - reports all n^2 object intersections
239
240struct dxSimpleSpace : public dxSpace {
241 dxSimpleSpace (dSpaceID _space);
242 void cleanGeoms();
243 void collide (void *data, dNearCallback *callback);
244 void collide2 (void *data, dxGeom *geom, dNearCallback *callback);
245};
246
247
248dxSimpleSpace::dxSimpleSpace (dSpaceID _space) : dxSpace (_space)
249{
250 type = dSimpleSpaceClass;
251}
252
253
254void dxSimpleSpace::cleanGeoms()
255{
256 // compute the AABBs of all dirty geoms, and clear the dirty flags
257 lock_count++;
258 for (dxGeom *g=first; g && (g->gflags & GEOM_DIRTY); g=g->next) {
259 if (IS_SPACE(g)) {
260 ((dxSpace*)g)->cleanGeoms();
261 }
262 g->recomputeAABB();
263 g->gflags &= (~(GEOM_DIRTY|GEOM_AABB_BAD));
264 }
265 lock_count--;
266}
267
268
269void dxSimpleSpace::collide (void *data, dNearCallback *callback)
270{
271 dAASSERT (callback);
272
273 lock_count++;
274 cleanGeoms();
275
276 // intersect all bounding boxes
277 for (dxGeom *g1=first; g1; g1=g1->next) {
278 if (GEOM_ENABLED(g1)){
279 for (dxGeom *g2=g1->next; g2; g2=g2->next) {
280 if (GEOM_ENABLED(g2)){
281 collideAABBs (g1,g2,data,callback);
282 }
283 }
284 }
285 }
286
287 lock_count--;
288}
289
290
291void dxSimpleSpace::collide2 (void *data, dxGeom *geom,
292 dNearCallback *callback)
293{
294 dAASSERT (geom && callback);
295
296 lock_count++;
297 cleanGeoms();
298 geom->recomputeAABB();
299
300 // intersect bounding boxes
301 for (dxGeom *g=first; g; g=g->next) {
302 if (GEOM_ENABLED(g)){
303 collideAABBs (g,geom,data,callback);
304 }
305 }
306
307 lock_count--;
308}
309
310//****************************************************************************
311// utility stuff for hash table space
312
313// kind of silly, but oh well...
314#ifndef MAXINT
315#define MAXINT ((int)((((unsigned int)(-1)) << 1) >> 1))
316#endif
317
318
319// prime[i] is the largest prime smaller than 2^i
320#define NUM_PRIMES 31
321static long int prime[NUM_PRIMES] = {1L,2L,3L,7L,13L,31L,61L,127L,251L,509L,
322 1021L,2039L,4093L,8191L,16381L,32749L,65521L,131071L,262139L,
323 524287L,1048573L,2097143L,4194301L,8388593L,16777213L,33554393L,
324 67108859L,134217689L,268435399L,536870909L,1073741789L};
325
326
327// an axis aligned bounding box in the hash table
328struct dxAABB {
329 dxAABB *next; // next in the list of all AABBs
330 int level; // the level this is stored in (cell size = 2^level)
331 int dbounds[6]; // AABB bounds, discretized to cell size
332 dxGeom *geom; // corresponding geometry object (AABB stored here)
333 int index; // index of this AABB, starting from 0
334};
335
336
337// a hash table node that represents an AABB that intersects a particular cell
338// at a particular level
339struct Node {
340 Node *next; // next node in hash table collision list, 0 if none
341 int x,y,z; // cell position in space, discretized to cell size
342 dxAABB *aabb; // axis aligned bounding box that intersects this cell
343};
344
345
346// return the `level' of an AABB. the AABB will be put into cells at this
347// level - the cell size will be 2^level. the level is chosen to be the
348// smallest value such that the AABB occupies no more than 8 cells, regardless
349// of its placement. this means that:
350// size/2 < q <= size
351// where q is the maximum AABB dimension.
352
353static int findLevel (dReal bounds[6])
354{
355 if (bounds[0] <= -dInfinity || bounds[1] >= dInfinity ||
356 bounds[2] <= -dInfinity || bounds[3] >= dInfinity ||
357 bounds[4] <= -dInfinity || bounds[5] >= dInfinity) {
358 return MAXINT;
359 }
360
361 // compute q
362 dReal q,q2;
363 q = bounds[1] - bounds[0]; // x bounds
364 q2 = bounds[3] - bounds[2]; // y bounds
365 if (q2 > q) q = q2;
366 q2 = bounds[5] - bounds[4]; // z bounds
367 if (q2 > q) q = q2;
368
369 // find level such that 0.5 * 2^level < q <= 2^level
370 int level;
371 frexp (q,&level); // q = (0.5 .. 1.0) * 2^level (definition of frexp)
372 return level;
373}
374
375
376// find a virtual memory address for a cell at the given level and x,y,z
377// position.
378// @@@ currently this is not very sophisticated, e.g. the scaling
379// factors could be better designed to avoid collisions, and they should
380// probably depend on the hash table physical size.
381
382static unsigned long getVirtualAddress (int level, int x, int y, int z)
383{
384 return level*1000 + x*100 + y*10 + z;
385}
386
387//****************************************************************************
388// hash space
389
390struct dxHashSpace : public dxSpace {
391 int global_minlevel; // smallest hash table level to put AABBs in
392 int global_maxlevel; // objects that need a level larger than this will be
393 // put in a "big objects" list instead of a hash table
394
395 dxHashSpace (dSpaceID _space);
396 void setLevels (int minlevel, int maxlevel);
397 void getLevels (int *minlevel, int *maxlevel);
398 void cleanGeoms();
399 void collide (void *data, dNearCallback *callback);
400 void collide2 (void *data, dxGeom *geom, dNearCallback *callback);
401};
402
403
404dxHashSpace::dxHashSpace (dSpaceID _space) : dxSpace (_space)
405{
406 type = dHashSpaceClass;
407 global_minlevel = -3;
408 global_maxlevel = 10;
409}
410
411
412void dxHashSpace::setLevels (int minlevel, int maxlevel)
413{
414 dAASSERT (minlevel <= maxlevel);
415 global_minlevel = minlevel;
416 global_maxlevel = maxlevel;
417}
418
419
420void dxHashSpace::getLevels (int *minlevel, int *maxlevel)
421{
422 if (minlevel) *minlevel = global_minlevel;
423 if (maxlevel) *maxlevel = global_maxlevel;
424}
425
426
427void dxHashSpace::cleanGeoms()
428{
429 // compute the AABBs of all dirty geoms, and clear the dirty flags
430 lock_count++;
431 for (dxGeom *g=first; g && (g->gflags & GEOM_DIRTY); g=g->next) {
432 if (IS_SPACE(g)) {
433 ((dxSpace*)g)->cleanGeoms();
434 }
435 g->recomputeAABB();
436 g->gflags &= (~(GEOM_DIRTY|GEOM_AABB_BAD));
437 }
438 lock_count--;
439}
440
441
442void dxHashSpace::collide (void *data, dNearCallback *callback)
443{
444 dAASSERT(this && callback);
445 dxGeom *geom;
446 dxAABB *aabb;
447 int i,maxlevel;
448
449 // 0 or 1 geoms can't collide with anything
450 if (count < 2) return;
451
452 lock_count++;
453 cleanGeoms();
454
455 // create a list of auxiliary information for all geom axis aligned bounding
456 // boxes. set the level for all AABBs. put AABBs larger than the space's
457 // global_maxlevel in the big_boxes list, check everything else against
458 // that list at the end. for AABBs that are not too big, record the maximum
459 // level that we need.
460
461 int n = 0; // number of AABBs in main list
462 dxAABB *first_aabb = 0; // list of AABBs in hash table
463 dxAABB *big_boxes = 0; // list of AABBs too big for hash table
464 maxlevel = global_minlevel - 1;
465 for (geom = first; geom; geom=geom->next) {
466 if (!GEOM_ENABLED(geom)){
467 continue;
468 }
469 dxAABB *aabb = (dxAABB*) ALLOCA (sizeof(dxAABB));
470 aabb->geom = geom;
471 // compute level, but prevent cells from getting too small
472 int level = findLevel (geom->aabb);
473 if (level < global_minlevel) level = global_minlevel;
474 if (level <= global_maxlevel) {
475 // aabb goes in main list
476 aabb->next = first_aabb;
477 first_aabb = aabb;
478 aabb->level = level;
479 if (level > maxlevel) maxlevel = level;
480 // cellsize = 2^level
481 dReal cellsize = (dReal) ldexp (1.0,level);
482 // discretize AABB position to cell size
483 for (i=0; i < 6; i++) aabb->dbounds[i] = (int)
484 floor (geom->aabb[i]/cellsize);
485 // set AABB index
486 aabb->index = n;
487 n++;
488 }
489 else {
490 // aabb is too big, put it in the big_boxes list. we don't care about
491 // setting level, dbounds, index, or the maxlevel
492 aabb->next = big_boxes;
493 big_boxes = aabb;
494 }
495 }
496
497 // for `n' objects, an n*n array of bits is used to record if those objects
498 // have been intersection-tested against each other yet. this array can
499 // grow large with high n, but oh well...
500 int tested_rowsize = (n+7) >> 3; // number of bytes needed for n bits
501 unsigned char *tested = (unsigned char *) ALLOCA (n * tested_rowsize);
502 memset (tested,0,n * tested_rowsize);
503
504 // create a hash table to store all AABBs. each AABB may take up to 8 cells.
505 // we use chaining to resolve collisions, but we use a relatively large table
506 // to reduce the chance of collisions.
507
508 // compute hash table size sz to be a prime > 8*n
509 for (i=0; i<NUM_PRIMES; i++) {
510 if (prime[i] >= (8*n)) break;
511 }
512 if (i >= NUM_PRIMES) i = NUM_PRIMES-1; // probably pointless
513 int sz = prime[i];
514
515 // allocate and initialize hash table node pointers
516 Node **table = (Node **) ALLOCA (sizeof(Node*) * sz);
517 for (i=0; i<sz; i++) table[i] = 0;
518
519 // add each AABB to the hash table (may need to add it to up to 8 cells)
520 for (aabb=first_aabb; aabb; aabb=aabb->next) {
521 int *dbounds = aabb->dbounds;
522 for (int xi = dbounds[0]; xi <= dbounds[1]; xi++) {
523 for (int yi = dbounds[2]; yi <= dbounds[3]; yi++) {
524 for (int zi = dbounds[4]; zi <= dbounds[5]; zi++) {
525 // get the hash index
526 unsigned long hi = getVirtualAddress (aabb->level,xi,yi,zi) % sz;
527 // add a new node to the hash table
528 Node *node = (Node*) ALLOCA (sizeof (Node));
529 node->x = xi;
530 node->y = yi;
531 node->z = zi;
532 node->aabb = aabb;
533 node->next = table[hi];
534 table[hi] = node;
535 }
536 }
537 }
538 }
539
540 // now that all AABBs are loaded into the hash table, we do the actual
541 // collision detection. for all AABBs, check for other AABBs in the
542 // same cells for collisions, and then check for other AABBs in all
543 // intersecting higher level cells.
544
545 int db[6]; // discrete bounds at current level
546 for (aabb=first_aabb; aabb; aabb=aabb->next) {
547 // we are searching for collisions with aabb
548 for (i=0; i<6; i++) db[i] = aabb->dbounds[i];
549 for (int level = aabb->level; level <= maxlevel; level++) {
550 for (int xi = db[0]; xi <= db[1]; xi++) {
551 for (int yi = db[2]; yi <= db[3]; yi++) {
552 for (int zi = db[4]; zi <= db[5]; zi++) {
553 // get the hash index
554 unsigned long hi = getVirtualAddress (level,xi,yi,zi) % sz;
555 // search all nodes at this index
556 Node *node;
557 for (node = table[hi]; node; node=node->next) {
558 // node points to an AABB that may intersect aabb
559 if (node->aabb == aabb) continue;
560 if (node->aabb->level == level &&
561 node->x == xi && node->y == yi && node->z == zi) {
562 // see if aabb and node->aabb have already been tested
563 // against each other
564 unsigned char mask;
565 if (aabb->index <= node->aabb->index) {
566 i = (aabb->index * tested_rowsize)+(node->aabb->index >> 3);
567 mask = 1 << (node->aabb->index & 7);
568 }
569 else {
570 i = (node->aabb->index * tested_rowsize)+(aabb->index >> 3);
571 mask = 1 << (aabb->index & 7);
572 }
573 dIASSERT (i >= 0 && i < (tested_rowsize*n));
574 if ((tested[i] & mask)==0) {
575 collideAABBs (aabb->geom,node->aabb->geom,data,callback);
576 }
577 tested[i] |= mask;
578 }
579 }
580 }
581 }
582 }
583 // get the discrete bounds for the next level up
584 for (i=0; i<6; i++) db[i] >>= 1;
585 }
586 }
587
588 // every AABB in the normal list must now be intersected against every
589 // AABB in the big_boxes list. so let's hope there are not too many objects
590 // in the big_boxes list.
591 for (aabb=first_aabb; aabb; aabb=aabb->next) {
592 for (dxAABB *aabb2=big_boxes; aabb2; aabb2=aabb2->next) {
593 collideAABBs (aabb->geom,aabb2->geom,data,callback);
594 }
595 }
596
597 // intersected all AABBs in the big_boxes list together
598 for (aabb=big_boxes; aabb; aabb=aabb->next) {
599 for (dxAABB *aabb2=aabb->next; aabb2; aabb2=aabb2->next) {
600 collideAABBs (aabb->geom,aabb2->geom,data,callback);
601 }
602 }
603
604 lock_count--;
605}
606
607
608void dxHashSpace::collide2 (void *data, dxGeom *geom,
609 dNearCallback *callback)
610{
611 dAASSERT (geom && callback);
612
613 // this could take advantage of the hash structure to avoid
614 // O(n2) complexity, but it does not yet.
615
616 lock_count++;
617 cleanGeoms();
618 geom->recomputeAABB();
619
620 // intersect bounding boxes
621 for (dxGeom *g=first; g; g=g->next) {
622 if (GEOM_ENABLED(g)) collideAABBs (g,geom,data,callback);
623 }
624
625 lock_count--;
626}
627
628//****************************************************************************
629// space functions
630
631dxSpace *dSimpleSpaceCreate (dxSpace *space)
632{
633 return new dxSimpleSpace (space);
634}
635
636
637dxSpace *dHashSpaceCreate (dxSpace *space)
638{
639 return new dxHashSpace (space);
640}
641
642
643void dHashSpaceSetLevels (dxSpace *space, int minlevel, int maxlevel)
644{
645 dAASSERT (space);
646 dUASSERT (minlevel <= maxlevel,"must have minlevel <= maxlevel");
647 dUASSERT (space->type == dHashSpaceClass,"argument must be a hash space");
648 dxHashSpace *hspace = (dxHashSpace*) space;
649 hspace->setLevels (minlevel,maxlevel);
650}
651
652
653void dHashSpaceGetLevels (dxSpace *space, int *minlevel, int *maxlevel)
654{
655 dAASSERT (space);
656 dUASSERT (space->type == dHashSpaceClass,"argument must be a hash space");
657 dxHashSpace *hspace = (dxHashSpace*) space;
658 hspace->getLevels (minlevel,maxlevel);
659}
660
661
662void dSpaceDestroy (dxSpace *space)
663{
664 dAASSERT (space);
665 dUASSERT (dGeomIsSpace(space),"argument not a space");
666 dGeomDestroy (space);
667}
668
669
670void dSpaceSetCleanup (dxSpace *space, int mode)
671{
672 dAASSERT (space);
673 dUASSERT (dGeomIsSpace(space),"argument not a space");
674 space->setCleanup (mode);
675}
676
677
678int dSpaceGetCleanup (dxSpace *space)
679{
680 dAASSERT (space);
681 dUASSERT (dGeomIsSpace(space),"argument not a space");
682 return space->getCleanup();
683}
684
685
686void dSpaceAdd (dxSpace *space, dxGeom *g)
687{
688 dAASSERT (space);
689 dUASSERT (dGeomIsSpace(space),"argument not a space");
690 CHECK_NOT_LOCKED (space);
691 space->add (g);
692}
693
694
695void dSpaceRemove (dxSpace *space, dxGeom *g)
696{
697 dAASSERT (space);
698 dUASSERT (dGeomIsSpace(space),"argument not a space");
699 CHECK_NOT_LOCKED (space);
700 space->remove (g);
701}
702
703
704int dSpaceQuery (dxSpace *space, dxGeom *g)
705{
706 dAASSERT (space);
707 dUASSERT (dGeomIsSpace(space),"argument not a space");
708 return space->query (g);
709}
710
711void dSpaceClean (dxSpace *space){
712 dAASSERT (space);
713 dUASSERT (dGeomIsSpace(space),"argument not a space");
714
715 space->cleanGeoms();
716}
717
718int dSpaceGetNumGeoms (dxSpace *space)
719{
720 dAASSERT (space);
721 dUASSERT (dGeomIsSpace(space),"argument not a space");
722 return space->getNumGeoms();
723}
724
725
726dGeomID dSpaceGetGeom (dxSpace *space, int i)
727{
728 dAASSERT (space);
729 dUASSERT (dGeomIsSpace(space),"argument not a space");
730 return space->getGeom (i);
731}
732
733
734void dSpaceCollide (dxSpace *space, void *data, dNearCallback *callback)
735{
736 dAASSERT (space && callback);
737 dUASSERT (dGeomIsSpace(space),"argument not a space");
738 space->collide (data,callback);
739}
740
741
742void dSpaceCollide2 (dxGeom *g1, dxGeom *g2, void *data,
743 dNearCallback *callback)
744{
745 dAASSERT (g1 && g2 && callback);
746 dxSpace *s1,*s2;
747
748 // see if either geom is a space
749 if (IS_SPACE(g1)) s1 = (dxSpace*) g1; else s1 = 0;
750 if (IS_SPACE(g2)) s2 = (dxSpace*) g2; else s2 = 0;
751
752 // handle the four space/geom cases
753 if (s1) {
754 if (s2) {
755 // g1 and g2 are spaces.
756 if (s1==s2) {
757 // collide a space with itself --> interior collision
758 s1->collide (data,callback);
759 }
760 else {
761 // iterate through the space that has the fewest geoms, calling
762 // collide2 in the other space for each one.
763 if (s1->count < s2->count) {
764 for (dxGeom *g = s1->first; g; g=g->next) {
765 s2->collide2 (data,g,callback);
766 }
767 }
768 else {
769 for (dxGeom *g = s2->first; g; g=g->next) {
770 s1->collide2 (data,g,callback);
771 }
772 }
773 }
774 }
775 else {
776 // g1 is a space, g2 is a geom
777 s1->collide2 (data,g2,callback);
778 }
779 }
780 else {
781 if (s2) {
782 // g1 is a geom, g2 is a space
783 s2->collide2 (data,g1,callback);
784 }
785 else {
786 // g1 and g2 are geoms, call the callback directly
787 callback (data,g1,g2);
788 }
789 }
790}
generated by cgit v1.1 at 2024-11-09 22:59:45 +0000