Added methods GetClosest, GetInSphere and GetInBox.

Changed signature of Set method to comply with the SpatialIndex interface

vcg/space/index/octree.h

@@ -102,11 +102,11 @@ namespace vcg
 	
 	
 	template < class OBJECT_TYPE, class SCALAR_TYPE>
-	class Octree : public OctreeTemplate< Voxel >, public vcg::SpatialIndex< OBJECT_TYPE, SCALAR_TYPE >
+	class Octree : public OctreeTemplate< Voxel, SCALAR_TYPE >, public vcg::SpatialIndex< OBJECT_TYPE, SCALAR_TYPE >
 	{
 	public: 
+		typedef						SCALAR_TYPE													ScalarType;
 		typedef						OBJECT_TYPE													ObjectType;
-		typedef						vcg::Box3<ScalarType>								BoundingBoxType;	
 		typedef typename	Octree::Leaf											* LeafPointer;
 		typedef typename	Octree::InnerNode									* InnerNodePointer;
 		typedef typename	ReferenceType<OBJECT_TYPE>::Type	* ObjectPointer;
@@ -178,10 +178,10 @@ namespace vcg
 		*/
 		struct ObjectReference
 		{
-			ObjectReference() {mark_position=-1; pObject=NULL;}
+			ObjectReference() {pMark=NULL; pObject=NULL;}
 
-			int						mark_position;
+			unsigned char *pMark;
-			ObjectPointer pObject;
+			ObjectPointer  pObject;
 		};
 
 		/*
@@ -195,7 +195,7 @@ namespace vcg
 				this->distance = -1.0f;
 			};
 
-			Neighbour(ObjectPointer &object, CoordType &point, float distance)
+			Neighbour(ObjectPointer &object, CoordType &point, ScalarType distance)
 			{
 				this->object	 = object;
 				this->point		 = point;
@@ -205,7 +205,7 @@ namespace vcg
 
 			ObjectPointer		object;
 			CoordType				point;
-			float						distance;
+			ScalarType			distance;
 		};
 
 		/*
@@ -232,17 +232,25 @@ public:
 		/*!
 		* Populate the octree
 		*/
-		template <class OBJECT_ITERATOR, class BOUNDING_BOX_FUNCTOR>
+		template <class OBJECT_ITERATOR/*, class BOUNDING_BOX_FUNCTOR*/>
-		void Set(const OBJECT_ITERATOR & bObj, const OBJECT_ITERATOR & eObj, const BoundingBoxType &bounding_box, const BOUNDING_BOX_FUNCTOR &bb_functor/*, vcg::CallBackPos *callback=NULL*/) 
+		void Set(const OBJECT_ITERATOR & bObj, const OBJECT_ITERATOR & eObj /*, const BoundingBoxType &bounding_box*/ /*, const BOUNDING_BOX_FUNCTOR &bb_functor*/ /*, vcg::CallBackPos *callback=NULL*/) 
 		{
-			typedef Dereferencer<typename ReferenceType<typename OBJECT_ITERATOR::value_type>::Type >	DereferencerType;
-
 			// Compute the bounding-box enclosing the whole dataset
+			typedef Dereferencer<typename ReferenceType<typename OBJECT_ITERATOR::value_type>::Type >	DereferencerType;
+			BoundingBoxType bounding_box, obj_bb;
+			bounding_box.SetNull();
+			for (OBJECT_ITERATOR iObj=bObj; iObj!=eObj; iObj++)
+			{
+				(*iObj).GetBBox(obj_bb);
+				bounding_box.Add(obj_bb);
+			}
+
+			//...and expand it a bit more
 			BoundingBoxType resulting_bb(bounding_box);
 			CoordType offset = bounding_box.Dim()*Octree::EXPANSION_FACTOR;
 			CoordType center = bounding_box.Center();
 			resulting_bb.Offset(offset);
-			float longest_side = vcg::math::Max( resulting_bb.DimX(), vcg::math::Max(resulting_bb.DimY(), resulting_bb.DimZ()) )/2.0f;
+			ScalarType longest_side = vcg::math::Max( resulting_bb.DimX(), vcg::math::Max(resulting_bb.DimY(), resulting_bb.DimZ()) )/2.0f;
 			resulting_bb.Set(center);
 			resulting_bb.Offset(longest_side);
 			boundingBox = resulting_bb;
@@ -250,11 +258,6 @@ public:
 			// Try to find a reasonable octree depth 
 			int dataset_dimension = std::distance(bObj, eObj);
 
-			// Allocate the mark array
-			global_mark				= 1;
-			marks							= new unsigned char[dataset_dimension];
-			memset(&marks[0], 0, sizeof(unsigned char)*dataset_dimension);
-
 			int primitives_per_voxel;
 			int depth = 4;
 			do 
@@ -265,7 +268,7 @@ public:
 				depth++;
 			}
 			while (primitives_per_voxel>25 && depth<15);
-			Initialize(depth);
+			Initialize(++depth);
 
 			// Sort the dataset (using the lebesgue space filling curve...)
 			std::string message("Indexing dataset...");
@@ -279,7 +282,7 @@ public:
 			vcg::Point3<ScalarType> hit_leaf;
 			for (int i=0; i<dataset_dimension; i++, iObj++)
 			{
-				object_bb = bb_functor(*iObj);
+				(*iObj).GetBBox(object_bb);
 				hit_leaf  = object_bb.min;
 
 				while (object_bb.IsIn(hit_leaf))
@@ -306,15 +309,21 @@ public:
 			delete []route;
 
 			int placeholder_count = int(placeholders.size());
+			
+			// Allocate the mark array
+			global_mark				= 1;
+			marks							= new unsigned char[placeholder_count];
+			memset(&marks[0], 0, sizeof(unsigned char)*placeholder_count);
+			
 			std::sort(placeholders.begin(), placeholders.end(), ObjectSorter< Octree::Node >());
 			std::vector< Octree::Node* > filled_leaves(placeholder_count);
-			sorted_dataset.resize( dataset_dimension );
+			sorted_dataset.resize( placeholder_count );
 			for (int i=0; i<placeholder_count; i++)
 			{
 				std::advance((iObj=bObj), placeholders[i].object_index);
-				sorted_dataset[i].pObject				= &DereferencerType::Ref(*iObj);
+				sorted_dataset[i].pObject	= &DereferencerType::Ref(*iObj);
-				sorted_dataset[i].mark_position = i;
+				sorted_dataset[i].pMark		= &marks[i];
-				filled_leaves[i]								= placeholders[i].leaf_pointer;
+				filled_leaves[i]					= placeholders[i].leaf_pointer;
 			}
 
 			// The dataset is sorted and the octree is built, but the indexing information aren't stored yet in the octree:
@@ -337,43 +346,116 @@ public:
 			IndexInnerNodes( Root() );
 		} //end of Set
 
+		/*!
+		* Finds the closest object to a given point.
+		*/
+		template <class OBJECT_POINT_DISTANCE_FUNCTOR, class OBJECT_MARKER>
+		ObjectPointer GetClosest
+		(
+			OBJECT_POINT_DISTANCE_FUNCTOR & distance_functor, 
+			OBJECT_MARKER									& /*marker*/, 
+			const CoordType								& query_point, 
+			const ScalarType							& max_distance,
+			ScalarType										& distance, 
+			CoordType											& point,
+			bool														allow_zero_distance = false
+		) 
+		{
+			BoundingBoxType query_bb;
+			ScalarType sphere_radius;
+			if (!GuessInitialBoundingBox(query_point, max_distance, sphere_radius, query_bb))
+				return NULL;
+			
+			std::vector< NodePointer > leaves;
+
+			unsigned int object_count;
+			int					 leaves_count;
+			ScalarType	 k_distance = leafDiagonal;
+
+			IncrementMark();
+			
+			do
+			{
+				leaves.clear();
+				object_count = 0;
+				query_bb.Offset(k_distance);
+				sphere_radius += vcg::math::Max<ScalarType>(leafDiagonal, k_distance);
+				
+				ContainedLeaves(query_bb, leaves, Root(), boundingBox);
+
+				leaves_count = int(leaves.size());
+				object_count = 0;
+				for (int i=0; i<leaves_count; i++)
+					object_count += Voxel( leaves[i] )->count;
+			}
+			while (object_count==0 && sphere_radius<max_distance);
+
+			if (sphere_radius>max_distance)
+				return NULL;
+			
+
+			CoordType				 closest_point;
+			VoxelType				*voxel;
+			ObjectReference *ref;
+			int							 begin;
+			int							 end;
+			ScalarType			 dist;
+
+			std::vector< Neighbour > neighbors;
+			object_count = 0;
+			for (int i=0; i<leaves_count; i++)
+			{
+				voxel = Voxel(leaves[i]);
+				begin = voxel->begin;
+				end		= voxel->end;
+				for ( ; begin<end; begin++)
+				{
+					ref				= &sorted_dataset[begin];
+					if (IsMarked(ref))
+						continue;
+
+					dist = max_distance;
+					if (!distance_functor(*ref->pObject, query_point, dist, closest_point))
+						continue;
+
+					Mark(ref);
+					if (dist!=ScalarType(0.0) || allow_zero_distance)
+						neighbors.push_back( Neighbour(ref->pObject, closest_point, dist) );
+
+				} //end of for ( ; begin<end; begin++)
+			} // end of for (int i=0; i<leavesCount; i++)
+
+			object_count = int(neighbors.size());
+			std::vector< Neighbour >::iterator first = neighbors.begin();
+			std::vector< Neighbour >::iterator last	 = neighbors.end();
+			std::partial_sort(first, first+object_count, last, DistanceCompare());
+			distance	= neighbors[0].distance;
+			point			= neighbors[0].point;
+			return			neighbors[0].object;
+		}; //end of GetClosest
+
 		/*!
 		* Retrive the k closest element to the query point
 		*/
-		template <class OBJECT_POINT_DISTANCE_FUNCTOR, class OBJECT_MARKER, class OBJECT_POINTER_CONTAINER, class DISTANCE_CONTAINER, class POINT_POINTER_CONTAINER>
+		template <class OBJECT_POINT_DISTANCE_FUNCTOR, class OBJECT_MARKER, class OBJECT_POINTER_CONTAINER, class DISTANCE_CONTAINER, class POINT_CONTAINER>
 		unsigned int GetKClosest
 			(
-			OBJECT_POINT_DISTANCE_FUNCTOR &distance_functor, 
+			OBJECT_POINT_DISTANCE_FUNCTOR & distance_functor, 
-			OBJECT_MARKER									&/*marker*/, 
+			OBJECT_MARKER									& /*marker*/, 
-			const unsigned int						 k, 
+			const unsigned int						  k, 
-			const CoordType								&query_point, 
+			const CoordType								& query_point, 
-			const ScalarType							&max_distance,
+			const ScalarType							& max_distance,
-			OBJECT_POINTER_CONTAINER			&objects, 
+			OBJECT_POINTER_CONTAINER			& objects, 
-			DISTANCE_CONTAINER						&distances, 
+			DISTANCE_CONTAINER						& distances, 
-			POINT_POINTER_CONTAINER				&points,
+			POINT_CONTAINER								& points,
-			bool													 sort_per_distance   = false,
+			bool													  sort_per_distance   = false,
-			bool													 allow_zero_distance = false
+			bool													  allow_zero_distance = false
 			)
 		{
-			// costruisco una bounging box centrata in query di dimensione pari a quella di una foglia.
-			// e controllo se in tale bounging box sono contenute un numero di elementi >= a k.
-			// Altrimenti espando il bounding box.
 			BoundingBoxType query_bb;
-			query_bb.Set(query_point);
+			ScalarType sphere_radius;
-
+			if (!GuessInitialBoundingBox(query_point, max_distance, sphere_radius, query_bb))
-			// Il raggio della sfera centrata nel punto query
+				return 0;
-			float sphere_radius = 0.0f;
-
-			// se il bounding-box non interseca il bounding-box dell'octree, lo espando subito
-			if (!query_bb.IsIn(query_point))
-			{
-				do
-				{ 
-					query_bb.Offset(leafDiagonal); 
-					sphere_radius += leafDiagonal;
-				}
-				while ( !boundingBox.Collide(query_bb) || sphere_radius>max_distance); // TODO check the termination condintion
-			}
 
 			std::vector< NodePointer > leaves;
 			std::vector< Neighbour	 > neighbors;
@@ -383,13 +465,7 @@ public:
 			float				 k_distance = leafDiagonal;
 			do
 			{
-				// update the marks
+				IncrementMark();
-				global_mark = (global_mark+1)%255;
-				if (global_mark == 0)
-				{
-					memset(&marks[0], 0, sizeof(unsigned char)*int(sorted_dataset.size()));
-					global_mark++;
-				}
 
 				do
 				{
@@ -405,7 +481,7 @@ public:
 					for (int i=0; i<leaves_count; i++)
 						object_count += Voxel( leaves[i] )->count;
 				}
-				while (object_count<k && sphere_radius>max_distance); // TODO check the termination condintion
+				while (object_count<k && sphere_radius<max_distance); // TODO check the termination condintion
 
 				// i punti contenuti nelle foglie sono sufficienti. 
 				// seleziono solamente i k punti più vicini.
@@ -417,6 +493,7 @@ public:
 				float						 distance;
 
 				neighbors.clear();
+				object_count = 0;
 				for (int i=0; i<leaves_count; i++)
 				{
 					voxel = Voxel(leaves[i]);
@@ -425,23 +502,23 @@ public:
 					for ( ; begin<end; begin++)
 					{
 						ref				= &sorted_dataset[begin];
-						if (marks[ref->mark_position]==global_mark)
+						if (IsMarked(ref))
 							continue;
 						
 						distance = max_distance;
 						if (!distance_functor(*ref->pObject, query_point, distance, closest_point))
 							continue;
 
-						marks[ref->mark_position]=global_mark;
+						Mark(ref);
-						if ((distance!=0.0f || allow_zero_distance)/* && distance<max_distance*/)
+						if ((distance!=0.0f || allow_zero_distance))
 							neighbors.push_back( Neighbour(ref->pObject, closest_point, distance) );
 					} //end of for ( ; begin<end; begin++)
 				} // end of for (int i=0; i<leavesCount; i++)
 
-
+				object_count = int(neighbors.size());
 				if (sphere_radius<max_distance)
 				{
-					if (int(neighbors.size())<k)
+					if (object_count<k)
 					{
 						k_distance = 2.0f*sphere_radius;
 						continue;
@@ -461,28 +538,142 @@ public:
 			} 
 			while (k_distance>sphere_radius && sphere_radius<max_distance);
 
-			// copy the nearest object into  
+			return CopyQueryResults<OBJECT_POINTER_CONTAINER, DISTANCE_CONTAINER, POINT_CONTAINER>(neighbors, object_count, objects, distances, points);
-			if (points.size()!=object_count) 
-			{
-				points.resize(object_count);
-				distances.resize(object_count);
-				objects.resize(object_count);
-			}
-
-			POINT_POINTER_CONTAINER::iterator  iPoint			= points.begin();
-			DISTANCE_CONTAINER::iterator			 iDistance	= distances.begin();
-			OBJECT_POINTER_CONTAINER::iterator iObject		= objects.begin();
-			for (unsigned int n=0; n<object_count; n++, iPoint++, iDistance++, iObject++)
-			{
-				(*iPoint)		 = neighbors[n].point;
-				(*iDistance) = neighbors[n].distance;
-				(*iObject)	 = neighbors[n].object;
-			}
-			return object_count;
 		}; //end of GetKClosest
 
 
 		/*!
+		* Returns all the objects contained inside a specified sphere
+		*/
+		template <class OBJECT_POINT_DISTANCE_FUNCTOR, class OBJECT_MARKER, class OBJECT_POINTER_CONTAINER, class DISTANCE_CONTAINER, class POINT_CONTAINER>
+		unsigned int GetInSphere
+			(
+				OBJECT_POINT_DISTANCE_FUNCTOR		&distance_functor, 
+				OBJECT_MARKER										&/*marker*/,
+				const CoordType									&sphere_center, 
+				const ScalarType								&sphere_radius,
+				OBJECT_POINTER_CONTAINER				&objects, 
+				DISTANCE_CONTAINER							&distances, 
+				POINT_CONTAINER									&points,
+				bool													 sort_per_distance   = false,
+				bool													 allow_zero_distance = false
+			)
+		{
+			// Define the minimum bounding-box containing the sphere
+			BoundingBoxType query_bb(sphere_center, sphere_radius);
+			
+			// If that bounding-box don't collide with the octree bounding-box, simply return 0
+			if (!boundingBox.Collide(query_bb))
+				return 0;
+
+			std::vector< NodePointer > leaves;
+			std::vector< Neighbour	 > neighbors;
+
+			unsigned int object_count = 0;
+			float				 k_distance		= leafDiagonal;
+
+			IncrementMark();
+			ContainedLeaves(query_bb, leaves, Root(), boundingBox);
+
+			int	leaves_countleaves_count = int(leaves.size());
+			if (leaves_count==0)
+				return 0;
+
+			CoordType				 closest_point;
+			VoxelType				*voxel;
+			ObjectReference *ref;
+			int							 begin;
+			int							 end;
+			float						 distance;
+			for (int i=0; i<leaves_count; i++)
+			{
+				voxel = Voxel(leaves[i]);
+				begin = voxel->begin;
+				end		= voxel->end;
+				for ( ; begin<end; begin++)
+				{
+					ref				= &sorted_dataset[begin];
+					if (IsMarked(ref))
+						continue;
+					
+					distance = sphere_radius;
+					if (!distance_functor(*ref->pObject, query_point, distance, closest_point))
+						continue;
+
+					object_count++;
+					Mark(ref);
+					if ((distance!=0.0f || allow_zero_distance) && distance<max_distance)
+						neighbors.push_back( Neighbour(ref->pObject, closest_point, distance) );
+				} //end of for ( ; begin<end; begin++)
+			} // end of for (int i=0; i<leavesCount; i++)
+
+			std::vector< Neighbour >::iterator first = neighbors.begin();
+			std::vector< Neighbour >::iterator last	 = neighbors.end();
+			if (sort_per_distance)  std::partial_sort(first, first+object_count, last, DistanceCompare());
+			else										std::nth_element (first, first+object_count, last, DistanceCompare()); 
+			
+			return CopyQueryResults<OBJECT_POINTER_CONTAINER, DISTANCE_CONTAINER, POINT_CONTAINER>(neighbors, object_count, objects, distances, points);
+		};//end of GetInSphere
+
+		/*!
+		* Returns all the objects lying inside the specified bbox
+		*/
+			template <class OBJECT_MARKER, class OBJECT_POINTER_CONTAINER>
+			unsigned int GetInBox
+				(
+					OBJECT_MARKER							&/*marker*/, 
+					const BoundingBoxType			&query_bounding_box,
+					OBJECT_POINTER_CONTAINER	&objects
+					) 
+			{
+				//if the query bounding-box don't collide with the octree bounding-box, simply return 0
+				if (!query_bounding_box.Collide())
+				{
+					objects.clear();
+					return 0;
+				}
+				
+				//otherwise, retrieve the leaves and fill the container with the objects contained
+				std::vector< NodePointer > leaves;
+				unsigned int object_count;
+				int					 leaves_count;
+			
+				ContainedLeaves(query_bounding_box, leaves, Root(), boundingBox);
+				leaves_count = int(leaves.size());
+				if (leaves_count==0)
+				{
+					objects.clear();
+					return 0;
+				}
+				
+				IncrementMark();
+
+				VoxelType				*voxel;
+				ObjectReference *ref;
+				int							 begin;
+				int							 end;
+				object_count = 0;
+				for (int i=0; i<leaves_count; i++)
+				{
+					voxel = Voxel(leaves[i]);
+					begin = voxel->begin;
+					end		= voxel->end;
+					for ( ; begin<end; begin++)
+					{
+						ref	= &sorted_dataset[begin];
+						if (IsMarked(ref))
+							continue;
+						
+						object_count++;
+						Mark(ref);
+						objects.push_back(ref->pObject);
+					} //end of for ( ; begin<end; begin++)
+				} // end of for (int i=0; i<leavesCount; i++)
+
+				return int(objects.size());
+			}; //end of GetInBox
+
+		/*
 		* Draw the octree in a valid OpenGL context according to the rendering settings
 		*/
 		void Octree::DrawOctree(vcg::Box3f &boundingBox, NodePointer n)
@@ -522,6 +713,89 @@ public:
 		OcreeRenderingSetting					rendering_settings;
 
 	protected:
+		/*!
+		*/
+		inline void IncrementMark()
+		{
+			// update the marks
+			global_mark = (global_mark+1)%255;
+			if (global_mark == 0)
+			{
+				memset(&marks[0], 0, sizeof(unsigned char)*int(sorted_dataset.size()));
+				global_mark++;
+			}
+		};//end of IncrementMark
+
+		/*
+		*/
+		inline bool IsMarked(const ObjectReference *ref) const 
+		{ return *ref->pMark == global_mark; };
+
+		/*
+		*/
+		inline void Mark(const ObjectReference *ref)
+		{ *ref->pMark = global_mark;};
+
+		/*!
+		* Guess an initial bounding-box from which starting the research of the closest point(s).
+		* \return true iff it's possibile to find a sphere, centered in query_point and having radius max_distance at most, which collide the octree bounding-box.
+		*/
+		inline bool GuessInitialBoundingBox(const CoordType &query_point, const ScalarType max_distance, ScalarType &sphere_radius, BoundingBoxType &query_bb)
+		{
+			// costruisco una bounging box centrata in query di dimensione pari a quella di una foglia.
+			// e controllo se in tale bounging box sono contenute un numero di elementi >= a k.
+			// Altrimenti espando il bounding box.
+			query_bb.Set(query_point);
+
+			// Il raggio della sfera centrata nel punto query
+			sphere_radius = 0.0f;
+
+			// se il bounding-box non interseca il bounding-box dell'octree, lo espando subito
+			if (!query_bb.IsIn(query_point))
+			{
+				do
+				{ 
+					query_bb.Offset(leafDiagonal); 
+					sphere_radius += leafDiagonal;
+				}
+				while ( !boundingBox.Collide(query_bb) || sphere_radius>max_distance); // TODO check the termination condintion
+			}
+			return (sphere_radius<=max_distance);
+		};
+
+		/*!
+		* Copy the results of a query
+		*/
+		template <class OBJECT_POINTER_CONTAINER, class DISTANCE_CONTAINER, class POINT_CONTAINER>
+		inline int CopyQueryResults
+		(
+			std::vector< Neighbour	 > &neighbors,
+			const unsigned int					object_count,
+			OBJECT_POINTER_CONTAINER	 &objects,
+			DISTANCE_CONTAINER				 &distances, 
+			POINT_CONTAINER						 &points
+		)
+		{
+			// copy the nearest object into  
+			if (int(points.size())!=object_count) 
+			{
+				points.resize(object_count);
+				distances.resize(object_count);
+				objects.resize(object_count);
+			}
+
+			POINT_CONTAINER::iterator					 iPoint			= points.begin();
+			DISTANCE_CONTAINER::iterator			 iDistance	= distances.begin();
+			OBJECT_POINTER_CONTAINER::iterator iObject		= objects.begin();
+			for (unsigned int n=0; n<object_count; n++, iPoint++, iDistance++, iObject++)
+			{
+				(*iPoint)		 = neighbors[n].point;
+				(*iDistance) = neighbors[n].distance;
+				(*iObject)	 = neighbors[n].object;
+			}
+			return object_count;
+		}
+		
 		/*!
 		* When all the leaves are indexed, this procedure is called in order to propagate the indexing information to the inner nodes
 		*/

vcg/space/index/octree_template.h

@@ -48,7 +48,7 @@ Si tiene int invece di puntatori per garantirsi reallocazione dinamica.
 I dati veri e propri stanno in un vettore di nodi
 */
 
-template <typename VOXEL_TYPE>
+template <typename VOXEL_TYPE, class SCALAR_TYPE>
 class OctreeTemplate
 {
 protected:
@@ -56,12 +56,15 @@ protected:
 
 public:
 	// Octree Type Definitions
-	typedef typename VOXEL_TYPE		VoxelType;
+	typedef SCALAR_TYPE								ScalarType;
-	typedef typename VOXEL_TYPE  *VoxelPointer;
+	typedef typename VOXEL_TYPE				VoxelType;
-	typedef vcg::Point3i					CenterType;	
+	typedef typename VOXEL_TYPE		   *VoxelPointer;
-	static const float						EXPANSION_FACTOR;
+	typedef vcg::Point3i							CenterType;	
-	typedef	int										NodeIndex;
+	static const ScalarType						EXPANSION_FACTOR;
-	typedef Node								 *NodePointer;
+	typedef	int												NodeIndex;
+	typedef Node										 *NodePointer;
+	typedef	vcg::Box3<ScalarType>			BoundingBoxType;	
+	typedef vcg::Point3<ScalarType>	CoordinateType;
 
 protected:
 	/*
@@ -154,14 +157,14 @@ public:
 		nodes.push_back( root );
 		root->center	= CenterType(size, size, size);
 		
-		float szf = (float) size;
+		ScalarType szf = (ScalarType) size;
 		leafDimension  = boundingBox.Dim();
 		leafDimension /= szf;
 		leafDiagonal	 = leafDimension.Norm();
 	};
 
 	// Return the octree bounding-box
-	inline vcg::Box3f		BoundingBox()											{ return boundingBox;				}
+	inline BoundingBoxType		BoundingBox()								{ return boundingBox;				}
 	
 	// Return the Voxel of the n-th node
 	inline VoxelPointer	Voxel(const NodePointer n)				{ return &(n->voxel);				}
@@ -199,15 +202,15 @@ public:
 	inline CenterType CenterInOctreeCoordinates(const NodePointer n) const { return n->center;}
 	
 	// Return the center of the n-th node expressed in world-coordinate
-	inline vcg::Point3f CenterInWorldCoordinates(const NodePointer n) const
+	inline CoordinateType CenterInWorldCoordinates(const NodePointer n) const
 	{
 		assert(0<=n && n<NodeCount());
 
 		int level = n->level;
 		int shift = maxDepth-level+1;
-		vcg::Point3f wcCenter; 
+		CoordinateType wcCenter; 
-		vcg::Point3f ocCenter = CenterInOctreeCoordinates(n);
+		CoordinateType ocCenter = CenterInOctreeCoordinates(n);
-		vcg::Point3f nodeSize = boundingBox.Dim()/float(1<<level);
+		CoordinateType nodeSize = boundingBox.Dim()/float(1<<level);
 		wcCenter.X() = boundingBox.min.X() + (nodeSize.X()*(0.5f+(ocCenter.X()>>shift)));
 		wcCenter.Y() = boundingBox.min.Y() + (nodeSize.Y()*(0.5f+(ocCenter.Y()>>shift)));
 		wcCenter.Z() = boundingBox.min.Z() + (nodeSize.Z()*(0.5f+(ocCenter.Z()>>shift)));
@@ -264,9 +267,9 @@ public:
 
 	The center of each cell can simply be obtained by adding .5 to the path of the leaves.
 	*/
-	vcg::Point3f Center(NodePointer n) const
+	CoordinateType Center(NodePointer n) const
 	{
-		vcg::Point3f center;
+		CoordinateType center;
 		center.Import(GetPath(n));
 		center+=Point3f(.5f,.5f,.5f);
 
@@ -277,15 +280,15 @@ public:
 	}
 
 	// Return the bounding-box of the n-th node expressed in world-coordinate
-	vcg::Box3f BoundingBoxInWorldCoordinates(const NodePointer n)
+	BoundingBoxType BoundingBoxInWorldCoordinates(const NodePointer n)
 	{
 		assert(0<=n && n<Size());
 
 		char level = Level(n);
 		int shift  = maximumDepth-level+1;
-		vcg::Point3f nodeDim = boundingBox.Dim()/float(1<<level);
+		CoordinateType	nodeDim = boundingBox.Dim()/float(1<<level);
-		CenterType   center	 = CenterInOctreeCoordinates(n);
+		CenterType			center	 = CenterInOctreeCoordinates(n);
-		vcg::Box3f	 nodeBB;
+		BoundingBoxType	nodeBB;
 		nodeBB.min.X() = boundingBox.min.X() + (nodeDim.X()*(center.X()>>shift));
 		nodeBB.min.Y() = boundingBox.min.Y() + (nodeDim.Y()*(center.Y()>>shift));
 		nodeBB.min.Z() = boundingBox.min.Z() + (nodeDim.Z()*(center.Z()>>shift));
@@ -294,9 +297,9 @@ public:
 	};
 
 	// Return one of the 8 subb box of a given box.
-	vcg::Box3f SubBox(vcg::Box3f &lbb, int i)
+	BoundingBoxType SubBox(BoundingBoxType &lbb, int i)
 	{
-		vcg::Box3f bs;
+		BoundingBoxType bs;
 		if (i&1)	bs.min.X()=(lbb.min.X()+(bs.max.X()=lbb.max.X()))/2.0f;
 		else			bs.max.X()=((bs.min.X()=lbb.min.X())+lbb.max.X())/2.0f;
 		if (i&2)	bs.min.Y()=(lbb.min.Y()+(bs.max.Y()=lbb.max.Y()))/2.0f;
@@ -309,9 +312,9 @@ public:
 
 	// Given the bounding-box and the center (both in world-coordinates)
 	// of a node, return the bounding-box (in world-coordinats) of the i-th son 
-	vcg::Box3f SubBoxAndCenterInWorldCoordinates(vcg::Box3f &lbb, vcg::Point3f &center, int i)
+	BoundingBoxType SubBoxAndCenterInWorldCoordinates(BoundingBoxType &lbb, CoordinateType &center, int i)
 	{
-		vcg::Box3f bs;
+		BoundingBoxType bs;
 		if (i&1)	
 		{
 			bs.min[0]=center[0];
@@ -407,9 +410,9 @@ public:
 
 	// Convert the point p coordinates to the integer based representation 
 	// in the range 0..size, where size is 2^maxdepth
-	vcg::Point3i Interize(const vcg::Point3f &pf) const  
+	CenterType Interize(const CoordinateType &pf) const  
 	{
-		vcg::Point3i pi;
+		CenterType pi;
 
 		assert(pf.X()>=boundingBox.min.X() &&  pf.X()<=boundingBox.max.X());
 		assert(pf.Y()>=boundingBox.min.Y() &&  pf.Y()<=boundingBox.max.Y());
@@ -424,9 +427,9 @@ public:
 
 	// Inverse function of Interize; 
 	// Return to the original coords space (not to the original values!!)
-	vcg::Point3f DeInterize(const vcg::Point3i &pi ) const 
+	CoordinateType DeInterize(const CenterType &pi ) const 
 	{
-		vcg::Point3f pf;
+		CoordinateType pf;
 
 		assert(pi.X()>=0 && pi.X()<size); 
 		assert(pi.Y()>=0 && pi.Y()<size); 
@@ -525,7 +528,7 @@ public:
 	// I nodi mancanti dalla radice fino a profondità maxDepth vengono aggiunti. 
 	// In posizione i ci sarà il nodo di livello i.
 	// Restituisce lo z-order del punto p
-	unsigned long long BuildRoute(const vcg::Point3f &p, NodePointer *&route)
+	unsigned long long BuildRoute(const CoordinateType &p, NodePointer *&route)
 	{
 		assert( boundingBox.min.X()<=p.X() && p.X()<=boundingBox.max.X() );
 		assert( boundingBox.min.Y()<=p.Y() && p.Y()<=boundingBox.max.Y() );
@@ -568,7 +571,7 @@ public:
 	// fino al nodo di profontidà massima presente; nelle eventuali posizioni rimaste
 	// libere è inserito il valore -1. Restituisce true se il punto p cade in una foglia
 	// dell'otree, false altrimenti
-	bool GetRoute(const vcg::Point3f &p, NodePointer *&route)
+	bool GetRoute(const CoordinateType &p, NodePointer *&route)
 	{
 		assert( boundingBox.min.X()<=p.X() && p.X()<=boundingBox.max.X() );
 		assert( boundingBox.min.Y()<=p.Y() && p.Y()<=boundingBox.max.Y() );
@@ -606,12 +609,13 @@ public:
 	// bb (la bounding-box dell'octree); i puntatori a tali nodi sono 
 	// inseriti progressivamente in contained_nodes.
 	// The vector nodes must be cleared before calling this method.
-	void ContainedNodes(
+	void ContainedNodes
-		vcg::Box3f								 &query,
+	(
+		BoundingBoxType						 &query,
 		std::vector< NodePointer > &nodes,
 		int													depth, 
 		NodePointer									n,
-		vcg::Box3f								 &nodeBB)
+		BoundingBoxType						 &nodeBB)
 	{
 		if (!query.Collide(nodeBB))
 			return;
@@ -621,8 +625,8 @@ public:
 		else
 		{
 			NodePointer	son;
-			vcg::Box3f	sonBB;
+			BoundingBoxType	sonBB;
-			vcg::Point3f nodeCenter = nodeBB.Center();
+			CoordinateType  nodeCenter = nodeBB.Center();
 			for (int s=0; s<8; s++)
 			{
 				son = Son(n, s);
@@ -640,15 +644,15 @@ public:
 	// bounding-box ha intersezione non nulla con bb; i loro indici
 	// sono inseriti all'interno di leaves.
 	void ContainedLeaves(
-		vcg::Box3f								 &query, 
+		BoundingBoxType						 &query, 
 		std::vector< NodePointer > &leaves,
 		NodePointer									node,
-		vcg::Box3f								 &nodeBB
+		BoundingBoxType						 &nodeBB
 		)
 	{
 		NodePointer	son;
-		vcg::Box3f	sonBB;
+		BoundingBoxType	sonBB;
-		vcg::Point3f nodeCenter = nodeBB.Center();
+		CoordinateType nodeCenter = nodeBB.Center();
 		for (int s=0; s<8; s++)
 		{
 			son = Son(node, s); //nodes[nodeIndex].sonIndex[s]
@@ -681,21 +685,20 @@ public:
 	int						maximumDepth;
 
 	// The dimension of a leaf
-	vcg::Point3f	leafDimension; 
+	CoordinateType	leafDimension; 
 
 	// The diagonal of a leaf
-	float					leafDiagonal;
+	ScalarType			leafDiagonal;
 	
 	// The Octree nodes
 	std::vector< Node* > nodes;
 
 	// The bounding box containing the octree (in world coordinate)
-	vcg::Box3f				boundingBox;
+	BoundingBoxType				boundingBox;
 }; //end of class OctreeTemplate
 
-template <typename VOXEL_TYPE>
+template <typename VOXEL_TYPE, class SCALAR_TYPE>
-const float OctreeTemplate<VOXEL_TYPE>::EXPANSION_FACTOR  = 0.035f;
+const SCALAR_TYPE OctreeTemplate<VOXEL_TYPE, SCALAR_TYPE>::EXPANSION_FACTOR  = SCALAR_TYPE(0.035);
-
 }
 
 #endif //OCTREETEMPLATE_H