add vcglib

vcglib/vcg/space/index/aabb_binary_tree/aabb_binary_tree.h

@@ -0,0 +1,144 @@
+/****************************************************************************
+* VCGLib                                                            o o     *
+* Visual and Computer Graphics Library                            o     o   *
+*                                                                _   O  _   *
+* Copyright(C) 2004-2016                                           \/)\/    *
+* Visual Computing Lab                                            /\/|      *
+* ISTI - Italian National Research Council                           |      *
+*                                                                    \      *
+* All rights reserved.                                                      *
+*                                                                           *
+* This program is free software; you can redistribute it and/or modify      *   
+* it under the terms of the GNU General Public License as published by      *
+* the Free Software Foundation; either version 2 of the License, or         *
+* (at your option) any later version.                                       *
+*                                                                           *
+* This program is distributed in the hope that it will be useful,           *
+* but WITHOUT ANY WARRANTY; without even the implied warranty of            *
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the             *
+* GNU General Public License (http://www.gnu.org/licenses/gpl.txt)          *
+* for more details.                                                         *
+*                                                                           *
+****************************************************************************/
+
+/****************************************************************************
+History
+
+$Log: not supported by cvs2svn $
+Revision 1.5  2005/09/29 22:18:16  m_di_benedetto
+Added frustum culling methods, renamed some parameters.
+
+Revision 1.4  2005/09/28 21:23:03  m_di_benedetto
+Added Import() to box and barycenter functors
+to handle tree and objects with different ScalarType.
+
+Revision 1.3  2005/09/28 20:10:41  m_di_benedetto
+First Commit.
+
+
+****************************************************************************/
+
+#ifndef __VCGLIB_AABBBINARYTREEINDEX_H
+#define __VCGLIB_AABBBINARYTREEINDEX_H
+
+// vcg headers
+#include <vcg/space/index/base.h>
+#include <vcg/space/index/aabb_binary_tree/base.h>
+#include <vcg/space/index/aabb_binary_tree/closest.h>
+#include <vcg/space/index/aabb_binary_tree/frustum_cull.h>
+#include <vcg/space/index/aabb_binary_tree/kclosest.h>
+#include <vcg/space/index/aabb_binary_tree/ray.h>
+#include <wrap/utils.h>
+
+/***************************************************************************/
+
+namespace vcg {
+
+template <class OBJTYPE, class SCALARTYPE, class NODEAUXDATA = EmptyClass>
+class AABBBinaryTreeIndex : public SpatialIndex<OBJTYPE, SCALARTYPE> {
+public:
+	typedef AABBBinaryTreeIndex<OBJTYPE, SCALARTYPE, NODEAUXDATA> ClassType;
+	typedef OBJTYPE ObjType;
+	typedef SCALARTYPE ScalarType;
+	typedef NODEAUXDATA NodeAuxData;
+	typedef ObjType * ObjPtr;
+	typedef Point3<ScalarType> CoordType;
+	typedef AABBBinaryTree<ObjType, ScalarType, NodeAuxData> TreeType;
+
+	inline TreeType & Tree(void) {
+		return (this->tree);
+	}
+
+	inline const TreeType & Tree(void) const {
+		return (this->tree);
+	}
+
+	bool Empty()
+	{
+		return (this->tree.pRoot == 0);
+	}
+
+	void Clear(void)
+	{
+		this->tree.Clear();
+	}
+
+	template <class OBJITER>
+	inline void Set(const OBJITER & _oBegin, const OBJITER & _oEnd) {
+		GetPointerFunctor getPtr;
+		GetBox3Functor getBox;
+		GetBarycenter3Functor getBarycenter;
+		//const unsigned int divs = 100;
+		//const unsigned int size = (unsigned int)(std::distance(_oBegin, _oEnd));
+		//const unsigned int maxObjectsPerLeaf = (size < divs) ? (size) : ((unsigned int)((float)(std::distance(_oBegin, _oEnd)) / (float)divs));
+		const unsigned int maxObjectsPerLeaf = 10;
+		const ScalarType leafBoxMaxVolume = ((ScalarType)0);
+		const bool useVariance = true;
+
+		(void)(this->tree.Set(_oBegin, _oEnd, getPtr, getBox, getBarycenter, maxObjectsPerLeaf, leafBoxMaxVolume, useVariance));
+	}
+
+	template <class OBJITERATOR, class OBJITERATORPTRFUNCT, class OBJBOXFUNCT, class OBJBARYCENTERFUNCT>
+	inline bool Set(const OBJITERATOR & _oBegin, const OBJITERATOR & _oEnd, OBJITERATORPTRFUNCT & _objPtr, OBJBOXFUNCT & _objBox, OBJBARYCENTERFUNCT & _objBarycenter, const unsigned int _maxElemsPerLeaf = 1, const ScalarType & _leafBoxMaxVolume = ((ScalarType)0), const bool _useVariance = true) {
+		return (this->tree.Set(_oBegin, _oEnd, _objPtr, _objBox, _objBarycenter, _maxElemsPerLeaf, _leafBoxMaxVolume, _useVariance));
+	}
+
+	template <class OBJPOINTDISTFUNCTOR, class OBJMARKER>
+	inline ObjPtr GetClosest(
+		OBJPOINTDISTFUNCTOR & _getPointDistance, OBJMARKER & _marker, 
+		const typename OBJPOINTDISTFUNCTOR::QueryType & _p, const ScalarType & _maxDist,
+		ScalarType & _minDist, CoordType & _closestPt) {
+		(void)_marker;
+		return (AABBBinaryTreeClosest<TreeType>::Closest(this->tree, _getPointDistance, _p, _maxDist, _minDist, _closestPt));
+	}
+
+	template <class OBJPOINTDISTFUNCTOR, class OBJMARKER, class OBJPTRCONTAINER, class DISTCONTAINER, class POINTCONTAINER>
+	inline unsigned int GetKClosest(
+		OBJPOINTDISTFUNCTOR & _getPointDistance, OBJMARKER & _marker, const unsigned int _k, const CoordType & _p, const ScalarType & _maxDist,
+		OBJPTRCONTAINER & _objectPtrs, DISTCONTAINER & _distances, POINTCONTAINER & _points) {
+		(void)_marker;
+		return (AABBBinaryTreeKClosest<TreeType>::KClosest(this->tree, _getPointDistance, _k, _p, _maxDist, _objectPtrs, _distances, _points));
+	}
+
+	template <class OBJRAYISECTFUNCTOR, class OBJMARKER>
+	inline ObjPtr DoRay(OBJRAYISECTFUNCTOR & _rayIntersector, OBJMARKER & _marker, const Ray3<ScalarType> & _ray, const ScalarType & _maxDist, ScalarType & _t) {
+		(void)_marker;
+		return (AABBBinaryTreeRay<TreeType>::Ray(this->tree, _rayIntersector, _ray, _maxDist, _t));
+	}
+
+	inline void InitializeFrustumCull(void) {
+		(void)(AABBBinaryTreeFrustumCull<TreeType>::Initialize(this->tree));
+	}
+
+	inline void FrustumCull(const Plane3<ScalarType> _frustumPlanes[6], const unsigned int _minNodeObjectsCount) {
+		(void)(AABBBinaryTreeFrustumCull<TreeType>::FrustumCull(this->tree, _frustumPlanes, _minNodeObjectsCount));
+	}
+
+protected:
+	TreeType tree;
+
+};
+
+}	// end namespace vcg
+
+#endif // #ifndef __VCGLIB_AABBBINARYTREEINDEX_H

vcglib/vcg/space/index/aabb_binary_tree/base.h

@@ -0,0 +1,412 @@
+/****************************************************************************
+* VCGLib                                                            o o     *
+* Visual and Computer Graphics Library                            o     o   *
+*                                                                _   O  _   *
+* Copyright(C) 2004-2016                                           \/)\/    *
+* Visual Computing Lab                                            /\/|      *
+* ISTI - Italian National Research Council                           |      *
+*                                                                    \      *
+* All rights reserved.                                                      *
+*                                                                           *
+* This program is free software; you can redistribute it and/or modify      *   
+* it under the terms of the GNU General Public License as published by      *
+* the Free Software Foundation; either version 2 of the License, or         *
+* (at your option) any later version.                                       *
+*                                                                           *
+* This program is distributed in the hope that it will be useful,           *
+* but WITHOUT ANY WARRANTY; without even the implied warranty of            *
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the             *
+* GNU General Public License (http://www.gnu.org/licenses/gpl.txt)          *
+* for more details.                                                         *
+*                                                                           *
+****************************************************************************/
+
+/****************************************************************************
+History
+
+$Log: not supported by cvs2svn $
+Revision 1.2  2005/10/05 01:43:28  m_di_benedetto
+Removed "parent" pointer class member in Node class.
+
+Revision 1.1  2005/09/28 19:44:49  m_di_benedetto
+First Commit.
+
+
+****************************************************************************/
+
+#ifndef __VCGLIB_AABBBINARYTREEBASE_H
+#define __VCGLIB_AABBBINARYTREEBASE_H
+
+// standard headers
+#include <assert.h>
+
+// stl headers
+#include <vector>
+
+// vcg headers
+#include <vcg/space/point3.h>
+#include <vcg/space/box3.h>
+
+/***************************************************************************/
+
+namespace vcg {
+
+template <class OBJTYPE, class SCALARTYPE, class NODEAUXDATATYPE>
+class AABBBinaryTree {
+	public:
+		typedef AABBBinaryTree<OBJTYPE, SCALARTYPE, NODEAUXDATATYPE> ClassType;
+		typedef OBJTYPE ObjType;
+		typedef ObjType * ObjPtr;
+		typedef SCALARTYPE ScalarType;
+		typedef NODEAUXDATATYPE NodeAuxDataType;
+		typedef Point3<ScalarType> CoordType;
+
+		typedef std::vector<ObjPtr> ObjPtrVector;
+		typedef typename ObjPtrVector::iterator ObjPtrVectorIterator;
+		typedef typename ObjPtrVector::const_iterator ObjPtrVectorConstIterator;
+
+	public:
+		class AABBBinaryTreeNode {
+			public:
+				CoordType boxCenter;
+				CoordType boxHalfDims;
+				ObjPtrVectorIterator oBegin;
+				ObjPtrVectorIterator oEnd;
+				AABBBinaryTreeNode * children[2];
+				unsigned char splitAxis;
+				NodeAuxDataType auxData;
+
+				inline AABBBinaryTreeNode(void);
+				inline ~AABBBinaryTreeNode(void);
+
+				inline void Clear(void);
+				inline bool IsLeaf(void) const;
+				inline unsigned int ObjectsCount(void) const;
+
+				inline unsigned int & Flags(void);
+				inline const unsigned int & Flags(void) const;
+
+				inline ScalarType & ScalarValue(void);
+				inline const ScalarType & ScalarValue(void) const;
+
+				inline int & IntValue(void);
+				inline const int & IntValue(void) const;
+
+				inline unsigned int & UIntValue(void);
+				inline const unsigned int & UIntValue(void) const;
+
+				inline void * & PtrValue(void);
+				inline const void * & PtrValue(void) const;
+
+			protected:
+				union SharedDataUnion {
+					unsigned int flags;
+					int intValue;
+					unsigned int uintValue;
+					ScalarType scalarValue;
+					void * ptrValue;
+				};
+
+				SharedDataUnion sharedData;
+		};
+
+		typedef AABBBinaryTreeNode NodeType;
+
+		ObjPtrVector pObjects;
+		NodeType * pRoot;
+
+		inline AABBBinaryTree(void);
+		inline ~AABBBinaryTree(void);
+
+		inline void Clear(void);
+
+		template <class OBJITERATOR, class OBJITERATORPTRFUNCT, class OBJBOXFUNCT, class OBJBARYCENTERFUNCT>
+		inline bool Set(const OBJITERATOR & oBegin, const OBJITERATOR & oEnd, OBJITERATORPTRFUNCT & objPtr, OBJBOXFUNCT & objBox, OBJBARYCENTERFUNCT & objBarycenter, const unsigned int maxElemsPerLeaf = 1, const ScalarType & leafBoxMaxVolume = ((ScalarType)0), const bool useVariance = true);
+
+	protected:
+		template <class OBJBOXFUNCT, class OBJBARYCENTERFUNCT>
+		inline static NodeType * BoundObjects(const ObjPtrVectorIterator & oBegin, const ObjPtrVectorIterator & oEnd, const unsigned int size, const unsigned int maxElemsPerLeaf, const ScalarType & leafBoxMaxVolume, const bool useVariance, OBJBOXFUNCT & getBox, OBJBARYCENTERFUNCT & getBarycenter);
+
+		template <class OBJBARYCENTERFUNCT>
+		inline static int BalanceMedian(const ObjPtrVectorIterator & oBegin, const ObjPtrVectorIterator & oEnd, const int size, const int splitAxis, OBJBARYCENTERFUNCT & getBarycenter, ObjPtrVectorIterator & medianIter);
+};
+
+
+
+template <class OBJTYPE, class SCALARTYPE, class NODEAUXDATATYPE>
+AABBBinaryTree<OBJTYPE, SCALARTYPE, NODEAUXDATATYPE>::AABBBinaryTree(void) {
+	this->pObjects.clear();
+	this->pRoot = 0;
+}
+
+template <class OBJTYPE, class SCALARTYPE, class NODEAUXDATATYPE>
+AABBBinaryTree<OBJTYPE, SCALARTYPE, NODEAUXDATATYPE>::~AABBBinaryTree(void) {
+	this->pObjects.clear();
+	delete this->pRoot;
+}
+
+template <class OBJTYPE, class SCALARTYPE, class NODEAUXDATATYPE>
+void AABBBinaryTree<OBJTYPE, SCALARTYPE, NODEAUXDATATYPE>::Clear(void) {
+	this->pObjects.clear();
+	delete this->pRoot;
+	this->pRoot = 0;
+}
+
+template <class OBJTYPE, class SCALARTYPE, class NODEAUXDATATYPE>
+template <class OBJITERATOR, class OBJITERATORPTRFUNCT, class OBJBOXFUNCT, class OBJBARYCENTERFUNCT>
+bool AABBBinaryTree<OBJTYPE, SCALARTYPE, NODEAUXDATATYPE>::Set(const OBJITERATOR & oBegin, const OBJITERATOR & oEnd, OBJITERATORPTRFUNCT & objPtr, OBJBOXFUNCT & objBox, OBJBARYCENTERFUNCT & objBarycenter, const unsigned int maxElemsPerLeaf, const ScalarType & leafBoxMaxVolume, const bool useVariance) {
+	this->Clear();
+
+	if ((maxElemsPerLeaf == 0) && (leafBoxMaxVolume <= ((ScalarType)0))) {
+		return (false);
+	}
+
+	const unsigned int size = (unsigned int)std::distance(oBegin, oEnd);
+
+	this->pObjects.reserve(size);
+	for (OBJITERATOR oi=oBegin; oi!=oEnd; ++oi) {
+		this->pObjects.push_back(objPtr(*oi));
+	}
+
+	this->pRoot = ClassType::BoundObjects(this->pObjects.begin(), this->pObjects.end(), size, maxElemsPerLeaf, leafBoxMaxVolume, useVariance, objBox, objBarycenter);
+
+	return (this->pRoot != 0);
+}
+
+template <class OBJTYPE, class SCALARTYPE, class NODEAUXDATATYPE>
+template <class OBJBOXFUNCT, class OBJBARYCENTERFUNCT>
+typename AABBBinaryTree<OBJTYPE, SCALARTYPE, NODEAUXDATATYPE>::NodeType * AABBBinaryTree<OBJTYPE, SCALARTYPE, NODEAUXDATATYPE>::BoundObjects(const ObjPtrVectorIterator & oBegin, const ObjPtrVectorIterator & oEnd, const unsigned int size, const unsigned int maxElemsPerLeaf, const ScalarType & leafBoxMaxVolume, const bool useVariance, OBJBOXFUNCT & getBox, OBJBARYCENTERFUNCT & getBarycenter) {
+	if (size <= 0) {
+		return (0);
+	}
+
+	NodeType * pNode = new NodeType();
+	if (pNode == 0) {
+		return (0);
+	}
+
+	pNode->children[0] = 0;
+	pNode->children[1] = 0;
+
+	pNode->oBegin = oBegin;
+	pNode->oEnd = oEnd;
+
+	Box3<ScalarType> bbox;
+	bbox.SetNull();
+	for (ObjPtrVectorConstIterator oi=pNode->oBegin; oi!=pNode->oEnd; ++oi) {
+		Box3<ScalarType> tbox;
+		getBox(*(*oi), tbox);
+		bbox.Add(tbox);
+	}
+
+	pNode->boxCenter = bbox.Center();
+	pNode->boxHalfDims = bbox.Dim() / ((ScalarType)2);
+
+	const bool bMaxObjectsReached = (((maxElemsPerLeaf > 0) && (size <= maxElemsPerLeaf)) || (size == 1));
+	const bool bMaxVolumeReached = ((leafBoxMaxVolume > ((ScalarType)0)) && (bbox.Volume() <= leafBoxMaxVolume));
+	const bool isLeaf = bMaxObjectsReached || bMaxVolumeReached;
+
+	if (isLeaf) {
+		pNode->splitAxis = 0;
+		return (pNode);
+	}
+
+	CoordType pSplit;
+
+	if (useVariance) {
+		CoordType mean((ScalarType)0, (ScalarType)0, (ScalarType)0);
+		CoordType variance((ScalarType)0, (ScalarType)0, (ScalarType)0);
+		for (ObjPtrVectorIterator oi=oBegin; oi!=oEnd; ++oi) {
+			CoordType bc;
+			getBarycenter(*(*oi), bc);
+			mean += bc;
+			variance[0] += bc[0] * bc[0];
+			variance[1] += bc[1] * bc[1];
+			variance[2] += bc[2] * bc[2];
+		}
+		variance[0] -= (mean[0] * mean[0]) / ((ScalarType)size);
+		variance[1] -= (mean[1] * mean[1]) / ((ScalarType)size);
+		variance[2] -= (mean[2] * mean[2]) / ((ScalarType)size);
+		pSplit = variance;
+	}
+	else {
+		pSplit = pNode->boxHalfDims;
+	}
+
+	ScalarType maxDim = pSplit[0];
+	unsigned char splitAxis = 0;
+	if (maxDim < pSplit[1]) {
+		maxDim = pSplit[1];
+		splitAxis = 1;
+	}
+	if (maxDim < pSplit[2]) {
+		maxDim = pSplit[2];
+		splitAxis = 2;
+	}
+
+	pNode->splitAxis = splitAxis;
+
+	ObjPtrVectorIterator median;
+	const int lSize = ClassType::BalanceMedian(pNode->oBegin, pNode->oEnd, size, splitAxis, getBarycenter, median);
+	const int rSize = size - lSize;
+
+	if (lSize > 0) {
+		pNode->children[0] = ClassType::BoundObjects(pNode->oBegin, median, lSize, maxElemsPerLeaf, leafBoxMaxVolume, useVariance, getBox, getBarycenter);
+		if (pNode->children[0] == 0) {
+			delete pNode;
+			return (0);
+		}
+	}
+
+	if (rSize > 0) {
+		pNode->children[1] = ClassType::BoundObjects(median, pNode->oEnd, rSize, maxElemsPerLeaf, leafBoxMaxVolume, useVariance, getBox, getBarycenter);
+		if (pNode->children[1] == 0) {
+			delete pNode;
+			return (0);
+		}
+	}
+
+	return (pNode);
+}
+
+template <class OBJTYPE, class SCALARTYPE, class NODEAUXDATATYPE>
+template <class OBJBARYCENTERFUNCT>
+int AABBBinaryTree<OBJTYPE, SCALARTYPE, NODEAUXDATATYPE>::BalanceMedian(const ObjPtrVectorIterator & oBegin, const ObjPtrVectorIterator & oEnd, const int size, const int splitAxis, OBJBARYCENTERFUNCT & getBarycenter, ObjPtrVectorIterator & medianIter) {
+	const int iMedian = (size + 1) / 2;
+
+	ObjPtrVectorIterator l, r, i, j;
+	ObjPtr iTmp;
+	ScalarType pos;
+	ObjPtrVectorIterator median = oBegin + iMedian;
+	CoordType bc;
+
+	l = oBegin;
+	r = oEnd - 1;
+
+	while (l < r) {
+		getBarycenter(*(*r), bc);
+		pos = bc[splitAxis];
+
+		i = l;
+		j = r - 1;
+
+		while (true) {
+			getBarycenter(*(*i), bc);
+			while ((bc[splitAxis] <= pos) && (i < r)) {
+				i++;
+				getBarycenter(*(*i), bc);
+			}
+			getBarycenter(*(*j), bc);
+			while ((bc[splitAxis] > pos) && (j > l)) {
+				j--;
+				getBarycenter(*(*j), bc);
+			}
+			if (i >= j) {
+				break;
+			}
+			iTmp = (*i);
+			(*i) = (*j);
+			(*j) = iTmp;
+		}
+
+		iTmp = (*i);
+		(*i) = (*r);
+		(*r) = iTmp;
+
+		if (i >= (median)) {
+			r = i - 1;
+		}
+		if (i <= (median)) {
+			l = i + 1;
+		}
+	}
+
+	medianIter = median;
+
+	return (iMedian);
+}
+
+template <class OBJTYPE, class SCALARTYPE, class NODEAUXDATATYPE>
+AABBBinaryTree<OBJTYPE, SCALARTYPE, NODEAUXDATATYPE>::AABBBinaryTreeNode::AABBBinaryTreeNode(void) {
+	this->children[0] = 0;
+	this->children[1] = 0;
+}
+
+template <class OBJTYPE, class SCALARTYPE, class NODEAUXDATATYPE>
+AABBBinaryTree<OBJTYPE, SCALARTYPE, NODEAUXDATATYPE>::AABBBinaryTreeNode::~AABBBinaryTreeNode(void) {
+	delete this->children[0];
+	delete this->children[1];
+}
+
+template <class OBJTYPE, class SCALARTYPE, class NODEAUXDATATYPE>
+void AABBBinaryTree<OBJTYPE, SCALARTYPE, NODEAUXDATATYPE>::AABBBinaryTreeNode::Clear(void) {
+	delete this->children[0];
+	this->children[0] = 0;
+
+	delete this->children[1];
+	this->children[1] = 0;
+}
+
+template <class OBJTYPE, class SCALARTYPE, class NODEAUXDATATYPE>
+bool AABBBinaryTree<OBJTYPE, SCALARTYPE, NODEAUXDATATYPE>::AABBBinaryTreeNode::IsLeaf(void) const {
+	return ((this->children[0] == 0) && (this->children[1] == 0));
+}
+
+template <class OBJTYPE, class SCALARTYPE, class NODEAUXDATATYPE>
+unsigned int AABBBinaryTree<OBJTYPE, SCALARTYPE, NODEAUXDATATYPE>::AABBBinaryTreeNode::ObjectsCount(void) const {
+	return ((unsigned int)(std::distance(this->oBegin, this->oEnd)));
+}
+
+template <class OBJTYPE, class SCALARTYPE, class NODEAUXDATATYPE>
+unsigned int & AABBBinaryTree<OBJTYPE, SCALARTYPE, NODEAUXDATATYPE>::AABBBinaryTreeNode::Flags(void) {
+	return (this->sharedData.flags);
+}
+
+template <class OBJTYPE, class SCALARTYPE, class NODEAUXDATATYPE>
+const unsigned int & AABBBinaryTree<OBJTYPE, SCALARTYPE, NODEAUXDATATYPE>::AABBBinaryTreeNode::Flags(void) const {
+	return (this->sharedData.flags);
+}
+
+template <class OBJTYPE, class SCALARTYPE, class NODEAUXDATATYPE>
+int & AABBBinaryTree<OBJTYPE, SCALARTYPE, NODEAUXDATATYPE>::AABBBinaryTreeNode::IntValue(void) {
+	return (this->sharedData.intValue);
+}
+
+template <class OBJTYPE, class SCALARTYPE, class NODEAUXDATATYPE>
+const int & AABBBinaryTree<OBJTYPE, SCALARTYPE, NODEAUXDATATYPE>::AABBBinaryTreeNode::IntValue(void) const {
+	return (this->sharedData.intValue);
+}
+
+template <class OBJTYPE, class SCALARTYPE, class NODEAUXDATATYPE>
+unsigned int & AABBBinaryTree<OBJTYPE, SCALARTYPE, NODEAUXDATATYPE>::AABBBinaryTreeNode::UIntValue(void) {
+	return (this->sharedData.uintValue);
+}
+
+template <class OBJTYPE, class SCALARTYPE, class NODEAUXDATATYPE>
+const unsigned int & AABBBinaryTree<OBJTYPE, SCALARTYPE, NODEAUXDATATYPE>::AABBBinaryTreeNode::UIntValue(void) const {
+	return (this->sharedData.uintValue);
+}
+
+template <class OBJTYPE, class SCALARTYPE, class NODEAUXDATATYPE>
+typename AABBBinaryTree<OBJTYPE, SCALARTYPE, NODEAUXDATATYPE>::ScalarType & AABBBinaryTree<OBJTYPE, SCALARTYPE, NODEAUXDATATYPE>::AABBBinaryTreeNode::ScalarValue(void) {
+	return (this->sharedData.scalarValue);
+}
+
+template <class OBJTYPE, class SCALARTYPE, class NODEAUXDATATYPE>
+const typename AABBBinaryTree<OBJTYPE, SCALARTYPE, NODEAUXDATATYPE>::ScalarType & AABBBinaryTree<OBJTYPE, SCALARTYPE, NODEAUXDATATYPE>::AABBBinaryTreeNode::ScalarValue(void) const {
+	return (this->sharedData.scalarValue);
+}
+
+template <class OBJTYPE, class SCALARTYPE, class NODEAUXDATATYPE>
+void * & AABBBinaryTree<OBJTYPE, SCALARTYPE, NODEAUXDATATYPE>::AABBBinaryTreeNode::PtrValue(void) {
+	return (this->sharedData.ptrValue);
+}
+
+template <class OBJTYPE, class SCALARTYPE, class NODEAUXDATATYPE>
+const void * & AABBBinaryTree<OBJTYPE, SCALARTYPE, NODEAUXDATATYPE>::AABBBinaryTreeNode::PtrValue(void) const {
+	return (this->sharedData.ptrValue);
+}
+
+}	// end namespace vcg
+
+#endif // #ifndef __VCGLIB_AABBBINARYTREEBASE_H

vcglib/vcg/space/index/aabb_binary_tree/closest.h

@@ -0,0 +1,153 @@
+/****************************************************************************
+* VCGLib                                                            o o     *
+* Visual and Computer Graphics Library                            o     o   *
+*                                                                _   O  _   *
+* Copyright(C) 2004-2016                                           \/)\/    *
+* Visual Computing Lab                                            /\/|      *
+* ISTI - Italian National Research Council                           |      *
+*                                                                    \      *
+* All rights reserved.                                                      *
+*                                                                           *
+* This program is free software; you can redistribute it and/or modify      *   
+* it under the terms of the GNU General Public License as published by      *
+* the Free Software Foundation; either version 2 of the License, or         *
+* (at your option) any later version.                                       *
+*                                                                           *
+* This program is distributed in the hope that it will be useful,           *
+* but WITHOUT ANY WARRANTY; without even the implied warranty of            *
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the             *
+* GNU General Public License (http://www.gnu.org/licenses/gpl.txt)          *
+* for more details.                                                         *
+*                                                                           *
+****************************************************************************/
+
+/****************************************************************************
+History
+
+$Log: not supported by cvs2svn $
+Revision 1.1  2005/09/26 18:33:16  m_di_benedetto
+First Commit.
+
+
+****************************************************************************/
+
+#ifndef __VCGLIB_AABBBINARYTREE_CLOSEST_H
+#define __VCGLIB_AABBBINARYTREE_CLOSEST_H
+
+// stl headers
+#include <limits>
+#include <vector>
+
+// vcg headers
+#include <vcg/math/base.h>
+#include <vcg/space/index/aabb_binary_tree/base.h>
+#include <wrap/utils.h>
+
+/***************************************************************************/
+
+namespace vcg {
+
+template <class TREETYPE>
+class AABBBinaryTreeClosest {
+public:
+	typedef AABBBinaryTreeClosest<TREETYPE> ClassType;
+	typedef TREETYPE TreeType;
+	typedef typename TreeType::ScalarType ScalarType;
+	typedef typename TreeType::CoordType CoordType;
+	typedef typename TreeType::NodeType NodeType;
+	typedef typename TreeType::ObjPtr ObjPtr;
+
+	template <class OBJPOINTDISTANCEFUNCT>
+	static inline ObjPtr Closest(TreeType & tree, OBJPOINTDISTANCEFUNCT & getPointDistance, const CoordType & p, const ScalarType & maxDist, ScalarType & minDist, CoordType & q) {
+		typedef std::vector<NodeType *> NodePtrVector;
+		typedef typename NodePtrVector::const_iterator NodePtrVector_ci;
+
+		NodeType * pRoot = tree.pRoot;
+
+		if (pRoot == 0) {
+			return (0);
+		}
+
+		NodePtrVector clist1;
+		NodePtrVector clist2;
+		NodePtrVector leaves;
+
+		NodePtrVector * candidates = &clist1;
+		NodePtrVector * newCandidates = &clist2;
+
+		clist1.reserve(tree.pObjects.size());
+		clist2.reserve(tree.pObjects.size());
+		leaves.reserve(tree.pObjects.size());
+
+		clist1.resize(0);
+		clist2.resize(0);
+		leaves.resize(0);
+
+		ScalarType minMaxDist = maxDist * maxDist;
+
+		candidates->push_back(pRoot);
+
+		while (!candidates->empty()) {
+			newCandidates->resize(0);
+
+			for (NodePtrVector_ci bv=candidates->begin(); bv!=candidates->end(); ++bv) {
+				const CoordType dc = Abs(p - (*bv)->boxCenter);
+				const ScalarType maxDist = (dc + (*bv)->boxHalfDims).SquaredNorm();
+				(*bv)->ScalarValue() = LowClampToZero(dc - (*bv)->boxHalfDims).SquaredNorm();
+				if (maxDist < minMaxDist) {
+					minMaxDist = maxDist;
+				}
+			}
+
+			for (NodePtrVector_ci ci=candidates->begin(); ci!=candidates->end(); ++ci) {
+				if ((*ci)->ScalarValue() < minMaxDist) {
+					if ((*ci)->IsLeaf()) {
+						leaves.push_back(*ci);
+					}
+					else {
+						if ((*ci)->children[0] != 0) {
+							newCandidates->push_back((*ci)->children[0]);
+						}
+						if ((*ci)->children[1] != 0) {
+							newCandidates->push_back((*ci)->children[1]);
+						}
+					}
+				}
+			}
+			NodePtrVector * cSwap = candidates;
+			candidates = newCandidates;
+			newCandidates = cSwap;
+		}
+
+		clist1.clear();
+		clist2.clear();
+
+		ObjPtr closestObject = 0;
+		CoordType closestPoint;
+		ScalarType closestDist = math::Sqrt(minMaxDist) + std::numeric_limits<ScalarType>::epsilon();
+		ScalarType closestDistSq = closestDist * closestDist;
+
+
+		for (NodePtrVector_ci ci=leaves.begin(); ci!=leaves.end(); ++ci) {
+			if ((*ci)->ScalarValue() < closestDistSq) {
+				for (typename TreeType::ObjPtrVectorConstIterator si=(*ci)->oBegin; si!=(*ci)->oEnd; ++si) {
+					if (getPointDistance(*(*si), p, closestDist, closestPoint)) {
+						closestDistSq = closestDist * closestDist;
+						closestObject = (*si);
+            q = closestPoint;
+            minDist = closestDist;
+					}
+				}
+			}
+		}
+
+		leaves.clear();
+
+		return (closestObject);
+	}
+
+};
+
+} // end namespace vcg
+
+#endif // #ifndef __VCGLIB_AABBBINARYTREE_CLOSEST_H

vcglib/vcg/space/index/aabb_binary_tree/frustum_cull.h

@@ -0,0 +1,284 @@
+/****************************************************************************
+* VCGLib                                                            o o     *
+* Visual and Computer Graphics Library                            o     o   *
+*                                                                _   O  _   *
+* Copyright(C) 2004-2016                                           \/)\/    *
+* Visual Computing Lab                                            /\/|      *
+* ISTI - Italian National Research Council                           |      *
+*                                                                    \      *
+* All rights reserved.                                                      *
+*                                                                           *
+* This program is free software; you can redistribute it and/or modify      *   
+* it under the terms of the GNU General Public License as published by      *
+* the Free Software Foundation; either version 2 of the License, or         *
+* (at your option) any later version.                                       *
+*                                                                           *
+* This program is distributed in the hope that it will be useful,           *
+* but WITHOUT ANY WARRANTY; without even the implied warranty of            *
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the             *
+* GNU General Public License (http://www.gnu.org/licenses/gpl.txt)          *
+* for more details.                                                         *
+*                                                                           *
+****************************************************************************/
+
+/****************************************************************************
+History
+
+$Log: not supported by cvs2svn $
+Revision 1.8  2005/11/30 09:57:13  m_di_benedetto
+Added methods to flag visibility.
+
+Revision 1.7  2005/10/26 11:42:03  m_di_benedetto
+Added PASS_THROUGH flags.
+
+Revision 1.6  2005/10/15 19:14:35  m_di_benedetto
+Modified objapplyfunctor to nodeapplyfunctor.
+
+Revision 1.5  2005/10/05 01:59:56  m_di_benedetto
+First Commit, new version.
+
+Revision 1.3  2005/09/29 22:20:49  m_di_benedetto
+Removed '&' in FrustumCull() method.
+
+Revision 1.2  2005/09/28 19:57:18  m_di_benedetto
+#included aabb tree base.
+
+Revision 1.1  2005/09/26 18:33:16  m_di_benedetto
+First Commit.
+
+
+****************************************************************************/
+
+#ifndef __VCGLIB_AABBBINARYTREE_FRUSTUMCULL_H
+#define __VCGLIB_AABBBINARYTREE_FRUSTUMCULL_H
+
+// std headers
+/* EMPTY */
+
+// vcg headers
+#include <vcg/space/point3.h>
+#include <vcg/space/plane3.h>
+#include <vcg/space/index/aabb_binary_tree/base.h>
+
+/***************************************************************************/
+
+namespace vcg {
+
+template <class TREETYPE>
+class AABBBinaryTreeFrustumCull {
+public:
+	typedef AABBBinaryTreeFrustumCull<TREETYPE> ClassType;
+	typedef TREETYPE TreeType;
+	typedef typename TreeType::ScalarType ScalarType;
+	typedef typename TreeType::CoordType CoordType;
+	typedef typename TreeType::NodeType NodeType;
+	typedef typename TreeType::ObjPtr ObjPtr;
+
+protected:
+	class VFrustumPlane {
+	public:
+		CoordType normal;
+		ScalarType offset;
+		unsigned int pVertexIndex[3];
+	};
+
+	class VFrustum {
+	public:
+		VFrustumPlane planes[6];
+	};
+
+public:
+	enum {
+		FC_FIRST_PLANE_BIT					= 0,
+		FC_PARTIALLY_VISIBLE_BIT		= (1 << (FC_FIRST_PLANE_BIT + 3)),
+		FC_FULLY_VISIBLE_BIT				= (1 << (FC_FIRST_PLANE_BIT + 4)),
+		FC_PASS_THROUGH_FIRST_BIT		= (FC_FIRST_PLANE_BIT + 5)
+	};
+
+	static inline bool IsPartiallyVisible(const NodeType * node) {
+		return ((node->Flags() & FC_PARTIALLY_VISIBLE_BIT) != 0);
+	}
+
+	static inline bool IsFullyVisible(const NodeType * node) {
+		return ((node->Flags() & FC_FULLY_VISIBLE_BIT) != 0);
+	}
+
+	static inline void SetFullyVisible(NodeType * node) {
+		node->Flags() |= FC_FULLY_VISIBLE_BIT;
+	}
+
+	static inline void SetInvisible(NodeType * node) {
+		node->Flags() &= ~(FC_FULLY_VISIBLE_BIT | FC_PARTIALLY_VISIBLE_BIT);
+	}
+
+	static inline bool IsVisible(const NodeType * node) {
+		return ((node->Flags() & (FC_PARTIALLY_VISIBLE_BIT | FC_FULLY_VISIBLE_BIT)) != 0);
+	}
+
+	static inline unsigned int PassThrough(const NodeType * node) {
+		return ((node->Flags() >> FC_PASS_THROUGH_FIRST_BIT) & 0x03);
+	}
+
+	static inline void Initialize(TreeType & tree) {
+		NodeType * pRoot = tree.pRoot;
+		if (pRoot == 0) {
+			return;
+		}
+		ClassType::InitializeNodeFlagsRec(pRoot);
+	}
+
+	template <class NODEAPPLYFUNCTOR>
+	static inline void FrustumCull(TreeType & tree, const Point3<ScalarType> & viewerPosition, const Plane3<ScalarType> frustumPlanes[6], const unsigned int minNodeObjectsCount, NODEAPPLYFUNCTOR & nodeApply) {
+		NodeType * pRoot = tree.pRoot;
+		if (pRoot == 0) {
+			return;
+		}
+
+		VFrustum frustum;
+		for (int i=0; i<6; ++i) {
+			frustum.planes[i].normal = frustumPlanes[i].Direction();
+			frustum.planes[i].offset = frustumPlanes[i].Offset();
+			frustum.planes[i].pVertexIndex[0] = (frustum.planes[i].normal[0] >= ((ScalarType)0)) ? (1) : (0);
+			frustum.planes[i].pVertexIndex[1] = (frustum.planes[i].normal[1] >= ((ScalarType)0)) ? (1) : (0);
+			frustum.planes[i].pVertexIndex[2] = (frustum.planes[i].normal[2] >= ((ScalarType)0)) ? (1) : (0);
+		}
+
+		const unsigned char inMask = 0x3F;
+		ClassType::NodeVsFrustum(tree.pRoot, viewerPosition, frustum, inMask, minNodeObjectsCount, nodeApply);
+	}
+
+protected:
+
+	static inline void InitializeNodeFlagsRec(NodeType * node) {
+		//node->Flags() &= ~(0x7F);
+		node->Flags() = 0;
+		if (node->children[0] != 0) {
+			ClassType::InitializeNodeFlagsRec(node->children[0]);
+		}
+		if (node->children[1] != 0) {
+			ClassType::InitializeNodeFlagsRec(node->children[1]);
+		}
+	}
+
+	template <class NODEAPPLYFUNCTOR>
+	static inline void NodeVsFrustum(NodeType * node, const Point3<ScalarType> & viewerPosition, const VFrustum & f, unsigned char inMask, unsigned int minNodeObjectsCount, NODEAPPLYFUNCTOR & nodeApply) {
+		if (node == 0) {
+			return;
+		}
+
+		const CoordType bminmax[2] = {
+			node->boxCenter - node->boxHalfDims,
+			node->boxCenter + node->boxHalfDims,
+		};
+		const unsigned int firstFail = (unsigned int)((node->Flags() >> FC_FIRST_PLANE_BIT) & 0x7);
+		const VFrustumPlane * fp = f.planes + firstFail;
+		unsigned char k = 1 << firstFail;
+		unsigned char newMask = 0x0;
+		bool fullInside = true;
+
+		node->Flags() &= ~(1 << 25);
+		node->Flags() &= ~(FC_PARTIALLY_VISIBLE_BIT | FC_FULLY_VISIBLE_BIT | (0x03 << FC_PASS_THROUGH_FIRST_BIT));
+
+		if ((k & inMask) != 0) {
+			if (
+				((fp->normal[0] * bminmax[fp->pVertexIndex[0]][0]) +
+				(fp->normal[1] * bminmax[fp->pVertexIndex[1]][1]) +
+				(fp->normal[2] * bminmax[fp->pVertexIndex[2]][2]) -
+				(fp->offset)) < ((ScalarType)0)
+			) {
+				return;
+			}
+
+			if (
+				((fp->normal[0] * bminmax[1 - fp->pVertexIndex[0]][0]) +
+				(fp->normal[1] * bminmax[1 - fp->pVertexIndex[1]][1]) +
+				(fp->normal[2] * bminmax[1 - fp->pVertexIndex[2]][2]) -
+				(fp->offset)) < ((ScalarType)0)
+			) {
+				newMask |=  k;
+				fullInside = false;
+			}
+		}
+
+		k = 1;
+		for (unsigned int i=0; k<=inMask; ++i, k<<=1) {
+			if ((i != firstFail) && ((k & inMask) != 0)) {
+				fp = f.planes + i;
+				if (
+					((fp->normal[0] * bminmax[fp->pVertexIndex[0]][0]) +
+					(fp->normal[1] * bminmax[fp->pVertexIndex[1]][1]) +
+					(fp->normal[2] * bminmax[fp->pVertexIndex[2]][2]) -
+					(fp->offset)) < ((ScalarType)0)
+				) {
+					node->Flags() = (node->Flags() & ((~0x0) & (0x7 << ClassType::FC_FIRST_PLANE_BIT))) | (i << ClassType::FC_FIRST_PLANE_BIT);
+					return;
+				}
+
+				if (
+					((fp->normal[0] * bminmax[1 - fp->pVertexIndex[0]][0]) +
+					(fp->normal[1] * bminmax[1 - fp->pVertexIndex[1]][1]) +
+					(fp->normal[2] * bminmax[1 - fp->pVertexIndex[2]][2]) -
+					(fp->offset)) < ((ScalarType)0)
+				) {
+					newMask |=  k;
+					fullInside = false;
+				}
+			}
+		}
+
+		// Intermediate BVs containing a sufficient number of objects are marked fully visible even if they don't
+		if (fullInside || (node->ObjectsCount() <= minNodeObjectsCount)) {
+			node->Flags() |= FC_FULLY_VISIBLE_BIT;
+			nodeApply(*node);
+			return;
+		}
+
+		node->Flags() |= FC_PARTIALLY_VISIBLE_BIT;
+
+		if ((node->IsLeaf()))
+		{
+			nodeApply(*node);
+			return;
+		}
+
+		//ClassType::NodeVsFrustum(node->children[0], viewerPosition, f, newMask, minNodeObjectsCount, nodeApply);
+		//ClassType::NodeVsFrustum(node->children[1], viewerPosition, f, newMask, minNodeObjectsCount, nodeApply);
+		ScalarType dt;
+		if (node->splitAxis == 0) {
+			dt = viewerPosition[0] - node->boxCenter[0];
+		}
+		else if (node->splitAxis == 1) {
+			dt = viewerPosition[1] - node->boxCenter[1];
+		}
+		else {
+			dt = viewerPosition[2] - node->boxCenter[2];
+		}
+
+		if (dt <= (ScalarType)0) {
+			ClassType::NodeVsFrustum(node->children[0], viewerPosition, f, newMask, minNodeObjectsCount, nodeApply);
+			ClassType::NodeVsFrustum(node->children[1], viewerPosition, f, newMask, minNodeObjectsCount, nodeApply);
+		}
+		else {
+			ClassType::NodeVsFrustum(node->children[1], viewerPosition, f, newMask, minNodeObjectsCount, nodeApply);
+			ClassType::NodeVsFrustum(node->children[0], viewerPosition, f, newMask, minNodeObjectsCount, nodeApply);
+		}
+
+		const bool c0 = (node->children[0] != 0) && ClassType::IsVisible(node->children[0]);
+		const bool c1 = (node->children[1] != 0) && ClassType::IsVisible(node->children[1]);
+
+		if (c0 != c1) {
+				node->Flags() &= ~(0x03 << FC_PASS_THROUGH_FIRST_BIT);
+			if (c0) {
+				node->Flags() |= (0x01 << FC_PASS_THROUGH_FIRST_BIT);
+			}
+			else {
+				node->Flags() |= (0x02 << FC_PASS_THROUGH_FIRST_BIT);
+			}
+		}
+	}
+
+};
+
+}	// end namespace vcg
+
+#endif // #ifndef __VCGLIB_AABBBINARYTREE_FRUSTUMCULL_H

vcglib/vcg/space/index/aabb_binary_tree/kclosest.h

@@ -0,0 +1,159 @@
+/****************************************************************************
+* VCGLib                                                            o o     *
+* Visual and Computer Graphics Library                            o     o   *
+*                                                                _   O  _   *
+* Copyright(C) 2004-2016                                           \/)\/    *
+* Visual Computing Lab                                            /\/|      *
+* ISTI - Italian National Research Council                           |      *
+*                                                                    \      *
+* All rights reserved.                                                      *
+*                                                                           *
+* This program is free software; you can redistribute it and/or modify      *   
+* it under the terms of the GNU General Public License as published by      *
+* the Free Software Foundation; either version 2 of the License, or         *
+* (at your option) any later version.                                       *
+*                                                                           *
+* This program is distributed in the hope that it will be useful,           *
+* but WITHOUT ANY WARRANTY; without even the implied warranty of            *
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the             *
+* GNU General Public License (http://www.gnu.org/licenses/gpl.txt)          *
+* for more details.                                                         *
+*                                                                           *
+****************************************************************************/
+
+/****************************************************************************
+History
+
+$Log: not supported by cvs2svn $
+Revision 1.1  2005/09/26 18:33:16  m_di_benedetto
+First Commit.
+
+
+****************************************************************************/
+
+#ifndef __VCGLIB_AABBBINARYTREE_KCLOSEST_H
+#define __VCGLIB_AABBBINARYTREE_KCLOSEST_H
+
+// stl headers
+#include <queue>
+#include <deque>
+
+// vcg headers
+#include <vcg/space/index/aabb_binary_tree/base.h>
+#include <wrap/utils.h>
+
+/***************************************************************************/
+
+namespace vcg {
+
+template <class TREETYPE>
+class AABBBinaryTreeKClosest {
+public:
+	typedef AABBBinaryTreeKClosest<TREETYPE> ClassType;
+	typedef TREETYPE TreeType;
+	typedef typename TreeType::ScalarType ScalarType;
+	typedef typename TreeType::CoordType CoordType;
+	typedef typename TreeType::NodeType NodeType;
+	typedef typename TreeType::ObjPtr ObjPtr;
+
+protected:
+		class ClosestObjType {
+		public:
+			ObjPtr pObj;
+			ScalarType minDist;
+			CoordType closestPt;
+		};
+
+		class CompareClosest {
+		public:
+			bool operator () (const ClosestObjType & a, const ClosestObjType & b) {
+				return (a.minDist < b.minDist);
+			}
+		};
+
+		typedef std::priority_queue<ClosestObjType, std::deque<ClosestObjType>, CompareClosest> PQueueType;
+
+public:
+	template <class OBJPOINTDISTANCEFUNCT, class OBJPTRCONTAINERTYPE, class DISTCONTAINERTYPE, class POINTCONTAINERTYPE>
+	static inline unsigned int KClosest(TreeType & tree, OBJPOINTDISTANCEFUNCT & getPointDistance, const unsigned int k, const CoordType & p, const ScalarType & maxDist, OBJPTRCONTAINERTYPE & objects, DISTCONTAINERTYPE & distances, POINTCONTAINERTYPE & points) {
+
+		if (k == 0) {
+			return (0);
+		}
+
+		NodeType * pRoot = tree.pRoot;
+
+		if (pRoot == 0) {
+			return (0);
+		}
+
+		PQueueType pq;
+		ScalarType mindmax = maxDist;
+
+		ClassType::DepthFirstCollect(pRoot, getPointDistance, k, p, mindmax, pq);
+
+		const unsigned int sz = (unsigned int)(pq.size());
+
+		while (!pq.empty()) {
+			ClosestObjType cobj = pq.top();
+			pq.pop();
+			objects.push_back(cobj.pObj);
+			distances.push_back(cobj.minDist);
+			points.push_back(cobj.closestPt);
+		}
+
+		return (sz);
+	}
+
+protected:
+	template <class OBJPOINTDISTANCEFUNCT>
+	static void DepthFirstCollect(NodeType * node, OBJPOINTDISTANCEFUNCT & getPointDistance, const unsigned int k, const CoordType & p, ScalarType & mindmax, PQueueType & pq) {
+		const CoordType dc = Abs(p - node->boxCenter);
+
+		if (pq.size() >= k) {
+			const ScalarType dmin = LowClampToZero(dc - node->boxHalfDims).SquaredNorm();
+			if (dmin >= mindmax) {
+				return;
+			}
+		}
+
+		if (node->IsLeaf()) {
+			bool someInserted = true;
+			for (typename TreeType::ObjPtrVectorConstIterator si=node->oBegin; si!=node->oEnd; ++si) {
+				ScalarType minDst = (pq.size() >= k) ? (pq.top().minDist) : (mindmax);
+				ClosestObjType cobj;
+				if (getPointDistance(*(*si), p, minDst, cobj.closestPt)) {
+					someInserted = true;
+					cobj.pObj = (*si);
+					cobj.minDist = minDst;
+					if (pq.size() >= k) {
+						pq.pop();
+					}
+					pq.push(cobj);
+				}
+			}
+			if (someInserted) {
+				if (pq.size() >= k) {
+					const ScalarType dmax = pq.top().minDist;
+					const ScalarType sqdmax = dmax * dmax;
+					if (sqdmax < mindmax) {
+						mindmax = sqdmax;
+					}
+				}
+			}
+		}
+		else {
+			if (node->children[0] != 0) {
+				ClassType::DepthFirstCollect(node->children[0], getPointDistance, k, p, mindmax, pq);
+			}
+			if (node->children[1] != 0) {
+				ClassType::DepthFirstCollect(node->children[1], getPointDistance, k, p, mindmax, pq);
+			}
+		}
+	}
+
+};
+
+} // end namespace vcg
+
+#endif // #ifndef __VCGLIB_AABBBINARYTREE_KCLOSEST_H

vcglib/vcg/space/index/aabb_binary_tree/ray.h

@@ -0,0 +1,163 @@
+/****************************************************************************
+* VCGLib                                                            o o     *
+* Visual and Computer Graphics Library                            o     o   *
+*                                                                _   O  _   *
+* Copyright(C) 2004-2016                                           \/)\/    *
+* Visual Computing Lab                                            /\/|      *
+* ISTI - Italian National Research Council                           |      *
+*                                                                    \      *
+* All rights reserved.                                                      *
+*                                                                           *
+* This program is free software; you can redistribute it and/or modify      *   
+* it under the terms of the GNU General Public License as published by      *
+* the Free Software Foundation; either version 2 of the License, or         *
+* (at your option) any later version.                                       *
+*                                                                           *
+* This program is distributed in the hope that it will be useful,           *
+* but WITHOUT ANY WARRANTY; without even the implied warranty of            *
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the             *
+* GNU General Public License (http://www.gnu.org/licenses/gpl.txt)          *
+* for more details.                                                         *
+*                                                                           *
+****************************************************************************/
+
+/****************************************************************************
+History
+
+$Log: not supported by cvs2svn $
+Revision 1.3  2005/10/05 01:40:56  m_di_benedetto
+Node children are now tested in ascending ray-T order.
+
+Revision 1.2  2005/09/28 19:48:31  m_di_benedetto
+Removed hit point parameter, #included aabbtree base.
+
+Revision 1.1  2005/09/26 18:33:16  m_di_benedetto
+First Commit.
+
+
+****************************************************************************/
+
+#ifndef __VCGLIB_AABBBINARYTREE_RAY_H
+#define __VCGLIB_AABBBINARYTREE_RAY_H
+
+// vcg headers
+#include <vcg/space/ray3.h>
+#include <vcg/space/index/aabb_binary_tree/base.h>
+
+/***************************************************************************/
+
+namespace vcg {
+
+template <class TREETYPE>
+class AABBBinaryTreeRay {
+public:
+	typedef AABBBinaryTreeRay<TREETYPE> ClassType;
+	typedef TREETYPE TreeType;
+	typedef typename TreeType::ScalarType ScalarType;
+	typedef typename TreeType::CoordType CoordType;
+	typedef typename TreeType::NodeType NodeType;
+	typedef typename TreeType::ObjPtr ObjPtr;
+
+	template <class OBJRAYISECTFUNCT>
+	static inline ObjPtr Ray(TreeType & tree, OBJRAYISECTFUNCT & rayIntersection, const Ray3<ScalarType> & ray, const ScalarType & maxDist, ScalarType & t) {
+
+		NodeType * pRoot = tree.pRoot;
+
+		if (pRoot == 0) {
+			return (0);
+		}
+
+		const CoordType & rayDirection = ray.Direction();
+
+		t = maxDist / rayDirection.Norm();
+
+		Ray3Ex rayex;
+		rayex.r = ray;
+		rayex.invDirection[0] = ((ScalarType)1) / rayDirection[0];
+		rayex.invDirection[1] = ((ScalarType)1) / rayDirection[1];
+		rayex.invDirection[2] = ((ScalarType)1) / rayDirection[2];
+		rayex.sign[0] = (rayex.invDirection[0] < ((ScalarType)0)) ? (1) : (0);
+		rayex.sign[1] = (rayex.invDirection[1] < ((ScalarType)0)) ? (1) : (0);
+		rayex.sign[2] = (rayex.invDirection[2] < ((ScalarType)0)) ? (1) : (0);
+
+		ObjPtr closestObj = 0;
+		ClassType::DepthFirstRayIsect(pRoot, rayIntersection, rayex, t, closestObj);
+
+		return (closestObj);
+	}
+
+protected:
+	class Ray3Ex {
+	public:
+		Ray3<ScalarType> r;
+		CoordType invDirection;
+		unsigned char sign[3];
+	};
+
+	static inline bool IntersectionBoxRay(const CoordType & boxCenter, const CoordType & boxHalfDims, const Ray3Ex & ray, ScalarType & t0) {
+		const CoordType bounds[2] = {
+			boxCenter - boxHalfDims,
+			boxCenter + boxHalfDims
+		};
+		ScalarType tmin, tmax;
+		ScalarType tcmin, tcmax;
+		const CoordType & origin = ray.r.Origin();
+
+		tmin = (bounds[ray.sign[0]][0] - origin[0]) * ray.invDirection[0];
+		tmax = (bounds[1 - ray.sign[0]][0] - origin[0]) * ray.invDirection[0];
+
+		tcmin = (bounds[ray.sign[1]][1] - origin[1]) * ray.invDirection[1];
+		tcmax = (bounds[1 - ray.sign[1]][1] - origin[1]) * ray.invDirection[1];
+
+		if ((tmin > tcmax) || (tcmin > tmax)) { return (false);	}
+
+		if (tcmin > tmin) { tmin = tcmin; }
+		if (tcmax < tmax) { tmax = tcmax; }
+		tcmin = (bounds[ray.sign[2]][2] - origin[2]) * ray.invDirection[2];
+		tcmax = (bounds[1-ray.sign[2]][2] - origin[2]) * ray.invDirection[2];
+
+		if ((tmin > tcmax) || (tcmin > tmax)) { return (false); }
+
+		if (tcmin > tmin) { tmin = tcmin; }
+		//if (tcmax < tmax) { tmax = tcmax; }
+		t0 = (tmin >= ((ScalarType)0)) ? (tmin) :((ScalarType)0);
+
+		return (true);
+	}
+
+	template <class OBJRAYISECTFUNCT>
+	static inline void DepthFirstRayIsect(const NodeType * node, OBJRAYISECTFUNCT & rayIntersection, const Ray3Ex & ray, ScalarType & rayT, ObjPtr & closestObj) {
+		if (node == 0) {
+			return;
+		}
+
+		ScalarType rt;
+		if (!ClassType::IntersectionBoxRay(node->boxCenter, node->boxHalfDims, ray, rt)) {
+			return;
+		}
+
+		if (rt >= rayT) {
+			return;
+		}
+
+		if (node->IsLeaf()) {
+			for (typename TreeType::ObjPtrVectorConstIterator si=node->oBegin; si!=node->oEnd; ++si) {
+				if (rayIntersection(*(*si), ray.r, rt)) {
+					if (rt < rayT) {
+						rayT = rt;
+						closestObj = (*si);
+					}
+				}
+			}
+			return;
+		}
+
+		ClassType::DepthFirstRayIsect(node->children[0], rayIntersection, ray, rayT, closestObj);
+		ClassType::DepthFirstRayIsect(node->children[1], rayIntersection, ray, rayT, closestObj);
+	}
+
+};
+
+}	// end namespace vcg
+
+#endif // #ifndef __VCGLIB_AABBBINARYTREE_RAY_H