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