OPCODE/OPC_SphereCollider.cpp

   1 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
   2 /*
   3  *      OPCODE - Optimized Collision Detection
   4  *      Copyright (C) 2001 Pierre Terdiman
   5  *      Homepage: http://www.codercorner.com/Opcode.htm
   6  */
   7 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
   8
   9 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  10 /**
  11  *      Contains code for a sphere collider.
  12  *      \file           OPC_SphereCollider.cpp
  13  *      \author         Pierre Terdiman
  14  *      \date           June, 2, 2001
  15  */
  16 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  17
  18 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  19 /**
  20  *      Contains a sphere-vs-tree collider.
  21  *      This class performs a collision test between a sphere and an AABB tree. You can use this to do a standard player vs world collision,
  22  *      in a Nettle/Telemachos way. It doesn't suffer from all reported bugs in those two classic codes - the "new" one by Paul Nettle is a
  23  *      debuggued version I think. Collision response can be driven by reported collision data - it works extremely well for me. In sake of
  24  *      efficiency, all meshes (that is, all AABB trees) should of course also be kept in an extra hierarchical structure (octree, whatever).
  25  *
  26  *      \class          SphereCollider
  27  *      \author         Pierre Terdiman
  28  *      \version        1.3
  29  *      \date           June, 2, 2001
  30 */
  31 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  32
  33 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  34 // Precompiled Header
  35 #include "Stdafx.h"
  36
  37 using namespace Opcode;
  38
  39 #include "OPC_SphereAABBOverlap.h"
  40 #include "OPC_SphereTriOverlap.h"
  41
  42 #define SET_CONTACT(prim_index, flag)                                                                   \
  43         /* Set contact status */                                                                                        \
  44         mFlags |= flag;                                                                                                         \
  45         mTouchedPrimitives->Add(udword(prim_index));
  46
  47 //! Sphere-triangle overlap test
  48 #define SPHERE_PRIM(prim_index, flag)                                                                   \
  49         /* Request vertices from the app */                                                                     \
  50         VertexPointers VP;      ConversionArea VC;      mIMesh->GetTriangle(VP, prim_index, VC); \
  51                                                                                                                                                 \
  52         /* Perform sphere-tri overlap test */                                                           \
  53         if(SphereTriOverlap(*VP.Vertex[0], *VP.Vertex[1], *VP.Vertex[2]))       \
  54         {                                                                                                                                       \
  55                 SET_CONTACT(prim_index, flag)                                                                   \
  56         }
  57
  58 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  59 /**
  60  *      Constructor.
  61  */
  62 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  63 SphereCollider::SphereCollider()
  64 {
  65         mCenter.Zero();
  66         mRadius2 = 0.0f;
  67 }
  68
  69 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  70 /**
  71  *      Destructor.
  72  */
  73 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  74 SphereCollider::~SphereCollider()
  75 {
  76 }
  77
  78 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  79 /**
  80  *      Generic collision query for generic OPCODE models. After the call, access the results:
  81  *      - with GetContactStatus()
  82  *      - with GetNbTouchedPrimitives()
  83  *      - with GetTouchedPrimitives()
  84  *
  85  *      \param          cache           [in/out] a sphere cache
  86  *      \param          sphere          [in] collision sphere in local space
  87  *      \param          model           [in] Opcode model to collide with
  88  *      \param          worlds          [in] sphere's world matrix, or null
  89  *      \param          worldm          [in] model's world matrix, or null
  90  *      \return         true if success
  91  *      \warning        SCALE NOT SUPPORTED. The matrices must contain rotation & translation parts only.
  92  */
  93 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  94 bool SphereCollider::Collide(SphereCache& cache, const Sphere& sphere, const Model& model, const Matrix4x4* worlds, const Matrix4x4* worldm)
  95 {
  96         // Checkings
  97         if(!Setup(&model))      return false;
  98
  99         // Init collision query
 100         if(InitQuery(cache, sphere, worlds, worldm))    return true;
 101
 102         // Special case for 1-leaf trees
 103         if(mCurrentModel && mCurrentModel->HasSingleNode())
 104         {
 105                 // Here we're supposed to perform a normal query, except our tree has a single node, i.e. just a few triangles
 106                 udword Nb = mIMesh->GetNbTriangles();
 107                 // Loop through all triangles
 108                 for(udword i=0;i<Nb;i++)
 109                 {
 110                         SPHERE_PRIM(i, OPC_CONTACT)
 111                 }
 112                 return true;
 113         }
 114
 115         if(!model.HasLeafNodes())
 116         {
 117                 if(model.IsQuantized())
 118                 {
 119                         const AABBQuantizedNoLeafTree* Tree = static_cast<const AABBQuantizedNoLeafTree *>(model.GetTree());
 120
 121                         // Setup dequantization coeffs
 122                         mCenterCoeff    = Tree->mCenterCoeff;
 123                         mExtentsCoeff   = Tree->mExtentsCoeff;
 124
 125                         // Perform collision query
 126                         if(SkipPrimitiveTests())        _CollideNoPrimitiveTest(Tree->GetNodes());
 127                         else                                            _Collide(Tree->GetNodes());
 128                 }
 129                 else
 130                 {
 131                         const AABBNoLeafTree* Tree = static_cast<const AABBNoLeafTree *>(model.GetTree());
 132
 133                         // Perform collision query
 134                         if(SkipPrimitiveTests())        _CollideNoPrimitiveTest(Tree->GetNodes());
 135                         else                                            _Collide(Tree->GetNodes());
 136                 }
 137         }
 138         else
 139         {
 140                 if(model.IsQuantized())
 141                 {
 142                         const AABBQuantizedTree* Tree = static_cast<const AABBQuantizedTree *>(model.GetTree());
 143
 144                         // Setup dequantization coeffs
 145                         mCenterCoeff    = Tree->mCenterCoeff;
 146                         mExtentsCoeff   = Tree->mExtentsCoeff;
 147
 148                         // Perform collision query
 149                         if(SkipPrimitiveTests())        _CollideNoPrimitiveTest(Tree->GetNodes());
 150                         else                                            _Collide(Tree->GetNodes());
 151                 }
 152                 else
 153                 {
 154                         const AABBCollisionTree* Tree = static_cast<const AABBCollisionTree *>(model.GetTree());
 155
 156                         // Perform collision query
 157                         if(SkipPrimitiveTests())        _CollideNoPrimitiveTest(Tree->GetNodes());
 158                         else                                            _Collide(Tree->GetNodes());
 159                 }
 160         }
 161         return true;
 162 }
 163
 164 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 165 /**
 166  *      Initializes a collision query :
 167  *      - reset stats & contact status
 168  *      - setup matrices
 169  *      - check temporal coherence
 170  *
 171  *      \param          cache           [in/out] a sphere cache
 172  *      \param          sphere          [in] sphere in local space
 173  *      \param          worlds          [in] sphere's world matrix, or null
 174  *      \param          worldm          [in] model's world matrix, or null
 175  *      \return         TRUE if we can return immediately
 176  *      \warning        SCALE NOT SUPPORTED. The matrices must contain rotation & translation parts only.
 177  */
 178 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 179 BOOL SphereCollider::InitQuery(SphereCache& cache, const Sphere& sphere, const Matrix4x4* worlds, const Matrix4x4* worldm)
 180 {
 181         // 1) Call the base method
 182         VolumeCollider::InitQuery();
 183
 184         // 2) Compute sphere in model space:
 185         // - Precompute R^2
 186         mRadius2 = sphere.mRadius * sphere.mRadius;
 187         // - Compute center position
 188         mCenter = sphere.mCenter;
 189         // -> to world space
 190         if(worlds)      mCenter *= *worlds;
 191         // -> to model space
 192         if(worldm)
 193         {
 194                 // Invert model matrix
 195                 Matrix4x4 InvWorldM;
 196                 InvertPRMatrix(InvWorldM, *worldm);
 197
 198                 mCenter *= InvWorldM;
 199         }
 200
 201         // 3) Setup destination pointer
 202         mTouchedPrimitives = &cache.TouchedPrimitives;
 203
 204         // 4) Special case: 1-triangle meshes [Opcode 1.3]
 205         if(mCurrentModel && mCurrentModel->HasSingleNode())
 206         {
 207                 if(!SkipPrimitiveTests())
 208                 {
 209                         // We simply perform the BV-Prim overlap test each time. We assume single triangle has index 0.
 210                         mTouchedPrimitives->Reset();
 211
 212                         // Perform overlap test between the unique triangle and the sphere (and set contact status if needed)
 213                         SPHERE_PRIM(udword(0), OPC_CONTACT)
 214
 215                         // Return immediately regardless of status
 216                         return TRUE;
 217                 }
 218         }
 219
 220         // 5) Check temporal coherence :
 221         if(TemporalCoherenceEnabled())
 222         {
 223                 // Here we use temporal coherence
 224                 // => check results from previous frame before performing the collision query
 225                 if(FirstContactEnabled())
 226                 {
 227                         // We're only interested in the first contact found => test the unique previously touched face
 228                         if(mTouchedPrimitives->GetNbEntries())
 229                         {
 230                                 // Get index of previously touched face = the first entry in the array
 231                                 udword PreviouslyTouchedFace = mTouchedPrimitives->GetEntry(0);
 232
 233                                 // Then reset the array:
 234                                 // - if the overlap test below is successful, the index we'll get added back anyway
 235                                 // - if it isn't, then the array should be reset anyway for the normal query
 236                                 mTouchedPrimitives->Reset();
 237
 238                                 // Perform overlap test between the cached triangle and the sphere (and set contact status if needed)
 239                                 SPHERE_PRIM(PreviouslyTouchedFace, OPC_TEMPORAL_CONTACT)
 240
 241                                 // Return immediately if possible
 242                                 if(GetContactStatus())  return TRUE;
 243                         }
 244                         // else no face has been touched during previous query
 245                         // => we'll have to perform a normal query
 246                 }
 247                 else
 248                 {
 249                         // We're interested in all contacts =>test the new real sphere N(ew) against the previous fat sphere P(revious):
 250                         float r = sqrtf(cache.FatRadius2) - sphere.mRadius;
 251                         if(IsCacheValid(cache) && cache.Center.SquareDistance(mCenter) < r*r)
 252                         {
 253                                 // - if N is included in P, return previous list
 254                                 // => we simply leave the list (mTouchedFaces) unchanged
 255
 256                                 // Set contact status if needed
 257                                 if(mTouchedPrimitives->GetNbEntries())  mFlags |= OPC_TEMPORAL_CONTACT;
 258
 259                                 // In any case we don't need to do a query
 260                                 return TRUE;
 261                         }
 262                         else
 263                         {
 264                                 // - else do the query using a fat N
 265
 266                                 // Reset cache since we'll about to perform a real query
 267                                 mTouchedPrimitives->Reset();
 268
 269                                 // Make a fat sphere so that coherence will work for subsequent frames
 270                                 mRadius2 *= cache.FatCoeff;
 271 //                              mRadius2 = (sphere.mRadius * cache.FatCoeff)*(sphere.mRadius * cache.FatCoeff);
 272
 273                                 // Update cache with query data (signature for cached faces)
 274                                 cache.Center = mCenter;
 275                                 cache.FatRadius2 = mRadius2;
 276                         }
 277                 }
 278         }
 279         else
 280         {
 281                 // Here we don't use temporal coherence => do a normal query
 282                 mTouchedPrimitives->Reset();
 283         }
 284
 285         return FALSE;
 286 }
 287
 288 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 289 /**
 290  *      Collision query for vanilla AABB trees.
 291  *      \param          cache           [in/out] a sphere cache
 292  *      \param          sphere          [in] collision sphere in world space
 293  *      \param          tree            [in] AABB tree
 294  *      \return         true if success
 295  */
 296 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 297 bool SphereCollider::Collide(SphereCache& cache, const Sphere& sphere, const AABBTree* tree)
 298 {
 299         // This is typically called for a scene tree, full of -AABBs-, not full of triangles.
 300         // So we don't really have "primitives" to deal with. Hence it doesn't work with
 301         // "FirstContact" + "TemporalCoherence".
 302         ASSERT( !(FirstContactEnabled() && TemporalCoherenceEnabled()) );
 303
 304         // Checkings
 305         if(!tree)       return false;
 306
 307         // Init collision query
 308         if(InitQuery(cache, sphere))    return true;
 309
 310         // Perform collision query
 311         _Collide(tree);
 312
 313         return true;
 314 }
 315
 316 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 317 /**
 318  *      Checks the sphere completely contains the box. In which case we can end the query sooner.
 319  *      \param          bc      [in] box center
 320  *      \param          be      [in] box extents
 321  *      \return         true if the sphere contains the whole box
 322  */
 323 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 324 inline_ BOOL SphereCollider::SphereContainsBox(const Point& bc, const Point& be)
 325 {
 326         // I assume if all 8 box vertices are inside the sphere, so does the whole box.
 327         // Sounds ok but maybe there's a better way?
 328         Point p;
 329         p.x=bc.x+be.x; p.y=bc.y+be.y; p.z=bc.z+be.z;    if(mCenter.SquareDistance(p)>=mRadius2) return FALSE;
 330         p.x=bc.x-be.x;                                                                  if(mCenter.SquareDistance(p)>=mRadius2) return FALSE;
 331         p.x=bc.x+be.x; p.y=bc.y-be.y;                                   if(mCenter.SquareDistance(p)>=mRadius2) return FALSE;
 332         p.x=bc.x-be.x;                                                                  if(mCenter.SquareDistance(p)>=mRadius2) return FALSE;
 333         p.x=bc.x+be.x; p.y=bc.y+be.y; p.z=bc.z-be.z;    if(mCenter.SquareDistance(p)>=mRadius2) return FALSE;
 334         p.x=bc.x-be.x;                                                                  if(mCenter.SquareDistance(p)>=mRadius2) return FALSE;
 335         p.x=bc.x+be.x; p.y=bc.y-be.y;                                   if(mCenter.SquareDistance(p)>=mRadius2) return FALSE;
 336         p.x=bc.x-be.x;                                                                  if(mCenter.SquareDistance(p)>=mRadius2) return FALSE;
 337
 338         return TRUE;
 339 }
 340
 341 #define TEST_BOX_IN_SPHERE(center, extents)     \
 342         if(SphereContainsBox(center, extents))  \
 343         {                                                                               \
 344                 /* Set contact status */                        \
 345                 mFlags |= OPC_CONTACT;                          \
 346                 _Dump(node);                                            \
 347                 return;                                                         \
 348         }
 349
 350 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 351 /**
 352  *      Recursive collision query for normal AABB trees.
 353  *      \param          node    [in] current collision node
 354  */
 355 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 356 void SphereCollider::_Collide(const AABBCollisionNode* node)
 357 {
 358         // Perform Sphere-AABB overlap test
 359         if(!SphereAABBOverlap(node->mAABB.mCenter, node->mAABB.mExtents))       return;
 360
 361         TEST_BOX_IN_SPHERE(node->mAABB.mCenter, node->mAABB.mExtents)
 362
 363         if(node->IsLeaf())
 364         {
 365                 SPHERE_PRIM(node->GetPrimitive(), OPC_CONTACT)
 366         }
 367         else
 368         {
 369                 _Collide(node->GetPos());
 370
 371                 if(ContactFound()) return;
 372
 373                 _Collide(node->GetNeg());
 374         }
 375 }
 376
 377 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 378 /**
 379  *      Recursive collision query for normal AABB trees, without primitive tests.
 380  *      \param          node    [in] current collision node
 381  */
 382 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 383 void SphereCollider::_CollideNoPrimitiveTest(const AABBCollisionNode* node)
 384 {
 385         // Perform Sphere-AABB overlap test
 386         if(!SphereAABBOverlap(node->mAABB.mCenter, node->mAABB.mExtents))       return;
 387
 388         TEST_BOX_IN_SPHERE(node->mAABB.mCenter, node->mAABB.mExtents)
 389
 390         if(node->IsLeaf())
 391         {
 392                 SET_CONTACT(node->GetPrimitive(), OPC_CONTACT)
 393         }
 394         else
 395         {
 396                 _CollideNoPrimitiveTest(node->GetPos());
 397
 398                 if(ContactFound()) return;
 399
 400                 _CollideNoPrimitiveTest(node->GetNeg());
 401         }
 402 }
 403
 404 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 405 /**
 406  *      Recursive collision query for quantized AABB trees.
 407  *      \param          node    [in] current collision node
 408  */
 409 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 410 void SphereCollider::_Collide(const AABBQuantizedNode* node)
 411 {
 412         // Dequantize box
 413         const QuantizedAABB& Box = node->mAABB;
 414         const Point Center(float(Box.mCenter[0]) * mCenterCoeff.x, float(Box.mCenter[1]) * mCenterCoeff.y, float(Box.mCenter[2]) * mCenterCoeff.z);
 415         const Point Extents(float(Box.mExtents[0]) * mExtentsCoeff.x, float(Box.mExtents[1]) * mExtentsCoeff.y, float(Box.mExtents[2]) * mExtentsCoeff.z);
 416
 417         // Perform Sphere-AABB overlap test
 418         if(!SphereAABBOverlap(Center, Extents)) return;
 419
 420         TEST_BOX_IN_SPHERE(Center, Extents)
 421
 422         if(node->IsLeaf())
 423         {
 424                 SPHERE_PRIM(node->GetPrimitive(), OPC_CONTACT)
 425         }
 426         else
 427         {
 428                 _Collide(node->GetPos());
 429
 430                 if(ContactFound()) return;
 431
 432                 _Collide(node->GetNeg());
 433         }
 434 }
 435
 436 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 437 /**
 438  *      Recursive collision query for quantized AABB trees, without primitive tests.
 439  *      \param          node    [in] current collision node
 440  */
 441 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 442 void SphereCollider::_CollideNoPrimitiveTest(const AABBQuantizedNode* node)
 443 {
 444         // Dequantize box
 445         const QuantizedAABB& Box = node->mAABB;
 446         const Point Center(float(Box.mCenter[0]) * mCenterCoeff.x, float(Box.mCenter[1]) * mCenterCoeff.y, float(Box.mCenter[2]) * mCenterCoeff.z);
 447         const Point Extents(float(Box.mExtents[0]) * mExtentsCoeff.x, float(Box.mExtents[1]) * mExtentsCoeff.y, float(Box.mExtents[2]) * mExtentsCoeff.z);
 448
 449         // Perform Sphere-AABB overlap test
 450         if(!SphereAABBOverlap(Center, Extents)) return;
 451
 452         TEST_BOX_IN_SPHERE(Center, Extents)
 453
 454         if(node->IsLeaf())
 455         {
 456                 SET_CONTACT(node->GetPrimitive(), OPC_CONTACT)
 457         }
 458         else
 459         {
 460                 _CollideNoPrimitiveTest(node->GetPos());
 461
 462                 if(ContactFound()) return;
 463
 464                 _CollideNoPrimitiveTest(node->GetNeg());
 465         }
 466 }
 467
 468 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 469 /**
 470  *      Recursive collision query for no-leaf AABB trees.
 471  *      \param          node    [in] current collision node
 472  */
 473 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 474 void SphereCollider::_Collide(const AABBNoLeafNode* node)
 475 {
 476         // Perform Sphere-AABB overlap test
 477         if(!SphereAABBOverlap(node->mAABB.mCenter, node->mAABB.mExtents))       return;
 478
 479         TEST_BOX_IN_SPHERE(node->mAABB.mCenter, node->mAABB.mExtents)
 480
 481         if(node->HasPosLeaf())  { SPHERE_PRIM(node->GetPosPrimitive(), OPC_CONTACT) }
 482         else                                    _Collide(node->GetPos());
 483
 484         if(ContactFound()) return;
 485
 486         if(node->HasNegLeaf())  { SPHERE_PRIM(node->GetNegPrimitive(), OPC_CONTACT) }
 487         else                                    _Collide(node->GetNeg());
 488 }
 489
 490 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 491 /**
 492  *      Recursive collision query for no-leaf AABB trees, without primitive tests.
 493  *      \param          node    [in] current collision node
 494  */
 495 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 496 void SphereCollider::_CollideNoPrimitiveTest(const AABBNoLeafNode* node)
 497 {
 498         // Perform Sphere-AABB overlap test
 499         if(!SphereAABBOverlap(node->mAABB.mCenter, node->mAABB.mExtents))       return;
 500
 501         TEST_BOX_IN_SPHERE(node->mAABB.mCenter, node->mAABB.mExtents)
 502
 503         if(node->HasPosLeaf())  { SET_CONTACT(node->GetPosPrimitive(), OPC_CONTACT) }
 504         else                                    _CollideNoPrimitiveTest(node->GetPos());
 505
 506         if(ContactFound()) return;
 507
 508         if(node->HasNegLeaf())  { SET_CONTACT(node->GetNegPrimitive(), OPC_CONTACT) }
 509         else                                    _CollideNoPrimitiveTest(node->GetNeg());
 510 }
 511
 512 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 513 /**
 514  *      Recursive collision query for quantized no-leaf AABB trees.
 515  *      \param          node    [in] current collision node
 516  */
 517 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 518 void SphereCollider::_Collide(const AABBQuantizedNoLeafNode* node)
 519 {
 520         // Dequantize box
 521         const QuantizedAABB& Box = node->mAABB;
 522         const Point Center(float(Box.mCenter[0]) * mCenterCoeff.x, float(Box.mCenter[1]) * mCenterCoeff.y, float(Box.mCenter[2]) * mCenterCoeff.z);
 523         const Point Extents(float(Box.mExtents[0]) * mExtentsCoeff.x, float(Box.mExtents[1]) * mExtentsCoeff.y, float(Box.mExtents[2]) * mExtentsCoeff.z);
 524
 525         // Perform Sphere-AABB overlap test
 526         if(!SphereAABBOverlap(Center, Extents)) return;
 527
 528         TEST_BOX_IN_SPHERE(Center, Extents)
 529
 530         if(node->HasPosLeaf())  { SPHERE_PRIM(node->GetPosPrimitive(), OPC_CONTACT) }
 531         else                                    _Collide(node->GetPos());
 532
 533         if(ContactFound()) return;
 534
 535         if(node->HasNegLeaf())  { SPHERE_PRIM(node->GetNegPrimitive(), OPC_CONTACT) }
 536         else                                    _Collide(node->GetNeg());
 537 }
 538
 539 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 540 /**
 541  *      Recursive collision query for quantized no-leaf AABB trees, without primitive tests.
 542  *      \param          node    [in] current collision node
 543  */
 544 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 545 void SphereCollider::_CollideNoPrimitiveTest(const AABBQuantizedNoLeafNode* node)
 546 {
 547         // Dequantize box
 548         const QuantizedAABB& Box = node->mAABB;
 549         const Point Center(float(Box.mCenter[0]) * mCenterCoeff.x, float(Box.mCenter[1]) * mCenterCoeff.y, float(Box.mCenter[2]) * mCenterCoeff.z);
 550         const Point Extents(float(Box.mExtents[0]) * mExtentsCoeff.x, float(Box.mExtents[1]) * mExtentsCoeff.y, float(Box.mExtents[2]) * mExtentsCoeff.z);
 551
 552         // Perform Sphere-AABB overlap test
 553         if(!SphereAABBOverlap(Center, Extents)) return;
 554
 555         TEST_BOX_IN_SPHERE(Center, Extents)
 556
 557         if(node->HasPosLeaf())  { SET_CONTACT(node->GetPosPrimitive(), OPC_CONTACT) }
 558         else                                    _CollideNoPrimitiveTest(node->GetPos());
 559
 560         if(ContactFound()) return;
 561
 562         if(node->HasNegLeaf())  { SET_CONTACT(node->GetNegPrimitive(), OPC_CONTACT) }
 563         else                                    _CollideNoPrimitiveTest(node->GetNeg());
 564 }
 565
 566 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 567 /**
 568  *      Recursive collision query for vanilla AABB trees.
 569  *      \param          node    [in] current collision node
 570  */
 571 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 572 void SphereCollider::_Collide(const AABBTreeNode* node)
 573 {
 574         // Perform Sphere-AABB overlap test
 575         Point Center, Extents;
 576         node->GetAABB()->GetCenter(Center);
 577         node->GetAABB()->GetExtents(Extents);
 578         if(!SphereAABBOverlap(Center, Extents)) return;
 579
 580         if(node->IsLeaf() || SphereContainsBox(Center, Extents))
 581         {
 582                 mFlags |= OPC_CONTACT;
 583                 mTouchedPrimitives->Add(node->GetPrimitives(), node->GetNbPrimitives());
 584         }
 585         else
 586         {
 587                 _Collide(node->GetPos());
 588                 _Collide(node->GetNeg());
 589         }
 590 }
 591
 592
 593
 594
 595
 596
 597
 598 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 599 /**
 600  *      Constructor.
 601  */
 602 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 603 HybridSphereCollider::HybridSphereCollider()
 604 {
 605 }
 606
 607 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 608 /**
 609  *      Destructor.
 610  */
 611 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 612 HybridSphereCollider::~HybridSphereCollider()
 613 {
 614 }
 615
 616 bool HybridSphereCollider::Collide(SphereCache& cache, const Sphere& sphere, const HybridModel& model, const Matrix4x4* worlds, const Matrix4x4* worldm)
 617 {
 618         // We don't want primitive tests here!
 619         mFlags |= OPC_NO_PRIMITIVE_TESTS;
 620
 621         // Checkings
 622         if(!Setup(&model))      return false;
 623
 624         // Init collision query
 625         if(InitQuery(cache, sphere, worlds, worldm))    return true;
 626
 627         // Special case for 1-leaf trees
 628         if(mCurrentModel && mCurrentModel->HasSingleNode())
 629         {
 630                 // Here we're supposed to perform a normal query, except our tree has a single node, i.e. just a few triangles
 631                 udword Nb = mIMesh->GetNbTriangles();
 632
 633                 // Loop through all triangles
 634                 for(udword i=0;i<Nb;i++)
 635                 {
 636                         SPHERE_PRIM(i, OPC_CONTACT)
 637                 }
 638                 return true;
 639         }
 640
 641         // Override destination array since we're only going to get leaf boxes here
 642         mTouchedBoxes.Reset();
 643         mTouchedPrimitives = &mTouchedBoxes;
 644
 645         // Now, do the actual query against leaf boxes
 646         if(!model.HasLeafNodes())
 647         {
 648                 if(model.IsQuantized())
 649                 {
 650                         const AABBQuantizedNoLeafTree* Tree = static_cast<const AABBQuantizedNoLeafTree *>(model.GetTree());
 651
 652                         // Setup dequantization coeffs
 653                         mCenterCoeff    = Tree->mCenterCoeff;
 654                         mExtentsCoeff   = Tree->mExtentsCoeff;
 655
 656                         // Perform collision query - we don't want primitive tests here!
 657                         _CollideNoPrimitiveTest(Tree->GetNodes());
 658                 }
 659                 else
 660                 {
 661                         const AABBNoLeafTree* Tree = static_cast<const AABBNoLeafTree *>(model.GetTree());
 662
 663                         // Perform collision query - we don't want primitive tests here!
 664                         _CollideNoPrimitiveTest(Tree->GetNodes());
 665                 }
 666         }
 667         else
 668         {
 669                 if(model.IsQuantized())
 670                 {
 671                         const AABBQuantizedTree* Tree = static_cast<const AABBQuantizedTree *>(model.GetTree());
 672
 673                         // Setup dequantization coeffs
 674                         mCenterCoeff    = Tree->mCenterCoeff;
 675                         mExtentsCoeff   = Tree->mExtentsCoeff;
 676
 677                         // Perform collision query - we don't want primitive tests here!
 678                         _CollideNoPrimitiveTest(Tree->GetNodes());
 679                 }
 680                 else
 681                 {
 682                         const AABBCollisionTree* Tree = static_cast<const AABBCollisionTree *>(model.GetTree());
 683
 684                         // Perform collision query - we don't want primitive tests here!
 685                         _CollideNoPrimitiveTest(Tree->GetNodes());
 686                 }
 687         }
 688
 689         // We only have a list of boxes so far
 690         if(GetContactStatus())
 691         {
 692                 // Reset contact status, since it currently only reflects collisions with leaf boxes
 693                 Collider::InitQuery();
 694
 695                 // Change dest container so that we can use built-in overlap tests and get collided primitives
 696                 cache.TouchedPrimitives.Reset();
 697                 mTouchedPrimitives = &cache.TouchedPrimitives;
 698
 699                 // Read touched leaf boxes
 700                 udword Nb = mTouchedBoxes.GetNbEntries();
 701                 const udword* Touched = mTouchedBoxes.GetEntries();
 702
 703                 const LeafTriangles* LT = model.GetLeafTriangles();
 704                 const udword* Indices = model.GetIndices();
 705
 706                 // Loop through touched leaves
 707                 while(Nb--)
 708                 {
 709                         const LeafTriangles& CurrentLeaf = LT[*Touched++];
 710
 711                         // Each leaf box has a set of triangles
 712                         udword NbTris = CurrentLeaf.GetNbTriangles();
 713                         if(Indices)
 714                         {
 715                                 const udword* T = &Indices[CurrentLeaf.GetTriangleIndex()];
 716
 717                                 // Loop through triangles and test each of them
 718                                 while(NbTris--)
 719                                 {
 720                                         udword TriangleIndex = *T++;
 721                                         SPHERE_PRIM(TriangleIndex, OPC_CONTACT)
 722                                 }
 723                         }
 724                         else
 725                         {
 726                                 udword BaseIndex = CurrentLeaf.GetTriangleIndex();
 727
 728                                 // Loop through triangles and test each of them
 729                                 while(NbTris--)
 730                                 {
 731                                         udword TriangleIndex = BaseIndex++;
 732                                         SPHERE_PRIM(TriangleIndex, OPC_CONTACT)
 733                                 }
 734                         }
 735                 }
 736         }
 737
 738         return true;
 739 }