OPC_AABBTree.cpp
Go to the documentation of this file.
1 /*
3  * OPCODE - Optimized Collision Detection
4  * Copyright (C) 2001 Pierre Terdiman
5  * Homepage: http://www.codercorner.com/Opcode.htm
6  */
8 
10 
16 
19 
27 
30 
43 
46 // Precompiled Header
47 #include "Stdafx.h"
48 
49 using namespace Opcode;
50 
52 
55 AABBTreeNode::AABBTreeNode() :
57  mPos (null),
59  mNeg (null),
60 #endif
61  mNodePrimitives (null),
62  mNbPrimitives (0)
63 {
64 #ifdef OPC_USE_TREE_COHERENCE
65  mBitmask = 0;
66 #endif
67 }
68 
70 
73 AABBTreeNode::~AABBTreeNode()
75 {
76  // Opcode 1.3:
77  const AABBTreeNode* Pos = GetPos();
78 #ifndef OPC_NO_NEG_VANILLA_TREE
79  const AABBTreeNode* Neg = GetNeg();
80  if(!(mPos&1)) DELETESINGLE(Pos);
81  if(!(mNeg&1)) DELETESINGLE(Neg);
82 #else
83  if(!(mPos&1)) DELETEARRAY(Pos);
84 #endif
85  mNodePrimitives = null; // This was just a shortcut to the global list => no release
86  mNbPrimitives = 0;
87 }
88 
90 
100 {
101  // Get node split value
102  float SplitValue = builder->GetSplittingValue(mNodePrimitives, mNbPrimitives, mBV, axis);
103 
104  udword NbPos = 0;
105  // Loop through all node-related primitives. Their indices range from mNodePrimitives[0] to mNodePrimitives[mNbPrimitives-1].
106  // Those indices map the global list in the tree builder.
107  for(udword i=0;i<mNbPrimitives;i++)
108  {
109  // Get index in global list
110  udword Index = mNodePrimitives[i];
111 
112  // Test against the splitting value. The primitive value is tested against the enclosing-box center.
113  // [We only need an approximate partition of the enclosing box here.]
114  float PrimitiveValue = builder->GetSplittingValue(Index, axis);
115 
116  // Reorganize the list of indices in this order: positive - negative.
117  if(PrimitiveValue > SplitValue)
118  {
119  // Swap entries
120  udword Tmp = mNodePrimitives[i];
121  mNodePrimitives[i] = mNodePrimitives[NbPos];
122  mNodePrimitives[NbPos] = Tmp;
123  // Count primitives assigned to positive space
124  NbPos++;
125  }
126  }
127  return NbPos;
128 }
129 
131 
151 {
152  // Checkings
153  if(!builder) return false;
154 
155  // Stop subdividing if we reach a leaf node. This is always performed here,
156  // else we could end in trouble if user overrides this.
157  if(mNbPrimitives==1) return true;
158 
159  // Let the user validate the subdivision
160  if(!builder->ValidateSubdivision(mNodePrimitives, mNbPrimitives, mBV)) return true;
161 
162  bool ValidSplit = true; // Optimism...
163  udword NbPos;
164  if(builder->mSettings.mRules & SPLIT_LARGEST_AXIS)
165  {
166  // Find the largest axis to split along
167  Point Extents; mBV.GetExtents(Extents); // Box extents
168  udword Axis = Extents.LargestAxis(); // Index of largest axis
169 
170  // Split along the axis
171  NbPos = Split(Axis, builder);
172 
173  // Check split validity
174  if(!NbPos || NbPos==mNbPrimitives) ValidSplit = false;
175  }
176  else if(builder->mSettings.mRules & SPLIT_SPLATTER_POINTS)
177  {
178  // Compute the means
179  Point Means(0.0f, 0.0f, 0.0f);
180  for(udword i=0;i<mNbPrimitives;i++)
181  {
182  udword Index = mNodePrimitives[i];
183  Means.x+=builder->GetSplittingValue(Index, 0);
184  Means.y+=builder->GetSplittingValue(Index, 1);
185  Means.z+=builder->GetSplittingValue(Index, 2);
186  }
187  Means/=float(mNbPrimitives);
188 
189  // Compute variances
190  Point Vars(0.0f, 0.0f, 0.0f);
191  for(udword i=0;i<mNbPrimitives;i++)
192  {
193  udword Index = mNodePrimitives[i];
194  volatile float Cx = builder->GetSplittingValue(Index, 0);
195  volatile float Cy = builder->GetSplittingValue(Index, 1);
196  volatile float Cz = builder->GetSplittingValue(Index, 2);
197  Vars.x += (Cx - Means.x)*(Cx - Means.x);
198  Vars.y += (Cy - Means.y)*(Cy - Means.y);
199  Vars.z += (Cz - Means.z)*(Cz - Means.z);
200  }
201  Vars/=float(mNbPrimitives-1);
202 
203  // Choose axis with greatest variance
204  udword Axis = Vars.LargestAxis();
205 
206  // Split along the axis
207  NbPos = Split(Axis, builder);
208 
209  // Check split validity
210  if(!NbPos || NbPos==mNbPrimitives) ValidSplit = false;
211  }
212  else if(builder->mSettings.mRules & SPLIT_BALANCED)
213  {
214  // Test 3 axis, take the best
215  float Results[3];
216  NbPos = Split(0, builder); Results[0] = float(NbPos)/float(mNbPrimitives);
217  NbPos = Split(1, builder); Results[1] = float(NbPos)/float(mNbPrimitives);
218  NbPos = Split(2, builder); Results[2] = float(NbPos)/float(mNbPrimitives);
219  Results[0]-=0.5f; Results[0]*=Results[0];
220  Results[1]-=0.5f; Results[1]*=Results[1];
221  Results[2]-=0.5f; Results[2]*=Results[2];
222  udword Min=0;
223  if(Results[1]<Results[Min]) Min = 1;
224  if(Results[2]<Results[Min]) Min = 2;
225 
226  // Split along the axis
227  NbPos = Split(Min, builder);
228 
229  // Check split validity
230  if(!NbPos || NbPos==mNbPrimitives) ValidSplit = false;
231  }
232  else if(builder->mSettings.mRules & SPLIT_BEST_AXIS)
233  {
234  // Test largest, then middle, then smallest axis...
235 
236  // Sort axis
237  Point Extents; mBV.GetExtents(Extents); // Box extents
238  udword SortedAxis[] = { 0, 1, 2 };
239  float* Keys = (float*)&Extents.x;
240  for(udword j=0;j<3;j++)
241  {
242  for(udword i=0;i<2;i++)
243  {
244  if(Keys[SortedAxis[i]]<Keys[SortedAxis[i+1]])
245  {
246  udword Tmp = SortedAxis[i];
247  SortedAxis[i] = SortedAxis[i+1];
248  SortedAxis[i+1] = Tmp;
249  }
250  }
251  }
252 
253  // Find the largest axis to split along
254  udword CurAxis = 0;
255  ValidSplit = false;
256  while(!ValidSplit && CurAxis!=3)
257  {
258  NbPos = Split(SortedAxis[CurAxis], builder);
259  // Check the subdivision has been successful
260  if(!NbPos || NbPos==mNbPrimitives) CurAxis++;
261  else ValidSplit = true;
262  }
263  }
264  else if(builder->mSettings.mRules & SPLIT_FIFTY)
265  {
266  // Don't even bother splitting (mainly a performance test)
267  NbPos = mNbPrimitives>>1;
268  }
269  else return false; // Unknown splitting rules
270 
271  // Check the subdivision has been successful
272  if(!ValidSplit)
273  {
274  // Here, all boxes lie in the same sub-space. Two strategies:
275  // - if the tree *must* be complete, make an arbitrary 50-50 split
276  // - else stop subdividing
277 // if(builder->mSettings.mRules&SPLIT_COMPLETE)
278  if(builder->mSettings.mLimit==1)
279  {
280  builder->IncreaseNbInvalidSplits();
281  NbPos = mNbPrimitives>>1;
282  }
283  else return true;
284  }
285 
286  // Now create children and assign their pointers.
287  if(builder->mNodeBase)
288  {
289  // We use a pre-allocated linear pool for complete trees [Opcode 1.3]
290  AABBTreeNode* Pool = (AABBTreeNode*)builder->mNodeBase;
291  udword Count = builder->GetCount() - 1; // Count begins to 1...
292  // Set last bit to tell it shouldn't be freed ### pretty ugly, find a better way. Maybe one bit in mNbPrimitives
293  ASSERT(!(udword(&Pool[Count+0])&1));
294  ASSERT(!(udword(&Pool[Count+1])&1));
295  mPos = EXWORD(&Pool[Count+0])|1;
296 #ifndef OPC_NO_NEG_VANILLA_TREE
297  mNeg = EXWORD(&Pool[Count+1])|1;
298 #endif
299  }
300  else
301  {
302  // Non-complete trees and/or Opcode 1.2 allocate nodes on-the-fly
303 #ifndef OPC_NO_NEG_VANILLA_TREE
304  mPos = (EXWORD)new AABBTreeNode; CHECKALLOC(mPos);
305  mNeg = (EXWORD)new AABBTreeNode; CHECKALLOC(mNeg);
306 #else
307  AABBTreeNode* PosNeg = new AABBTreeNode[2];
308  CHECKALLOC(PosNeg);
309  mPos = (EXWORD)PosNeg;
310 #endif
311  }
312 
313  // Update stats
314  builder->IncreaseCount(2);
315 
316  // Assign children
317  AABBTreeNode* Pos = (AABBTreeNode*)GetPos();
318  AABBTreeNode* Neg = (AABBTreeNode*)GetNeg();
319  Pos->mNodePrimitives = &mNodePrimitives[0];
320  Pos->mNbPrimitives = NbPos;
321  Neg->mNodePrimitives = &mNodePrimitives[NbPos];
322  Neg->mNbPrimitives = mNbPrimitives - NbPos;
323 
324  return true;
325 }
326 
328 
334 {
335  // 1) Compute the global box for current node. The box is stored in mBV.
336  builder->ComputeGlobalBox(mNodePrimitives, mNbPrimitives, mBV);
337 
338  // 2) Subdivide current node
339  Subdivide(builder);
340 
341  // 3) Recurse
342  AABBTreeNode* Pos = (AABBTreeNode*)GetPos();
343  AABBTreeNode* Neg = (AABBTreeNode*)GetNeg();
344  if(Pos) Pos->_BuildHierarchy(builder);
345  if(Neg) Neg->_BuildHierarchy(builder);
346 }
347 
349 
355 {
356  // 1) Recompute the new global box for current node
357  builder->ComputeGlobalBox(mNodePrimitives, mNbPrimitives, mBV);
358 
359  // 2) Recurse
360  AABBTreeNode* Pos = (AABBTreeNode*)GetPos();
361  AABBTreeNode* Neg = (AABBTreeNode*)GetNeg();
362  if(Pos) Pos->_Refit(builder);
363  if(Neg) Neg->_Refit(builder);
364 }
365 
366 
367 
369 
372 AABBTree::AABBTree() : mIndices(null), mPool(null), mTotalNbNodes(0)
374 {
375 }
376 
378 
383 {
384  Release();
385 }
386 
388 
391 void AABBTree::Release()
393 {
394  DELETEARRAY(mPool);
395  DELETEARRAY(mIndices);
396 }
397 
399 
404 bool AABBTree::Build(AABBTreeBuilder* builder)
406 {
407  // Checkings
408  if(!builder || !builder->mNbPrimitives) return false;
409 
410  // Release previous tree
411  Release();
412 
413  // Init stats
414  builder->SetCount(1);
415  builder->SetNbInvalidSplits(0);
416 
417  // Initialize indices. This list will be modified during build.
418  mIndices = new udword[builder->mNbPrimitives];
419  CHECKALLOC(mIndices);
420  // Identity permutation
421  for(udword i=0;i<builder->mNbPrimitives;i++) mIndices[i] = i;
422 
423  // Setup initial node. Here we have a complete permutation of the app's primitives.
424  mNodePrimitives = mIndices;
425  mNbPrimitives = builder->mNbPrimitives;
426 
427  // Use a linear array for complete trees (since we can predict the final number of nodes) [Opcode 1.3]
428 // if(builder->mRules&SPLIT_COMPLETE)
429  if(builder->mSettings.mLimit==1)
430  {
431  // Allocate a pool of nodes
432  mPool = new AABBTreeNode[builder->mNbPrimitives*2 - 1];
433 
434  builder->mNodeBase = mPool; // ### ugly !
435  }
436 
437  // Build the hierarchy
438  _BuildHierarchy(builder);
439 
440  // Get back total number of nodes
441  mTotalNbNodes = builder->GetCount();
442 
443  // For complete trees, check the correct number of nodes has been created [Opcode 1.3]
444  if(mPool) ASSERT(mTotalNbNodes==builder->mNbPrimitives*2 - 1);
445 
446  return true;
447 }
448 
450 
457 {
458  return Walk(null, null); // Use the walking code without callback
459 }
460 
462 
465 udword AABBTree::Walk(WalkingCallback callback, void* user_data) const
467 {
468  // Call it without callback to compute max depth
469  udword MaxDepth = 0;
470  udword CurrentDepth = 0;
471 
472  struct Local
473  {
474  static void _Walk(const AABBTreeNode* current_node, udword& max_depth, udword& current_depth, WalkingCallback callback, void* user_data)
475  {
476  // Checkings
477  if(!current_node) return;
478  // Entering a new node => increase depth
479  current_depth++;
480  // Keep track of max depth
481  if(current_depth>max_depth) max_depth = current_depth;
482 
483  // Callback
484  if(callback && !(callback)(current_node, current_depth, user_data)) return;
485 
486  // Recurse
487  if(current_node->GetPos()) { _Walk(current_node->GetPos(), max_depth, current_depth, callback, user_data); current_depth--; }
488  if(current_node->GetNeg()) { _Walk(current_node->GetNeg(), max_depth, current_depth, callback, user_data); current_depth--; }
489  }
490  };
491  Local::_Walk(this, MaxDepth, CurrentDepth, callback, user_data);
492  return MaxDepth;
493 }
494 
496 
500 bool AABBTree::Refit(AABBTreeBuilder* builder)
502 {
503  if(!builder) return false;
504  _Refit(builder);
505  return true;
506 }
507 
509 
513 bool AABBTree::Refit2(AABBTreeBuilder* builder)
515 {
516  // Checkings
517  if(!builder) return false;
518 
519  ASSERT(mPool);
520 
521  // Bottom-up update
522  Point Min,Max;
523  Point Min_,Max_;
524  udword Index = mTotalNbNodes;
525  while(Index--)
526  {
527  AABBTreeNode& Current = mPool[Index];
528 
529  if(Current.IsLeaf())
530  {
531  builder->ComputeGlobalBox(Current.GetPrimitives(), Current.GetNbPrimitives(), *(AABB*)Current.GetAABB());
532  }
533  else
534  {
535  Current.GetPos()->GetAABB()->GetMin(Min);
536  Current.GetPos()->GetAABB()->GetMax(Max);
537 
538  Current.GetNeg()->GetAABB()->GetMin(Min_);
539  Current.GetNeg()->GetAABB()->GetMax(Max_);
540 
541  Min.Min(Min_);
542  Max.Max(Max_);
543 
544  ((AABB*)Current.GetAABB())->SetMinMax(Min, Max);
545  }
546  }
547  return true;
548 }
549 
551 
557 {
558  udword TotalSize = mTotalNbNodes*GetNodeSize();
559  if(mIndices) TotalSize+=mNbPrimitives*sizeof(udword);
560  return TotalSize;
561 }
562 
564 
569 bool AABBTree::IsComplete() const
571 {
572  return (GetNbNodes()==GetNbPrimitives()*2-1);
573 }
AABBTreeBuilder::SetNbInvalidSplits
inline_ void SetNbInvalidSplits(udword nb)
Definition: OPC_TreeBuilders.h:120
AABBTree::~AABBTree
~AABBTree()
Definition: OPC_AABBTree.cpp:382
Pos
ush Pos
Definition: deflate.h:86
CHECKALLOC
#define CHECKALLOC(x)
Definition: IceMemoryMacros.h:103
i
png_uint_32 i
Definition: png.h:2732
AABBTree::AABBTree
AABBTree()
Definition: OPC_AABBTree.cpp:373
AABBTree::Build
bool Build(AABBTreeBuilder *builder)
Definition: OPC_AABBTree.cpp:405
AABBTreeNode::GetPrimitives
const inline_ udword * GetPrimitives() const
Definition: OPC_AABBTree.h:88
ASSERT
#define ASSERT(exp)
Definition: OPC_IceHook.h:24
DELETESINGLE
#define DELETESINGLE(x)
Deletes an instance of a class.
Definition: IceMemoryMacros.h:95
AABBTreeNode::_BuildHierarchy
void _BuildHierarchy(AABBTreeBuilder *builder)
Definition: OPC_AABBTree.cpp:333
AABBTree::ComputeDepth
udword ComputeDepth() const
Definition: OPC_AABBTree.cpp:456
AABBTreeBuilder::mNbPrimitives
udword mNbPrimitives
Total number of primitives.
Definition: OPC_TreeBuilders.h:114
AABBTree::Refit2
bool Refit2(AABBTreeBuilder *builder)
Definition: OPC_AABBTree.cpp:514
SPLIT_BALANCED
@ SPLIT_BALANCED
Try to keep a well-balanced tree.
Definition: OPC_TreeBuilders.h:30
udword
unsigned int udword
sizeof(udword) must be 4
Definition: IceTypes.h:65
SPLIT_FIFTY
@ SPLIT_FIFTY
Arbitrary 50-50 split.
Definition: OPC_TreeBuilders.h:31
AABBTree::Release
void Release()
Definition: OPC_AABBTree.cpp:392
AABBTreeNode::GetNbPrimitives
inline_ udword GetNbPrimitives() const
Definition: OPC_AABBTree.h:89
AABBTree::IsComplete
bool IsComplete() const
Definition: OPC_AABBTree.cpp:570
swingTest.f
f
Definition: swingTest.py:6
AABBTreeBuilder::mNodeBase
void * mNodeBase
Address of node pool [Opcode 1.3].
Definition: OPC_TreeBuilders.h:115
AABBTreeNode::Split
udword Split(udword axis, AABBTreeBuilder *builder)
Definition: OPC_AABBTree.cpp:99
OPC_NO_NEG_VANILLA_TREE
#define OPC_NO_NEG_VANILLA_TREE
Use tree-coherence or not [not implemented yet].
Definition: OPC_Settings.h:42
AABBTreeBuilder::GetCount
inline_ udword GetCount() const
Definition: OPC_TreeBuilders.h:119
AABBTreeBuilder::GetSplittingValue
virtual float GetSplittingValue(udword index, udword axis) const =0
AABBTreeNode::mNodePrimitives
udword * mNodePrimitives
Node-related primitives (shortcut to a position in mIndices below)
Definition: OPC_AABBTree.h:93
AABBTreeNode
Definition: OPC_AABBTree.h:83
AABBTreeBuilder::IncreaseCount
inline_ void IncreaseCount(udword nb)
Definition: OPC_TreeBuilders.h:118
SPLIT_LARGEST_AXIS
@ SPLIT_LARGEST_AXIS
Split along the largest axis.
Definition: OPC_TreeBuilders.h:27
Stdafx.h
AABBTreeNode::_Refit
void _Refit(AABBTreeBuilder *builder)
Definition: OPC_AABBTree.cpp:354
AABBTreeBuilder::ComputeGlobalBox
virtual bool ComputeGlobalBox(const udword *primitives, udword nb_prims, AABB &global_box) const =0
AABBTreeBuilder::mSettings
BuildSettings mSettings
Splitting rules & split limit [Opcode 1.3].
Definition: OPC_TreeBuilders.h:113
AABBTreeBuilder::ValidateSubdivision
virtual BOOL ValidateSubdivision(const udword *primitives, udword nb_prims, const AABB &global_box)
Definition: OPC_TreeBuilders.h:105
SPLIT_SPLATTER_POINTS
@ SPLIT_SPLATTER_POINTS
Splatter primitive centers (QuickCD-style)
Definition: OPC_TreeBuilders.h:28
EXWORD
#define EXWORD
Definition: OPC_AABBTree.h:26
AABBTree::Refit
bool Refit(AABBTreeBuilder *builder)
Definition: OPC_AABBTree.cpp:501
AABBTreeNode::mNbPrimitives
udword mNbPrimitives
Number of primitives for this node.
Definition: OPC_AABBTree.h:94
AABBTreeBuilder::IncreaseNbInvalidSplits
inline_ void IncreaseNbInvalidSplits()
Definition: OPC_TreeBuilders.h:121
AABBTreeNode::Subdivide
bool Subdivide(AABBTreeBuilder *builder)
Definition: OPC_AABBTree.cpp:150
BuildSettings::mRules
udword mRules
Building/Splitting rules (a combination of SplittingRules flags)
Definition: OPC_TreeBuilders.h:44
WalkingCallback
bool(* WalkingCallback)(const AABBTreeNode *current, udword depth, void *user_data)
Definition: OPC_AABBTree.h:111
AABBTreeBuilder
Definition: OPC_TreeBuilders.h:47
AABBTree::Walk
udword Walk(WalkingCallback callback, void *user_data) const
Definition: OPC_AABBTree.cpp:466
SPLIT_BEST_AXIS
@ SPLIT_BEST_AXIS
Try largest axis, then second, then last.
Definition: OPC_TreeBuilders.h:29
DELETEARRAY
#define DELETEARRAY(x)
Deletes an array.
Definition: IceMemoryMacros.h:96
AABBTree::GetUsedBytes
udword GetUsedBytes() const
Definition: OPC_AABBTree.cpp:556
AABB
Definition: IceAABB.h:267
Opcode
Definition: CollisionPairInserterBase.h:18
null
#define null
our own NULL pointer
Definition: IceTypes.h:57
AABBTreeBuilder::SetCount
inline_ void SetCount(udword nb)
Definition: OPC_TreeBuilders.h:117
BuildSettings::mLimit
udword mLimit
Limit number of primitives / node. If limit is 1, build a complete tree (2*N-1 nodes)
Definition: OPC_TreeBuilders.h:43


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Wed Sep 7 2022 02:51:03