OPC_OptimizedTree.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 
00021 
00022 
00024 
00032 
00033 
00035 
00043 
00044 
00046 
00054 
00055 
00057 
00065 
00066 
00068 // Precompiled Header
00069 #include "Stdafx.h"
00070 
00071 using namespace Opcode;
00072 
00076 static bool gFixQuantized = true;
00077 
00079 
00098 
00099 static void _BuildCollisionTree(AABBCollisionNode* linear, const udword box_id, udword& current_id, const AABBTreeNode* current_node)
00100 {
00101         // Current node from input tree is "current_node". Must be flattened into "linear[boxid]".
00102 
00103         // Store the AABB
00104         current_node->GetAABB()->GetCenter(linear[box_id].mAABB.mCenter);
00105         current_node->GetAABB()->GetExtents(linear[box_id].mAABB.mExtents);
00106 #if 1 // Added by AIST
00107         linear[box_id].mAABB.CreateSSV();
00108 #endif
00109         // Store remaining info
00110         if(current_node->IsLeaf())
00111         {
00112                 // The input tree must be complete => i.e. one primitive/leaf
00113                 ASSERT(current_node->GetNbPrimitives()==1);
00114                 // Get the primitive index from the input tree
00115                 udword PrimitiveIndex = current_node->GetPrimitives()[0];
00116                 // Setup box data as the primitive index, marked as leaf
00117                 linear[box_id].mData = (PrimitiveIndex<<1)|1;
00118         }
00119         else
00120         {
00121                 // To make the negative one implicit, we must store P and N in successive order
00122                 udword PosID = current_id++;    // Get a new id for positive child
00123                 udword NegID = current_id++;    // Get a new id for negative child
00124                 // Setup box data as the forthcoming new P pointer
00125                 linear[box_id].mData = (EXWORD)&linear[PosID];
00126                 // Make sure it's not marked as leaf
00127                 ASSERT(!(linear[box_id].mData&1));
00128                 
00129                 //Modified by S-cubed, Inc.
00130                 //examine_normal_vector@InsertCollisionPair.cpp $B$G?F$rDI$($k$h$&$K$9$k(B
00131                 ((AABBCollisionNode*)linear[box_id].mData)->mB     = &linear[box_id];
00132                 (((AABBCollisionNode*)linear[box_id].mData)+1)->mB = &linear[box_id];
00133                 // Recurse with new IDs
00134                 _BuildCollisionTree(linear, PosID, current_id, current_node->GetPos());
00135                 _BuildCollisionTree(linear, NegID, current_id, current_node->GetNeg());
00136         }
00137 }
00138 
00140 
00157 
00158 static void _BuildNoLeafTree(AABBNoLeafNode* linear, const udword box_id, udword& current_id, const AABBTreeNode* current_node)
00159 {
00160         const AABBTreeNode* P = current_node->GetPos();
00161         const AABBTreeNode* N = current_node->GetNeg();
00162         // Leaf nodes here?!
00163         ASSERT(P);
00164         ASSERT(N);
00165         // Internal node => keep the box
00166         current_node->GetAABB()->GetCenter(linear[box_id].mAABB.mCenter);
00167         current_node->GetAABB()->GetExtents(linear[box_id].mAABB.mExtents);
00168 
00169         if(P->IsLeaf())
00170         {
00171                 // The input tree must be complete => i.e. one primitive/leaf
00172                 ASSERT(P->GetNbPrimitives()==1);
00173                 // Get the primitive index from the input tree
00174                 udword PrimitiveIndex = P->GetPrimitives()[0];
00175                 // Setup prev box data as the primitive index, marked as leaf
00176                 linear[box_id].mPosData = (PrimitiveIndex<<1)|1;
00177         }
00178         else
00179         {
00180                 // Get a new id for positive child
00181                 udword PosID = current_id++;
00182                 // Setup box data
00183                 linear[box_id].mPosData = (EXWORD)&linear[PosID];
00184                 // Make sure it's not marked as leaf
00185                 ASSERT(!(linear[box_id].mPosData&1));
00186                 // Recurse
00187                 _BuildNoLeafTree(linear, PosID, current_id, P);
00188         }
00189 
00190         if(N->IsLeaf())
00191         {
00192                 // The input tree must be complete => i.e. one primitive/leaf
00193                 ASSERT(N->GetNbPrimitives()==1);
00194                 // Get the primitive index from the input tree
00195                 udword PrimitiveIndex = N->GetPrimitives()[0];
00196                 // Setup prev box data as the primitive index, marked as leaf
00197                 linear[box_id].mNegData = (PrimitiveIndex<<1)|1;
00198         }
00199         else
00200         {
00201                 // Get a new id for negative child
00202                 udword NegID = current_id++;
00203                 // Setup box data
00204                 linear[box_id].mNegData = (EXWORD)&linear[NegID];
00205                 // Make sure it's not marked as leaf
00206                 ASSERT(!(linear[box_id].mNegData&1));
00207                 // Recurse
00208                 _BuildNoLeafTree(linear, NegID, current_id, N);
00209         }
00210 }
00211 
00213 
00216 
00217 AABBCollisionTree::AABBCollisionTree() : mNodes(null)
00218 {
00219 }
00220 
00222 
00225 
00226 AABBCollisionTree::~AABBCollisionTree()
00227 {
00228         DELETEARRAY(mNodes);
00229 }
00230 
00232 
00237 
00238 bool AABBCollisionTree::Build(AABBTree* tree)
00239 {
00240         // Checkings
00241         if(!tree)       return false;
00242         // Check the input tree is complete
00243         udword NbTriangles      = tree->GetNbPrimitives();
00244         udword NbNodes          = tree->GetNbNodes();
00245         if(NbNodes!=NbTriangles*2-1)    return false;
00246 
00247         // Get nodes
00248         if(mNbNodes!=NbNodes)   // Same number of nodes => keep moving
00249         {
00250                 mNbNodes = NbNodes;
00251                 DELETEARRAY(mNodes);
00252                 mNodes = new AABBCollisionNode[mNbNodes];
00253                 CHECKALLOC(mNodes);
00254         }
00255 
00256         // Build the tree
00257         udword CurID = 1;
00258 
00259         // Modified by S-cubed, Inc.
00260         //$B%k!<%H$N?F$O<+J,<+?H(B
00261         mNodes[0].mB = &mNodes[0];
00262 
00263         _BuildCollisionTree(mNodes, 0, CurID, tree);
00264         ASSERT(CurID==mNbNodes);
00265 
00266         return true;
00267 }
00268 
00270 
00275 
00276 bool AABBCollisionTree::Refit(const MeshInterface* mesh_interface)
00277 {
00278         ASSERT(!"Not implemented since AABBCollisionTrees have twice as more nodes to refit as AABBNoLeafTrees!");
00279         return false;
00280 }
00281 
00283 
00289 
00290 bool AABBCollisionTree::Walk(GenericWalkingCallback callback, void* user_data) const
00291 {
00292         if(!callback)   return false;
00293 
00294         struct Local
00295         {
00296                 static void _Walk(const AABBCollisionNode* current_node, GenericWalkingCallback callback, void* user_data)
00297                 {
00298                         if(!current_node || !(callback)(current_node, user_data))       return;
00299 
00300                         if(!current_node->IsLeaf())
00301                         {
00302                                 _Walk(current_node->GetPos(), callback, user_data);
00303                                 _Walk(current_node->GetNeg(), callback, user_data);
00304                         }
00305                 }
00306         };
00307         Local::_Walk(mNodes, callback, user_data);
00308         return true;
00309 }
00310 
00311 
00313 
00316 
00317 AABBNoLeafTree::AABBNoLeafTree() : mNodes(null)
00318 {
00319 }
00320 
00322 
00325 
00326 AABBNoLeafTree::~AABBNoLeafTree()
00327 {
00328         DELETEARRAY(mNodes);
00329 }
00330 
00332 
00337 
00338 bool AABBNoLeafTree::Build(AABBTree* tree)
00339 {
00340         // Checkings
00341         if(!tree)       return false;
00342         // Check the input tree is complete
00343         udword NbTriangles      = tree->GetNbPrimitives();
00344         udword NbNodes          = tree->GetNbNodes();
00345         if(NbNodes!=NbTriangles*2-1)    return false;
00346 
00347         // Get nodes
00348         if(mNbNodes!=NbTriangles-1)     // Same number of nodes => keep moving
00349         {
00350                 mNbNodes = NbTriangles-1;
00351                 DELETEARRAY(mNodes);
00352                 mNodes = new AABBNoLeafNode[mNbNodes];
00353                 CHECKALLOC(mNodes);
00354         }
00355 
00356         // Build the tree
00357         udword CurID = 1;
00358         _BuildNoLeafTree(mNodes, 0, CurID, tree);
00359         ASSERT(CurID==mNbNodes);
00360 
00361         return true;
00362 }
00363 
00364 inline_ void ComputeMinMax(Point& min, Point& max, const VertexPointers& vp)
00365 {
00366         // Compute triangle's AABB = a leaf box
00367 #ifdef OPC_USE_FCOMI    // a 15% speedup on my machine, not much
00368         min.x = FCMin3(vp.Vertex[0]->x, vp.Vertex[1]->x, vp.Vertex[2]->x);
00369         max.x = FCMax3(vp.Vertex[0]->x, vp.Vertex[1]->x, vp.Vertex[2]->x);
00370 
00371         min.y = FCMin3(vp.Vertex[0]->y, vp.Vertex[1]->y, vp.Vertex[2]->y);
00372         max.y = FCMax3(vp.Vertex[0]->y, vp.Vertex[1]->y, vp.Vertex[2]->y);
00373 
00374         min.z = FCMin3(vp.Vertex[0]->z, vp.Vertex[1]->z, vp.Vertex[2]->z);
00375         max.z = FCMax3(vp.Vertex[0]->z, vp.Vertex[1]->z, vp.Vertex[2]->z);
00376 #else
00377         min = *vp.Vertex[0];
00378         max = *vp.Vertex[0];
00379         min.Min(*vp.Vertex[1]);
00380         max.Max(*vp.Vertex[1]);
00381         min.Min(*vp.Vertex[2]);
00382         max.Max(*vp.Vertex[2]);
00383 #endif
00384 }
00385 
00387 
00392 
00393 bool AABBNoLeafTree::Refit(const MeshInterface* mesh_interface)
00394 {
00395         // Checkings
00396         if(!mesh_interface)     return false;
00397 
00398         // Bottom-up update
00399         VertexPointers VP;
00400         Point Min,Max;
00401         Point Min_,Max_;
00402         udword Index = mNbNodes;
00403         while(Index--)
00404         {
00405                 AABBNoLeafNode& Current = mNodes[Index];
00406 
00407                 if(Current.HasPosLeaf())
00408                 {
00409                         mesh_interface->GetTriangle(VP, Current.GetPosPrimitive());
00410                         ComputeMinMax(Min, Max, VP);
00411                 }
00412                 else
00413                 {
00414                         const CollisionAABB& CurrentBox = Current.GetPos()->mAABB;
00415                         CurrentBox.GetMin(Min);
00416                         CurrentBox.GetMax(Max);
00417                 }
00418 
00419                 if(Current.HasNegLeaf())
00420                 {
00421                         mesh_interface->GetTriangle(VP, Current.GetNegPrimitive());
00422                         ComputeMinMax(Min_, Max_, VP);
00423                 }
00424                 else
00425                 {
00426                         const CollisionAABB& CurrentBox = Current.GetNeg()->mAABB;
00427                         CurrentBox.GetMin(Min_);
00428                         CurrentBox.GetMax(Max_);
00429                 }
00430 #ifdef OPC_USE_FCOMI
00431                 Min.x = FCMin2(Min.x, Min_.x);
00432                 Max.x = FCMax2(Max.x, Max_.x);
00433                 Min.y = FCMin2(Min.y, Min_.y);
00434                 Max.y = FCMax2(Max.y, Max_.y);
00435                 Min.z = FCMin2(Min.z, Min_.z);
00436                 Max.z = FCMax2(Max.z, Max_.z);
00437 #else
00438                 Min.Min(Min_);
00439                 Max.Max(Max_);
00440 #endif
00441                 Current.mAABB.SetMinMax(Min, Max);
00442         }
00443         return true;
00444 }
00445 
00447 
00453 
00454 bool AABBNoLeafTree::Walk(GenericWalkingCallback callback, void* user_data) const
00455 {
00456         if(!callback)   return false;
00457 
00458         struct Local
00459         {
00460                 static void _Walk(const AABBNoLeafNode* current_node, GenericWalkingCallback callback, void* user_data)
00461                 {
00462                         if(!current_node || !(callback)(current_node, user_data))       return;
00463 
00464                         if(!current_node->HasPosLeaf()) _Walk(current_node->GetPos(), callback, user_data);
00465                         if(!current_node->HasNegLeaf()) _Walk(current_node->GetNeg(), callback, user_data);
00466                 }
00467         };
00468         Local::_Walk(mNodes, callback, user_data);
00469         return true;
00470 }
00471 
00472 // Quantization notes:
00473 // - We could use the highest bits of mData to store some more quantized bits. Dequantization code
00474 //   would be slightly more complex, but number of overlap tests would be reduced (and anyhow those
00475 //   bits are currently wasted). Of course it's not possible if we move to 16 bits mData.
00476 // - Something like "16 bits floats" could be tested, to bypass the int-to-float conversion.
00477 // - A dedicated BV-BV test could be used, dequantizing while testing for overlap. (i.e. it's some
00478 //   lazy-dequantization which may save some work in case of early exits). At the very least some
00479 //   muls could be saved by precomputing several more matrices. But maybe not worth the pain.
00480 // - Do we need to dequantize anyway? Not doing the extents-related muls only implies the box has
00481 //   been scaled, for example.
00482 // - The deeper we move into the hierarchy, the smaller the extents should be. May not need a fixed
00483 //   number of quantization bits. Even better, could probably be best delta-encoded.
00484 
00485 
00486 // Find max values. Some people asked why I wasn't simply using the first node. Well, I can't.
00487 // I'm not looking for (min, max) values like in a standard AABB, I'm looking for the extremal
00488 // centers/extents in order to quantize them. The first node would only give a single center and
00489 // a single extents. While extents would be the biggest, the center wouldn't.
00490 #define FIND_MAX_VALUES                                                                                                                                                 \
00491         /* Get max values */                                                                                                                                            \
00492         Point CMax(MIN_FLOAT, MIN_FLOAT, MIN_FLOAT);                                                                                            \
00493         Point EMax(MIN_FLOAT, MIN_FLOAT, MIN_FLOAT);                                                                                            \
00494         for(udword i=0;i<mNbNodes;i++)                                                                                                                          \
00495         {                                                                                                                                                                                       \
00496                 if(fabsf(Nodes[i].mAABB.mCenter.x)>CMax.x)      CMax.x = fabsf(Nodes[i].mAABB.mCenter.x);       \
00497                 if(fabsf(Nodes[i].mAABB.mCenter.y)>CMax.y)      CMax.y = fabsf(Nodes[i].mAABB.mCenter.y);       \
00498                 if(fabsf(Nodes[i].mAABB.mCenter.z)>CMax.z)      CMax.z = fabsf(Nodes[i].mAABB.mCenter.z);       \
00499                 if(fabsf(Nodes[i].mAABB.mExtents.x)>EMax.x)     EMax.x = fabsf(Nodes[i].mAABB.mExtents.x);      \
00500                 if(fabsf(Nodes[i].mAABB.mExtents.y)>EMax.y)     EMax.y = fabsf(Nodes[i].mAABB.mExtents.y);      \
00501                 if(fabsf(Nodes[i].mAABB.mExtents.z)>EMax.z)     EMax.z = fabsf(Nodes[i].mAABB.mExtents.z);      \
00502         }
00503 
00504 #define INIT_QUANTIZATION                                                                                                       \
00505         udword nbc=15;  /* Keep one bit for sign */                                                             \
00506         udword nbe=15;  /* Keep one bit for fix */                                                              \
00507         if(!gFixQuantized) nbe++;                                                                                               \
00508                                                                                                                                                         \
00509         /* Compute quantization coeffs */                                                                               \
00510         Point CQuantCoeff, EQuantCoeff;                                                                                 \
00511         CQuantCoeff.x = CMax.x!=0.0f ? float((1<<nbc)-1)/CMax.x : 0.0f;                 \
00512         CQuantCoeff.y = CMax.y!=0.0f ? float((1<<nbc)-1)/CMax.y : 0.0f;                 \
00513         CQuantCoeff.z = CMax.z!=0.0f ? float((1<<nbc)-1)/CMax.z : 0.0f;                 \
00514         EQuantCoeff.x = EMax.x!=0.0f ? float((1<<nbe)-1)/EMax.x : 0.0f;                 \
00515         EQuantCoeff.y = EMax.y!=0.0f ? float((1<<nbe)-1)/EMax.y : 0.0f;                 \
00516         EQuantCoeff.z = EMax.z!=0.0f ? float((1<<nbe)-1)/EMax.z : 0.0f;                 \
00517         /* Compute and save dequantization coeffs */                                                    \
00518         mCenterCoeff.x = CQuantCoeff.x!=0.0f ? 1.0f / CQuantCoeff.x : 0.0f;             \
00519         mCenterCoeff.y = CQuantCoeff.y!=0.0f ? 1.0f / CQuantCoeff.y : 0.0f;             \
00520         mCenterCoeff.z = CQuantCoeff.z!=0.0f ? 1.0f / CQuantCoeff.z : 0.0f;             \
00521         mExtentsCoeff.x = EQuantCoeff.x!=0.0f ? 1.0f / EQuantCoeff.x : 0.0f;    \
00522         mExtentsCoeff.y = EQuantCoeff.y!=0.0f ? 1.0f / EQuantCoeff.y : 0.0f;    \
00523         mExtentsCoeff.z = EQuantCoeff.z!=0.0f ? 1.0f / EQuantCoeff.z : 0.0f;    \
00524 
00525 #define PERFORM_QUANTIZATION                                                                                                            \
00526         /* Quantize */                                                                                                                                  \
00527         mNodes[i].mAABB.mCenter[0] = sword(Nodes[i].mAABB.mCenter.x * CQuantCoeff.x);   \
00528         mNodes[i].mAABB.mCenter[1] = sword(Nodes[i].mAABB.mCenter.y * CQuantCoeff.y);   \
00529         mNodes[i].mAABB.mCenter[2] = sword(Nodes[i].mAABB.mCenter.z * CQuantCoeff.z);   \
00530         mNodes[i].mAABB.mExtents[0] = uword(Nodes[i].mAABB.mExtents.x * EQuantCoeff.x); \
00531         mNodes[i].mAABB.mExtents[1] = uword(Nodes[i].mAABB.mExtents.y * EQuantCoeff.y); \
00532         mNodes[i].mAABB.mExtents[2] = uword(Nodes[i].mAABB.mExtents.z * EQuantCoeff.z); \
00533         /* Fix quantized boxes */                                                                                                               \
00534         if(gFixQuantized)                                                                                                                               \
00535         {                                                                                                                                                               \
00536                 /* Make sure the quantized box is still valid */                                                        \
00537                 Point Max = Nodes[i].mAABB.mCenter + Nodes[i].mAABB.mExtents;                           \
00538                 Point Min = Nodes[i].mAABB.mCenter - Nodes[i].mAABB.mExtents;                           \
00539                 /* For each axis */                                                                                                                     \
00540                 for(udword j=0;j<3;j++)                                                                                                         \
00541                 {       /* Dequantize the box center */                                                                                 \
00542                         float qc = float(mNodes[i].mAABB.mCenter[j]) * mCenterCoeff[j];                 \
00543                         bool FixMe=true;                                                                                                                \
00544                         do                                                                                                                                              \
00545                         {       /* Dequantize the box extent */                                                                         \
00546                                 float qe = float(mNodes[i].mAABB.mExtents[j]) * mExtentsCoeff[j];       \
00547                                 /* Compare real & dequantized values */                                                         \
00548                                 if(qc+qe<Max[j] || qc-qe>Min[j])        mNodes[i].mAABB.mExtents[j]++;  \
00549                                 else                                                            FixMe=false;                                    \
00550                                 /* Prevent wrapping */                                                                                          \
00551                                 if(!mNodes[i].mAABB.mExtents[j])                                                                        \
00552                                 {                                                                                                                                       \
00553                                         mNodes[i].mAABB.mExtents[j]=0xffff;                                                             \
00554                                         FixMe=false;                                                                                                    \
00555                                 }                                                                                                                                       \
00556                         }while(FixMe);                                                                                                                  \
00557                 }                                                                                                                                                       \
00558         }
00559 
00560 #if (defined __x86_64) || (defined __aarch64__)
00561 #define REMAP_DATA(member)                                                                                      \
00562         /* Fix data */                                                                                                  \
00563         Data = Nodes[i].member;                                                                                 \
00564         if(!(Data&1))                                                                                                   \
00565         {                                                                                                                               \
00566                 /* Compute box number */                                                                        \
00567                 uqword Nb = (Data - uqword(Nodes))/Nodes[i].GetNodeSize();      \
00568                 Data = uqword(&mNodes[Nb]);                                                                     \
00569         }                                                                                                                               \
00570         /* ...remapped */                                                                                               \
00571         mNodes[i].member = Data;
00572 #else
00573 #define REMAP_DATA(member)                                                                                      \
00574         /* Fix data */                                                                                                  \
00575         Data = Nodes[i].member;                                                                                 \
00576         if(!(Data&1))                                                                                                   \
00577         {                                                                                                                               \
00578                 /* Compute box number */                                                                        \
00579                 udword Nb = (Data - udword(Nodes))/Nodes[i].GetNodeSize();      \
00580                 Data = udword(&mNodes[Nb]);                                                                     \
00581         }                                                                                                                               \
00582         /* ...remapped */                                                                                               \
00583         mNodes[i].member = Data;
00584 #endif
00585 
00587 
00590 
00591 AABBQuantizedTree::AABBQuantizedTree() : mNodes(null)
00592 {
00593 }
00594 
00596 
00599 
00600 AABBQuantizedTree::~AABBQuantizedTree()
00601 {
00602         DELETEARRAY(mNodes);
00603 }
00604 
00606 
00611 
00612 bool AABBQuantizedTree::Build(AABBTree* tree)
00613 {
00614         // Checkings
00615         if(!tree)       return false;
00616         // Check the input tree is complete
00617         udword NbTriangles      = tree->GetNbPrimitives();
00618         udword NbNodes          = tree->GetNbNodes();
00619         if(NbNodes!=NbTriangles*2-1)    return false;
00620 
00621         // Get nodes
00622         mNbNodes = NbNodes;
00623         DELETEARRAY(mNodes);
00624         AABBCollisionNode* Nodes = new AABBCollisionNode[mNbNodes];
00625         CHECKALLOC(Nodes);
00626 
00627         // Build the tree
00628         udword CurID = 1;
00629         _BuildCollisionTree(Nodes, 0, CurID, tree);
00630 
00631         // Quantize
00632         {
00633                 mNodes = new AABBQuantizedNode[mNbNodes];
00634                 CHECKALLOC(mNodes);
00635 
00636                 // Get max values
00637                 FIND_MAX_VALUES
00638 
00639                 // Quantization
00640                 INIT_QUANTIZATION
00641 
00642                 // Quantize
00643                 EXWORD Data;
00644                 for(udword i=0;i<mNbNodes;i++)
00645                 {
00646                         PERFORM_QUANTIZATION
00647                         REMAP_DATA(mData)
00648                 }
00649 
00650                 DELETEARRAY(Nodes);
00651         }
00652 
00653         return true;
00654 }
00655 
00657 
00662 
00663 bool AABBQuantizedTree::Refit(const MeshInterface* mesh_interface)
00664 {
00665         ASSERT(!"Not implemented since requantizing is painful !");
00666         return false;
00667 }
00668 
00670 
00676 
00677 bool AABBQuantizedTree::Walk(GenericWalkingCallback callback, void* user_data) const
00678 {
00679         if(!callback)   return false;
00680 
00681         struct Local
00682         {
00683                 static void _Walk(const AABBQuantizedNode* current_node, GenericWalkingCallback callback, void* user_data)
00684                 {
00685                         if(!current_node || !(callback)(current_node, user_data))       return;
00686 
00687                         if(!current_node->IsLeaf())
00688                         {
00689                                 _Walk(current_node->GetPos(), callback, user_data);
00690                                 _Walk(current_node->GetNeg(), callback, user_data);
00691                         }
00692                 }
00693         };
00694         Local::_Walk(mNodes, callback, user_data);
00695         return true;
00696 }
00697 
00698 
00699 
00701 
00704 
00705 AABBQuantizedNoLeafTree::AABBQuantizedNoLeafTree() : mNodes(null)
00706 {
00707 }
00708 
00710 
00713 
00714 AABBQuantizedNoLeafTree::~AABBQuantizedNoLeafTree()
00715 {
00716         DELETEARRAY(mNodes);
00717 }
00718 
00720 
00725 
00726 bool AABBQuantizedNoLeafTree::Build(AABBTree* tree)
00727 {
00728         // Checkings
00729         if(!tree)       return false;
00730         // Check the input tree is complete
00731         udword NbTriangles      = tree->GetNbPrimitives();
00732         udword NbNodes          = tree->GetNbNodes();
00733         if(NbNodes!=NbTriangles*2-1)    return false;
00734 
00735         // Get nodes
00736         mNbNodes = NbTriangles-1;
00737         DELETEARRAY(mNodes);
00738         AABBNoLeafNode* Nodes = new AABBNoLeafNode[mNbNodes];
00739         CHECKALLOC(Nodes);
00740 
00741         // Build the tree
00742         udword CurID = 1;
00743         _BuildNoLeafTree(Nodes, 0, CurID, tree);
00744         ASSERT(CurID==mNbNodes);
00745 
00746         // Quantize
00747         {
00748                 mNodes = new AABBQuantizedNoLeafNode[mNbNodes];
00749                 CHECKALLOC(mNodes);
00750 
00751                 // Get max values
00752                 FIND_MAX_VALUES
00753 
00754                 // Quantization
00755                 INIT_QUANTIZATION
00756 
00757                 // Quantize
00758                 EXWORD Data;
00759                 for(udword i=0;i<mNbNodes;i++)
00760                 {
00761                         PERFORM_QUANTIZATION
00762                         REMAP_DATA(mPosData)
00763                         REMAP_DATA(mNegData)
00764                 }
00765 
00766                 DELETEARRAY(Nodes);
00767         }
00768 
00769         return true;
00770 }
00771 
00773 
00778 
00779 bool AABBQuantizedNoLeafTree::Refit(const MeshInterface* mesh_interface)
00780 {
00781         ASSERT(!"Not implemented since requantizing is painful !");
00782         return false;
00783 }
00784 
00786 
00792 
00793 bool AABBQuantizedNoLeafTree::Walk(GenericWalkingCallback callback, void* user_data) const
00794 {
00795         if(!callback)   return false;
00796 
00797         struct Local
00798         {
00799                 static void _Walk(const AABBQuantizedNoLeafNode* current_node, GenericWalkingCallback callback, void* user_data)
00800                 {
00801                         if(!current_node || !(callback)(current_node, user_data))       return;
00802 
00803                         if(!current_node->HasPosLeaf()) _Walk(current_node->GetPos(), callback, user_data);
00804                         if(!current_node->HasNegLeaf()) _Walk(current_node->GetNeg(), callback, user_data);
00805                 }
00806         };
00807         Local::_Walk(mNodes, callback, user_data);
00808         return true;
00809 }


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Sun Apr 2 2017 03:43:55