Extras/CDTestFramework/Opcode/OPC_RayCollider.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 ray collider.
  21  *      \file           OPC_RayCollider.cpp
  22  *      \author         Pierre Terdiman
  23  *      \date           June, 2, 2001
  24  */
  25 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  26
  27 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  28 /**
  29  *      Contains a ray-vs-tree collider.
  30  *      This class performs a stabbing query on an AABB tree, i.e. does a ray-mesh collision.
  31  *
  32  *      HIGHER DISTANCE BOUND:
  33  *
  34  *              If P0 and P1 are two 3D points, let's define:
  35  *              - d = distance between P0 and P1
  36  *              - Origin        = P0
  37  *              - Direction     = (P1 - P0) / d = normalized direction vector
  38  *              - A parameter t such as a point P on the line (P0,P1) is P = Origin + t * Direction
  39  *              - t = 0  -->  P = P0
  40  *              - t = d  -->  P = P1
  41  *
  42  *              Then we can define a general "ray" as:
  43  *
  44  *                      struct Ray
  45  *                      {
  46  *                              Point   Origin;
  47  *                              Point   Direction;
  48  *                      };
  49  *
  50  *              But it actually maps three different things:
  51  *              - a segment,   when 0 <= t <= d
  52  *              - a half-line, when 0 <= t < +infinity, or -infinity < t <= d
  53  *              - a line,      when -infinity < t < +infinity
  54  *
  55  *              In Opcode, we support segment queries, which yield half-line queries by setting d = +infinity.
  56  *              We don't support line-queries. If you need them, shift the origin along the ray by an appropriate margin.
  57  *
  58  *              In short, the lower bound is always 0, and you can setup the higher bound "d" with RayCollider::SetMaxDist().
  59  *
  60  *              Query   |segment                        |half-line              |line
  61  *              --------|-------------------|---------------|----------------
  62  *              Usages  |-shadow feelers        |-raytracing    |-
  63  *                              |-sweep tests           |-in/out tests  |
  64  *
  65  *      FIRST CONTACT:
  66  *
  67  *              - You can setup "first contact" mode or "all contacts" mode with RayCollider::SetFirstContact().
  68  *              - In "first contact" mode we return as soon as the ray hits one face. If can be useful e.g. for shadow feelers, where
  69  *              you want to know whether the path to the light is free or not (a boolean answer is enough).
  70  *              - In "all contacts" mode we return all faces hit by the ray.
  71  *
  72  *      TEMPORAL COHERENCE:
  73  *
  74  *              - You can enable or disable temporal coherence with RayCollider::SetTemporalCoherence().
  75  *              - It currently only works in "first contact" mode.
  76  *              - If temporal coherence is enabled, the previously hit triangle is cached during the first query. Then, next queries
  77  *              start by colliding the ray against the cached triangle. If they still collide, we return immediately.
  78  *
  79  *      CLOSEST HIT:
  80  *
  81  *              - You can enable or disable "closest hit" with RayCollider::SetClosestHit().
  82  *              - It currently only works in "all contacts" mode.
  83  *              - If closest hit is enabled, faces are sorted by distance on-the-fly and the closest one only is reported.
  84  *
  85  *      BACKFACE CULLING:
  86  *
  87  *              - You can enable or disable backface culling with RayCollider::SetCulling().
  88  *              - If culling is enabled, ray will not hit back faces (only front faces).
  89  *
  90  *
  91  *
  92  *      \class          RayCollider
  93  *      \author         Pierre Terdiman
  94  *      \version        1.3
  95  *      \date           June, 2, 2001
  96 */
  97 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  98
  99 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 100 /**
 101  *      This class describes a face hit by a ray or segment.
 102  *      This is a particular class dedicated to stabbing queries.
 103  *
 104  *      \class          CollisionFace
 105  *      \author         Pierre Terdiman
 106  *      \version        1.3
 107  *      \date           March, 20, 2001
 108 */
 109 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 110
 111 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 112 /**
 113  *      This class is a dedicated collection of CollisionFace.
 114  *
 115  *      \class          CollisionFaces
 116  *      \author         Pierre Terdiman
 117  *      \version        1.3
 118  *      \date           March, 20, 2001
 119 */
 120 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 121
 122 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 123 // Precompiled Header
 124 #include "Stdafx.h"
 125
 126 using namespace Opcode;
 127
 128 #include "OPC_RayAABBOverlap.h"
 129 #include "OPC_RayTriOverlap.h"
 130
 131 #define SET_CONTACT(prim_index, flag)                                                                                   \
 132         mNbIntersections++;                                                                                                                     \
 133         /* Set contact status */                                                                                                        \
 134         mFlags |= flag;                                                                                                                         \
 135         /* In any case the contact has been found and recorded in mStabbedFace  */      \
 136         mStabbedFace.mFaceID = prim_index;
 137
 138 #ifdef OPC_RAYHIT_CALLBACK
 139
 140         #define HANDLE_CONTACT(prim_index, flag)                                                                                                        \
 141                 SET_CONTACT(prim_index, flag)                                                                                                                   \
 142                                                                                                                                                                                                 \
 143                 if(mHitCallback)        (mHitCallback)(mStabbedFace, mUserData);
 144
 145         #define UPDATE_CACHE                                    \
 146                 if(cache && GetContactStatus())         \
 147                 {                                                                       \
 148                         *cache  = mStabbedFace.mFaceID; \
 149                 }
 150 #else
 151
 152         #define HANDLE_CONTACT(prim_index, flag)                                                                                                        \
 153                 SET_CONTACT(prim_index, flag)                                                                                                                   \
 154                                                                                                                                                                                                 \
 155                 /* Now we can also record it in mStabbedFaces if available */                                                   \
 156                 if(mStabbedFaces)                                                                                                                                               \
 157                 {                                                                                                                                                                               \
 158                         /* If we want all faces or if that's the first one we hit */                                            \
 159                         if(!mClosestHit || !mStabbedFaces->GetNbFaces())                                                                        \
 160                         {                                                                                                                                                                       \
 161                                 mStabbedFaces->AddFace(mStabbedFace);                                                                                   \
 162                         }                                                                                                                                                                       \
 163                         else                                                                                                                                                            \
 164                         {                                                                                                                                                                       \
 165                                 /* We only keep closest hit */                                                                                                  \
 166                                 CollisionFace* Current = const_cast<CollisionFace*>(mStabbedFaces->GetFaces()); \
 167                                 if(Current && mStabbedFace.mDistance<Current->mDistance)                                                \
 168                                 {                                                                                                                                                               \
 169                                         *Current = mStabbedFace;                                                                                                        \
 170                                 }                                                                                                                                                               \
 171                         }                                                                                                                                                                       \
 172                 }
 173
 174         #define UPDATE_CACHE                                                                                            \
 175                 if(cache && GetContactStatus() && mStabbedFaces)                                \
 176                 {                                                                                                                               \
 177                         const CollisionFace* Current = mStabbedFaces->GetFaces();       \
 178                         if(Current)     *cache  = Current->mFaceID;                                             \
 179                         else            *cache  = INVALID_ID;                                                   \
 180                 }
 181 #endif
 182
 183 #define SEGMENT_PRIM(prim_index, flag)                                                                                                          \
 184         /* Request vertices from the app */                                                                                                             \
 185         VertexPointers VP;      mIMesh->GetTriangle(VP, prim_index);                                                            \
 186                                                                                                                                                                                         \
 187         /* Perform ray-tri overlap test and return */                                                                                   \
 188         if(RayTriOverlap(*VP.Vertex[0], *VP.Vertex[1], *VP.Vertex[2]))                                                  \
 189         {                                                                                                                                                                               \
 190                 /* Intersection point is valid if dist < segment's length */                                            \
 191                 /* We know dist>0 so we can use integers */                                                                                     \
 192                 if(IR(mStabbedFace.mDistance)<IR(mMaxDist))                                                                                     \
 193                 {                                                                                                                                                                       \
 194                         HANDLE_CONTACT(prim_index, flag)                                                                                                \
 195                 }                                                                                                                                                                       \
 196         }
 197
 198 #define RAY_PRIM(prim_index, flag)                                                                                                                      \
 199         /* Request vertices from the app */                                                                                                             \
 200         VertexPointers VP;      mIMesh->GetTriangle(VP, prim_index);                                                            \
 201                                                                                                                                                                                         \
 202         /* Perform ray-tri overlap test and return */                                                                                   \
 203         if(RayTriOverlap(*VP.Vertex[0], *VP.Vertex[1], *VP.Vertex[2]))                                                  \
 204         {                                                                                                                                                                               \
 205                 HANDLE_CONTACT(prim_index, flag)                                                                                                        \
 206         }
 207
 208
 209 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 210 /**
 211  *      Constructor.
 212  */
 213 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 214 RayCollider::RayCollider() :
 215         mNbRayBVTests           (0),
 216         mNbRayPrimTests         (0),
 217         mNbIntersections        (0),
 218         mCulling                        (true),
 219 #ifdef OPC_RAYHIT_CALLBACK
 220         mHitCallback            (null),
 221         mUserData                       (0),
 222 #else
 223         mClosestHit                     (false),
 224         mStabbedFaces           (null),
 225 #endif
 226         mMaxDist                        (MAX_FLOAT)
 227 {
 228 }
 229
 230 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 231 /**
 232  *      Destructor.
 233  */
 234 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 235 RayCollider::~RayCollider()
 236 {
 237 }
 238
 239 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 240 /**
 241  *      Validates current settings. You should call this method after all the settings and callbacks have been defined.
 242  *      \return         null if everything is ok, else a string describing the problem
 243  */
 244 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 245 const char* RayCollider::ValidateSettings()
 246 {
 247         if(mMaxDist<0.0f)                                                                                       return "Higher distance bound must be positive!";
 248         if(TemporalCoherenceEnabled() && !FirstContactEnabled())        return "Temporal coherence only works with ""First contact"" mode!";
 249 #ifndef OPC_RAYHIT_CALLBACK
 250         if(mClosestHit && FirstContactEnabled())                                        return "Closest hit doesn't work with ""First contact"" mode!";
 251         if(TemporalCoherenceEnabled() && mClosestHit)                           return "Temporal coherence can't guarantee to report closest hit!";
 252 #endif
 253         if(SkipPrimitiveTests())                                                                        return "SkipPrimitiveTests not possible for RayCollider ! (not implemented)";
 254         return null;
 255 }
 256
 257 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 258 /**
 259  *      Generic stabbing query for generic OPCODE models. After the call, access the results:
 260  *      - with GetContactStatus()
 261  *      - in the user-provided destination array
 262  *
 263  *      \param          world_ray               [in] stabbing ray in world space
 264  *      \param          model                   [in] Opcode model to collide with
 265  *      \param          world                   [in] model's world matrix, or null
 266  *      \param          cache                   [in] a possibly cached face index, or null
 267  *      \return         true if success
 268  *      \warning        SCALE NOT SUPPORTED. The matrices must contain rotation & translation parts only.
 269  */
 270 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 271 bool RayCollider::Collide(const Ray& world_ray, const Model& model, const Matrix4x4* world, udword* cache)
 272 {
 273         // Checkings
 274         if(!Setup(&model))      return false;
 275
 276         // Init collision query
 277         if(InitQuery(world_ray, world, cache))  return true;
 278
 279         if(!model.HasLeafNodes())
 280         {
 281                 if(model.IsQuantized())
 282                 {
 283                         const AABBQuantizedNoLeafTree* Tree = (const AABBQuantizedNoLeafTree*)model.GetTree();
 284
 285                         // Setup dequantization coeffs
 286                         mCenterCoeff    = Tree->mCenterCoeff;
 287                         mExtentsCoeff   = Tree->mExtentsCoeff;
 288
 289                         // Perform stabbing query
 290                         if(IR(mMaxDist)!=IEEE_MAX_FLOAT)        _SegmentStab(Tree->GetNodes());
 291                         else                                                            _RayStab(Tree->GetNodes());
 292                 }
 293                 else
 294                 {
 295                         const AABBNoLeafTree* Tree = (const AABBNoLeafTree*)model.GetTree();
 296
 297                         // Perform stabbing query
 298                         if(IR(mMaxDist)!=IEEE_MAX_FLOAT)        _SegmentStab(Tree->GetNodes());
 299                         else                                                            _RayStab(Tree->GetNodes());
 300                 }
 301         }
 302         else
 303         {
 304                 if(model.IsQuantized())
 305                 {
 306                         const AABBQuantizedTree* Tree = (const AABBQuantizedTree*)model.GetTree();
 307
 308                         // Setup dequantization coeffs
 309                         mCenterCoeff    = Tree->mCenterCoeff;
 310                         mExtentsCoeff   = Tree->mExtentsCoeff;
 311
 312                         // Perform stabbing query
 313                         if(IR(mMaxDist)!=IEEE_MAX_FLOAT)        _SegmentStab(Tree->GetNodes());
 314                         else                                                            _RayStab(Tree->GetNodes());
 315                 }
 316                 else
 317                 {
 318                         const AABBCollisionTree* Tree = (const AABBCollisionTree*)model.GetTree();
 319
 320                         // Perform stabbing query
 321                         if(IR(mMaxDist)!=IEEE_MAX_FLOAT)        _SegmentStab(Tree->GetNodes());
 322                         else                                                            _RayStab(Tree->GetNodes());
 323                 }
 324         }
 325
 326         // Update cache if needed
 327         UPDATE_CACHE
 328         return true;
 329 }
 330
 331
 332 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 333 /**
 334  *      Initializes a stabbing query :
 335  *      - reset stats & contact status
 336  *      - compute ray in local space
 337  *      - check temporal coherence
 338  *
 339  *      \param          world_ray       [in] stabbing ray in world space
 340  *      \param          world           [in] object's world matrix, or null
 341  *      \param          face_id         [in] index of previously stabbed triangle
 342  *      \return         TRUE if we can return immediately
 343  *      \warning        SCALE NOT SUPPORTED. The matrix must contain rotation & translation parts only.
 344  */
 345 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 346 BOOL RayCollider::InitQuery(const Ray& world_ray, const Matrix4x4* world, udword* face_id)
 347 {
 348         // Reset stats & contact status
 349         Collider::InitQuery();
 350         mNbRayBVTests           = 0;
 351         mNbRayPrimTests         = 0;
 352         mNbIntersections        = 0;
 353 #ifndef OPC_RAYHIT_CALLBACK
 354         if(mStabbedFaces)       mStabbedFaces->Reset();
 355 #endif
 356
 357         // Compute ray in local space
 358         // The (Origin/Dir) form is needed for the ray-triangle test anyway (even for segment tests)
 359         if(world)
 360         {
 361                 Matrix3x3 InvWorld = *world;
 362                 mDir = InvWorld * world_ray.mDir;
 363
 364                 Matrix4x4 World;
 365                 InvertPRMatrix(World, *world);
 366                 mOrigin = world_ray.mOrig * World;
 367         }
 368         else
 369         {
 370                 mDir    = world_ray.mDir;
 371                 mOrigin = world_ray.mOrig;
 372         }
 373
 374         // 4) Special case: 1-triangle meshes [Opcode 1.3]
 375         if(mCurrentModel && mCurrentModel->HasSingleNode())
 376         {
 377                 // We simply perform the BV-Prim overlap test each time. We assume single triangle has index 0.
 378                 if(!SkipPrimitiveTests())
 379                 {
 380                         // Perform overlap test between the unique triangle and the ray (and set contact status if needed)
 381                         SEGMENT_PRIM(udword(0), OPC_CONTACT)
 382
 383                         // Return immediately regardless of status
 384                         return TRUE;
 385                 }
 386         }
 387
 388         // Check temporal coherence :
 389
 390         // Test previously colliding primitives first
 391         if(TemporalCoherenceEnabled() && FirstContactEnabled() && face_id && *face_id!=INVALID_ID)
 392         {
 393 #ifdef OLD_CODE
 394 #ifndef OPC_RAYHIT_CALLBACK
 395                 if(!mClosestHit)
 396 #endif
 397                 {
 398                         // Request vertices from the app
 399                         VertexPointers VP;
 400                         mIMesh->GetTriangle(VP, *face_id);
 401                         // Perform ray-cached tri overlap test
 402                         if(RayTriOverlap(*VP.Vertex[0], *VP.Vertex[1], *VP.Vertex[2]))
 403                         {
 404                                 // Intersection point is valid if:
 405                                 // - distance is positive (else it can just be a face behind the orig point)
 406                                 // - distance is smaller than a given max distance (useful for shadow feelers)
 407 //                              if(mStabbedFace.mDistance>0.0f && mStabbedFace.mDistance<mMaxDist)
 408                                 if(IR(mStabbedFace.mDistance)<IR(mMaxDist))     // The other test is already performed in RayTriOverlap
 409                                 {
 410                                         // Set contact status
 411                                         mFlags |= OPC_TEMPORAL_CONTACT;
 412
 413                                         mStabbedFace.mFaceID = *face_id;
 414
 415 #ifndef OPC_RAYHIT_CALLBACK
 416                                         if(mStabbedFaces)       mStabbedFaces->AddFace(mStabbedFace);
 417 #endif
 418                                         return TRUE;
 419                                 }
 420                         }
 421                 }
 422 #else
 423                 // New code
 424                 // We handle both Segment/ray queries with the same segment code, and a possible infinite limit
 425                 SEGMENT_PRIM(*face_id, OPC_TEMPORAL_CONTACT)
 426
 427                 // Return immediately if possible
 428                 if(GetContactStatus())  return TRUE;
 429 #endif
 430         }
 431
 432         // Precompute data (moved after temporal coherence since only needed for ray-AABB)
 433         if(IR(mMaxDist)!=IEEE_MAX_FLOAT)
 434         {
 435                 // For Segment-AABB overlap
 436                 mData = 0.5f * mDir * mMaxDist;
 437                 mData2 = mOrigin + mData;
 438
 439                 // Precompute mFDir;
 440                 mFDir.x = fabsf(mData.x);
 441                 mFDir.y = fabsf(mData.y);
 442                 mFDir.z = fabsf(mData.z);
 443         }
 444         else
 445         {
 446                 // For Ray-AABB overlap
 447 //              udword x = SIR(mDir.x)-1;
 448 //              udword y = SIR(mDir.y)-1;
 449 //              udword z = SIR(mDir.z)-1;
 450 //              mData.x = FR(x);
 451 //              mData.y = FR(y);
 452 //              mData.z = FR(z);
 453
 454                 // Precompute mFDir;
 455                 mFDir.x = fabsf(mDir.x);
 456                 mFDir.y = fabsf(mDir.y);
 457                 mFDir.z = fabsf(mDir.z);
 458         }
 459
 460         return FALSE;
 461 }
 462
 463 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 464 /**
 465  *      Stabbing query for vanilla AABB trees.
 466  *      \param          world_ray               [in] stabbing ray in world space
 467  *      \param          tree                    [in] AABB tree
 468  *      \param          box_indices             [out] indices of stabbed boxes
 469  *      \return         true if success
 470  */
 471 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 472 bool RayCollider::Collide(const Ray& world_ray, const AABBTree* tree, Container& box_indices)
 473 {
 474         // ### bad design here
 475
 476         // This is typically called for a scene tree, full of -AABBs-, not full of triangles.
 477         // So we don't really have "primitives" to deal with. Hence it doesn't work with
 478         // "FirstContact" + "TemporalCoherence".
 479         ASSERT( !(FirstContactEnabled() && TemporalCoherenceEnabled()) );
 480
 481         // Checkings
 482         if(!tree)                                       return false;
 483
 484         // Init collision query
 485         // Basically this is only called to initialize precomputed data
 486         if(InitQuery(world_ray))        return true;
 487
 488         // Perform stabbing query
 489         if(IR(mMaxDist)!=IEEE_MAX_FLOAT)        _SegmentStab(tree, box_indices);
 490         else                                                            _RayStab(tree, box_indices);
 491
 492         return true;
 493 }
 494
 495
 496 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 497 /**
 498  *      Recursive stabbing query for normal AABB trees.
 499  *      \param          node    [in] current collision node
 500  */
 501 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 502 void RayCollider::_SegmentStab(const AABBCollisionNode* node)
 503 {
 504         // Perform Segment-AABB overlap test
 505         if(!SegmentAABBOverlap(node->mAABB.mCenter, node->mAABB.mExtents))      return;
 506
 507         if(node->IsLeaf())
 508         {
 509                 SEGMENT_PRIM(node->GetPrimitive(), OPC_CONTACT)
 510         }
 511         else
 512         {
 513                 _SegmentStab(node->GetPos());
 514
 515                 if(ContactFound()) return;
 516
 517                 _SegmentStab(node->GetNeg());
 518         }
 519 }
 520
 521 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 522 /**
 523  *      Recursive stabbing query for quantized AABB trees.
 524  *      \param          node    [in] current collision node
 525  */
 526 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 527 void RayCollider::_SegmentStab(const AABBQuantizedNode* node)
 528 {
 529         // Dequantize box
 530         const QuantizedAABB& Box = node->mAABB;
 531         const Point Center(float(Box.mCenter[0]) * mCenterCoeff.x, float(Box.mCenter[1]) * mCenterCoeff.y, float(Box.mCenter[2]) * mCenterCoeff.z);
 532         const Point Extents(float(Box.mExtents[0]) * mExtentsCoeff.x, float(Box.mExtents[1]) * mExtentsCoeff.y, float(Box.mExtents[2]) * mExtentsCoeff.z);
 533
 534         // Perform Segment-AABB overlap test
 535         if(!SegmentAABBOverlap(Center, Extents))        return;
 536
 537         if(node->IsLeaf())
 538         {
 539                 SEGMENT_PRIM(node->GetPrimitive(), OPC_CONTACT)
 540         }
 541         else
 542         {
 543                 _SegmentStab(node->GetPos());
 544
 545                 if(ContactFound()) return;
 546
 547                 _SegmentStab(node->GetNeg());
 548         }
 549 }
 550
 551 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 552 /**
 553  *      Recursive stabbing query for no-leaf AABB trees.
 554  *      \param          node    [in] current collision node
 555  */
 556 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 557 void RayCollider::_SegmentStab(const AABBNoLeafNode* node)
 558 {
 559         // Perform Segment-AABB overlap test
 560         if(!SegmentAABBOverlap(node->mAABB.mCenter, node->mAABB.mExtents))      return;
 561
 562         if(node->HasPosLeaf())
 563         {
 564                 SEGMENT_PRIM(node->GetPosPrimitive(), OPC_CONTACT)
 565         }
 566         else _SegmentStab(node->GetPos());
 567
 568         if(ContactFound()) return;
 569
 570         if(node->HasNegLeaf())
 571         {
 572                 SEGMENT_PRIM(node->GetNegPrimitive(), OPC_CONTACT)
 573         }
 574         else _SegmentStab(node->GetNeg());
 575 }
 576
 577 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 578 /**
 579  *      Recursive stabbing query for quantized no-leaf AABB trees.
 580  *      \param          node    [in] current collision node
 581  */
 582 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 583 void RayCollider::_SegmentStab(const AABBQuantizedNoLeafNode* node)
 584 {
 585         // Dequantize box
 586         const QuantizedAABB& Box = node->mAABB;
 587         const Point Center(float(Box.mCenter[0]) * mCenterCoeff.x, float(Box.mCenter[1]) * mCenterCoeff.y, float(Box.mCenter[2]) * mCenterCoeff.z);
 588         const Point Extents(float(Box.mExtents[0]) * mExtentsCoeff.x, float(Box.mExtents[1]) * mExtentsCoeff.y, float(Box.mExtents[2]) * mExtentsCoeff.z);
 589
 590         // Perform Segment-AABB overlap test
 591         if(!SegmentAABBOverlap(Center, Extents))        return;
 592
 593         if(node->HasPosLeaf())
 594         {
 595                 SEGMENT_PRIM(node->GetPosPrimitive(), OPC_CONTACT)
 596         }
 597         else _SegmentStab(node->GetPos());
 598
 599         if(ContactFound()) return;
 600
 601         if(node->HasNegLeaf())
 602         {
 603                 SEGMENT_PRIM(node->GetNegPrimitive(), OPC_CONTACT)
 604         }
 605         else _SegmentStab(node->GetNeg());
 606 }
 607
 608 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 609 /**
 610  *      Recursive stabbing query for vanilla AABB trees.
 611  *      \param          node            [in] current collision node
 612  *      \param          box_indices     [out] indices of stabbed boxes
 613  */
 614 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 615 void RayCollider::_SegmentStab(const AABBTreeNode* node, Container& box_indices)
 616 {
 617         // Test the box against the segment
 618         Point Center, Extents;
 619         node->GetAABB()->GetCenter(Center);
 620         node->GetAABB()->GetExtents(Extents);
 621         if(!SegmentAABBOverlap(Center, Extents))        return;
 622
 623         if(node->IsLeaf())
 624         {
 625                 box_indices.Add(node->GetPrimitives(), node->GetNbPrimitives());
 626         }
 627         else
 628         {
 629                 _SegmentStab(node->GetPos(), box_indices);
 630                 _SegmentStab(node->GetNeg(), box_indices);
 631         }
 632 }
 633
 634 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 635 /**
 636  *      Recursive stabbing query for normal AABB trees.
 637  *      \param          node    [in] current collision node
 638  */
 639 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 640 void RayCollider::_RayStab(const AABBCollisionNode* node)
 641 {
 642         // Perform Ray-AABB overlap test
 643         if(!RayAABBOverlap(node->mAABB.mCenter, node->mAABB.mExtents))  return;
 644
 645         if(node->IsLeaf())
 646         {
 647                 RAY_PRIM(node->GetPrimitive(), OPC_CONTACT)
 648         }
 649         else
 650         {
 651                 _RayStab(node->GetPos());
 652
 653                 if(ContactFound()) return;
 654
 655                 _RayStab(node->GetNeg());
 656         }
 657 }
 658
 659 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 660 /**
 661  *      Recursive stabbing query for quantized AABB trees.
 662  *      \param          node    [in] current collision node
 663  */
 664 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 665 void RayCollider::_RayStab(const AABBQuantizedNode* node)
 666 {
 667         // Dequantize box
 668         const QuantizedAABB& Box = node->mAABB;
 669         const Point Center(float(Box.mCenter[0]) * mCenterCoeff.x, float(Box.mCenter[1]) * mCenterCoeff.y, float(Box.mCenter[2]) * mCenterCoeff.z);
 670         const Point Extents(float(Box.mExtents[0]) * mExtentsCoeff.x, float(Box.mExtents[1]) * mExtentsCoeff.y, float(Box.mExtents[2]) * mExtentsCoeff.z);
 671
 672         // Perform Ray-AABB overlap test
 673         if(!RayAABBOverlap(Center, Extents))    return;
 674
 675         if(node->IsLeaf())
 676         {
 677                 RAY_PRIM(node->GetPrimitive(), OPC_CONTACT)
 678         }
 679         else
 680         {
 681                 _RayStab(node->GetPos());
 682
 683                 if(ContactFound()) return;
 684
 685                 _RayStab(node->GetNeg());
 686         }
 687 }
 688
 689 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 690 /**
 691  *      Recursive stabbing query for no-leaf AABB trees.
 692  *      \param          node    [in] current collision node
 693  */
 694 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 695 void RayCollider::_RayStab(const AABBNoLeafNode* node)
 696 {
 697         // Perform Ray-AABB overlap test
 698         if(!RayAABBOverlap(node->mAABB.mCenter, node->mAABB.mExtents))  return;
 699
 700         if(node->HasPosLeaf())
 701         {
 702                 RAY_PRIM(node->GetPosPrimitive(), OPC_CONTACT)
 703         }
 704         else _RayStab(node->GetPos());
 705
 706         if(ContactFound()) return;
 707
 708         if(node->HasNegLeaf())
 709         {
 710                 RAY_PRIM(node->GetNegPrimitive(), OPC_CONTACT)
 711         }
 712         else _RayStab(node->GetNeg());
 713 }
 714
 715 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 716 /**
 717  *      Recursive stabbing query for quantized no-leaf AABB trees.
 718  *      \param          node    [in] current collision node
 719  */
 720 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 721 void RayCollider::_RayStab(const AABBQuantizedNoLeafNode* node)
 722 {
 723         // Dequantize box
 724         const QuantizedAABB& Box = node->mAABB;
 725         const Point Center(float(Box.mCenter[0]) * mCenterCoeff.x, float(Box.mCenter[1]) * mCenterCoeff.y, float(Box.mCenter[2]) * mCenterCoeff.z);
 726         const Point Extents(float(Box.mExtents[0]) * mExtentsCoeff.x, float(Box.mExtents[1]) * mExtentsCoeff.y, float(Box.mExtents[2]) * mExtentsCoeff.z);
 727
 728         // Perform Ray-AABB overlap test
 729         if(!RayAABBOverlap(Center, Extents))    return;
 730
 731         if(node->HasPosLeaf())
 732         {
 733                 RAY_PRIM(node->GetPosPrimitive(), OPC_CONTACT)
 734         }
 735         else _RayStab(node->GetPos());
 736
 737         if(ContactFound()) return;
 738
 739         if(node->HasNegLeaf())
 740         {
 741                 RAY_PRIM(node->GetNegPrimitive(), OPC_CONTACT)
 742         }
 743         else _RayStab(node->GetNeg());
 744 }
 745
 746 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 747 /**
 748  *      Recursive stabbing query for vanilla AABB trees.
 749  *      \param          node            [in] current collision node
 750  *      \param          box_indices     [out] indices of stabbed boxes
 751  */
 752 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 753 void RayCollider::_RayStab(const AABBTreeNode* node, Container& box_indices)
 754 {
 755         // Test the box against the ray
 756         Point Center, Extents;
 757         node->GetAABB()->GetCenter(Center);
 758         node->GetAABB()->GetExtents(Extents);
 759         if(!RayAABBOverlap(Center, Extents))    return;
 760
 761         if(node->IsLeaf())
 762         {
 763                 mFlags |= OPC_CONTACT;
 764                 box_indices.Add(node->GetPrimitives(), node->GetNbPrimitives());
 765         }
 766         else
 767         {
 768                 _RayStab(node->GetPos(), box_indices);
 769                 _RayStab(node->GetNeg(), box_indices);
 770         }
 771 }