Subversion Repositories gelsvn

Rev

Rev 324 | Rev 334 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 324 Rev 330
Line 47... Line 47...
47 
				return dot(dist_vec, dist_vec);
 47 
				return dot(dist_vec, dist_vec);
48 
			}
 48 
			}
49 
		};
 49 
		};
50 
 
 50 
 
51 
		typedef std::vector<KDNode> NodeVecType;
 51 
		typedef std::vector<KDNode> NodeVecType;
- 
 
 52 
		bool is_built;
52 
		NodeVecType init_nodes;
 53 
		NodeVecType init_nodes;
53 
		NodeVecType nodes;
 54 
		NodeVecType nodes;
54 
		bool is_built;
 - 
 
55 
 
 - 
 
56 
		/// The greatest depth of KD tree.
 - 
 
57 
		int max_depth;
 - 
 
58 
 
 - 
 
59 
		/// The dimension -- K
 - 
 
60 
		const int DIM;
 - 
 
61 
 
 - 
 
62 
		/// Total number of elements in tree
 - 
 
63 
		int elements;
 - 
 
64 
 
 55 
 
65 
		/** Comp is a class used for comparing two keys. Comp is constructed
 56 
		/** Comp is a class used for comparing two keys. Comp is constructed
66 
				with the discriminator - i.e. the coordinate of the key that is used
 57 
				with the discriminator - i.e. the coordinate of the key that is used
67 
				for comparing keys - Comp objects are passed to the sort algorithm.*/
 58 
				for comparing keys - Comp objects are passed to the sort algorithm.*/
68 
		class Comp
 59 
		class Comp
Line 90... Line 81...
90 
			}
 81 
			}
91 
		};
 82 
		};
92 
 
 83 
 
93 
 
 84 
 
94 
		/** Passed a vector of keys, this function will construct an optimal tree.
 85 
		/** Passed a vector of keys, this function will construct an optimal tree.
95 
				It is called recursively - second argument is level in tree. */
 86 
				It is called recursively */
96 
		void optimize(int, int, int, int);
 87 
		void optimize(int, int, int);
97 
 
 88 
 
98 
		/** Finde nearest neighbour. */
 89 
		/** Finde nearest neighbour. */
99 
		int closest_point_priv(int, const KeyType&, ScalarType&) const;
 90 
		int closest_point_priv(int, const KeyType&, ScalarType&) const;
100 
 
 91 
 
101
92
Line 112... Line 103...
112 
		int opt_disc(int,int) const;
 103 
		int opt_disc(int,int) const;
113
104
114 
	public:
 105 
	public:
115 
 
 106 
 
116 
		/** Build tree from vector of keys passed as argument. */
 107 
		/** Build tree from vector of keys passed as argument. */
117 
		KDTree():
 - 
 
118 
			is_built(false), max_depth(0), DIM(KeyType::get_dim()), elements(0)
 108 
		KDTree(): is_built(false), init_nodes(1) {}
119 
		{
 - 
 
120 
		}
 - 
 
121 
 
 109 
 
122 
		/** Insert a key value pair into the tree. Note that the tree needs to
110 
		/** Insert a key value pair into the tree. Note that the tree needs to
123 
				be built - by calling the build function - before you can search. */
 111 
				be built - by calling the build function - before you can search. */
124 
		void insert(const KeyT& key, const ValT& val)
 112 
		void insert(const KeyT& key, const ValT& val)
125 
		{
 113 
		{
126 
			assert(!is_built);
 114 
				if(is_built)
- 
 
 115 
				{
- 
 
 116 
						assert(init_nodes.size()==1);
- 
 
 117 
						init_nodes.swap(nodes);
- 
 
 118 
						is_built=false;
- 
 
 119 
				}
127 
			init_nodes.push_back(KDNode(key,val));
 120 
				init_nodes.push_back(KDNode(key,val));
128 
		}
 121 
		}
129 
 
 122 
 
130 
		/** Build the tree. After this function have been called, it is no longer
123 
		/** Build the tree. After this function has been called, it is no longer
131 
				legal to insert elements, but you can perform searches. */
 124 
				legal to insert elements, but you can perform searches. */
132 
		void build()
 125 
		void build()
133 
		{
 126 
		{
134 
			assert(!is_built);
 127 
			assert(!is_built);
135 
			nodes.resize(init_nodes.size()+1);
 128 
			nodes.resize(init_nodes.size());
136 
			if(init_nodes.size() > 0)
129 
			if(init_nodes.size() > 1)
137 
				optimize(1,0,init_nodes.size(),0);
 130 
				optimize(1,1,init_nodes.size());
138 
			NodeVecType v(0);
 131 
			NodeVecType v(1);
139 
			init_nodes.swap(v);
 132 
			init_nodes.swap(v);
140 
			is_built = true;
 133 
			is_built = true;
141 
		}
 134 
		}
142 
 
 135 
 
143 
		/** Find the key value pair closest to the key given as first
136 
		/** Find the key value pair closest to the key given as first
Line 145... Line 138...
145 
				The final two arguments contain the closest key and its
138 
				The final two arguments contain the closest key and its
146 
				associated value upon return. */
 139 
				associated value upon return. */
147 
		bool closest_point(const KeyT& p, ScalarType& dist, KeyT&k, ValT&v) const
 140 
		bool closest_point(const KeyT& p, ScalarType& dist, KeyT&k, ValT&v) const
148 
		{
 141 
		{
149 
			assert(is_built);
 142 
			assert(is_built);
- 
 
 143 
			if(nodes.size()>1)
- 
 
 144 
			{
150 
			ScalarType max_sq_dist = CGLA::sqr(dist);
 145 
					ScalarType max_sq_dist = CGLA::sqr(dist);
151 
			if(int n = closest_point_priv(1, p, max_sq_dist))
 146 
					if(int n = closest_point_priv(1, p, max_sq_dist))
152 
				{
 147 
					{
153 
					k = nodes[n].key;
 148 
							k = nodes[n].key;
154 
					v = nodes[n].val;
 149 
							v = nodes[n].val;
155 
					dist = std::sqrt(max_sq_dist);
 150 
							dist = std::sqrt(max_sq_dist);
156 
					return true;
 151 
							return true;
157 
				}
 152 
					}
- 
 
 153 
			}
158 
			return false;
 154 
			return false;
159 
		}
 155 
		}
160 
 
 156 
 
161 
		/** Find all the elements within a given radius (second argument) of
 157 
		/** Find all the elements within a given radius (second argument) of
162 
				the key (first argument). The key value pairs inside the sphere are
 158 
				the key (first argument). The key value pairs inside the sphere are
163 
				returned in a pair of vectors passed as the two last arguments. */
 159 
				returned in a pair of vectors passed as the two last arguments.
- 
 
 160 
				Note that we don't resize the two last arguments to zero - so either
- 
 
 161 
				they should be empty vectors or you should desire appending the newly
- 
 
 162 
				found elements onto these vectors.
- 
 
 163 
		*/
164 
		int in_sphere(const KeyType& p,
164 
		int in_sphere(const KeyType& p,
165 
									ScalarType dist,
 165 
									ScalarType dist,
166 
									std::vector<KeyT>& keys,
 166 
									std::vector<KeyT>& keys,
167 
									std::vector<ValT>& vals) const
 167 
									std::vector<ValT>& vals) const
168 
		{
 168 
		{
169 
			assert(is_built);
 169 
			assert(is_built);
- 
 
 170 
			if(nodes.size()>1)
- 
 
 171 
			{
170 
			ScalarType max_sq_dist = CGLA::sqr(dist);
 172 
					ScalarType max_sq_dist = CGLA::sqr(dist);
171 
			in_sphere_priv(1,p,max_sq_dist,keys,vals);
 173 
					in_sphere_priv(1,p,max_sq_dist,keys,vals);
172 
			return keys.size();
 174 
					return keys.size();
- 
 
 175 
			}
- 
 
 176 
			return 0;
173 
		}
 177 
		}
174
178
175 
 
 179 
 
176 
	};
 180 
	};
177 
 
 181 
 
Line 194... Line 198...
194 
	}
198 
	}
195 
 
 199 
 
196 
	template<class KeyT, class ValT>
 200 
	template<class KeyT, class ValT>
197 
	void KDTree<KeyT,ValT>::optimize(int cur,
 201 
	void KDTree<KeyT,ValT>::optimize(int cur,
198 
																	 int kvec_beg,
202 
																	 int kvec_beg,
199 
																	 int kvec_end,
203 
																	 int kvec_end)
200 
																	 int level)
 - 
 
201 
	{
 204 
	{
202 
		// Assert that we are not inserting beyond capacity.
 205 
		// Assert that we are not inserting beyond capacity.
203 
		assert(cur < nodes.size());
 206 
		assert(cur < nodes.size());
204 
 
 207 
 
205 
		// If there is just a single element, we simply insert.
 208 
		// If there is just a single element, we simply insert.
206 
		if(kvec_beg+1==kvec_end)
209 
		if(kvec_beg+1==kvec_end)
207 
			{
 210 
			{
208 
				max_depth  = std::max(level,max_depth);
 - 
 
209 
				nodes[cur] = init_nodes[kvec_beg];
 211 
				nodes[cur] = init_nodes[kvec_beg];
210 
				nodes[cur].dsc = -1;
 212 
				nodes[cur].dsc = -1;
211 
				return;
 213 
				return;
212 
			}
 214 
			}
213
215
Line 246... Line 248...
246 
		nodes[cur] = init_nodes[median];
 248 
		nodes[cur] = init_nodes[median];
247 
		nodes[cur].dsc = disc;
 249 
		nodes[cur].dsc = disc;
248 
 
 250 
 
249 
		// Recursively build left and right tree.
 251 
		// Recursively build left and right tree.
250 
		if(left_size>0)
252 
		if(left_size>0)
251 
			optimize(2*cur, kvec_beg, median,level+1);
 253 
			optimize(2*cur, kvec_beg, median);
252
254
253 
		if(right_size>0)
255 
		if(right_size>0)
254 
			optimize(2*cur+1, median+1, kvec_end,level+1);
 256 
			optimize(2*cur+1, median+1, kvec_end);
255 
	}
 257 
	}
256 
 
 258 
 
257 
	template<class KeyT, class ValT>
 259 
	template<class KeyT, class ValT>
258 
	int KDTree<KeyT,ValT>::closest_point_priv(int n, const KeyType& p,
260 
	int KDTree<KeyT,ValT>::closest_point_priv(int n, const KeyType& p,
259 
																						ScalarType& dist) const
 261 
																						ScalarType& dist) const