OPCODE/OPC_OBBCollider.cpp

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