PageRenderTime 57ms CodeModel.GetById 17ms app.highlight 34ms RepoModel.GetById 1ms app.codeStats 0ms

/indra/llmath/lloctree.h

https://bitbucket.org/lindenlab/viewer-beta/
C++ Header | 783 lines | 580 code | 125 blank | 78 comment | 99 complexity | 0e107fa818ed930cef58c988380ad24d MD5 | raw file
  1/** 
  2 * @file lloctree.h
  3 * @brief Octree declaration. 
  4 *
  5 * $LicenseInfo:firstyear=2005&license=viewerlgpl$
  6 * Second Life Viewer Source Code
  7 * Copyright (C) 2010, Linden Research, Inc.
  8 * 
  9 * This library is free software; you can redistribute it and/or
 10 * modify it under the terms of the GNU Lesser General Public
 11 * License as published by the Free Software Foundation;
 12 * version 2.1 of the License only.
 13 * 
 14 * This library is distributed in the hope that it will be useful,
 15 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 16 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 17 * Lesser General Public License for more details.
 18 * 
 19 * You should have received a copy of the GNU Lesser General Public
 20 * License along with this library; if not, write to the Free Software
 21 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
 22 * 
 23 * Linden Research, Inc., 945 Battery Street, San Francisco, CA  94111  USA
 24 * $/LicenseInfo$
 25 */
 26
 27#ifndef LL_LLOCTREE_H
 28#define LL_LLOCTREE_H
 29
 30#include "lltreenode.h"
 31#include "v3math.h"
 32#include "llvector4a.h"
 33#include <vector>
 34#include <set>
 35
 36#define OCT_ERRS LL_WARNS("OctreeErrors")
 37
 38
 39extern U32 gOctreeMaxCapacity;
 40/*#define LL_OCTREE_PARANOIA_CHECK 0
 41#if LL_DARWIN
 42#define LL_OCTREE_MAX_CAPACITY 32
 43#else
 44#define LL_OCTREE_MAX_CAPACITY 128
 45#endif*/
 46
 47template <class T> class LLOctreeNode;
 48
 49template <class T>
 50class LLOctreeListener: public LLTreeListener<T>
 51{
 52public:
 53	typedef LLTreeListener<T> BaseType;
 54	typedef LLOctreeNode<T> oct_node;
 55
 56	virtual void handleChildAddition(const oct_node* parent, oct_node* child) = 0;
 57	virtual void handleChildRemoval(const oct_node* parent, const oct_node* child) = 0;
 58};
 59
 60template <class T>
 61class LLOctreeTraveler
 62{
 63public:
 64	virtual void traverse(const LLOctreeNode<T>* node);
 65	virtual void visit(const LLOctreeNode<T>* branch) = 0;
 66};
 67
 68template <class T>
 69class LLOctreeTravelerDepthFirst : public LLOctreeTraveler<T>
 70{
 71public:
 72	virtual void traverse(const LLOctreeNode<T>* node);
 73};
 74
 75template <class T>
 76class LLOctreeNode : public LLTreeNode<T>
 77{
 78public:
 79
 80	typedef LLOctreeTraveler<T>									oct_traveler;
 81	typedef LLTreeTraveler<T>									tree_traveler;
 82	typedef typename std::set<LLPointer<T> >					element_list;
 83	typedef typename element_list::iterator						element_iter;
 84	typedef typename element_list::const_iterator	const_element_iter;
 85	typedef typename std::vector<LLTreeListener<T>*>::iterator	tree_listener_iter;
 86	typedef typename std::vector<LLOctreeNode<T>* >				child_list;
 87	typedef LLTreeNode<T>		BaseType;
 88	typedef LLOctreeNode<T>		oct_node;
 89	typedef LLOctreeListener<T>	oct_listener;
 90
 91	/*void* operator new(size_t size)
 92	{
 93		return ll_aligned_malloc_16(size);
 94	}
 95
 96	void operator delete(void* ptr)
 97	{
 98		ll_aligned_free_16(ptr);
 99	}*/
100
101	LLOctreeNode(	const LLVector4a& center, 
102					const LLVector4a& size, 
103					BaseType* parent, 
104					U8 octant = 255)
105	:	mParent((oct_node*)parent), 
106		mOctant(octant) 
107	{ 
108		mCenter = center;
109		mSize = size;
110
111		updateMinMax();
112		if ((mOctant == 255) && mParent)
113		{
114			mOctant = ((oct_node*) mParent)->getOctant(mCenter);
115		}
116
117		mElementCount = 0;
118
119		clearChildren();
120	}
121
122	virtual ~LLOctreeNode()								
123	{ 
124		BaseType::destroyListeners(); 
125		
126		for (U32 i = 0; i < getChildCount(); i++)
127		{
128			delete getChild(i);
129		} 
130	}
131
132	inline const BaseType* getParent()	const			{ return mParent; }
133	inline void setParent(BaseType* parent)				{ mParent = (oct_node*) parent; }
134	inline const LLVector4a& getCenter() const			{ return mCenter; }
135	inline const LLVector4a& getSize() const			{ return mSize; }
136	inline void setCenter(const LLVector4a& center)		{ mCenter = center; }
137	inline void setSize(const LLVector4a& size)			{ mSize = size; }
138    inline oct_node* getNodeAt(T* data)					{ return getNodeAt(data->getPositionGroup(), data->getBinRadius()); }
139	inline U8 getOctant() const							{ return mOctant; }
140	inline const oct_node*	getOctParent() const		{ return (const oct_node*) getParent(); }
141	inline oct_node* getOctParent() 					{ return (oct_node*) getParent(); }
142	
143	U8 getOctant(const LLVector4a& pos) const			//get the octant pos is in
144	{
145		return (U8) (pos.greaterThan(mCenter).getGatheredBits() & 0x7);
146	}
147	
148	inline bool isInside(const LLVector4a& pos, const F32& rad) const
149	{
150		return rad <= mSize[0]*2.f && isInside(pos); 
151	}
152
153	inline bool isInside(T* data) const			
154	{ 
155		return isInside(data->getPositionGroup(), data->getBinRadius());
156	}
157
158	bool isInside(const LLVector4a& pos) const
159	{
160		S32 gt = pos.greaterThan(mMax).getGatheredBits() & 0x7;
161		if (gt)
162		{
163			return false;
164		}
165
166		S32 lt = pos.lessEqual(mMin).getGatheredBits() & 0x7;
167		if (lt)
168		{
169			return false;
170		}
171				
172		return true;
173	}
174	
175	void updateMinMax()
176	{
177		mMax.setAdd(mCenter, mSize);
178		mMin.setSub(mCenter, mSize);
179	}
180
181	inline oct_listener* getOctListener(U32 index) 
182	{ 
183		return (oct_listener*) BaseType::getListener(index); 
184	}
185
186	inline bool contains(T* xform)
187	{
188		return contains(xform->getBinRadius());
189	}
190
191	bool contains(F32 radius)
192	{
193		if (mParent == NULL)
194		{	//root node contains nothing
195			return false;
196		}
197
198		F32 size = mSize[0];
199		F32 p_size = size * 2.f;
200
201		return (radius <= 0.001f && size <= 0.001f) ||
202				(radius <= p_size && radius > size);
203	}
204
205	static void pushCenter(LLVector4a &center, const LLVector4a &size, const T* data)
206	{
207		const LLVector4a& pos = data->getPositionGroup();
208
209		LLVector4Logical gt = pos.greaterThan(center);
210
211		LLVector4a up;
212		up = _mm_and_ps(size, gt);
213
214		LLVector4a down;
215		down = _mm_andnot_ps(gt, size);
216
217		center.add(up);
218		center.sub(down);
219	}
220
221	void accept(oct_traveler* visitor)				{ visitor->visit(this); }
222	virtual bool isLeaf() const						{ return mChild.empty(); }
223	
224	U32 getElementCount() const						{ return mElementCount; }
225	element_list& getData()							{ return mData; }
226	const element_list& getData() const				{ return mData; }
227	
228	U32 getChildCount()	const						{ return mChildCount; }
229	oct_node* getChild(U32 index)					{ return mChild[index]; }
230	const oct_node* getChild(U32 index) const		{ return mChild[index]; }
231	child_list& getChildren()						{ return mChild; }
232	const child_list& getChildren() const			{ return mChild; }
233	
234	void accept(tree_traveler* visitor) const		{ visitor->visit(this); }
235	void accept(oct_traveler* visitor) const		{ visitor->visit(this); }
236	
237	void validateChildMap()
238	{
239		for (U32 i = 0; i < 8; i++)
240		{
241			U8 idx = mChildMap[i];
242			if (idx != 255)
243			{
244				LLOctreeNode<T>* child = mChild[idx];
245
246				if (child->getOctant() != i)
247				{
248					llerrs << "Invalid child map, bad octant data." << llendl;
249				}
250
251				if (getOctant(child->getCenter()) != child->getOctant())
252				{
253					llerrs << "Invalid child octant compared to position data." << llendl;
254				}
255			}
256		}
257	}
258
259
260	oct_node* getNodeAt(const LLVector4a& pos, const F32& rad)
261	{ 
262		LLOctreeNode<T>* node = this;
263
264		if (node->isInside(pos, rad))
265		{		
266			//do a quick search by octant
267			U8 octant = node->getOctant(pos);
268			
269			//traverse the tree until we find a node that has no node
270			//at the appropriate octant or is smaller than the object.  
271			//by definition, that node is the smallest node that contains 
272			// the data
273			U8 next_node = node->mChildMap[octant];
274			
275			while (next_node != 255 && node->getSize()[0] >= rad)
276			{	
277				node = node->getChild(next_node);
278				octant = node->getOctant(pos);
279				next_node = node->mChildMap[octant];
280			}
281		}
282		else if (!node->contains(rad) && node->getParent())
283		{ //if we got here, data does not exist in this node
284			return ((LLOctreeNode<T>*) node->getParent())->getNodeAt(pos, rad);
285		}
286
287		return node;
288	}
289	
290	virtual bool insert(T* data)
291	{
292		if (data == NULL)
293		{
294			OCT_ERRS << "!!! INVALID ELEMENT ADDED TO OCTREE BRANCH !!!" << llendl;
295			return false;
296		}
297		LLOctreeNode<T>* parent = getOctParent();
298
299		//is it here?
300		if (isInside(data->getPositionGroup()))
301		{
302			if ((getElementCount() < gOctreeMaxCapacity && contains(data->getBinRadius()) ||
303				(data->getBinRadius() > getSize()[0] &&	parent && parent->getElementCount() >= gOctreeMaxCapacity))) 
304			{ //it belongs here
305				//if this is a redundant insertion, error out (should never happen)
306				llassert(mData.find(data) == mData.end());
307
308				mData.insert(data);
309				BaseType::insert(data);
310
311				mElementCount = mData.size();
312				return true;
313			}
314			else
315			{ 	
316				//find a child to give it to
317				oct_node* child = NULL;
318				for (U32 i = 0; i < getChildCount(); i++)
319				{
320					child = getChild(i);
321					if (child->isInside(data->getPositionGroup()))
322					{
323						child->insert(data);
324						return false;
325					}
326				}
327				
328				//it's here, but no kids are in the right place, make a new kid
329				LLVector4a center = getCenter();
330				LLVector4a size = getSize();
331				size.mul(0.5f);
332		        		
333				//push center in direction of data
334				LLOctreeNode<T>::pushCenter(center, size, data);
335
336				// handle case where floating point number gets too small
337				LLVector4a val;
338				val.setSub(center, getCenter());
339				val.setAbs(val);
340								
341				S32 lt = val.lessThan(LLVector4a::getEpsilon()).getGatheredBits() & 0x7;
342
343				if( lt == 0x7 )
344				{
345					mData.insert(data);
346					BaseType::insert(data);
347
348					mElementCount = mData.size();
349					return true;
350				}
351
352#if LL_OCTREE_PARANOIA_CHECK
353				if (getChildCount() == 8)
354				{
355					//this really isn't possible, something bad has happened
356					OCT_ERRS << "Octree detected floating point error and gave up." << llendl;
357					return false;
358				}
359				
360				//make sure no existing node matches this position
361				for (U32 i = 0; i < getChildCount(); i++)
362				{
363					if (mChild[i]->getCenter().equals3(center))
364					{
365						OCT_ERRS << "Octree detected duplicate child center and gave up." << llendl;
366						return false;
367					}
368				}
369#endif
370
371				//make the new kid
372				child = new LLOctreeNode<T>(center, size, this);
373				addChild(child);
374								
375				child->insert(data);
376			}
377		}
378		else 
379		{
380			//it's not in here, give it to the root
381			OCT_ERRS << "Octree insertion failed, starting over from root!" << llendl;
382
383			oct_node* node = this;
384
385			while (parent)
386			{
387				node = parent;
388				parent = node->getOctParent();
389			}
390
391			node->insert(data);
392		}
393
394		return false;
395	}
396
397	bool remove(T* data)
398	{
399		if (mData.find(data) != mData.end())
400		{	//we have data
401			mData.erase(data);
402			mElementCount = mData.size();
403			notifyRemoval(data);
404			checkAlive();
405			return true;
406		}
407		else if (isInside(data))
408		{
409			oct_node* dest = getNodeAt(data);
410
411			if (dest != this)
412			{
413				return dest->remove(data);
414			}
415		}
416
417		//SHE'S GONE MISSING...
418		//none of the children have it, let's just brute force this bastard out
419		//starting with the root node (UGLY CODE COMETH!)
420		oct_node* parent = getOctParent();
421		oct_node* node = this;
422
423		while (parent != NULL)
424		{
425			node = parent;
426			parent = node->getOctParent();
427		}
428
429		//node is now root
430		llwarns << "!!! OCTREE REMOVING FACE BY ADDRESS, SEVERE PERFORMANCE PENALTY |||" << llendl;
431		node->removeByAddress(data);
432		return true;
433	}
434
435	void removeByAddress(T* data)
436	{
437        if (mData.find(data) != mData.end())
438		{
439			mData.erase(data);
440			mElementCount = mData.size();
441			notifyRemoval(data);
442			llwarns << "FOUND!" << llendl;
443			checkAlive();
444			return;
445		}
446		
447		for (U32 i = 0; i < getChildCount(); i++)
448		{	//we don't contain data, so pass this guy down
449			LLOctreeNode<T>* child = (LLOctreeNode<T>*) getChild(i);
450			child->removeByAddress(data);
451		}
452	}
453
454	void clearChildren()
455	{
456		mChild.clear();
457		mChildCount = 0;
458		U32* foo = (U32*) mChildMap;
459		foo[0] = foo[1] = 0xFFFFFFFF;
460	}
461
462	void validate()
463	{
464#if LL_OCTREE_PARANOIA_CHECK
465		for (U32 i = 0; i < getChildCount(); i++)
466		{
467			mChild[i]->validate();
468			if (mChild[i]->getParent() != this)
469			{
470				llerrs << "Octree child has invalid parent." << llendl;
471			}
472		}
473#endif
474	}
475
476	virtual bool balance()
477	{	
478		return false;
479	}
480
481	void destroy()
482	{
483		for (U32 i = 0; i < getChildCount(); i++) 
484		{	
485			mChild[i]->destroy();
486			delete mChild[i];
487		}
488	}
489
490	void addChild(oct_node* child, BOOL silent = FALSE) 
491	{
492#if LL_OCTREE_PARANOIA_CHECK
493
494		if (child->getSize().equals3(getSize()))
495		{
496			OCT_ERRS << "Child size is same as parent size!" << llendl;
497		}
498
499		for (U32 i = 0; i < getChildCount(); i++)
500		{
501			if(!mChild[i]->getSize().equals3(child->getSize())) 
502			{
503				OCT_ERRS <<"Invalid octree child size." << llendl;
504			}
505			if (mChild[i]->getCenter().equals3(child->getCenter()))
506			{
507				OCT_ERRS <<"Duplicate octree child position." << llendl;
508			}
509		}
510
511		if (mChild.size() >= 8)
512		{
513			OCT_ERRS <<"Octree node has too many children... why?" << llendl;
514		}
515#endif
516
517		mChildMap[child->getOctant()] = mChildCount;
518
519		mChild.push_back(child);
520		++mChildCount;
521		child->setParent(this);
522
523		if (!silent)
524		{
525			for (U32 i = 0; i < this->getListenerCount(); i++)
526			{
527				oct_listener* listener = getOctListener(i);
528				listener->handleChildAddition(this, child);
529			}
530		}
531	}
532
533	void removeChild(S32 index, BOOL destroy = FALSE)
534	{
535		for (U32 i = 0; i < this->getListenerCount(); i++)
536		{
537			oct_listener* listener = getOctListener(i);
538			listener->handleChildRemoval(this, getChild(index));
539		}
540		
541		if (destroy)
542		{
543			mChild[index]->destroy();
544			delete mChild[index];
545		}
546		mChild.erase(mChild.begin() + index);
547		--mChildCount;
548
549		//rebuild child map
550		U32* foo = (U32*) mChildMap;
551		foo[0] = foo[1] = 0xFFFFFFFF;
552
553		for (U32 i = 0; i < mChildCount; ++i)
554		{
555			mChildMap[mChild[i]->getOctant()] = i;
556		}
557
558		checkAlive();
559	}
560
561	void checkAlive()
562	{
563		if (getChildCount() == 0 && getElementCount() == 0)
564		{
565			oct_node* parent = getOctParent();
566			if (parent)
567			{
568				parent->deleteChild(this);
569			}
570		}
571	}
572
573	void deleteChild(oct_node* node)
574	{
575		for (U32 i = 0; i < getChildCount(); i++)
576		{
577			if (getChild(i) == node)
578			{
579				removeChild(i, TRUE);
580				return;
581			}
582		}
583
584		OCT_ERRS << "Octree failed to delete requested child." << llendl;
585	}
586
587protected:	
588	typedef enum
589	{
590		CENTER = 0,
591		SIZE = 1,
592		MAX = 2,
593		MIN = 3
594	} eDName;
595
596	LLVector4a mCenter;
597	LLVector4a mSize;
598	LLVector4a mMax;
599	LLVector4a mMin;
600	
601	oct_node* mParent;
602	U8 mOctant;
603
604	child_list mChild;
605	U8 mChildMap[8];
606	U32 mChildCount;
607
608	element_list mData;
609	U32 mElementCount;
610		
611}; 
612
613//just like a regular node, except it might expand on insert and compress on balance
614template <class T>
615class LLOctreeRoot : public LLOctreeNode<T>
616{
617public:
618	typedef LLOctreeNode<T>	BaseType;
619	typedef LLOctreeNode<T>		oct_node;
620
621	LLOctreeRoot(const LLVector4a& center, 
622				 const LLVector4a& size, 
623				 BaseType* parent)
624	:	BaseType(center, size, parent)
625	{
626	}
627	
628	bool balance()
629	{	
630		if (this->getChildCount() == 1 && 
631			!(this->mChild[0]->isLeaf()) &&
632			this->mChild[0]->getElementCount() == 0) 
633		{ //if we have only one child and that child is an empty branch, make that child the root
634			oct_node* child = this->mChild[0];
635					
636			//make the root node look like the child
637			this->setCenter(this->mChild[0]->getCenter());
638			this->setSize(this->mChild[0]->getSize());
639			this->updateMinMax();
640
641			//reset root node child list
642			this->clearChildren();
643
644			//copy the child's children into the root node silently 
645			//(don't notify listeners of addition)
646			for (U32 i = 0; i < child->getChildCount(); i++)
647			{
648				addChild(child->getChild(i), TRUE);
649			}
650
651			//destroy child
652			child->clearChildren();
653			delete child;
654
655			return false;
656		}
657		
658		return true;
659	}
660
661	// LLOctreeRoot::insert
662	bool insert(T* data)
663	{
664		if (data == NULL) 
665		{
666			OCT_ERRS << "!!! INVALID ELEMENT ADDED TO OCTREE ROOT !!!" << llendl;
667			return false;
668		}
669		
670		if (data->getBinRadius() > 4096.0)
671		{
672			OCT_ERRS << "!!! ELEMENT EXCEEDS MAXIMUM SIZE IN OCTREE ROOT !!!" << llendl;
673			return false;
674		}
675		
676		LLVector4a MAX_MAG;
677		MAX_MAG.splat(1024.f*1024.f);
678
679		const LLVector4a& v = data->getPositionGroup();
680
681		LLVector4a val;
682		val.setSub(v, BaseType::mCenter);
683		val.setAbs(val);
684		S32 lt = val.lessThan(MAX_MAG).getGatheredBits() & 0x7;
685
686		if (lt != 0x7)
687		{
688			//OCT_ERRS << "!!! ELEMENT EXCEEDS RANGE OF SPATIAL PARTITION !!!" << llendl;
689			return false;
690		}
691
692		if (this->getSize()[0] > data->getBinRadius() && isInside(data->getPositionGroup()))
693		{
694			//we got it, just act like a branch
695			oct_node* node = getNodeAt(data);
696			if (node == this)
697			{
698				LLOctreeNode<T>::insert(data);
699			}
700			else
701			{
702				node->insert(data);
703			}
704		}
705		else if (this->getChildCount() == 0)
706		{
707			//first object being added, just wrap it up
708			while (!(this->getSize()[0] > data->getBinRadius() && isInside(data->getPositionGroup())))
709			{
710				LLVector4a center, size;
711				center = this->getCenter();
712				size = this->getSize();
713				LLOctreeNode<T>::pushCenter(center, size, data);
714				this->setCenter(center);
715				size.mul(2.f);
716				this->setSize(size);
717				this->updateMinMax();
718			}
719			LLOctreeNode<T>::insert(data);
720		}
721		else
722		{
723			while (!(this->getSize()[0] > data->getBinRadius() && isInside(data->getPositionGroup())))
724			{
725				//the data is outside the root node, we need to grow
726				LLVector4a center(this->getCenter());
727				LLVector4a size(this->getSize());
728
729				//expand this node
730				LLVector4a newcenter(center);
731				LLOctreeNode<T>::pushCenter(newcenter, size, data);
732				this->setCenter(newcenter);
733				LLVector4a size2 = size;
734				size2.mul(2.f);
735				this->setSize(size2);
736				this->updateMinMax();
737
738				//copy our children to a new branch
739				LLOctreeNode<T>* newnode = new LLOctreeNode<T>(center, size, this);
740				
741				for (U32 i = 0; i < this->getChildCount(); i++)
742				{
743					LLOctreeNode<T>* child = this->getChild(i);
744					newnode->addChild(child);
745				}
746
747				//clear our children and add the root copy
748				this->clearChildren();
749				addChild(newnode);
750			}
751
752			//insert the data
753			insert(data);
754		}
755
756		return false;
757	}
758};
759
760//========================
761//		LLOctreeTraveler
762//========================
763template <class T>
764void LLOctreeTraveler<T>::traverse(const LLOctreeNode<T>* node)
765{
766	node->accept(this);
767	for (U32 i = 0; i < node->getChildCount(); i++)
768	{
769		traverse(node->getChild(i));
770	}
771}
772
773template <class T>
774void LLOctreeTravelerDepthFirst<T>::traverse(const LLOctreeNode<T>* node)
775{
776	for (U32 i = 0; i < node->getChildCount(); i++)
777	{
778		traverse(node->getChild(i));
779	}
780	node->accept(this);
781}
782
783#endif