Fixed: Some incorrect changes from #2114 were reverted and full reorder of constraint...
[ode.git] / OPCODE / OPC_AABBTree.cpp
blob48129a8bf095215a30546d6ea4a389da91cb53e8
1 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
2 /*
3 * OPCODE - Optimized Collision Detection
4 * Copyright (C) 2001 Pierre Terdiman
5 * Homepage: http://www.codercorner.com/Opcode.htm
6 */
7 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
9 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
10 /**
11 * Contains code for a versatile AABB tree.
12 * \file OPC_AABBTree.cpp
13 * \author Pierre Terdiman
14 * \date March, 20, 2001
16 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
18 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
19 /**
20 * Contains a generic AABB tree node.
22 * \class AABBTreeNode
23 * \author Pierre Terdiman
24 * \version 1.3
25 * \date March, 20, 2001
27 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
29 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
30 /**
31 * Contains a generic AABB tree.
32 * This is a vanilla AABB tree, without any particular optimization. It contains anonymous references to
33 * user-provided primitives, which can theoretically be anything - triangles, boxes, etc. Each primitive
34 * is surrounded by an AABB, regardless of the primitive's nature. When the primitive is a triangle, the
35 * resulting tree can be converted into an optimized tree. If the primitive is a box, the resulting tree
36 * can be used for culling - VFC or occlusion -, assuming you cull on a mesh-by-mesh basis (modern way).
38 * \class AABBTree
39 * \author Pierre Terdiman
40 * \version 1.3
41 * \date March, 20, 2001
43 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
45 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
46 // Precompiled Header
47 #include "Stdafx.h"
49 using namespace Opcode;
51 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
52 /**
53 * Constructor.
55 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
56 AABBTreeNode::AABBTreeNode() :
57 mPos (null),
58 #ifndef OPC_NO_NEG_VANILLA_TREE
59 mNeg (null),
60 #endif
61 mNodePrimitives (null),
62 mNbPrimitives (0)
64 #ifdef OPC_USE_TREE_COHERENCE
65 mBitmask = 0;
66 #endif
69 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
70 /**
71 * Destructor.
73 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
74 AABBTreeNode::~AABBTreeNode()
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;
89 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
90 /**
91 * Splits the node along a given axis.
92 * The list of indices is reorganized according to the split values.
93 * \param axis [in] splitting axis index
94 * \param builder [in] the tree builder
95 * \return the number of primitives assigned to the first child
96 * \warning this method reorganizes the internal list of primitives
98 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
99 udword AABBTreeNode::Split(udword axis, AABBTreeBuilder* builder)
101 // Get node split value
102 float SplitValue = builder->GetSplittingValue(mNodePrimitives, mNbPrimitives, mBV, axis);
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++)
109 // Get index in global list
110 udword Index = mNodePrimitives[i];
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);
116 // Reorganize the list of indices in this order: positive - negative.
117 if(PrimitiveValue > SplitValue)
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++;
127 return NbPos;
130 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
132 * Subdivides the node.
135 * / \
136 * / \
137 * N/2 N/2
138 * / \ / \
139 * N/4 N/4 N/4 N/4
140 * (etc)
142 * A well-balanced tree should have a O(log n) depth.
143 * A degenerate tree would have a O(n) depth.
144 * Note a perfectly-balanced tree is not well-suited to collision detection anyway.
146 * \param builder [in] the tree builder
147 * \return true if success
149 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
150 bool AABBTreeNode::Subdivide(AABBTreeBuilder* builder)
152 // Checkings
153 if(!builder) return false;
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;
159 // Let the user validate the subdivision
160 if(!builder->ValidateSubdivision(mNodePrimitives, mNbPrimitives, mBV)) return true;
162 bool ValidSplit = true; // Optimism...
163 udword NbPos;
164 if(builder->mSettings.mRules & SPLIT_LARGEST_AXIS)
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
170 // Split along the axis
171 NbPos = Split(Axis, builder);
173 // Check split validity
174 if(!NbPos || NbPos==mNbPrimitives) ValidSplit = false;
176 else if(builder->mSettings.mRules & SPLIT_SPLATTER_POINTS)
178 // Compute the means
179 Point Means(0.0f, 0.0f, 0.0f);
180 for(udword i=0;i<mNbPrimitives;i++)
182 udword Index = mNodePrimitives[i];
183 Means += builder->GetSplittingValues(Index);
185 Means/=float(mNbPrimitives);
187 // Compute variances
188 Point Vars(0.0f, 0.0f, 0.0f);
189 for(udword i=0;i<mNbPrimitives;i++)
191 udword Index = mNodePrimitives[i];
192 Point Center = builder->GetSplittingValues(Index);
193 Point Delta = Center - Means;
194 Vars += Delta * Delta;
196 Vars/=float(mNbPrimitives-1);
198 // Choose axis with greatest variance
199 udword Axis = Vars.LargestAxis();
201 // Split along the axis
202 NbPos = Split(Axis, builder);
204 // Check split validity
205 if(!NbPos || NbPos==mNbPrimitives) ValidSplit = false;
207 else if(builder->mSettings.mRules & SPLIT_BALANCED)
209 // Test 3 axis, take the best
210 float Results[3];
211 NbPos = Split(0, builder); Results[0] = float(NbPos)/float(mNbPrimitives);
212 NbPos = Split(1, builder); Results[1] = float(NbPos)/float(mNbPrimitives);
213 NbPos = Split(2, builder); Results[2] = float(NbPos)/float(mNbPrimitives);
214 Results[0]-=0.5f; Results[0]*=Results[0];
215 Results[1]-=0.5f; Results[1]*=Results[1];
216 Results[2]-=0.5f; Results[2]*=Results[2];
217 udword Min=0;
218 if(Results[1]<Results[Min]) Min = 1;
219 if(Results[2]<Results[Min]) Min = 2;
221 // Split along the axis
222 NbPos = Split(Min, builder);
224 // Check split validity
225 if(!NbPos || NbPos==mNbPrimitives) ValidSplit = false;
227 else if(builder->mSettings.mRules & SPLIT_BEST_AXIS)
229 // Test largest, then middle, then smallest axis...
231 // Sort axis
232 Point Extents; mBV.GetExtents(Extents); // Box extents
233 udword SortedAxis[] = { 0, 1, 2 };
234 float* Keys = (float*)&Extents.x;
235 for(udword j=0;j<3;j++)
237 for(udword i=0;i<2;i++)
239 if(Keys[SortedAxis[i]]<Keys[SortedAxis[i+1]])
241 udword Tmp = SortedAxis[i];
242 SortedAxis[i] = SortedAxis[i+1];
243 SortedAxis[i+1] = Tmp;
248 // Find the largest axis to split along
249 udword CurAxis = 0;
250 ValidSplit = false;
251 while(!ValidSplit && CurAxis!=3)
253 NbPos = Split(SortedAxis[CurAxis], builder);
254 // Check the subdivision has been successful
255 if(!NbPos || NbPos==mNbPrimitives) CurAxis++;
256 else ValidSplit = true;
259 else if(builder->mSettings.mRules & SPLIT_FIFTY)
261 // Don't even bother splitting (mainly a performance test)
262 NbPos = mNbPrimitives>>1;
264 else return false; // Unknown splitting rules
266 // Check the subdivision has been successful
267 if(!ValidSplit)
269 // Here, all boxes lie in the same sub-space. Two strategies:
270 // - if the tree *must* be complete, make an arbitrary 50-50 split
271 // - else stop subdividing
272 // if(builder->mSettings.mRules&SPLIT_COMPLETE)
273 if(builder->mSettings.mLimit==1)
275 builder->IncreaseNbInvalidSplits();
276 NbPos = mNbPrimitives>>1;
278 else return true;
281 // Now create children and assign their pointers.
282 if(builder->mNodeBase)
284 // We use a pre-allocated linear pool for complete trees [Opcode 1.3]
285 AABBTreeNode* Pool = (AABBTreeNode*)builder->mNodeBase;
286 udword Count = builder->GetCount() - 1; // Count begins to 1...
287 // Set last bit to tell it shouldn't be freed ### pretty ugly, find a better way. Maybe one bit in mNbPrimitives
288 ASSERT(!(udword(&Pool[Count+0])&1));
289 ASSERT(!(udword(&Pool[Count+1])&1));
290 mPos = size_t(&Pool[Count+0])|1;
291 #ifndef OPC_NO_NEG_VANILLA_TREE
292 mNeg = size_t(&Pool[Count+1])|1;
293 #endif
295 else
297 // Non-complete trees and/or Opcode 1.2 allocate nodes on-the-fly
298 #ifndef OPC_NO_NEG_VANILLA_TREE
299 mPos = (size_t)new AABBTreeNode; CHECKALLOC(mPos);
300 mNeg = (size_t)new AABBTreeNode; CHECKALLOC(mNeg);
301 #else
302 AABBTreeNode* PosNeg = new AABBTreeNode[2];
303 CHECKALLOC(PosNeg);
304 mPos = (size_t)PosNeg;
305 #endif
308 // Update stats
309 builder->IncreaseCount(2);
311 // Assign children
312 AABBTreeNode* Pos = const_cast<AABBTreeNode *>(GetPos());
313 AABBTreeNode* Neg = const_cast<AABBTreeNode *>(GetNeg());
314 Pos->mNodePrimitives = &mNodePrimitives[0];
315 Pos->mNbPrimitives = NbPos;
316 Neg->mNodePrimitives = &mNodePrimitives[NbPos];
317 Neg->mNbPrimitives = mNbPrimitives - NbPos;
319 return true;
322 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
324 * Recursive hierarchy building in a top-down fashion.
325 * \param builder [in] the tree builder
327 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
328 void AABBTreeNode::_BuildHierarchy(AABBTreeBuilder* builder)
330 // 1) Compute the global box for current node. The box is stored in mBV.
331 builder->ComputeGlobalBox(mNodePrimitives, mNbPrimitives, mBV);
333 // 2) Subdivide current node
334 Subdivide(builder);
336 // 3) Recurse
337 AABBTreeNode* Pos = const_cast<AABBTreeNode *>(GetPos());
338 AABBTreeNode* Neg = const_cast<AABBTreeNode *>(GetNeg());
339 if(Pos) Pos->_BuildHierarchy(builder);
340 if(Neg) Neg->_BuildHierarchy(builder);
343 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
345 * Refits the tree (top-down).
346 * \param builder [in] the tree builder
348 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
349 void AABBTreeNode::_Refit(AABBTreeBuilder* builder)
351 // 1) Recompute the new global box for current node
352 builder->ComputeGlobalBox(mNodePrimitives, mNbPrimitives, mBV);
354 // 2) Recurse
355 AABBTreeNode* Pos = const_cast<AABBTreeNode *>(GetPos());
356 AABBTreeNode* Neg = const_cast<AABBTreeNode *>(GetNeg());
357 if(Pos) Pos->_Refit(builder);
358 if(Neg) Neg->_Refit(builder);
363 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
365 * Constructor.
367 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
368 AABBTree::AABBTree() : mIndices(null), mPool(null), mTotalNbNodes(0)
372 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
374 * Destructor.
376 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
377 AABBTree::~AABBTree()
379 Release();
382 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
384 * Releases the tree.
386 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
387 void AABBTree::Release()
389 DELETEARRAY(mPool);
390 DELETEARRAY(mIndices);
393 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
395 * Builds a generic AABB tree from a tree builder.
396 * \param builder [in] the tree builder
397 * \return true if success
399 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
400 bool AABBTree::Build(AABBTreeBuilder* builder)
402 // Checkings
403 if(!builder || !builder->mNbPrimitives) return false;
405 // Release previous tree
406 Release();
408 // Init stats
409 builder->SetCount(1);
410 builder->SetNbInvalidSplits(0);
412 // Initialize indices. This list will be modified during build.
413 mIndices = new dTriIndex[builder->mNbPrimitives];
414 CHECKALLOC(mIndices);
415 // Identity permutation
416 for(udword i=0;i<builder->mNbPrimitives;i++) mIndices[i] = i;
418 // Setup initial node. Here we have a complete permutation of the app's primitives.
419 mNodePrimitives = mIndices;
420 mNbPrimitives = builder->mNbPrimitives;
422 // Use a linear array for complete trees (since we can predict the final number of nodes) [Opcode 1.3]
423 // if(builder->mRules&SPLIT_COMPLETE)
424 if(builder->mSettings.mLimit==1)
426 // Allocate a pool of nodes
427 mPool = new AABBTreeNode[builder->mNbPrimitives*2 - 1];
429 builder->mNodeBase = mPool; // ### ugly !
432 // Build the hierarchy
433 _BuildHierarchy(builder);
435 // Get back total number of nodes
436 mTotalNbNodes = builder->GetCount();
438 // For complete trees, check the correct number of nodes has been created [Opcode 1.3]
439 if(mPool) ASSERT(mTotalNbNodes==builder->mNbPrimitives*2 - 1);
441 return true;
444 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
446 * Computes the depth of the tree.
447 * A well-balanced tree should have a log(n) depth. A degenerate tree O(n) depth.
448 * \return depth of the tree
450 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
451 udword AABBTree::ComputeDepth() const
453 return Walk(null, null); // Use the walking code without callback
456 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
458 * Walks the tree, calling the user back for each node.
460 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
461 udword AABBTree::Walk(WalkingCallback callback, void* user_data) const
463 // Call it without callback to compute max depth
464 udword MaxDepth = 0;
465 udword CurrentDepth = 0;
467 struct Local
469 static void _Walk(const AABBTreeNode* current_node, udword& max_depth, udword& current_depth, WalkingCallback callback, void* user_data)
471 // Checkings
472 if(!current_node) return;
473 // Entering a new node => increase depth
474 current_depth++;
475 // Keep track of max depth
476 if(current_depth>max_depth) max_depth = current_depth;
478 // Callback
479 if(callback && !(callback)(current_node, current_depth, user_data)) return;
481 // Recurse
482 if(current_node->GetPos()) { _Walk(current_node->GetPos(), max_depth, current_depth, callback, user_data); current_depth--; }
483 if(current_node->GetNeg()) { _Walk(current_node->GetNeg(), max_depth, current_depth, callback, user_data); current_depth--; }
486 Local::_Walk(this, MaxDepth, CurrentDepth, callback, user_data);
487 return MaxDepth;
490 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
492 * Refits the tree in a top-down way.
493 * \param builder [in] the tree builder
495 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
496 bool AABBTree::Refit(AABBTreeBuilder* builder)
498 if(!builder) return false;
499 _Refit(builder);
500 return true;
503 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
505 * Refits the tree in a bottom-up way.
506 * \param builder [in] the tree builder
508 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
509 bool AABBTree::Refit2(AABBTreeBuilder* builder)
511 // Checkings
512 if(!builder) return false;
514 ASSERT(mPool);
516 // Bottom-up update
517 Point Min,Max;
518 Point Min_,Max_;
519 udword Index = mTotalNbNodes;
520 while(Index--)
522 AABBTreeNode& Current = mPool[Index];
524 if(Current.IsLeaf())
526 builder->ComputeGlobalBox(Current.GetPrimitives(), Current.GetNbPrimitives(), *const_cast<AABB*>(Current.GetAABB()));
528 else
530 Current.GetPos()->GetAABB()->GetMin(Min);
531 Current.GetPos()->GetAABB()->GetMax(Max);
533 Current.GetNeg()->GetAABB()->GetMin(Min_);
534 Current.GetNeg()->GetAABB()->GetMax(Max_);
536 Min.Min(Min_);
537 Max.Max(Max_);
539 const_cast<AABB*>(Current.GetAABB())->SetMinMax(Min, Max);
542 return true;
545 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
547 * Computes the number of bytes used by the tree.
548 * \return number of bytes used
550 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
551 udword AABBTree::GetUsedBytes() const
553 udword TotalSize = mTotalNbNodes*GetNodeSize();
554 if(mIndices) TotalSize+=mNbPrimitives*sizeof(udword);
555 return TotalSize;
558 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
560 * Checks the tree is a complete tree or not.
561 * A complete tree is made of 2*N-1 nodes, where N is the number of primitives in the tree.
562 * \return true for complete trees
564 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
565 bool AABBTree::IsComplete() const
567 return (GetNbNodes()==GetNbPrimitives()*2-1);