Extras/CDTestFramework/Opcode/OPC_SphereCollider.cpp

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