OPC_PlanesCollider.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 
00027 
00028 
00030 // Precompiled Header
00031 #include "Stdafx.h"
00032 
00033 using namespace Opcode;
00034 
00035 #include "OPC_PlanesAABBOverlap.h"
00036 #include "OPC_PlanesTriOverlap.h"
00037 
00038 #define SET_CONTACT(prim_index, flag)           \
00039         /* Set contact status */                                \
00040         mFlags |= flag;                                                 \
00041         mTouchedPrimitives->Add(prim_index);
00042 
00044 #define PLANES_PRIM(prim_index, flag)           \
00045         /* Request vertices from the app */             \
00046         mIMesh->GetTriangle(mVP, prim_index);   \
00047         /* Perform triangle-box overlap test */ \
00048         if(PlanesTriOverlap(clip_mask))                 \
00049         {                                                                               \
00050                 SET_CONTACT(prim_index, flag)           \
00051         }
00052 
00054 
00057 
00058 PlanesCollider::PlanesCollider() :
00059         collisionPairInserter(NULL),
00060         mNbPlanes       (0),
00061         mPlanes         (null)
00062 {
00063 }
00064 
00066 
00069 
00070 PlanesCollider::~PlanesCollider()
00071 {
00072         DELETEARRAY(mPlanes);
00073 }
00074 
00076 
00080 
00081 const char* PlanesCollider::ValidateSettings()
00082 {
00083         if(TemporalCoherenceEnabled() && !FirstContactEnabled())        return "Temporal coherence only works with ""First contact"" mode!";
00084 
00085         return VolumeCollider::ValidateSettings();
00086 }
00087 
00089 
00103 
00104 bool PlanesCollider::Collide(PlanesCache& cache, const Plane* planes, udword nb_planes, const Model& model, const Matrix4x4* worldm)
00105 {
00106         // Checkings
00107         if(!Setup(&model))      return false;
00108 
00109         // Init collision query
00110         if(InitQuery(cache, planes, nb_planes, worldm)) return true;
00111 
00112         udword PlaneMask = (1<<nb_planes)-1;
00113 
00114         if(!model.HasLeafNodes())
00115         {
00116                 if(model.IsQuantized())
00117                 {
00118                         const AABBQuantizedNoLeafTree* Tree = (const AABBQuantizedNoLeafTree*)model.GetTree();
00119 
00120                         // Setup dequantization coeffs
00121                         mCenterCoeff    = Tree->mCenterCoeff;
00122                         mExtentsCoeff   = Tree->mExtentsCoeff;
00123 
00124                         // Perform collision query
00125                         if(SkipPrimitiveTests())        _CollideNoPrimitiveTest(Tree->GetNodes(), PlaneMask);
00126                         else                                            _Collide(Tree->GetNodes(), PlaneMask);
00127                 }
00128                 else
00129                 {
00130                         const AABBNoLeafTree* Tree = (const AABBNoLeafTree*)model.GetTree();
00131 
00132                         // Perform collision query
00133                         if(SkipPrimitiveTests())        _CollideNoPrimitiveTest(Tree->GetNodes(), PlaneMask);
00134                         else                                            _Collide(Tree->GetNodes(), PlaneMask);
00135                 }
00136         }
00137         else
00138         {
00139                 if(model.IsQuantized())
00140                 {
00141                         const AABBQuantizedTree* Tree = (const AABBQuantizedTree*)model.GetTree();
00142 
00143                         // Setup dequantization coeffs
00144                         mCenterCoeff    = Tree->mCenterCoeff;
00145                         mExtentsCoeff   = Tree->mExtentsCoeff;
00146 
00147                         // Perform collision query
00148                         if(SkipPrimitiveTests())        _CollideNoPrimitiveTest(Tree->GetNodes(), PlaneMask);
00149                         else                                            _Collide(Tree->GetNodes(), PlaneMask);
00150                 }
00151                 else
00152                 {
00153                         const AABBCollisionTree* Tree = (const AABBCollisionTree*)model.GetTree();
00154 
00155                         // Perform collision query
00156                         if(SkipPrimitiveTests())        _CollideNoPrimitiveTest(Tree->GetNodes(), PlaneMask);
00157                         else                                            _Collide(Tree->GetNodes(), PlaneMask);
00158                 }
00159         }
00160         return true;
00161 }
00162 
00164 
00177 
00178 BOOL PlanesCollider::InitQuery(PlanesCache& cache, const Plane* planes, udword nb_planes, const Matrix4x4* worldm)
00179 {
00180         // 1) Call the base method
00181         VolumeCollider::InitQuery();
00182 
00183         // 2) Compute planes in model space
00184         if(nb_planes>mNbPlanes)
00185         {
00186                 DELETEARRAY(mPlanes);
00187                 mPlanes = new Plane[nb_planes];
00188         }
00189         mNbPlanes = nb_planes;
00190 
00191         if(worldm)
00192         {
00193                 Matrix4x4 InvWorldM;
00194                 InvertPRMatrix(InvWorldM, *worldm);
00195 
00196 //              for(udword i=0;i<nb_planes;i++) mPlanes[i] = planes[i] * InvWorldM;
00197                 for(udword i=0;i<nb_planes;i++) TransformPlane(mPlanes[i], planes[i], InvWorldM);
00198         }
00199         else CopyMemory(mPlanes, planes, nb_planes*sizeof(Plane));
00200 
00201         // 3) Setup destination pointer
00202         mTouchedPrimitives = &cache.TouchedPrimitives;
00203 
00204         // 4) Special case: 1-triangle meshes [Opcode 1.3]
00205         if(mCurrentModel && mCurrentModel->HasSingleNode())
00206         {
00207                 if(!SkipPrimitiveTests())
00208                 {
00209                         // We simply perform the BV-Prim overlap test each time. We assume single triangle has index 0.
00210                         mTouchedPrimitives->Reset();
00211 
00212                         // Perform overlap test between the unique triangle and the planes (and set contact status if needed)
00213                         udword clip_mask = (1<<mNbPlanes)-1;
00214                         PLANES_PRIM(udword(0), OPC_CONTACT)
00215 
00216                         // Return immediately regardless of status
00217                         return TRUE;
00218                 }
00219         }
00220 
00221         // 4) Check temporal coherence:
00222         if(TemporalCoherenceEnabled())
00223         {
00224                 // Here we use temporal coherence
00225                 // => check results from previous frame before performing the collision query
00226                 if(FirstContactEnabled())
00227                 {
00228                         // We're only interested in the first contact found => test the unique previously touched face
00229                         if(mTouchedPrimitives->GetNbEntries())
00230                         {
00231                                 // Get index of previously touched face = the first entry in the array
00232                                 udword PreviouslyTouchedFace = mTouchedPrimitives->GetEntry(0);
00233 
00234                                 // Then reset the array:
00235                                 // - if the overlap test below is successful, the index we'll get added back anyway
00236                                 // - if it isn't, then the array should be reset anyway for the normal query
00237                                 mTouchedPrimitives->Reset();
00238 
00239                                 // Perform overlap test between the cached triangle and the planes (and set contact status if needed)
00240                                 udword clip_mask = (1<<mNbPlanes)-1;
00241                                 PLANES_PRIM(PreviouslyTouchedFace, OPC_TEMPORAL_CONTACT)
00242 
00243                                 // Return immediately if possible
00244                                 if(GetContactStatus())  return TRUE;
00245                         }
00246                         // else no face has been touched during previous query
00247                         // => we'll have to perform a normal query
00248                 }
00249                 else mTouchedPrimitives->Reset();
00250         }
00251         else
00252         {
00253                 // Here we don't use temporal coherence => do a normal query
00254                 mTouchedPrimitives->Reset();
00255         }
00256 
00257         return FALSE;
00258 }
00259 
00260 #define TEST_CLIP_MASK                                                                                                                                                                  \
00261         /* If the box is completely included, so are its children. We don't need to do extra tests, we */       \
00262         /* can immediately output a list of visible children. Those ones won't need to be clipped. */           \
00263         if(!OutClipMask)                                                                                                                                                                        \
00264         {                                                                                                                                                                                                       \
00265                 /* Set contact status */                                                                                                                                                \
00266                 mFlags |= OPC_CONTACT;                                                                                                                                                  \
00267                 _Dump(node);                                                                                                                                                                    \
00268                 return;                                                                                                                                                                                 \
00269         }
00270 
00272 
00276 
00277 void PlanesCollider::_Collide(const AABBCollisionNode* node, udword clip_mask)
00278 {
00279         // Test the box against the planes. If the box is completely culled, so are its children, hence we exit.
00280         udword OutClipMask;
00281         if(!PlanesAABBOverlap(node->mAABB.mCenter, node->mAABB.mExtents, OutClipMask, clip_mask))       return;
00282 
00283         TEST_CLIP_MASK
00284 
00285         // Else the box straddles one or several planes, so we need to recurse down the tree.
00286         if(node->IsLeaf())
00287         {
00288                 PLANES_PRIM(node->GetPrimitive(), OPC_CONTACT)
00289         }
00290         else
00291         {
00292                 _Collide(node->GetPos(), OutClipMask);
00293 
00294                 if(ContactFound()) return;
00295 
00296                 _Collide(node->GetNeg(), OutClipMask);
00297         }
00298 }
00299 
00301 
00305 
00306 void PlanesCollider::_CollideNoPrimitiveTest(const AABBCollisionNode* node, udword clip_mask)
00307 {
00308         // Test the box against the planes. If the box is completely culled, so are its children, hence we exit.
00309         udword OutClipMask;
00310         if(!PlanesAABBOverlap(node->mAABB.mCenter, node->mAABB.mExtents, OutClipMask, clip_mask))       return;
00311 
00312         TEST_CLIP_MASK
00313 
00314         // Else the box straddles one or several planes, so we need to recurse down the tree.
00315         if(node->IsLeaf())
00316         {
00317                 SET_CONTACT(node->GetPrimitive(), OPC_CONTACT)
00318         }
00319         else
00320         {
00321                 _CollideNoPrimitiveTest(node->GetPos(), OutClipMask);
00322 
00323                 if(ContactFound()) return;
00324 
00325                 _CollideNoPrimitiveTest(node->GetNeg(), OutClipMask);
00326         }
00327 }
00328 
00330 
00334 
00335 void PlanesCollider::_Collide(const AABBQuantizedNode* node, udword clip_mask)
00336 {
00337         // Dequantize box
00338         const QuantizedAABB& Box = node->mAABB;
00339         const Point Center(float(Box.mCenter[0]) * mCenterCoeff.x, float(Box.mCenter[1]) * mCenterCoeff.y, float(Box.mCenter[2]) * mCenterCoeff.z);
00340         const Point Extents(float(Box.mExtents[0]) * mExtentsCoeff.x, float(Box.mExtents[1]) * mExtentsCoeff.y, float(Box.mExtents[2]) * mExtentsCoeff.z);
00341 
00342         // Test the box against the planes. If the box is completely culled, so are its children, hence we exit.
00343         udword OutClipMask;
00344         if(!PlanesAABBOverlap(Center, Extents, OutClipMask, clip_mask)) return;
00345 
00346         TEST_CLIP_MASK
00347 
00348         // Else the box straddles one or several planes, so we need to recurse down the tree.
00349         if(node->IsLeaf())
00350         {
00351                 PLANES_PRIM(node->GetPrimitive(), OPC_CONTACT)
00352         }
00353         else
00354         {
00355                 _Collide(node->GetPos(), OutClipMask);
00356 
00357                 if(ContactFound()) return;
00358 
00359                 _Collide(node->GetNeg(), OutClipMask);
00360         }
00361 }
00362 
00364 
00368 
00369 void PlanesCollider::_CollideNoPrimitiveTest(const AABBQuantizedNode* node, udword clip_mask)
00370 {
00371         // Dequantize box
00372         const QuantizedAABB& Box = node->mAABB;
00373         const Point Center(float(Box.mCenter[0]) * mCenterCoeff.x, float(Box.mCenter[1]) * mCenterCoeff.y, float(Box.mCenter[2]) * mCenterCoeff.z);
00374         const Point Extents(float(Box.mExtents[0]) * mExtentsCoeff.x, float(Box.mExtents[1]) * mExtentsCoeff.y, float(Box.mExtents[2]) * mExtentsCoeff.z);
00375 
00376         // Test the box against the planes. If the box is completely culled, so are its children, hence we exit.
00377         udword OutClipMask;
00378         if(!PlanesAABBOverlap(Center, Extents, OutClipMask, clip_mask)) return;
00379 
00380         TEST_CLIP_MASK
00381 
00382         // Else the box straddles one or several planes, so we need to recurse down the tree.
00383         if(node->IsLeaf())
00384         {
00385                 SET_CONTACT(node->GetPrimitive(), OPC_CONTACT)
00386         }
00387         else
00388         {
00389                 _CollideNoPrimitiveTest(node->GetPos(), OutClipMask);
00390 
00391                 if(ContactFound()) return;
00392 
00393                 _CollideNoPrimitiveTest(node->GetNeg(), OutClipMask);
00394         }
00395 }
00396 
00398 
00402 
00403 void PlanesCollider::_Collide(const AABBNoLeafNode* node, udword clip_mask)
00404 {
00405         // Test the box against the planes. If the box is completely culled, so are its children, hence we exit.
00406         udword OutClipMask;
00407         if(!PlanesAABBOverlap(node->mAABB.mCenter, node->mAABB.mExtents, OutClipMask, clip_mask))       return;
00408 
00409         TEST_CLIP_MASK
00410 
00411         // Else the box straddles one or several planes, so we need to recurse down the tree.
00412         if(node->HasPosLeaf())  { PLANES_PRIM(node->GetPosPrimitive(), OPC_CONTACT) }
00413         else                                    _Collide(node->GetPos(), OutClipMask);
00414 
00415         if(ContactFound()) return;
00416 
00417         if(node->HasNegLeaf())  { PLANES_PRIM(node->GetNegPrimitive(), OPC_CONTACT) }
00418         else                                    _Collide(node->GetNeg(), OutClipMask);
00419 }
00420 
00422 
00426 
00427 void PlanesCollider::_CollideNoPrimitiveTest(const AABBNoLeafNode* node, udword clip_mask)
00428 {
00429         // Test the box against the planes. If the box is completely culled, so are its children, hence we exit.
00430         udword OutClipMask;
00431         if(!PlanesAABBOverlap(node->mAABB.mCenter, node->mAABB.mExtents, OutClipMask, clip_mask))       return;
00432 
00433         TEST_CLIP_MASK
00434 
00435         // Else the box straddles one or several planes, so we need to recurse down the tree.
00436         if(node->HasPosLeaf())  { SET_CONTACT(node->GetPosPrimitive(), OPC_CONTACT) }
00437         else                                    _CollideNoPrimitiveTest(node->GetPos(), OutClipMask);
00438 
00439         if(ContactFound()) return;
00440 
00441         if(node->HasNegLeaf())  { SET_CONTACT(node->GetNegPrimitive(), OPC_CONTACT) }
00442         else                                    _CollideNoPrimitiveTest(node->GetNeg(), OutClipMask);
00443 }
00444 
00446 
00450 
00451 void PlanesCollider::_Collide(const AABBQuantizedNoLeafNode* node, udword clip_mask)
00452 {
00453         // Dequantize box
00454         const QuantizedAABB& Box = node->mAABB;
00455         const Point Center(float(Box.mCenter[0]) * mCenterCoeff.x, float(Box.mCenter[1]) * mCenterCoeff.y, float(Box.mCenter[2]) * mCenterCoeff.z);
00456         const Point Extents(float(Box.mExtents[0]) * mExtentsCoeff.x, float(Box.mExtents[1]) * mExtentsCoeff.y, float(Box.mExtents[2]) * mExtentsCoeff.z);
00457 
00458         // Test the box against the planes. If the box is completely culled, so are its children, hence we exit.
00459         udword OutClipMask;
00460         if(!PlanesAABBOverlap(Center, Extents, OutClipMask, clip_mask)) return;
00461 
00462         TEST_CLIP_MASK
00463 
00464         // Else the box straddles one or several planes, so we need to recurse down the tree.
00465         if(node->HasPosLeaf())  { PLANES_PRIM(node->GetPosPrimitive(), OPC_CONTACT) }
00466         else                                    _Collide(node->GetPos(), OutClipMask);
00467 
00468         if(ContactFound()) return;
00469 
00470         if(node->HasNegLeaf())  { PLANES_PRIM(node->GetNegPrimitive(), OPC_CONTACT) }
00471         else                                    _Collide(node->GetNeg(), OutClipMask);
00472 }
00473 
00475 
00479 
00480 void PlanesCollider::_CollideNoPrimitiveTest(const AABBQuantizedNoLeafNode* node, udword clip_mask)
00481 {
00482         // Dequantize box
00483         const QuantizedAABB& Box = node->mAABB;
00484         const Point Center(float(Box.mCenter[0]) * mCenterCoeff.x, float(Box.mCenter[1]) * mCenterCoeff.y, float(Box.mCenter[2]) * mCenterCoeff.z);
00485         const Point Extents(float(Box.mExtents[0]) * mExtentsCoeff.x, float(Box.mExtents[1]) * mExtentsCoeff.y, float(Box.mExtents[2]) * mExtentsCoeff.z);
00486 
00487         // Test the box against the planes. If the box is completely culled, so are its children, hence we exit.
00488         udword OutClipMask;
00489         if(!PlanesAABBOverlap(Center, Extents, OutClipMask, clip_mask)) return;
00490 
00491         TEST_CLIP_MASK
00492 
00493         // Else the box straddles one or several planes, so we need to recurse down the tree.
00494         if(node->HasPosLeaf())  { SET_CONTACT(node->GetPosPrimitive(), OPC_CONTACT) }
00495         else                                    _CollideNoPrimitiveTest(node->GetPos(), OutClipMask);
00496 
00497         if(ContactFound()) return;
00498 
00499         if(node->HasNegLeaf())  { SET_CONTACT(node->GetNegPrimitive(), OPC_CONTACT) }
00500         else                                    _CollideNoPrimitiveTest(node->GetNeg(), OutClipMask);
00501 }
00502 
00503 
00504 
00505 
00506 
00507 
00508 
00510 
00513 
00514 HybridPlanesCollider::HybridPlanesCollider()
00515 {
00516 }
00517 
00519 
00522 
00523 HybridPlanesCollider::~HybridPlanesCollider()
00524 {
00525 }
00526 
00527 bool HybridPlanesCollider::Collide(PlanesCache& cache, const Plane* planes, udword nb_planes, const HybridModel& model, const Matrix4x4* worldm)
00528 {
00529         // We don't want primitive tests here!
00530         mFlags |= OPC_NO_PRIMITIVE_TESTS;
00531 
00532         // Checkings
00533         if(!Setup(&model))      return false;
00534 
00535         // Init collision query
00536         if(InitQuery(cache, planes, nb_planes, worldm)) return true;
00537 
00538         // Special case for 1-leaf trees
00539         if(mCurrentModel && mCurrentModel->HasSingleNode())
00540         {
00541                 // Here we're supposed to perform a normal query, except our tree has a single node, i.e. just a few triangles
00542                 udword Nb = mIMesh->GetNbTriangles();
00543 
00544                 // Loop through all triangles
00545                 udword clip_mask = (1<<mNbPlanes)-1;
00546                 for(udword i=0;i<Nb;i++)
00547                 {
00548                         PLANES_PRIM(i, OPC_CONTACT)
00549                 }
00550                 return true;
00551         }
00552 
00553         // Override destination array since we're only going to get leaf boxes here
00554         mTouchedBoxes.Reset();
00555         mTouchedPrimitives = &mTouchedBoxes;
00556 
00557         udword PlaneMask = (1<<nb_planes)-1;
00558 
00559         // Now, do the actual query against leaf boxes
00560         if(!model.HasLeafNodes())
00561         {
00562                 if(model.IsQuantized())
00563                 {
00564                         const AABBQuantizedNoLeafTree* Tree = (const AABBQuantizedNoLeafTree*)model.GetTree();
00565 
00566                         // Setup dequantization coeffs
00567                         mCenterCoeff    = Tree->mCenterCoeff;
00568                         mExtentsCoeff   = Tree->mExtentsCoeff;
00569 
00570                         // Perform collision query - we don't want primitive tests here!
00571                         _CollideNoPrimitiveTest(Tree->GetNodes(), PlaneMask);
00572                 }
00573                 else
00574                 {
00575                         const AABBNoLeafTree* Tree = (const AABBNoLeafTree*)model.GetTree();
00576 
00577                         // Perform collision query - we don't want primitive tests here!
00578                         _CollideNoPrimitiveTest(Tree->GetNodes(), PlaneMask);
00579                 }
00580         }
00581         else
00582         {
00583                 if(model.IsQuantized())
00584                 {
00585                         const AABBQuantizedTree* Tree = (const AABBQuantizedTree*)model.GetTree();
00586 
00587                         // Setup dequantization coeffs
00588                         mCenterCoeff    = Tree->mCenterCoeff;
00589                         mExtentsCoeff   = Tree->mExtentsCoeff;
00590 
00591                         // Perform collision query - we don't want primitive tests here!
00592                         _CollideNoPrimitiveTest(Tree->GetNodes(), PlaneMask);
00593                 }
00594                 else
00595                 {
00596                         const AABBCollisionTree* Tree = (const AABBCollisionTree*)model.GetTree();
00597 
00598                         // Perform collision query - we don't want primitive tests here!
00599                         _CollideNoPrimitiveTest(Tree->GetNodes(), PlaneMask);
00600                 }
00601         }
00602 
00603         // We only have a list of boxes so far
00604         if(GetContactStatus())
00605         {
00606                 // Reset contact status, since it currently only reflects collisions with leaf boxes
00607                 Collider::InitQuery();
00608 
00609                 // Change dest container so that we can use built-in overlap tests and get collided primitives
00610                 cache.TouchedPrimitives.Reset();
00611                 mTouchedPrimitives = &cache.TouchedPrimitives;
00612 
00613                 // Read touched leaf boxes
00614                 udword Nb = mTouchedBoxes.GetNbEntries();
00615                 const udword* Touched = mTouchedBoxes.GetEntries();
00616 
00617                 const LeafTriangles* LT = model.GetLeafTriangles();
00618                 const udword* Indices = model.GetIndices();
00619 
00620                 // Loop through touched leaves
00621                 udword clip_mask = (1<<mNbPlanes)-1;
00622                 while(Nb--)
00623                 {
00624                         const LeafTriangles& CurrentLeaf = LT[*Touched++];
00625 
00626                         // Each leaf box has a set of triangles
00627                         udword NbTris = CurrentLeaf.GetNbTriangles();
00628                         if(Indices)
00629                         {
00630                                 const udword* T = &Indices[CurrentLeaf.GetTriangleIndex()];
00631 
00632                                 // Loop through triangles and test each of them
00633                                 while(NbTris--)
00634                                 {
00635                                         udword TriangleIndex = *T++;
00636                                         PLANES_PRIM(TriangleIndex, OPC_CONTACT)
00637                                 }
00638                         }
00639                         else
00640                         {
00641                                 udword BaseIndex = CurrentLeaf.GetTriangleIndex();
00642 
00643                                 // Loop through triangles and test each of them
00644                                 while(NbTris--)
00645                                 {
00646                                         udword TriangleIndex = BaseIndex++;
00647                                         PLANES_PRIM(TriangleIndex, OPC_CONTACT)
00648                                 }
00649                         }
00650                 }
00651         }
00652 
00653         return true;
00654 }


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