00001
00002
00003
00004
00005
00006
00008
00010
00016
00017
00019
00028
00029
00030 #include"../CollisionPairInserter.h"
00031
00033
00034 #include "Stdafx.h"
00035
00036 #include<iostream>
00037
00038 using namespace Opcode;
00039
00040 #include "OPC_BoxBoxOverlap.h"
00041
00042 #include "OPC_TriTriOverlap.h"
00043
00045
00048
00049 AABBTreeCollider::AABBTreeCollider() :
00050 mIMesh0 (null),
00051 mIMesh1 (null),
00052 mNbBVBVTests (0),
00053 mNbPrimPrimTests (0),
00054 mNbBVPrimTests (0),
00055 mFullBoxBoxTest (true),
00056 mFullPrimBoxTest (true),
00057 collisionPairInserter(0)
00058 {
00059 }
00060
00061
00063
00066
00067 AABBTreeCollider::~AABBTreeCollider()
00068 {
00069 }
00070
00072
00076
00077 const char* AABBTreeCollider::ValidateSettings()
00078 {
00079 if(TemporalCoherenceEnabled() && !FirstContactEnabled()) return "Temporal coherence only works with ""First contact"" mode!";
00080 return null;
00081 }
00082
00084
00096
00097 bool AABBTreeCollider::Collide(BVTCache& cache, const Matrix4x4* world0, const Matrix4x4* world1)
00098 {
00099
00100 if(!cache.Model0 || !cache.Model1) return false;
00101 if(cache.Model0->HasLeafNodes()!=cache.Model1->HasLeafNodes()) return false;
00102 if(cache.Model0->IsQuantized()!=cache.Model1->IsQuantized()) return false;
00103
00104
00105
00106
00107
00108
00109
00110
00111
00112
00113
00114 #ifdef __MESHMERIZER_H__
00115
00116 if(cache.HullTest)
00117 {
00118 if(cache.Model0->GetHull() && cache.Model1->GetHull())
00119 {
00120 struct Local
00121 {
00122 static Point* SVCallback(const Point& sv, udword& previndex, udword user_data)
00123 {
00124 CollisionHull* Hull = (CollisionHull*)user_data;
00125 previndex = Hull->ComputeSupportingVertex(sv, previndex);
00126 return (Point*)&Hull->GetVerts()[previndex];
00127 }
00128 };
00129
00130 bool Collide;
00131
00132 if(0)
00133 {
00134 static GJKEngine GJK;
00135 static bool GJKInitDone=false;
00136 if(!GJKInitDone)
00137 {
00138 GJK.Enable(GJK_BACKUP_PROCEDURE);
00139 GJK.Enable(GJK_DEGENERATE);
00140 GJK.Enable(GJK_HILLCLIMBING);
00141 GJKInitDone = true;
00142 }
00143 GJK.SetCallbackObj0(Local::SVCallback);
00144 GJK.SetCallbackObj1(Local::SVCallback);
00145 GJK.SetUserData0(udword(cache.Model0->GetHull()));
00146 GJK.SetUserData1(udword(cache.Model1->GetHull()));
00147 Collide = GJK.Collide(*world0, *world1, &cache.SepVector);
00148 }
00149 else
00150 {
00151 static SVEngine SVE;
00152 SVE.SetCallbackObj0(Local::SVCallback);
00153 SVE.SetCallbackObj1(Local::SVCallback);
00154 SVE.SetUserData0(udword(cache.Model0->GetHull()));
00155 SVE.SetUserData1(udword(cache.Model1->GetHull()));
00156 Collide = SVE.Collide(*world0, *world1, &cache.SepVector);
00157 }
00158
00159 if(!Collide)
00160 {
00161
00162 mFlags &= ~OPC_CONTACT;
00163 mNbBVBVTests = 0;
00164 mNbPrimPrimTests = 0;
00165 mNbBVPrimTests = 0;
00166 mPairs.Reset();
00167 return true;
00168 }
00169 }
00170 }
00171
00172
00173 cache.HullTest = false;
00174 #endif // __MESHMERIZER_H__
00175
00176
00177 if(!Setup(cache.Model0->GetMeshInterface(), cache.Model1->GetMeshInterface())) return false;
00178
00179
00180 bool Status;
00181 if(!cache.Model0->HasLeafNodes())
00182 {
00183 if(cache.Model0->IsQuantized())
00184 {
00185 const AABBQuantizedNoLeafTree* T0 = (const AABBQuantizedNoLeafTree*)cache.Model0->GetTree();
00186 const AABBQuantizedNoLeafTree* T1 = (const AABBQuantizedNoLeafTree*)cache.Model1->GetTree();
00187 Status = Collide(T0, T1, world0, world1, &cache);
00188 }
00189 else
00190 {
00191 const AABBNoLeafTree* T0 = (const AABBNoLeafTree*)cache.Model0->GetTree();
00192 const AABBNoLeafTree* T1 = (const AABBNoLeafTree*)cache.Model1->GetTree();
00193 Status = Collide(T0, T1, world0, world1, &cache);
00194 }
00195 }
00196 else
00197 {
00198 if(cache.Model0->IsQuantized())
00199 {
00200 const AABBQuantizedTree* T0 = (const AABBQuantizedTree*)cache.Model0->GetTree();
00201 const AABBQuantizedTree* T1 = (const AABBQuantizedTree*)cache.Model1->GetTree();
00202 Status = Collide(T0, T1, world0, world1, &cache);
00203 }
00204 else
00205 {
00206 const AABBCollisionTree* T0 = (const AABBCollisionTree*)cache.Model0->GetTree();
00207 const AABBCollisionTree* T1 = (const AABBCollisionTree*)cache.Model1->GetTree();
00208 Status = Collide(T0, T1, world0, world1, &cache);
00209 }
00210 }
00211
00212 #ifdef __MESHMERIZER_H__
00213 if(Status)
00214 {
00215
00216 if(GetContactStatus()) cache.ResetCountDown();
00217
00218
00219 cache.CountDown--;
00220 if(!cache.CountDown)
00221 {
00222 cache.ResetCountDown();
00223 cache.HullTest = true;
00224 }
00225 }
00226 #endif
00227 return Status;
00228 }
00229
00231
00240
00241 void AABBTreeCollider::InitQuery(const Matrix4x4* world0, const Matrix4x4* world1)
00242 {
00243
00244 Collider::InitQuery();
00245 mNbBVBVTests = 0;
00246 mNbPrimPrimTests = 0;
00247 mNbBVPrimTests = 0;
00248 mPairs.Reset();
00249
00250
00251 Matrix4x4 InvWorld0, InvWorld1;
00252 if(world0) InvertPRMatrix(InvWorld0, *world0);
00253 else InvWorld0.Identity();
00254
00255 if(world1) InvertPRMatrix(InvWorld1, *world1);
00256 else InvWorld1.Identity();
00257
00258 Matrix4x4 World0to1 = world0 ? (*world0 * InvWorld1) : InvWorld1;
00259 Matrix4x4 World1to0 = world1 ? (*world1 * InvWorld0) : InvWorld0;
00260
00261 mR0to1 = World0to1; World0to1.GetTrans(mT0to1);
00262 mR1to0 = World1to0; World1to0.GetTrans(mT1to0);
00263
00264
00265 for(udword i=0;i<3;i++)
00266 {
00267 for(udword j=0;j<3;j++)
00268 {
00269
00270 mAR.m[i][j] = 1e-6f + fabsf(mR1to0.m[i][j]);
00271 }
00272 }
00273
00274
00275
00276
00277
00278 if(collisionPairInserter){
00279
00280 for(udword i=0; i<3; i++){
00281 for(udword j=0; j<3; j++){
00282 collisionPairInserter->CD_Rot1(i,j) = (double) world0->m[j][i];
00283 collisionPairInserter->CD_Rot2(i,j) = (double) world1->m[j][i];
00284 }
00285 }
00286
00287 IceMaths::Point t0;
00288 IceMaths::Point t1;
00289 world0->GetTrans(t0);
00290 world1->GetTrans(t1);
00291
00292
00293 collisionPairInserter->CD_Trans1[0] = (double) t0.x;
00294 collisionPairInserter->CD_Trans1[1] = (double) t0.y;
00295 collisionPairInserter->CD_Trans1[2] = (double) t0.z;
00296
00297 collisionPairInserter->CD_Trans2[0] = (double) t1.x;
00298 collisionPairInserter->CD_Trans2[1] = (double) t1.y;
00299 collisionPairInserter->CD_Trans2[2] = (double) t1.z;
00300
00301
00302 collisionPairInserter->CD_s1 = 1.0;
00303 collisionPairInserter->CD_s2 = 1.0;
00304 }
00305 }
00306
00307
00309
00315
00316 bool AABBTreeCollider::CheckTemporalCoherence(Pair* cache)
00317 {
00318
00319 if(!cache) return false;
00320
00321
00322 if(TemporalCoherenceEnabled() && FirstContactEnabled())
00323 {
00324 PrimTest(cache->id0, cache->id1);
00325 if(GetContactStatus()) return true;
00326 }
00327 return false;
00328 }
00329
00330 #define UPDATE_CACHE \
00331 if(cache && GetContactStatus()) \
00332 { \
00333 cache->id0 = mPairs.GetEntry(0); \
00334 cache->id1 = mPairs.GetEntry(1); \
00335 }
00336
00338
00348
00349 bool AABBTreeCollider::Collide(const AABBCollisionTree* tree0, const AABBCollisionTree* tree1, const Matrix4x4* world0, const Matrix4x4* world1, Pair* cache)
00350 {
00351
00352 InitQuery(world0, world1);
00353
00354
00355 if(CheckTemporalCoherence(cache)) return true;
00356
00357
00358 _Collide(tree0->GetNodes(), tree1->GetNodes());
00359
00360 UPDATE_CACHE
00361
00362 return true;
00363 }
00364
00366
00376
00377 bool AABBTreeCollider::Collide(const AABBNoLeafTree* tree0, const AABBNoLeafTree* tree1, const Matrix4x4* world0, const Matrix4x4* world1, Pair* cache)
00378 {
00379
00380 InitQuery(world0, world1);
00381
00382
00383 if(CheckTemporalCoherence(cache)) return true;
00384
00385
00386 _Collide(tree0->GetNodes(), tree1->GetNodes());
00387
00388 UPDATE_CACHE
00389
00390 return true;
00391 }
00392
00394
00404
00405 bool AABBTreeCollider::Collide(const AABBQuantizedTree* tree0, const AABBQuantizedTree* tree1, const Matrix4x4* world0, const Matrix4x4* world1, Pair* cache)
00406 {
00407
00408 InitQuery(world0, world1);
00409
00410
00411 if(CheckTemporalCoherence(cache)) return true;
00412
00413
00414 mCenterCoeff0 = tree0->mCenterCoeff;
00415 mExtentsCoeff0 = tree0->mExtentsCoeff;
00416 mCenterCoeff1 = tree1->mCenterCoeff;
00417 mExtentsCoeff1 = tree1->mExtentsCoeff;
00418
00419
00420 const AABBQuantizedNode* N0 = tree0->GetNodes();
00421 const Point a(float(N0->mAABB.mExtents[0]) * mExtentsCoeff0.x, float(N0->mAABB.mExtents[1]) * mExtentsCoeff0.y, float(N0->mAABB.mExtents[2]) * mExtentsCoeff0.z);
00422 const Point Pa(float(N0->mAABB.mCenter[0]) * mCenterCoeff0.x, float(N0->mAABB.mCenter[1]) * mCenterCoeff0.y, float(N0->mAABB.mCenter[2]) * mCenterCoeff0.z);
00423
00424 const AABBQuantizedNode* N1 = tree1->GetNodes();
00425 const Point b(float(N1->mAABB.mExtents[0]) * mExtentsCoeff1.x, float(N1->mAABB.mExtents[1]) * mExtentsCoeff1.y, float(N1->mAABB.mExtents[2]) * mExtentsCoeff1.z);
00426 const Point Pb(float(N1->mAABB.mCenter[0]) * mCenterCoeff1.x, float(N1->mAABB.mCenter[1]) * mCenterCoeff1.y, float(N1->mAABB.mCenter[2]) * mCenterCoeff1.z);
00427
00428
00429 _Collide(N0, N1, a, Pa, b, Pb);
00430
00431 UPDATE_CACHE
00432
00433 return true;
00434 }
00435
00437
00447
00448 bool AABBTreeCollider::Collide(const AABBQuantizedNoLeafTree* tree0, const AABBQuantizedNoLeafTree* tree1, const Matrix4x4* world0, const Matrix4x4* world1, Pair* cache)
00449 {
00450
00451 InitQuery(world0, world1);
00452
00453
00454 if(CheckTemporalCoherence(cache)) return true;
00455
00456
00457 mCenterCoeff0 = tree0->mCenterCoeff;
00458 mExtentsCoeff0 = tree0->mExtentsCoeff;
00459 mCenterCoeff1 = tree1->mCenterCoeff;
00460 mExtentsCoeff1 = tree1->mExtentsCoeff;
00461
00462
00463 _Collide(tree0->GetNodes(), tree1->GetNodes());
00464
00465 UPDATE_CACHE
00466
00467 return true;
00468 }
00469
00471
00473
00474
00475
00476 #define ALTERNATIVE_CODE //!< Alternative descent rules
00477
00478 #ifdef ORIGINAL_CODE
00479
00480
00485
00486 void AABBTreeCollider::_Collide(const AABBCollisionNode* b0, const AABBCollisionNode* b1)
00487 {
00488 mNowNode0 = b0;
00489 mNowNode1 = b1;
00490
00491 if(!BoxBoxOverlap(b0->mAABB.mExtents, b0->mAABB.mCenter, b1->mAABB.mExtents, b1->mAABB.mCenter)) return;
00492
00493 if(b0->IsLeaf() && b1->IsLeaf()) { PrimTest(b0->GetPrimitive(), b1->GetPrimitive()); return; }
00494
00495 if(b1->IsLeaf() || (!b0->IsLeaf() && (b0->GetSize() > b1->GetSize())))
00496 {
00497 _Collide(b0->GetNeg(), b1);
00498 if(ContactFound()) return;
00499 _Collide(b0->GetPos(), b1);
00500 }
00501 else
00502 {
00503 _Collide(b0, b1->GetNeg());
00504 if(ContactFound()) return;
00505 _Collide(b0, b1->GetPos());
00506 }
00507 }
00508 #endif
00509
00510 #ifdef ALTERNATIVE_CODE
00511
00512
00517
00518 void AABBTreeCollider::_Collide(const AABBCollisionNode* b0, const AABBCollisionNode* b1)
00519 {
00520
00521 if(!BoxBoxOverlap(b0->mAABB.mExtents, b0->mAABB.mCenter, b1->mAABB.mExtents, b1->mAABB.mCenter))
00522 {
00523 return;
00524 }
00525
00526 if(b0->IsLeaf())
00527 {
00528 if(b1->IsLeaf())
00529 {
00530 mNowNode0 = b0;
00531 mNowNode1 = b1;
00532 PrimTest(b0->GetPrimitive(), b1->GetPrimitive());
00533 }
00534 else
00535 {
00536 _Collide(b0, b1->GetNeg());
00537 if(ContactFound()) return;
00538 _Collide(b0, b1->GetPos());
00539 }
00540 }
00541 else if(b1->IsLeaf())
00542 {
00543 _Collide(b0->GetNeg(), b1);
00544 if(ContactFound()) return;
00545 _Collide(b0->GetPos(), b1);
00546 }
00547 else
00548 {
00549 _Collide(b0->GetNeg(), b1->GetNeg());
00550 if(ContactFound()) return;
00551 _Collide(b0->GetNeg(), b1->GetPos());
00552 if(ContactFound()) return;
00553 _Collide(b0->GetPos(), b1->GetNeg());
00554 if(ContactFound()) return;
00555 _Collide(b0->GetPos(), b1->GetPos());
00556 }
00557 }
00558 #endif
00559
00561
00563
00565
00570
00571 void AABBTreeCollider::PrimTest(udword id0, udword id1)
00572 {
00573 mId0 = id0;
00574 mId1 = id1;
00575
00576 VertexPointers VP0;
00577 VertexPointers VP1;
00578 mIMesh0->GetTriangle(VP0, id0);
00579 mIMesh1->GetTriangle(VP1, id1);
00580
00581
00582
00583
00584 Point u0,u1,u2;
00585 TransformPoint(u0, *VP0.Vertex[0], mR0to1, mT0to1);
00586 TransformPoint(u1, *VP0.Vertex[1], mR0to1, mT0to1);
00587 TransformPoint(u2, *VP0.Vertex[2], mR0to1, mT0to1);
00588
00589
00590 if(TriTriOverlap(u0, u1, u2,
00591 *VP1.Vertex[0], *VP1.Vertex[1], *VP1.Vertex[2]))
00592 {
00593
00594 mPairs.Add(id0).Add(id1);
00595
00596 mFlags |= OPC_CONTACT;
00597 }
00598 }
00599
00601
00605
00606 inline_ void AABBTreeCollider::PrimTestTriIndex(udword id1)
00607 {
00608
00609 VertexPointers VP;
00610 mIMesh1->GetTriangle(VP, id1);
00611
00612
00613 if(TriTriOverlap(mLeafVerts[0], mLeafVerts[1], mLeafVerts[2], *VP.Vertex[0], *VP.Vertex[1], *VP.Vertex[2]))
00614 {
00615
00616 mPairs.Add(mLeafIndex).Add(id1);
00617
00618 mFlags |= OPC_CONTACT;
00619 }
00620 }
00621
00623
00627
00628 inline_ void AABBTreeCollider::PrimTestIndexTri(udword id0)
00629 {
00630
00631 VertexPointers VP;
00632 mIMesh0->GetTriangle(VP, id0);
00633
00634
00635 if(TriTriOverlap(mLeafVerts[0], mLeafVerts[1], mLeafVerts[2], *VP.Vertex[0], *VP.Vertex[1], *VP.Vertex[2]))
00636 {
00637
00638 mPairs.Add(id0).Add(mLeafIndex);
00639
00640 mFlags |= OPC_CONTACT;
00641 }
00642 }
00643
00645
00649
00650 void AABBTreeCollider::_CollideTriBox(const AABBNoLeafNode* b)
00651 {
00652
00653
00654
00655
00656
00657
00658 if(b->HasPosLeaf()) PrimTestTriIndex(b->GetPosPrimitive());
00659 else _CollideTriBox(b->GetPos());
00660
00661 if(ContactFound()) return;
00662
00663
00664 if(b->HasNegLeaf()) PrimTestTriIndex(b->GetNegPrimitive());
00665 else _CollideTriBox(b->GetNeg());
00666 }
00667
00669
00673
00674 void AABBTreeCollider::_CollideBoxTri(const AABBNoLeafNode* b)
00675 {
00676
00677
00678
00679
00680
00681
00682 if(b->HasPosLeaf()) PrimTestIndexTri(b->GetPosPrimitive());
00683 else _CollideBoxTri(b->GetPos());
00684
00685 if(ContactFound()) return;
00686
00687
00688 if(b->HasNegLeaf()) PrimTestIndexTri(b->GetNegPrimitive());
00689 else _CollideBoxTri(b->GetNeg());
00690 }
00691
00693 #define FETCH_LEAF(prim_index, imesh, rot, trans) \
00694 mLeafIndex = prim_index; \
00695 \
00696 VertexPointers VP; imesh->GetTriangle(VP, prim_index); \
00697 \
00698 TransformPoint(mLeafVerts[0], *VP.Vertex[0], rot, trans); \
00699 TransformPoint(mLeafVerts[1], *VP.Vertex[1], rot, trans); \
00700 TransformPoint(mLeafVerts[2], *VP.Vertex[2], rot, trans);
00701
00703
00708
00709 void AABBTreeCollider::_Collide(const AABBNoLeafNode* a, const AABBNoLeafNode* b)
00710 {
00711
00712 if(!BoxBoxOverlap(a->mAABB.mExtents, a->mAABB.mCenter, b->mAABB.mExtents, b->mAABB.mCenter)) return;
00713
00714
00715 BOOL BHasPosLeaf = b->HasPosLeaf();
00716 BOOL BHasNegLeaf = b->HasNegLeaf();
00717
00718 if(a->HasPosLeaf())
00719 {
00720 FETCH_LEAF(a->GetPosPrimitive(), mIMesh0, mR0to1, mT0to1)
00721
00722 if(BHasPosLeaf) PrimTestTriIndex(b->GetPosPrimitive());
00723 else _CollideTriBox(b->GetPos());
00724
00725 if(ContactFound()) return;
00726
00727 if(BHasNegLeaf) PrimTestTriIndex(b->GetNegPrimitive());
00728 else _CollideTriBox(b->GetNeg());
00729 }
00730 else
00731 {
00732 if(BHasPosLeaf)
00733 {
00734 FETCH_LEAF(b->GetPosPrimitive(), mIMesh1, mR1to0, mT1to0)
00735
00736 _CollideBoxTri(a->GetPos());
00737 }
00738 else _Collide(a->GetPos(), b->GetPos());
00739
00740 if(ContactFound()) return;
00741
00742 if(BHasNegLeaf)
00743 {
00744 FETCH_LEAF(b->GetNegPrimitive(), mIMesh1, mR1to0, mT1to0)
00745
00746 _CollideBoxTri(a->GetPos());
00747 }
00748 else _Collide(a->GetPos(), b->GetNeg());
00749 }
00750
00751 if(ContactFound()) return;
00752
00753 if(a->HasNegLeaf())
00754 {
00755 FETCH_LEAF(a->GetNegPrimitive(), mIMesh0, mR0to1, mT0to1)
00756
00757 if(BHasPosLeaf) PrimTestTriIndex(b->GetPosPrimitive());
00758 else _CollideTriBox(b->GetPos());
00759
00760 if(ContactFound()) return;
00761
00762 if(BHasNegLeaf) PrimTestTriIndex(b->GetNegPrimitive());
00763 else _CollideTriBox(b->GetNeg());
00764 }
00765 else
00766 {
00767 if(BHasPosLeaf)
00768 {
00769
00770 FETCH_LEAF(b->GetPosPrimitive(), mIMesh1, mR1to0, mT1to0)
00771
00772 _CollideBoxTri(a->GetNeg());
00773 }
00774 else _Collide(a->GetNeg(), b->GetPos());
00775
00776 if(ContactFound()) return;
00777
00778 if(BHasNegLeaf)
00779 {
00780
00781 FETCH_LEAF(b->GetNegPrimitive(), mIMesh1, mR1to0, mT1to0)
00782
00783 _CollideBoxTri(a->GetNeg());
00784 }
00785 else _Collide(a->GetNeg(), b->GetNeg());
00786 }
00787 }
00788
00790
00792
00794
00803
00804 void AABBTreeCollider::_Collide(const AABBQuantizedNode* b0, const AABBQuantizedNode* b1, const Point& a, const Point& Pa, const Point& b, const Point& Pb)
00805 {
00806
00807 if(!BoxBoxOverlap(a, Pa, b, Pb)) return;
00808
00809 if(b0->IsLeaf() && b1->IsLeaf()) { PrimTest(b0->GetPrimitive(), b1->GetPrimitive()); return; }
00810
00811 if(b1->IsLeaf() || (!b0->IsLeaf() && (b0->GetSize() > b1->GetSize())))
00812 {
00813
00814 const QuantizedAABB* Box = &b0->GetNeg()->mAABB;
00815 const Point negPa(float(Box->mCenter[0]) * mCenterCoeff0.x, float(Box->mCenter[1]) * mCenterCoeff0.y, float(Box->mCenter[2]) * mCenterCoeff0.z);
00816 const Point nega(float(Box->mExtents[0]) * mExtentsCoeff0.x, float(Box->mExtents[1]) * mExtentsCoeff0.y, float(Box->mExtents[2]) * mExtentsCoeff0.z);
00817 _Collide(b0->GetNeg(), b1, nega, negPa, b, Pb);
00818
00819 if(ContactFound()) return;
00820
00821
00822 Box = &b0->GetPos()->mAABB;
00823 const Point posPa(float(Box->mCenter[0]) * mCenterCoeff0.x, float(Box->mCenter[1]) * mCenterCoeff0.y, float(Box->mCenter[2]) * mCenterCoeff0.z);
00824 const Point posa(float(Box->mExtents[0]) * mExtentsCoeff0.x, float(Box->mExtents[1]) * mExtentsCoeff0.y, float(Box->mExtents[2]) * mExtentsCoeff0.z);
00825 _Collide(b0->GetPos(), b1, posa, posPa, b, Pb);
00826 }
00827 else
00828 {
00829
00830 const QuantizedAABB* Box = &b1->GetNeg()->mAABB;
00831 const Point negPb(float(Box->mCenter[0]) * mCenterCoeff1.x, float(Box->mCenter[1]) * mCenterCoeff1.y, float(Box->mCenter[2]) * mCenterCoeff1.z);
00832 const Point negb(float(Box->mExtents[0]) * mExtentsCoeff1.x, float(Box->mExtents[1]) * mExtentsCoeff1.y, float(Box->mExtents[2]) * mExtentsCoeff1.z);
00833 _Collide(b0, b1->GetNeg(), a, Pa, negb, negPb);
00834
00835 if(ContactFound()) return;
00836
00837
00838 Box = &b1->GetPos()->mAABB;
00839 const Point posPb(float(Box->mCenter[0]) * mCenterCoeff1.x, float(Box->mCenter[1]) * mCenterCoeff1.y, float(Box->mCenter[2]) * mCenterCoeff1.z);
00840 const Point posb(float(Box->mExtents[0]) * mExtentsCoeff1.x, float(Box->mExtents[1]) * mExtentsCoeff1.y, float(Box->mExtents[2]) * mExtentsCoeff1.z);
00841 _Collide(b0, b1->GetPos(), a, Pa, posb, posPb);
00842 }
00843 }
00844
00846
00848
00850
00855
00856 void AABBTreeCollider::_CollideTriBox(const AABBQuantizedNoLeafNode* b)
00857 {
00858
00859
00860 const QuantizedAABB* bb = &b->mAABB;
00861 const Point Pb(float(bb->mCenter[0]) * mCenterCoeff1.x, float(bb->mCenter[1]) * mCenterCoeff1.y, float(bb->mCenter[2]) * mCenterCoeff1.z);
00862 const Point eb(float(bb->mExtents[0]) * mExtentsCoeff1.x, float(bb->mExtents[1]) * mExtentsCoeff1.y, float(bb->mExtents[2]) * mExtentsCoeff1.z);
00863
00864
00865
00866
00867
00868
00869 if(b->HasPosLeaf()) PrimTestTriIndex(b->GetPosPrimitive());
00870 else _CollideTriBox(b->GetPos());
00871
00872 if(ContactFound()) return;
00873
00874 if(b->HasNegLeaf()) PrimTestTriIndex(b->GetNegPrimitive());
00875 else _CollideTriBox(b->GetNeg());
00876 }
00877
00879
00884
00885 void AABBTreeCollider::_CollideBoxTri(const AABBQuantizedNoLeafNode* b)
00886 {
00887
00888 const QuantizedAABB* bb = &b->mAABB;
00889 const Point Pa(float(bb->mCenter[0]) * mCenterCoeff0.x, float(bb->mCenter[1]) * mCenterCoeff0.y, float(bb->mCenter[2]) * mCenterCoeff0.z);
00890 const Point ea(float(bb->mExtents[0]) * mExtentsCoeff0.x, float(bb->mExtents[1]) * mExtentsCoeff0.y, float(bb->mExtents[2]) * mExtentsCoeff0.z);
00891
00892
00893
00894
00895
00896
00897
00898 if(b->HasPosLeaf()) PrimTestIndexTri(b->GetPosPrimitive());
00899 else _CollideBoxTri(b->GetPos());
00900
00901 if(ContactFound()) return;
00902
00903 if(b->HasNegLeaf()) PrimTestIndexTri(b->GetNegPrimitive());
00904 else _CollideBoxTri(b->GetNeg());
00905 }
00906
00908
00913
00914 void AABBTreeCollider::_Collide(const AABBQuantizedNoLeafNode* a, const AABBQuantizedNoLeafNode* b)
00915 {
00916
00917 const QuantizedAABB* ab = &a->mAABB;
00918 const Point Pa(float(ab->mCenter[0]) * mCenterCoeff0.x, float(ab->mCenter[1]) * mCenterCoeff0.y, float(ab->mCenter[2]) * mCenterCoeff0.z);
00919 const Point ea(float(ab->mExtents[0]) * mExtentsCoeff0.x, float(ab->mExtents[1]) * mExtentsCoeff0.y, float(ab->mExtents[2]) * mExtentsCoeff0.z);
00920
00921 const QuantizedAABB* bb = &b->mAABB;
00922 const Point Pb(float(bb->mCenter[0]) * mCenterCoeff1.x, float(bb->mCenter[1]) * mCenterCoeff1.y, float(bb->mCenter[2]) * mCenterCoeff1.z);
00923 const Point eb(float(bb->mExtents[0]) * mExtentsCoeff1.x, float(bb->mExtents[1]) * mExtentsCoeff1.y, float(bb->mExtents[2]) * mExtentsCoeff1.z);
00924
00925
00926 if(!BoxBoxOverlap(ea, Pa, eb, Pb)) return;
00927
00928
00929 BOOL BHasPosLeaf = b->HasPosLeaf();
00930 BOOL BHasNegLeaf = b->HasNegLeaf();
00931
00932 if(a->HasPosLeaf())
00933 {
00934 FETCH_LEAF(a->GetPosPrimitive(), mIMesh0, mR0to1, mT0to1)
00935
00936 if(BHasPosLeaf) PrimTestTriIndex(b->GetPosPrimitive());
00937 else _CollideTriBox(b->GetPos());
00938
00939 if(ContactFound()) return;
00940
00941 if(BHasNegLeaf) PrimTestTriIndex(b->GetNegPrimitive());
00942 else _CollideTriBox(b->GetNeg());
00943 }
00944 else
00945 {
00946 if(BHasPosLeaf)
00947 {
00948 FETCH_LEAF(b->GetPosPrimitive(), mIMesh1, mR1to0, mT1to0)
00949
00950 _CollideBoxTri(a->GetPos());
00951 }
00952 else _Collide(a->GetPos(), b->GetPos());
00953
00954 if(ContactFound()) return;
00955
00956 if(BHasNegLeaf)
00957 {
00958 FETCH_LEAF(b->GetNegPrimitive(), mIMesh1, mR1to0, mT1to0)
00959
00960 _CollideBoxTri(a->GetPos());
00961 }
00962 else _Collide(a->GetPos(), b->GetNeg());
00963 }
00964
00965 if(ContactFound()) return;
00966
00967 if(a->HasNegLeaf())
00968 {
00969 FETCH_LEAF(a->GetNegPrimitive(), mIMesh0, mR0to1, mT0to1)
00970
00971 if(BHasPosLeaf) PrimTestTriIndex(b->GetPosPrimitive());
00972 else _CollideTriBox(b->GetPos());
00973
00974 if(ContactFound()) return;
00975
00976 if(BHasNegLeaf) PrimTestTriIndex(b->GetNegPrimitive());
00977 else _CollideTriBox(b->GetNeg());
00978 }
00979 else
00980 {
00981 if(BHasPosLeaf)
00982 {
00983
00984 FETCH_LEAF(b->GetPosPrimitive(), mIMesh1, mR1to0, mT1to0)
00985
00986 _CollideBoxTri(a->GetNeg());
00987 }
00988 else _Collide(a->GetNeg(), b->GetPos());
00989
00990 if(ContactFound()) return;
00991
00992 if(BHasNegLeaf)
00993 {
00994
00995 FETCH_LEAF(b->GetNegPrimitive(), mIMesh1, mR1to0, mT1to0)
00996
00997 _CollideBoxTri(a->GetNeg());
00998 }
00999 else _Collide(a->GetNeg(), b->GetNeg());
01000 }
01001 }