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 

    25 spaces

    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 //****************************************************************************

    38 // make the geom dirty by setting the GEOM_DIRTY and GEOM_BAD_AABB flags

    39 // and moving it to the front of the space's list. all the parents of a

    40 // dirty geom also become dirty.

    41 

    42 void dGeomMoved (dxGeom *geom)

    43 {

    44   

    45   // if geom is offset, mark it as needing a calculate

    46   if (geom->offset_posr) {

    47     geom->gflags |= GEOM_POSR_BAD;

    48   }

    49   

    50   // from the bottom of the space heirarchy up, process all clean geoms

    51   // turning them into dirty geoms.

    52   dxSpace *parent = geom->parent_space;

    53 

    54   while (parent && (geom->gflags & GEOM_DIRTY)==0) {

    55 

    56     geom->gflags |= GEOM_DIRTY | GEOM_AABB_BAD;

    57     parent->dirty (geom);

    58     geom = parent;

    59     parent = parent->parent_space;

    60   }

    61 

    62   // all the remaining dirty geoms must have their AABB_BAD flags set, to

    63   // ensure that their AABBs get recomputed

    64   while (geom) {

    65     geom->gflags |= GEOM_DIRTY | GEOM_AABB_BAD;

    66 

    67     geom = geom->parent_space;

    68   }

    69 }

    70 

    71 #define GEOM_ENABLED(g) ((g)->gflags & GEOM_ENABLED)

    72 

    73 //****************************************************************************

    74 // dxSpace

    75 

    76 dxSpace::dxSpace (dSpaceID _space) : dxGeom (_space,0)

    77 {

    78   count = 0;

    79   first = 0;

    80   cleanup = 1;

    81   current_index = 0;

    82   current_geom = 0;

    83   lock_count = 0;

    84 }

    85 

    86 

    87 dxSpace::~dxSpace()

    88 {

    89   if (cleanup) {

    90     // note that destroying each geom will call remove()

    91     dxGeom *g,*n;

    92     for (g = first; g; g=n) {

    93       n = g->next;

    94       dGeomDestroy (g);

    95     }

    96   }

    97   else {

    98     dxGeom *g,*n;

    99     for (g = first; g; g=n) {

   100       n = g->next;

   101       remove (g);

   102     }

   103   }

   104 }

   105 

   106 

   107 void dxSpace::computeAABB()

   108 {

   109   if (first) {

   110     int i;

   111     dReal a[6];

   112     a[0] = dInfinity;

   113     a[1] = -dInfinity;

   114     a[2] = dInfinity;

   115     a[3] = -dInfinity;

   116     a[4] = dInfinity;

   117     a[5] = -dInfinity;

   118     for (dxGeom *g=first; g; g=g->next) {

   119       g->recomputeAABB();

   120       for (i=0; i<6; i += 2) if (g->aabb[i] < a[i]) a[i] = g->aabb[i];

   121       for (i=1; i<6; i += 2) if (g->aabb[i] > a[i]) a[i] = g->aabb[i];

   122     }

   123     memcpy(aabb,a,6*sizeof(dReal));

   124   }

   125   else {

   126     dSetZero (aabb,6);

   127   }

   128 }

   129 

   130 

   131 void dxSpace::setCleanup (int mode)

   132 {

   133   cleanup = (mode != 0);

   134 }

   135 

   136 

   137 int dxSpace::getCleanup()

   138 {

   139   return cleanup;

   140 }

   141 

   142 

   143 int dxSpace::query (dxGeom *geom)

   144 {

   145   return (geom->parent_space == this);

   146 }

   147 

   148 

   149 int dxSpace::getNumGeoms()

   150 {

   151   return count;

   152 }

   153 

   154 

   155 // the dirty geoms are numbered 0..k, the clean geoms are numbered k+1..count-1

   156 

   157 dxGeom *dxSpace::getGeom (int i)

   158 {

   159   if (current_geom && current_index == i-1) {

   160     current_geom = current_geom->next;

   161     current_index = i;

   162     return current_geom;

   163   }

   164   else {

   165     dxGeom *g=first;

   166     for (int j=0; j<i; j++) {

   167       if (g) g = g->next; else return 0;

   168     }

   169     current_geom = g;

   170     current_index = i;

   171     return g;

   172   }

   173 }

   174 

   175 

   176 void dxSpace::add (dxGeom *geom)

   177 {

   178 

   179   // add

   180   geom->parent_space = this;

   181   geom->spaceAdd (&first);

   182   count++;

   183 

   184   // enumerator has been invalidated

   185   current_geom = 0;

   186 

   187   // new geoms are added to the front of the list and are always

   188   // considered to be dirty. as a consequence, this space and all its

   189   // parents are dirty too.

   190   geom->gflags |= GEOM_DIRTY | GEOM_AABB_BAD;

   191   dGeomMoved (this);

   192 }

   193 

   194 

   195 void dxSpace::remove (dxGeom *geom)

   196 {

   197 

   198   // remove

   199   geom->spaceRemove();

   200   count--;

   201 

   202   // safeguard

   203   geom->next = 0;

   204   geom->tome = 0;

   205   geom->parent_space = 0;

   206 

   207   // enumerator has been invalidated

   208   current_geom = 0;

   209 

   210   // the bounding box of this space (and that of all the parents) may have

   211   // changed as a consequence of the removal.

   212   dGeomMoved (this);

   213 }

   214 

   215 

   216 void dxSpace::dirty (dxGeom *geom)

   217 {

   218   geom->spaceRemove();

   219   geom->spaceAdd (&first);

   220 }

   221 

   222 //****************************************************************************

   223 // simple space - reports all n^2 object intersections

   224 

   225 struct dxSimpleSpace : public dxSpace {

   226   dxSimpleSpace (dSpaceID _space);

   227   void cleanGeoms();

   228   void collide (void *data, dNearCallback *callback);

   229   void collide2 (void *data, dxGeom *geom, dNearCallback *callback);

   230 };

   231 

   232 

   233 dxSimpleSpace::dxSimpleSpace (dSpaceID _space) : dxSpace (_space)

   234 {

   235   type = dSimpleSpaceClass;

   236 }

   237 

   238 

   239 void dxSimpleSpace::cleanGeoms()

   240 {

   241   // compute the AABBs of all dirty geoms, and clear the dirty flags

   242   lock_count++;

   243   for (dxGeom *g=first; g && (g->gflags & GEOM_DIRTY); g=g->next) {

   244     if (IS_SPACE(g)) {

   245       ((dxSpace*)g)->cleanGeoms();

   246     }

   247     g->recomputeAABB();

   248     g->gflags &= (~(GEOM_DIRTY|GEOM_AABB_BAD));

   249   }

   250   lock_count--;

   251 }

   252 

   253 

   254 void dxSimpleSpace::collide (void *data, dNearCallback *callback)

   255 {

   256 

   257   lock_count++;

   258   cleanGeoms();

   259 

   260   // intersect all bounding boxes

   261   for (dxGeom *g1=first; g1; g1=g1->next) {

   262     if (GEOM_ENABLED(g1)){

   263       for (dxGeom *g2=g1->next; g2; g2=g2->next) {

   264 	if (GEOM_ENABLED(g2)){

   265 	  collideAABBs (g1,g2,data,callback);

   266 	}

   267       }

   268     }

   269   }

   270 

   271   lock_count--;

   272 }

   273 

   274 

   275 void dxSimpleSpace::collide2 (void *data, dxGeom *geom,

   276 			      dNearCallback *callback)

   277 {

   278 

   279   lock_count++;

   280   cleanGeoms();

   281   geom->recomputeAABB();

   282 

   283   // intersect bounding boxes

   284   for (dxGeom *g=first; g; g=g->next) {

   285     if (GEOM_ENABLED(g)){

   286       collideAABBs (g,geom,data,callback);

   287     }

   288   }

   289 

   290   lock_count--;

   291 }

   292 

   293 //****************************************************************************

   294 // utility stuff for hash table space

   295 

   296 // kind of silly, but oh well...

   297 #ifndef MAXINT

   298 #define MAXINT ((int)((((unsigned int)(-1)) << 1) >> 1))

   299 #endif

   300 

   301 

   302 // prime[i] is the largest prime smaller than 2^i

   303 #define NUM_PRIMES 31

   304 

   305 

   306 // an axis aligned bounding box in the hash table

   307 struct dxAABB {

   308   dxAABB *next;		// next in the list of all AABBs

   309   int level;		// the level this is stored in (cell size = 2^level)

   310   int dbounds[6];	// AABB bounds, discretized to cell size

   311   dxGeom *geom;		// corresponding geometry object (AABB stored here)

   312   int index;		// index of this AABB, starting from 0

   313 };

   314 

   315 

   316 // a hash table node that represents an AABB that intersects a particular cell

   317 // at a particular level

   318 struct Node {

   319   Node *next;		// next node in hash table collision list, 0 if none

   320   int x,y,z;		// cell position in space, discretized to cell size

   321   dxAABB *aabb;		// axis aligned bounding box that intersects this cell

   322 };

   323 

   324 

   325 

   326 //****************************************************************************

   327 // space functions

   328 

   329 EXPORT_C dxSpace *dSimpleSpaceCreate (dxSpace *space)

   330 {

   331   return new dxSimpleSpace (space);

   332 }

   333 

   334 EXPORT_C void dSpaceDestroy (dxSpace *space)

   335 {

   336   dGeomDestroy (space);

   337 }

   338 

   339 

   340 EXPORT_C void dSpaceSetCleanup (dxSpace *space, int mode)

   341 {

   342 

   343   space->setCleanup (mode);

   344 }

   345 

   346 

   347 EXPORT_C int dSpaceGetCleanup (dxSpace *space)

   348 {

   349 

   350   return space->getCleanup();

   351 }

   352 

   353 

   354 EXPORT_C void dSpaceAdd (dxSpace *space, dxGeom *g)

   355 {

   356 

   357   space->add (g);

   358 }

   359 

   360 

   361 EXPORT_C void dSpaceRemove (dxSpace *space, dxGeom *g)

   362 {

   363   space->remove (g);

   364 }

   365 

   366 

   367 EXPORT_C int dSpaceQuery (dxSpace *space, dxGeom *g)

   368 {

   369 

   370   return space->query (g);

   371 }

   372 

   373 EXPORT_C void dSpaceClean (dxSpace *space){

   374 

   375   space->cleanGeoms();

   376 }

   377 

   378 EXPORT_C int dSpaceGetNumGeoms (dxSpace *space)

   379 {

   380   return space->getNumGeoms();

   381 }

   382 

   383 

   384 EXPORT_C dGeomID dSpaceGetGeom (dxSpace *space, int i)

   385 {

   386   return space->getGeom (i);

   387 }

   388 

   389 

   390 EXPORT_C void dSpaceCollide (dxSpace *space, void *data, dNearCallback *callback)

   391 {

   392 

   393   space->collide (data,callback);

   394 }

   395 

   396 

   397 EXPORT_C void dSpaceCollide2 (dxGeom *g1, dxGeom *g2, void *data,

   398 		     dNearCallback *callback)

   399 {

   400 

   401   dxSpace *s1,*s2;

   402 

   403   // see if either geom is a space

   404   if (IS_SPACE(g1)) s1 = (dxSpace*) g1; else s1 = 0;

   405   if (IS_SPACE(g2)) s2 = (dxSpace*) g2; else s2 = 0;

   406 

   407   // handle the four space/geom cases

   408   if (s1) {

   409     if (s2) {

   410       // g1 and g2 are spaces.

   411       if (s1==s2) {

   412 	// collide a space with itself --> interior collision

   413 	s1->collide (data,callback);

   414       }

   415       else {

   416 	// iterate through the space that has the fewest geoms, calling

   417 	// collide2 in the other space for each one.

   418 	if (s1->count < s2->count) {

   419 	  for (dxGeom *g = s1->first; g; g=g->next) {

   420 	    s2->collide2 (data,g,callback);

   421 	  }

   422 	}

   423 	else {

   424 	  for (dxGeom *g = s2->first; g; g=g->next) {

   425 	    s1->collide2 (data,g,callback);

   426 	  }

   427 	}

   428       }

   429     }

   430     else {

   431       // g1 is a space, g2 is a geom

   432       s1->collide2 (data,g2,callback);

   433     }

   434   }

   435   else {

   436     if (s2) {

   437       // g1 is a geom, g2 is a space

   438       s2->collide2 (data,g1,callback);

   439     }

   440     else {

   441       // g1 and g2 are geoms, call the callback directly

   442       callback (data,g1,g2);

   443     }

   444   }

   445 }