OPC_AABBTree.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 
00043 
00044 
00046 // Precompiled Header
00047 #include "Stdafx.h"
00048 
00049 using namespace Opcode;
00050 
00052 
00055 
00056 AABBTreeNode::AABBTreeNode() :
00057         mPos                    (null),
00058 #ifndef OPC_NO_NEG_VANILLA_TREE
00059         mNeg                    (null),
00060 #endif
00061         mNodePrimitives (null),
00062         mNbPrimitives   (0)
00063 {
00064 #ifdef OPC_USE_TREE_COHERENCE
00065         mBitmask = 0;
00066 #endif
00067 }
00068 
00070 
00073 
00074 AABBTreeNode::~AABBTreeNode()
00075 {
00076         // Opcode 1.3:
00077         const AABBTreeNode* Pos = GetPos();
00078 #ifndef OPC_NO_NEG_VANILLA_TREE
00079         const AABBTreeNode* Neg = GetNeg();
00080         if(!(mPos&1))   DELETESINGLE(Pos);
00081         if(!(mNeg&1))   DELETESINGLE(Neg);
00082 #else
00083         if(!(mPos&1))   DELETEARRAY(Pos);
00084 #endif
00085         mNodePrimitives = null; // This was just a shortcut to the global list => no release
00086         mNbPrimitives   = 0;
00087 }
00088 
00090 
00098 
00099 udword AABBTreeNode::Split(udword axis, AABBTreeBuilder* builder)
00100 {
00101         // Get node split value
00102         float SplitValue = builder->GetSplittingValue(mNodePrimitives, mNbPrimitives, mBV, axis);
00103 
00104         udword NbPos = 0;
00105         // Loop through all node-related primitives. Their indices range from mNodePrimitives[0] to mNodePrimitives[mNbPrimitives-1].
00106         // Those indices map the global list in the tree builder.
00107         for(udword i=0;i<mNbPrimitives;i++)
00108         {
00109                 // Get index in global list
00110                 udword Index = mNodePrimitives[i];
00111 
00112                 // Test against the splitting value. The primitive value is tested against the enclosing-box center.
00113                 // [We only need an approximate partition of the enclosing box here.]
00114                 float PrimitiveValue = builder->GetSplittingValue(Index, axis);
00115 
00116                 // Reorganize the list of indices in this order: positive - negative.
00117                 if(PrimitiveValue > SplitValue)
00118                 {
00119                         // Swap entries
00120                         udword Tmp = mNodePrimitives[i];
00121                         mNodePrimitives[i] = mNodePrimitives[NbPos];
00122                         mNodePrimitives[NbPos] = Tmp;
00123                         // Count primitives assigned to positive space
00124                         NbPos++;
00125                 }
00126         }
00127         return NbPos;
00128 }
00129 
00131 
00149 
00150 bool AABBTreeNode::Subdivide(AABBTreeBuilder* builder)
00151 {
00152         // Checkings
00153         if(!builder)    return false;
00154 
00155         // Stop subdividing if we reach a leaf node. This is always performed here,
00156         // else we could end in trouble if user overrides this.
00157         if(mNbPrimitives==1)    return true;
00158 
00159         // Let the user validate the subdivision
00160         if(!builder->ValidateSubdivision(mNodePrimitives, mNbPrimitives, mBV))  return true;
00161 
00162         bool ValidSplit = true; // Optimism...
00163         udword NbPos;
00164         if(builder->mSettings.mRules & SPLIT_LARGEST_AXIS)
00165         {
00166                 // Find the largest axis to split along
00167                 Point Extents;  mBV.GetExtents(Extents);        // Box extents
00168                 udword Axis     = Extents.LargestAxis();                // Index of largest axis
00169 
00170                 // Split along the axis
00171                 NbPos = Split(Axis, builder);
00172 
00173                 // Check split validity
00174                 if(!NbPos || NbPos==mNbPrimitives)      ValidSplit = false;
00175         }
00176         else if(builder->mSettings.mRules & SPLIT_SPLATTER_POINTS)
00177         {
00178                 // Compute the means
00179                 Point Means(0.0f, 0.0f, 0.0f);
00180                 for(udword i=0;i<mNbPrimitives;i++)
00181                 {
00182                         udword Index = mNodePrimitives[i];
00183                         Means.x+=builder->GetSplittingValue(Index, 0);
00184                         Means.y+=builder->GetSplittingValue(Index, 1);
00185                         Means.z+=builder->GetSplittingValue(Index, 2);
00186                 }
00187                 Means/=float(mNbPrimitives);
00188 
00189                 // Compute variances
00190                 Point Vars(0.0f, 0.0f, 0.0f);
00191                 for(udword i=0;i<mNbPrimitives;i++)
00192                 {
00193                         udword Index = mNodePrimitives[i];
00194                         volatile float Cx = builder->GetSplittingValue(Index, 0);
00195                         volatile float Cy = builder->GetSplittingValue(Index, 1);
00196                         volatile float Cz = builder->GetSplittingValue(Index, 2);
00197                         Vars.x += (Cx - Means.x)*(Cx - Means.x);
00198                         Vars.y += (Cy - Means.y)*(Cy - Means.y);
00199                         Vars.z += (Cz - Means.z)*(Cz - Means.z);
00200                 }
00201                 Vars/=float(mNbPrimitives-1);
00202 
00203                 // Choose axis with greatest variance
00204                 udword Axis = Vars.LargestAxis();
00205 
00206                 // Split along the axis
00207                 NbPos = Split(Axis, builder);
00208 
00209                 // Check split validity
00210                 if(!NbPos || NbPos==mNbPrimitives)      ValidSplit = false;
00211         }
00212         else if(builder->mSettings.mRules & SPLIT_BALANCED)
00213         {
00214                 // Test 3 axis, take the best
00215                 float Results[3];
00216                 NbPos = Split(0, builder);      Results[0] = float(NbPos)/float(mNbPrimitives);
00217                 NbPos = Split(1, builder);      Results[1] = float(NbPos)/float(mNbPrimitives);
00218                 NbPos = Split(2, builder);      Results[2] = float(NbPos)/float(mNbPrimitives);
00219                 Results[0]-=0.5f;       Results[0]*=Results[0];
00220                 Results[1]-=0.5f;       Results[1]*=Results[1];
00221                 Results[2]-=0.5f;       Results[2]*=Results[2];
00222                 udword Min=0;
00223                 if(Results[1]<Results[Min])     Min = 1;
00224                 if(Results[2]<Results[Min])     Min = 2;
00225                 
00226                 // Split along the axis
00227                 NbPos = Split(Min, builder);
00228 
00229                 // Check split validity
00230                 if(!NbPos || NbPos==mNbPrimitives)      ValidSplit = false;
00231         }
00232         else if(builder->mSettings.mRules & SPLIT_BEST_AXIS)
00233         {
00234                 // Test largest, then middle, then smallest axis...
00235 
00236                 // Sort axis
00237                 Point Extents;  mBV.GetExtents(Extents);        // Box extents
00238                 udword SortedAxis[] = { 0, 1, 2 };
00239                 float* Keys = (float*)&Extents.x;
00240                 for(udword j=0;j<3;j++)
00241                 {
00242                         for(udword i=0;i<2;i++)
00243                         {
00244                                 if(Keys[SortedAxis[i]]<Keys[SortedAxis[i+1]])
00245                                 {
00246                                         udword Tmp = SortedAxis[i];
00247                                         SortedAxis[i] = SortedAxis[i+1];
00248                                         SortedAxis[i+1] = Tmp;
00249                                 }
00250                         }
00251                 }
00252 
00253                 // Find the largest axis to split along
00254                 udword CurAxis = 0;
00255                 ValidSplit = false;
00256                 while(!ValidSplit && CurAxis!=3)
00257                 {
00258                         NbPos = Split(SortedAxis[CurAxis], builder);
00259                         // Check the subdivision has been successful
00260                         if(!NbPos || NbPos==mNbPrimitives)      CurAxis++;
00261                         else                                                            ValidSplit = true;
00262                 }
00263         }
00264         else if(builder->mSettings.mRules & SPLIT_FIFTY)
00265         {
00266                 // Don't even bother splitting (mainly a performance test)
00267                 NbPos = mNbPrimitives>>1;
00268         }
00269         else return false;      // Unknown splitting rules
00270 
00271         // Check the subdivision has been successful
00272         if(!ValidSplit)
00273         {
00274                 // Here, all boxes lie in the same sub-space. Two strategies:
00275                 // - if the tree *must* be complete, make an arbitrary 50-50 split
00276                 // - else stop subdividing
00277 //              if(builder->mSettings.mRules&SPLIT_COMPLETE)
00278                 if(builder->mSettings.mLimit==1)
00279                 {
00280                         builder->IncreaseNbInvalidSplits();
00281                         NbPos = mNbPrimitives>>1;
00282                 }
00283                 else return true;
00284         }
00285 
00286         // Now create children and assign their pointers.
00287         if(builder->mNodeBase)
00288         {
00289                 // We use a pre-allocated linear pool for complete trees [Opcode 1.3]
00290                 AABBTreeNode* Pool = (AABBTreeNode*)builder->mNodeBase;
00291                 udword Count = builder->GetCount() - 1; // Count begins to 1...
00292                 // Set last bit to tell it shouldn't be freed ### pretty ugly, find a better way. Maybe one bit in mNbPrimitives
00293                 ASSERT(!(udword(&Pool[Count+0])&1));
00294                 ASSERT(!(udword(&Pool[Count+1])&1));
00295                 mPos = EXWORD(&Pool[Count+0])|1;
00296 #ifndef OPC_NO_NEG_VANILLA_TREE
00297                 mNeg = EXWORD(&Pool[Count+1])|1;
00298 #endif
00299         }
00300         else
00301         {
00302                 // Non-complete trees and/or Opcode 1.2 allocate nodes on-the-fly
00303 #ifndef OPC_NO_NEG_VANILLA_TREE
00304                 mPos = (EXWORD)new AABBTreeNode;        CHECKALLOC(mPos);
00305                 mNeg = (EXWORD)new AABBTreeNode;        CHECKALLOC(mNeg);
00306 #else
00307                 AABBTreeNode* PosNeg = new AABBTreeNode[2];
00308                 CHECKALLOC(PosNeg);
00309                 mPos = (EXWORD)PosNeg;
00310 #endif
00311         }
00312 
00313         // Update stats
00314         builder->IncreaseCount(2);
00315 
00316         // Assign children
00317         AABBTreeNode* Pos = (AABBTreeNode*)GetPos();
00318         AABBTreeNode* Neg = (AABBTreeNode*)GetNeg();
00319         Pos->mNodePrimitives    = &mNodePrimitives[0];
00320         Pos->mNbPrimitives              = NbPos;
00321         Neg->mNodePrimitives    = &mNodePrimitives[NbPos];
00322         Neg->mNbPrimitives              = mNbPrimitives - NbPos;
00323 
00324         return true;
00325 }
00326 
00328 
00332 
00333 void AABBTreeNode::_BuildHierarchy(AABBTreeBuilder* builder)
00334 {
00335         // 1) Compute the global box for current node. The box is stored in mBV.
00336         builder->ComputeGlobalBox(mNodePrimitives, mNbPrimitives, mBV);
00337 
00338         // 2) Subdivide current node
00339         Subdivide(builder);
00340 
00341         // 3) Recurse
00342         AABBTreeNode* Pos = (AABBTreeNode*)GetPos();
00343         AABBTreeNode* Neg = (AABBTreeNode*)GetNeg();
00344         if(Pos) Pos->_BuildHierarchy(builder);
00345         if(Neg) Neg->_BuildHierarchy(builder);
00346 }
00347 
00349 
00353 
00354 void AABBTreeNode::_Refit(AABBTreeBuilder* builder)
00355 {
00356         // 1) Recompute the new global box for current node
00357         builder->ComputeGlobalBox(mNodePrimitives, mNbPrimitives, mBV);
00358 
00359         // 2) Recurse
00360         AABBTreeNode* Pos = (AABBTreeNode*)GetPos();
00361         AABBTreeNode* Neg = (AABBTreeNode*)GetNeg();
00362         if(Pos) Pos->_Refit(builder);
00363         if(Neg) Neg->_Refit(builder);
00364 }
00365 
00366 
00367 
00369 
00372 
00373 AABBTree::AABBTree() : mIndices(null), mPool(null), mTotalNbNodes(0)
00374 {
00375 }
00376 
00378 
00381 
00382 AABBTree::~AABBTree()
00383 {
00384         Release();
00385 }
00386 
00388 
00391 
00392 void AABBTree::Release()
00393 {
00394         DELETEARRAY(mPool);
00395         DELETEARRAY(mIndices);
00396 }
00397 
00399 
00404 
00405 bool AABBTree::Build(AABBTreeBuilder* builder)
00406 {
00407         // Checkings
00408         if(!builder || !builder->mNbPrimitives) return false;
00409 
00410         // Release previous tree
00411         Release();
00412 
00413         // Init stats
00414         builder->SetCount(1);
00415         builder->SetNbInvalidSplits(0);
00416 
00417         // Initialize indices. This list will be modified during build.
00418         mIndices = new udword[builder->mNbPrimitives];
00419         CHECKALLOC(mIndices);
00420         // Identity permutation
00421         for(udword i=0;i<builder->mNbPrimitives;i++)    mIndices[i] = i;
00422 
00423         // Setup initial node. Here we have a complete permutation of the app's primitives.
00424         mNodePrimitives = mIndices;
00425         mNbPrimitives   = builder->mNbPrimitives;
00426 
00427         // Use a linear array for complete trees (since we can predict the final number of nodes) [Opcode 1.3]
00428 //      if(builder->mRules&SPLIT_COMPLETE)
00429         if(builder->mSettings.mLimit==1)
00430         {
00431                 // Allocate a pool of nodes
00432                 mPool = new AABBTreeNode[builder->mNbPrimitives*2 - 1];
00433 
00434                 builder->mNodeBase = mPool;     // ### ugly !
00435         }
00436 
00437         // Build the hierarchy
00438         _BuildHierarchy(builder);
00439 
00440         // Get back total number of nodes
00441         mTotalNbNodes   = builder->GetCount();
00442 
00443         // For complete trees, check the correct number of nodes has been created [Opcode 1.3]
00444         if(mPool)       ASSERT(mTotalNbNodes==builder->mNbPrimitives*2 - 1);
00445 
00446         return true;
00447 }
00448 
00450 
00455 
00456 udword AABBTree::ComputeDepth() const
00457 {
00458         return Walk(null, null);        // Use the walking code without callback
00459 }
00460 
00462 
00465 
00466 udword AABBTree::Walk(WalkingCallback callback, void* user_data) const
00467 {
00468         // Call it without callback to compute max depth
00469         udword MaxDepth = 0;
00470         udword CurrentDepth = 0;
00471 
00472         struct Local
00473         {
00474                 static void _Walk(const AABBTreeNode* current_node, udword& max_depth, udword& current_depth, WalkingCallback callback, void* user_data)
00475                 {
00476                         // Checkings
00477                         if(!current_node)       return;
00478                         // Entering a new node => increase depth
00479                         current_depth++;
00480                         // Keep track of max depth
00481                         if(current_depth>max_depth)     max_depth = current_depth;
00482 
00483                         // Callback
00484                         if(callback && !(callback)(current_node, current_depth, user_data))     return;
00485 
00486                         // Recurse
00487                         if(current_node->GetPos())      { _Walk(current_node->GetPos(), max_depth, current_depth, callback, user_data); current_depth--;        }
00488                         if(current_node->GetNeg())      { _Walk(current_node->GetNeg(), max_depth, current_depth, callback, user_data); current_depth--;        }
00489                 }
00490         };
00491         Local::_Walk(this, MaxDepth, CurrentDepth, callback, user_data);
00492         return MaxDepth;
00493 }
00494 
00496 
00500 
00501 bool AABBTree::Refit(AABBTreeBuilder* builder)
00502 {
00503         if(!builder)    return false;
00504         _Refit(builder);
00505         return true;
00506 }
00507 
00509 
00513 
00514 bool AABBTree::Refit2(AABBTreeBuilder* builder)
00515 {
00516         // Checkings
00517         if(!builder)    return false;
00518 
00519         ASSERT(mPool);
00520 
00521         // Bottom-up update
00522         Point Min,Max;
00523         Point Min_,Max_;
00524         udword Index = mTotalNbNodes;
00525         while(Index--)
00526         {
00527                 AABBTreeNode& Current = mPool[Index];
00528 
00529                 if(Current.IsLeaf())
00530                 {
00531                         builder->ComputeGlobalBox(Current.GetPrimitives(), Current.GetNbPrimitives(), *(AABB*)Current.GetAABB());
00532                 }
00533                 else
00534                 {
00535                         Current.GetPos()->GetAABB()->GetMin(Min);
00536                         Current.GetPos()->GetAABB()->GetMax(Max);
00537 
00538                         Current.GetNeg()->GetAABB()->GetMin(Min_);
00539                         Current.GetNeg()->GetAABB()->GetMax(Max_);
00540 
00541                         Min.Min(Min_);
00542                         Max.Max(Max_);
00543 
00544                         ((AABB*)Current.GetAABB())->SetMinMax(Min, Max);
00545                 }
00546         }
00547         return true;
00548 }
00549 
00551 
00555 
00556 udword AABBTree::GetUsedBytes() const
00557 {
00558         udword TotalSize = mTotalNbNodes*GetNodeSize();
00559         if(mIndices)    TotalSize+=mNbPrimitives*sizeof(udword);
00560         return TotalSize;
00561 }
00562 
00564 
00569 
00570 bool AABBTree::IsComplete() const
00571 {
00572         return (GetNbNodes()==GetNbPrimitives()*2-1);
00573 }


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