OPC_RayCollider.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 
00088 
00089 
00091 
00100 
00101 
00103 
00111 
00112 
00114 // Precompiled Header
00115 #include "Stdafx.h"
00116 
00117 using namespace Opcode;
00118 
00119 #include "OPC_RayAABBOverlap.h"
00120 #include "OPC_RayTriOverlap.h"
00121 
00122 #define SET_CONTACT(prim_index, flag)                                                                                   \
00123         mNbIntersections++;                                                                                                                     \
00124         /* Set contact status */                                                                                                        \
00125         mFlags |= flag;                                                                                                                         \
00126         /* In any case the contact has been found and recorded in mStabbedFace  */      \
00127         mStabbedFace.mFaceID = prim_index;
00128 
00129 #ifdef OPC_RAYHIT_CALLBACK
00130 
00131         #define HANDLE_CONTACT(prim_index, flag)                                                                                                        \
00132                 SET_CONTACT(prim_index, flag)                                                                                                                   \
00133                                                                                                                                                                                                 \
00134                 if(mHitCallback)        (mHitCallback)(mStabbedFace, mUserData);
00135 
00136         #define UPDATE_CACHE                                    \
00137                 if(cache && GetContactStatus())         \
00138                 {                                                                       \
00139                         *cache  = mStabbedFace.mFaceID; \
00140                 }
00141 #else
00142 
00143         #define HANDLE_CONTACT(prim_index, flag)                                                                                                        \
00144                 SET_CONTACT(prim_index, flag)                                                                                                                   \
00145                                                                                                                                                                                                 \
00146                 /* Now we can also record it in mStabbedFaces if available */                                                   \
00147                 if(mStabbedFaces)                                                                                                                                               \
00148                 {                                                                                                                                                                               \
00149                         /* If we want all faces or if that's the first one we hit */                                            \
00150                         if(!mClosestHit || !mStabbedFaces->GetNbFaces())                                                                        \
00151                         {                                                                                                                                                                       \
00152                                 mStabbedFaces->AddFace(mStabbedFace);                                                                                   \
00153                         }                                                                                                                                                                       \
00154                         else                                                                                                                                                            \
00155                         {                                                                                                                                                                       \
00156                                 /* We only keep closest hit */                                                                                                  \
00157                                 CollisionFace* Current = const_cast<CollisionFace*>(mStabbedFaces->GetFaces()); \
00158                                 if(Current && mStabbedFace.mDistance<Current->mDistance)                                                \
00159                                 {                                                                                                                                                               \
00160                                         *Current = mStabbedFace;                                                                                                        \
00161                                 }                                                                                                                                                               \
00162                         }                                                                                                                                                                       \
00163                 }
00164 
00165         #define UPDATE_CACHE                                                                                            \
00166                 if(cache && GetContactStatus() && mStabbedFaces)                                \
00167                 {                                                                                                                               \
00168                         const CollisionFace* Current = mStabbedFaces->GetFaces();       \
00169                         if(Current)     *cache  = Current->mFaceID;                                             \
00170                         else            *cache  = INVALID_ID;                                                   \
00171                 }
00172 #endif
00173 
00174 #define SEGMENT_PRIM(prim_index, flag)                                                                                                          \
00175         /* Request vertices from the app */                                                                                                             \
00176         VertexPointers VP;      mIMesh->GetTriangle(VP, prim_index);                                                            \
00177                                                                                                                                                                                         \
00178         /* Perform ray-tri overlap test and return */                                                                                   \
00179         if(RayTriOverlap(*VP.Vertex[0], *VP.Vertex[1], *VP.Vertex[2]))                                                  \
00180         {                                                                                                                                                                               \
00181                 /* Intersection point is valid if dist < segment's length */                                            \
00182                 /* We know dist>0 so we can use integers */                                                                                     \
00183                 if(IR(mStabbedFace.mDistance)<IR(mMaxDist))                                                                                     \
00184                 {                                                                                                                                                                       \
00185                         HANDLE_CONTACT(prim_index, flag)                                                                                                \
00186                 }                                                                                                                                                                       \
00187         }
00188 
00189 #define RAY_PRIM(prim_index, flag)                                                                                                                      \
00190         /* Request vertices from the app */                                                                                                             \
00191         VertexPointers VP;      mIMesh->GetTriangle(VP, prim_index);                                                            \
00192                                                                                                                                                                                         \
00193         /* Perform ray-tri overlap test and return */                                                                                   \
00194         if(RayTriOverlap(*VP.Vertex[0], *VP.Vertex[1], *VP.Vertex[2]))                                                  \
00195         {                                                                                                                                                                               \
00196                 HANDLE_CONTACT(prim_index, flag)                                                                                                        \
00197         }
00198 
00199 
00201 
00204 
00205 RayCollider::RayCollider() :
00206 #ifdef OPC_RAYHIT_CALLBACK
00207         mHitCallback            (null),
00208         mUserData                       (0),
00209 #else
00210         mStabbedFaces           (null),
00211 #endif
00212         mNbRayBVTests           (0),
00213         mNbRayPrimTests         (0),
00214         mNbIntersections        (0),
00215         mMaxDist                        (MAX_FLOAT),
00216 #ifndef OPC_RAYHIT_CALLBACK
00217         mClosestHit                     (false),
00218 #endif
00219         mCulling                        (true)
00220 {
00221 }
00222 
00224 
00227 
00228 RayCollider::~RayCollider()
00229 {
00230 }
00231 
00233 
00237 
00238 const char* RayCollider::ValidateSettings()
00239 {
00240         if(mMaxDist<0.0f)                                                                                       return "Higher distance bound must be positive!";
00241         if(TemporalCoherenceEnabled() && !FirstContactEnabled())        return "Temporal coherence only works with ""First contact"" mode!";
00242 #ifndef OPC_RAYHIT_CALLBACK
00243         if(mClosestHit && FirstContactEnabled())                                        return "Closest hit doesn't work with ""First contact"" mode!";
00244         if(TemporalCoherenceEnabled() && mClosestHit)                           return "Temporal coherence can't guarantee to report closest hit!";
00245 #endif
00246         if(SkipPrimitiveTests())                                                                        return "SkipPrimitiveTests not possible for RayCollider ! (not implemented)";
00247         return null;
00248 }
00249 
00251 
00263 
00264 bool RayCollider::Collide(const Ray& world_ray, const Model& model, const Matrix4x4* world, udword* cache)
00265 {
00266         // Checkings
00267         if(!Setup(&model))      return false;
00268 
00269         // Init collision query
00270         if(InitQuery(world_ray, world, cache))  return true;
00271 
00272         if(!model.HasLeafNodes())
00273         {
00274                 if(model.IsQuantized())
00275                 {
00276                         const AABBQuantizedNoLeafTree* Tree = (const AABBQuantizedNoLeafTree*)model.GetTree();
00277 
00278                         // Setup dequantization coeffs
00279                         mCenterCoeff    = Tree->mCenterCoeff;
00280                         mExtentsCoeff   = Tree->mExtentsCoeff;
00281 
00282                         // Perform stabbing query
00283                         if(IR(mMaxDist)!=IEEE_MAX_FLOAT)        _SegmentStab(Tree->GetNodes());
00284                         else                                                            _RayStab(Tree->GetNodes());
00285                 }
00286                 else
00287                 {
00288                         const AABBNoLeafTree* Tree = (const AABBNoLeafTree*)model.GetTree();
00289 
00290                         // Perform stabbing query
00291                         if(IR(mMaxDist)!=IEEE_MAX_FLOAT)        _SegmentStab(Tree->GetNodes());
00292                         else                                                            _RayStab(Tree->GetNodes());
00293                 }
00294         }
00295         else
00296         {
00297                 if(model.IsQuantized())
00298                 {
00299                         const AABBQuantizedTree* Tree = (const AABBQuantizedTree*)model.GetTree();
00300 
00301                         // Setup dequantization coeffs
00302                         mCenterCoeff    = Tree->mCenterCoeff;
00303                         mExtentsCoeff   = Tree->mExtentsCoeff;
00304 
00305                         // Perform stabbing query
00306                         if(IR(mMaxDist)!=IEEE_MAX_FLOAT)        _SegmentStab(Tree->GetNodes());
00307                         else                                                            _RayStab(Tree->GetNodes());
00308                 }
00309                 else
00310                 {
00311                         const AABBCollisionTree* Tree = (const AABBCollisionTree*)model.GetTree();
00312 
00313                         // Perform stabbing query
00314                         if(IR(mMaxDist)!=IEEE_MAX_FLOAT)        _SegmentStab(Tree->GetNodes());
00315                         else                                                            _RayStab(Tree->GetNodes());
00316                 }
00317         }
00318 
00319         // Update cache if needed
00320         UPDATE_CACHE
00321         return true;
00322 }
00323 
00324 
00326 
00338 
00339 BOOL RayCollider::InitQuery(const Ray& world_ray, const Matrix4x4* world, udword* face_id)
00340 {
00341         // Reset stats & contact status
00342         Collider::InitQuery();
00343         mNbRayBVTests           = 0;
00344         mNbRayPrimTests         = 0;
00345         mNbIntersections        = 0;
00346 #ifndef OPC_RAYHIT_CALLBACK
00347         if(mStabbedFaces)       mStabbedFaces->Reset();
00348 #endif
00349 
00350         // Compute ray in local space
00351         // The (Origin/Dir) form is needed for the ray-triangle test anyway (even for segment tests)
00352         if(world)
00353         {
00354                 Matrix3x3 InvWorld = *world;
00355                 mDir = InvWorld * world_ray.mDir;
00356 
00357                 Matrix4x4 World;
00358                 InvertPRMatrix(World, *world);
00359                 mOrigin = world_ray.mOrig * World;
00360         }
00361         else
00362         {
00363                 mDir    = world_ray.mDir;
00364                 mOrigin = world_ray.mOrig;
00365         }
00366 
00367         // 4) Special case: 1-triangle meshes [Opcode 1.3]
00368         if(mCurrentModel && mCurrentModel->HasSingleNode())
00369         {
00370                 // We simply perform the BV-Prim overlap test each time. We assume single triangle has index 0.
00371                 if(!SkipPrimitiveTests())
00372                 {
00373                         // Perform overlap test between the unique triangle and the ray (and set contact status if needed)
00374                         SEGMENT_PRIM(udword(0), OPC_CONTACT)
00375 
00376                         // Return immediately regardless of status
00377                         return TRUE;
00378                 }
00379         }
00380 
00381         // Check temporal coherence :
00382 
00383         // Test previously colliding primitives first
00384         if(TemporalCoherenceEnabled() && FirstContactEnabled() && face_id && *face_id!=INVALID_ID)
00385         {
00386 #ifdef OLD_CODE
00387 #ifndef OPC_RAYHIT_CALLBACK
00388                 if(!mClosestHit)
00389 #endif
00390                 {
00391                         // Request vertices from the app
00392                         VertexPointers VP;
00393                         mIMesh->GetTriangle(VP, *face_id);
00394                         // Perform ray-cached tri overlap test
00395                         if(RayTriOverlap(*VP.Vertex[0], *VP.Vertex[1], *VP.Vertex[2]))
00396                         {
00397                                 // Intersection point is valid if:
00398                                 // - distance is positive (else it can just be a face behind the orig point)
00399                                 // - distance is smaller than a given max distance (useful for shadow feelers)
00400 //                              if(mStabbedFace.mDistance>0.0f && mStabbedFace.mDistance<mMaxDist)
00401                                 if(IR(mStabbedFace.mDistance)<IR(mMaxDist))     // The other test is already performed in RayTriOverlap
00402                                 {
00403                                         // Set contact status
00404                                         mFlags |= OPC_TEMPORAL_CONTACT;
00405 
00406                                         mStabbedFace.mFaceID = *face_id;
00407 
00408 #ifndef OPC_RAYHIT_CALLBACK
00409                                         if(mStabbedFaces)       mStabbedFaces->AddFace(mStabbedFace);
00410 #endif
00411                                         return TRUE;
00412                                 }
00413                         }
00414                 }
00415 #else
00416                 // New code
00417                 // We handle both Segment/ray queries with the same segment code, and a possible infinite limit
00418                 SEGMENT_PRIM(*face_id, OPC_TEMPORAL_CONTACT)
00419 
00420                 // Return immediately if possible
00421                 if(GetContactStatus())  return TRUE;
00422 #endif
00423         }
00424 
00425         // Precompute data (moved after temporal coherence since only needed for ray-AABB)
00426         if(IR(mMaxDist)!=IEEE_MAX_FLOAT)
00427         {
00428                 // For Segment-AABB overlap
00429                 mData = 0.5f * mDir * mMaxDist;
00430                 mData2 = mOrigin + mData;
00431 
00432                 // Precompute mFDir;
00433                 mFDir.x = fabsf(mData.x);
00434                 mFDir.y = fabsf(mData.y);
00435                 mFDir.z = fabsf(mData.z);
00436         }
00437         else
00438         {
00439                 // For Ray-AABB overlap
00440 //              udword x = SIR(mDir.x)-1;
00441 //              udword y = SIR(mDir.y)-1;
00442 //              udword z = SIR(mDir.z)-1;
00443 //              mData.x = FR(x);
00444 //              mData.y = FR(y);
00445 //              mData.z = FR(z);
00446 
00447                 // Precompute mFDir;
00448                 mFDir.x = fabsf(mDir.x);
00449                 mFDir.y = fabsf(mDir.y);
00450                 mFDir.z = fabsf(mDir.z);
00451         }
00452 
00453         return FALSE;
00454 }
00455 
00457 
00464 
00465 bool RayCollider::Collide(const Ray& world_ray, const AABBTree* tree, Container& box_indices)
00466 {
00467         // ### bad design here
00468 
00469         // This is typically called for a scene tree, full of -AABBs-, not full of triangles.
00470         // So we don't really have "primitives" to deal with. Hence it doesn't work with
00471         // "FirstContact" + "TemporalCoherence".
00472         ASSERT( !(FirstContactEnabled() && TemporalCoherenceEnabled()) );
00473 
00474         // Checkings
00475         if(!tree)                                       return false;
00476 
00477         // Init collision query
00478         // Basically this is only called to initialize precomputed data
00479         if(InitQuery(world_ray))        return true;
00480 
00481         // Perform stabbing query
00482         if(IR(mMaxDist)!=IEEE_MAX_FLOAT)        _SegmentStab(tree, box_indices);
00483         else                                                            _RayStab(tree, box_indices);
00484 
00485         return true;
00486 }
00487 
00488 
00490 
00494 
00495 void RayCollider::_SegmentStab(const AABBCollisionNode* node)
00496 {
00497         // Perform Segment-AABB overlap test
00498         if(!SegmentAABBOverlap(node->mAABB.mCenter, node->mAABB.mExtents))      return;
00499 
00500         if(node->IsLeaf())
00501         {
00502                 SEGMENT_PRIM(node->GetPrimitive(), OPC_CONTACT)
00503         }
00504         else
00505         {
00506                 _SegmentStab(node->GetPos());
00507 
00508                 if(ContactFound()) return;
00509 
00510                 _SegmentStab(node->GetNeg());
00511         }
00512 }
00513 
00515 
00519 
00520 void RayCollider::_SegmentStab(const AABBQuantizedNode* node)
00521 {
00522         // Dequantize box
00523         const QuantizedAABB& Box = node->mAABB;
00524         const Point Center(float(Box.mCenter[0]) * mCenterCoeff.x, float(Box.mCenter[1]) * mCenterCoeff.y, float(Box.mCenter[2]) * mCenterCoeff.z);
00525         const Point Extents(float(Box.mExtents[0]) * mExtentsCoeff.x, float(Box.mExtents[1]) * mExtentsCoeff.y, float(Box.mExtents[2]) * mExtentsCoeff.z);
00526 
00527         // Perform Segment-AABB overlap test
00528         if(!SegmentAABBOverlap(Center, Extents))        return;
00529 
00530         if(node->IsLeaf())
00531         {
00532                 SEGMENT_PRIM(node->GetPrimitive(), OPC_CONTACT)
00533         }
00534         else
00535         {
00536                 _SegmentStab(node->GetPos());
00537 
00538                 if(ContactFound()) return;
00539 
00540                 _SegmentStab(node->GetNeg());
00541         }
00542 }
00543 
00545 
00549 
00550 void RayCollider::_SegmentStab(const AABBNoLeafNode* node)
00551 {
00552         // Perform Segment-AABB overlap test
00553         if(!SegmentAABBOverlap(node->mAABB.mCenter, node->mAABB.mExtents))      return;
00554 
00555         if(node->HasPosLeaf())
00556         {
00557                 SEGMENT_PRIM(node->GetPosPrimitive(), OPC_CONTACT)
00558         }
00559         else _SegmentStab(node->GetPos());
00560 
00561         if(ContactFound()) return;
00562 
00563         if(node->HasNegLeaf())
00564         {
00565                 SEGMENT_PRIM(node->GetNegPrimitive(), OPC_CONTACT)
00566         }
00567         else _SegmentStab(node->GetNeg());
00568 }
00569 
00571 
00575 
00576 void RayCollider::_SegmentStab(const AABBQuantizedNoLeafNode* node)
00577 {
00578         // Dequantize box
00579         const QuantizedAABB& Box = node->mAABB;
00580         const Point Center(float(Box.mCenter[0]) * mCenterCoeff.x, float(Box.mCenter[1]) * mCenterCoeff.y, float(Box.mCenter[2]) * mCenterCoeff.z);
00581         const Point Extents(float(Box.mExtents[0]) * mExtentsCoeff.x, float(Box.mExtents[1]) * mExtentsCoeff.y, float(Box.mExtents[2]) * mExtentsCoeff.z);
00582 
00583         // Perform Segment-AABB overlap test
00584         if(!SegmentAABBOverlap(Center, Extents))        return;
00585 
00586         if(node->HasPosLeaf())
00587         {
00588                 SEGMENT_PRIM(node->GetPosPrimitive(), OPC_CONTACT)
00589         }
00590         else _SegmentStab(node->GetPos());
00591 
00592         if(ContactFound()) return;
00593 
00594         if(node->HasNegLeaf())
00595         {
00596                 SEGMENT_PRIM(node->GetNegPrimitive(), OPC_CONTACT)
00597         }
00598         else _SegmentStab(node->GetNeg());
00599 }
00600 
00602 
00607 
00608 void RayCollider::_SegmentStab(const AABBTreeNode* node, Container& box_indices)
00609 {
00610         // Test the box against the segment
00611         Point Center, Extents;
00612         node->GetAABB()->GetCenter(Center);
00613         node->GetAABB()->GetExtents(Extents);
00614         if(!SegmentAABBOverlap(Center, Extents))        return;
00615 
00616         if(node->IsLeaf())
00617         {
00618                 box_indices.Add(node->GetPrimitives(), node->GetNbPrimitives());
00619         }
00620         else
00621         {
00622                 _SegmentStab(node->GetPos(), box_indices);
00623                 _SegmentStab(node->GetNeg(), box_indices);
00624         }
00625 }
00626 
00628 
00632 
00633 void RayCollider::_RayStab(const AABBCollisionNode* node)
00634 {
00635         // Perform Ray-AABB overlap test
00636         if(!RayAABBOverlap(node->mAABB.mCenter, node->mAABB.mExtents))  return;
00637 
00638         if(node->IsLeaf())
00639         {
00640                 RAY_PRIM(node->GetPrimitive(), OPC_CONTACT)
00641         }
00642         else
00643         {
00644                 _RayStab(node->GetPos());
00645 
00646                 if(ContactFound()) return;
00647 
00648                 _RayStab(node->GetNeg());
00649         }
00650 }
00651 
00653 
00657 
00658 void RayCollider::_RayStab(const AABBQuantizedNode* node)
00659 {
00660         // Dequantize box
00661         const QuantizedAABB& Box = node->mAABB;
00662         const Point Center(float(Box.mCenter[0]) * mCenterCoeff.x, float(Box.mCenter[1]) * mCenterCoeff.y, float(Box.mCenter[2]) * mCenterCoeff.z);
00663         const Point Extents(float(Box.mExtents[0]) * mExtentsCoeff.x, float(Box.mExtents[1]) * mExtentsCoeff.y, float(Box.mExtents[2]) * mExtentsCoeff.z);
00664 
00665         // Perform Ray-AABB overlap test
00666         if(!RayAABBOverlap(Center, Extents))    return;
00667 
00668         if(node->IsLeaf())
00669         {
00670                 RAY_PRIM(node->GetPrimitive(), OPC_CONTACT)
00671         }
00672         else
00673         {
00674                 _RayStab(node->GetPos());
00675 
00676                 if(ContactFound()) return;
00677 
00678                 _RayStab(node->GetNeg());
00679         }
00680 }
00681 
00683 
00687 
00688 void RayCollider::_RayStab(const AABBNoLeafNode* node)
00689 {
00690         // Perform Ray-AABB overlap test
00691         if(!RayAABBOverlap(node->mAABB.mCenter, node->mAABB.mExtents))  return;
00692 
00693         if(node->HasPosLeaf())
00694         {
00695                 RAY_PRIM(node->GetPosPrimitive(), OPC_CONTACT)
00696         }
00697         else _RayStab(node->GetPos());
00698 
00699         if(ContactFound()) return;
00700 
00701         if(node->HasNegLeaf())
00702         {
00703                 RAY_PRIM(node->GetNegPrimitive(), OPC_CONTACT)
00704         }
00705         else _RayStab(node->GetNeg());
00706 }
00707 
00709 
00713 
00714 void RayCollider::_RayStab(const AABBQuantizedNoLeafNode* node)
00715 {
00716         // Dequantize box
00717         const QuantizedAABB& Box = node->mAABB;
00718         const Point Center(float(Box.mCenter[0]) * mCenterCoeff.x, float(Box.mCenter[1]) * mCenterCoeff.y, float(Box.mCenter[2]) * mCenterCoeff.z);
00719         const Point Extents(float(Box.mExtents[0]) * mExtentsCoeff.x, float(Box.mExtents[1]) * mExtentsCoeff.y, float(Box.mExtents[2]) * mExtentsCoeff.z);
00720 
00721         // Perform Ray-AABB overlap test
00722         if(!RayAABBOverlap(Center, Extents))    return;
00723 
00724         if(node->HasPosLeaf())
00725         {
00726                 RAY_PRIM(node->GetPosPrimitive(), OPC_CONTACT)
00727         }
00728         else _RayStab(node->GetPos());
00729 
00730         if(ContactFound()) return;
00731 
00732         if(node->HasNegLeaf())
00733         {
00734                 RAY_PRIM(node->GetNegPrimitive(), OPC_CONTACT)
00735         }
00736         else _RayStab(node->GetNeg());
00737 }
00738 
00740 
00745 
00746 void RayCollider::_RayStab(const AABBTreeNode* node, Container& box_indices)
00747 {
00748         // Test the box against the ray
00749         Point Center, Extents;
00750         node->GetAABB()->GetCenter(Center);
00751         node->GetAABB()->GetExtents(Extents);
00752         if(!RayAABBOverlap(Center, Extents))    return;
00753 
00754         if(node->IsLeaf())
00755         {
00756                 mFlags |= OPC_CONTACT;
00757                 box_indices.Add(node->GetPrimitives(), node->GetNbPrimitives());
00758         }
00759         else
00760         {
00761                 _RayStab(node->GetPos(), box_indices);
00762                 _RayStab(node->GetNeg(), box_indices);
00763         }
00764 }


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