OPC_TreeCollider.cpp
Go to the documentation of this file.
00001 
00002 /*
00003  *      OPCODE - Optimized Collision Detection
00004  *      Copyright (C) 2001 Pierre Terdiman
00005  *      Homepage: http://www.codercorner.com/Opcode.htm
00006  */
00008 
00010 
00016 
00017 
00019 
00028 
00029 
00030 #include"../CollisionPairInserter.h"
00031 
00033 // Precompiled Header
00034 #include "Stdafx.h"
00035 
00036 #include<iostream>
00037 
00038 using namespace Opcode;
00039 
00040 #include "OPC_BoxBoxOverlap.h"
00041 //#include "OPC_TriBoxOverlap.h"
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         // Checkings
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           Rules:
00107                 - perform hull test
00108                 - when hulls collide, disable hull test
00109                 - if meshes overlap, reset countdown
00110                 - if countdown reaches 0, enable hull test
00111 
00112         */
00113 
00114 #ifdef __MESHMERIZER_H__
00115         // Handle hulls
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                 // Reset stats & contact status
00162                 mFlags &= ~OPC_CONTACT;
00163                 mNbBVBVTests            = 0;
00164                 mNbPrimPrimTests        = 0;
00165                 mNbBVPrimTests          = 0;
00166                 mPairs.Reset();
00167                 return true;
00168                         }
00169                 }
00170         }
00171 
00172         // Here, hulls collide
00173         cache.HullTest = false;
00174 #endif // __MESHMERIZER_H__
00175 
00176         // Checkings
00177         if(!Setup(cache.Model0->GetMeshInterface(), cache.Model1->GetMeshInterface()))  return false;
00178 
00179         // Simple double-dispatch
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                 // Reset counter as long as overlap occurs
00216                 if(GetContactStatus())  cache.ResetCountDown();
00217 
00218                 // Enable hull test again when counter reaches zero
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         // Reset stats & contact status
00244         Collider::InitQuery();
00245         mNbBVBVTests            = 0;
00246         mNbPrimPrimTests        = 0;
00247         mNbBVPrimTests          = 0;
00248         mPairs.Reset();
00249 
00250         // Setup matrices
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         // Precompute absolute 1-to-0 rotation matrix
00265         for(udword i=0;i<3;i++)
00266         {
00267                 for(udword j=0;j<3;j++)
00268                 {
00269                         // Epsilon value prevents floating-point inaccuracies (strategy borrowed from CD)
00270                         mAR.m[i][j] = 1e-6f + fabsf(mR1to0.m[i][j]);
00271                 }
00272         }
00273 
00274 
00275         //Modified by S-cubed, Inc.
00276         //$B9TNs$r(B Boost $B$N7A<0$K$"$o$;$k!#(B
00277         //  Prim-Prim $B%F%9%H$r(B TriOverlap.cpp $B$G9T$&$?$a!#(B
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             //  Matrix3x3 w1 = world1;
00292             //  mT0to1 = w1 * (t1 - t0)
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             //s1, s2 $B$O(B 1.0 $B$N$_$J$N$G!"8GDj!#(B
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         // Checkings
00319         if(!cache)      return false;
00320 
00321         // Test previously colliding primitives first
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         // Init collision query
00352         InitQuery(world0, world1);
00353 
00354         // Check previous state
00355         if(CheckTemporalCoherence(cache))               return true;
00356 
00357         // Perform collision query
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         // Init collision query
00380         InitQuery(world0, world1);
00381 
00382         // Check previous state
00383         if(CheckTemporalCoherence(cache))               return true;
00384 
00385         // Perform collision query
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         // Init collision query
00408         InitQuery(world0, world1);
00409 
00410         // Check previous state
00411         if(CheckTemporalCoherence(cache))               return true;
00412 
00413         // Setup dequantization coeffs
00414         mCenterCoeff0   = tree0->mCenterCoeff;
00415         mExtentsCoeff0  = tree0->mExtentsCoeff;
00416         mCenterCoeff1   = tree1->mCenterCoeff;
00417         mExtentsCoeff1  = tree1->mExtentsCoeff;
00418 
00419         // Dequantize box A
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         // Dequantize box B
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         // Perform collision query
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         // Init collision query
00451         InitQuery(world0, world1);
00452 
00453         // Check previous state
00454         if(CheckTemporalCoherence(cache))               return true;
00455 
00456         // Setup dequantization coeffs
00457         mCenterCoeff0   = tree0->mCenterCoeff;
00458         mExtentsCoeff0  = tree0->mExtentsCoeff;
00459         mCenterCoeff1   = tree1->mCenterCoeff;
00460         mExtentsCoeff1  = tree1->mExtentsCoeff;
00461 
00462         // Perform collision query
00463         _Collide(tree0->GetNodes(), tree1->GetNodes());
00464 
00465         UPDATE_CACHE
00466 
00467         return true;
00468 }
00469 
00471 // Standard trees
00473 
00474 // The normal AABB tree can use 2 different descent rules (with different performances)
00475 //#define ORIGINAL_CODE                 //!< UNC-like descent rules
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         // Perform BV-BV overlap test
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         // Perform BV-BV overlap test
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 // No-leaf trees
00563 
00565 
00570 
00571 void AABBTreeCollider::PrimTest(udword id0, udword id1)
00572 {
00573     mId0 = id0;
00574     mId1 = id1;
00575         // Request vertices from the app
00576         VertexPointers VP0;
00577         VertexPointers VP1;
00578         mIMesh0->GetTriangle(VP0, id0);
00579         mIMesh1->GetTriangle(VP1, id1);
00580 
00581         // Modified by S-cubed, Inc.
00582         // Transform from space 0 (old : 1) to space 1 (old : 0)
00583         // CD $B$G$OJQ49$,5U$J$N$G$"$o$;$k!#(B
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         // Perform triangle-triangle overlap test
00590         if(TriTriOverlap(u0, u1, u2,
00591                                          *VP1.Vertex[0], *VP1.Vertex[1], *VP1.Vertex[2]))
00592         {
00593                 // Keep track of colliding pairs
00594                 mPairs.Add(id0).Add(id1);
00595                 // Set contact status
00596                 mFlags |= OPC_CONTACT;
00597         }
00598 }
00599 
00601 
00605 
00606 inline_ void AABBTreeCollider::PrimTestTriIndex(udword id1)
00607 {
00608         // Request vertices from the app
00609         VertexPointers VP;
00610         mIMesh1->GetTriangle(VP, id1);
00611 
00612         // Perform triangle-triangle overlap test
00613         if(TriTriOverlap(mLeafVerts[0], mLeafVerts[1], mLeafVerts[2], *VP.Vertex[0], *VP.Vertex[1], *VP.Vertex[2]))
00614         {
00615                 // Keep track of colliding pairs
00616                 mPairs.Add(mLeafIndex).Add(id1);
00617                 // Set contact status
00618                 mFlags |= OPC_CONTACT;
00619         }
00620 }
00621 
00623 
00627 
00628 inline_ void AABBTreeCollider::PrimTestIndexTri(udword id0)
00629 {
00630         // Request vertices from the app
00631         VertexPointers VP;
00632         mIMesh0->GetTriangle(VP, id0);
00633 
00634         // Perform triangle-triangle overlap test
00635         if(TriTriOverlap(mLeafVerts[0], mLeafVerts[1], mLeafVerts[2], *VP.Vertex[0], *VP.Vertex[1], *VP.Vertex[2]))
00636         {
00637                 // Keep track of colliding pairs
00638                 mPairs.Add(id0).Add(mLeafIndex);
00639                 // Set contact status
00640                 mFlags |= OPC_CONTACT;
00641         }
00642 }
00643 
00645 
00649 
00650 void AABBTreeCollider::_CollideTriBox(const AABBNoLeafNode* b)
00651 {
00652         // Perform triangle-box overlap test
00653   // Modified by S-cubed, Inc.
00654   //NoleafNode $B$O;HMQ$7$J$$(B
00655   //    if(!TriBoxOverlap(b->mAABB.mCenter, b->mAABB.mExtents)) return;
00656 
00657         // Keep same triangle, deal with first child
00658         if(b->HasPosLeaf())     PrimTestTriIndex(b->GetPosPrimitive());
00659         else                            _CollideTriBox(b->GetPos());
00660 
00661         if(ContactFound()) return;
00662 
00663         // Keep same triangle, deal with second child
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   // Modified by S-cubed, Inc.
00677   //NoleafNode $B$O;HMQ$7$J$$(B
00678         // Perform triangle-box overlap test
00679         //if(!TriBoxOverlap(b->mAABB.mCenter, b->mAABB.mExtents))       return;
00680 
00681         // Keep same triangle, deal with first child
00682         if(b->HasPosLeaf())     PrimTestIndexTri(b->GetPosPrimitive());
00683         else                            _CollideBoxTri(b->GetPos());
00684 
00685         if(ContactFound()) return;
00686 
00687         // Keep same triangle, deal with second child
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         /* Request vertices from the app */                                                     \
00696         VertexPointers VP;      imesh->GetTriangle(VP, prim_index);             \
00697         /* Transform them in a common space */                                          \
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         // Perform BV-BV overlap test
00712         if(!BoxBoxOverlap(a->mAABB.mExtents, a->mAABB.mCenter, b->mAABB.mExtents, b->mAABB.mCenter))    return;
00713 
00714         // Catch leaf status
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                         // ### That leaf has possibly already been fetched
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                         // ### That leaf has possibly already been fetched
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 // Quantized trees
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         // Perform BV-BV overlap test
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                 // Dequantize box
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                 // Dequantize box
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                 // Dequantize box
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                 // Dequantize box
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 // Quantized no-leaf trees
00848 
00850 
00855 
00856 void AABBTreeCollider::_CollideTriBox(const AABBQuantizedNoLeafNode* b)
00857 {
00858 
00859         // Dequantize box
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         // Perform triangle-box overlap test
00865         // Modified by S-cubed, Inc.
00866         //NoleafNode $B$O;HMQ$7$J$$(B
00867         //if(!TriBoxOverlap(Pb, eb))    return;
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         // Dequantize box
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         // Modified by S-cubed, Inc.
00894         //NoleafNode $B$O;HMQ$7$J$$(B
00895         // Perform triangle-box overlap test
00896         //      if(!TriBoxOverlap(Pa, ea))      return;
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         // Dequantize box A
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         // Dequantize box B
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         // Perform BV-BV overlap test
00926         if(!BoxBoxOverlap(ea, Pa, eb, Pb))      return;
00927 
00928         // Catch leaf status
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                         // ### That leaf has possibly already been fetched
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                         // ### That leaf has possibly already been fetched
00995                         FETCH_LEAF(b->GetNegPrimitive(), mIMesh1, mR1to0, mT1to0)
00996 
00997                         _CollideBoxTri(a->GetNeg());
00998                 }
00999                 else _Collide(a->GetNeg(), b->GetNeg());
01000         }
01001 }


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Thu Apr 11 2019 03:30:18