00001
00007 #ifndef INCLUDE_KDTREE_NODE_HPP
00008 #define INCLUDE_KDTREE_NODE_HPP
00009
00010 #ifdef KDTREE_DEFINE_OSTREAM_OPERATORS
00011 # include <ostream>
00012 #endif
00013
00014 #include <cstddef>
00015 #include <cmath>
00016
00017 namespace KDTree
00018 {
00019 struct _Node_base
00020 {
00021 typedef _Node_base* _Base_ptr;
00022 typedef _Node_base const* _Base_const_ptr;
00023
00024 _Base_ptr _M_parent;
00025 _Base_ptr _M_left;
00026 _Base_ptr _M_right;
00027
00028 _Node_base(_Base_ptr const __PARENT = NULL,
00029 _Base_ptr const __LEFT = NULL,
00030 _Base_ptr const __RIGHT = NULL)
00031 : _M_parent(__PARENT), _M_left(__LEFT), _M_right(__RIGHT) {}
00032
00033 static _Base_ptr
00034 _S_minimum(_Base_ptr __x)
00035 {
00036 while (__x->_M_left) __x = __x->_M_left;
00037 return __x;
00038 }
00039
00040 static _Base_ptr
00041 _S_maximum(_Base_ptr __x)
00042 {
00043 while (__x->_M_right) __x = __x->_M_right;
00044 return __x;
00045 }
00046 };
00047
00048 template <typename _Val>
00049 struct _Node : public _Node_base
00050 {
00051 using _Node_base::_Base_ptr;
00052 typedef _Node* _Link_type;
00053
00054 _Val _M_value;
00055
00056 _Node(_Val const& __VALUE = _Val(),
00057 _Base_ptr const __PARENT = NULL,
00058 _Base_ptr const __LEFT = NULL,
00059 _Base_ptr const __RIGHT = NULL)
00060 : _Node_base(__PARENT, __LEFT, __RIGHT), _M_value(__VALUE) {}
00061
00062 #ifdef KDTREE_DEFINE_OSTREAM_OPERATORS
00063
00064 template <typename Char, typename Traits>
00065 friend
00066 std::basic_ostream<Char, Traits>&
00067 operator<<(typename std::basic_ostream<Char, Traits>& out,
00068 _Node_base const& node)
00069 {
00070 out << &node;
00071 out << " parent: " << node._M_parent;
00072 out << "; left: " << node._M_left;
00073 out << "; right: " << node._M_right;
00074 return out;
00075 }
00076
00077 template <typename Char, typename Traits>
00078 friend
00079 std::basic_ostream<Char, Traits>&
00080 operator<<(typename std::basic_ostream<Char, Traits>& out,
00081 _Node<_Val> const& node)
00082 {
00083 out << &node;
00084 out << ' ' << node._M_value;
00085 out << "; parent: " << node._M_parent;
00086 out << "; left: " << node._M_left;
00087 out << "; right: " << node._M_right;
00088 return out;
00089 }
00090
00091 #endif
00092 };
00093
00094 template <typename _Val, typename _Acc, typename _Cmp>
00095 class _Node_compare
00096 {
00097 public:
00098 _Node_compare(size_t const __DIM, _Acc const& acc, _Cmp const& cmp)
00099 : _M_DIM(__DIM), _M_acc(acc), _M_cmp(cmp) {}
00100
00101 bool
00102 operator()(_Val const& __A, _Val const& __B) const
00103 {
00104 return _M_cmp(_M_acc(__A, _M_DIM), _M_acc(__B, _M_DIM));
00105 }
00106
00107 private:
00108 size_t _M_DIM;
00109 _Acc _M_acc;
00110 _Cmp _M_cmp;
00111 };
00112
00119 template <typename _ValA, typename _ValB, typename _Cmp,
00120 typename _Acc>
00121 inline
00122 bool
00123 _S_node_compare (const size_t __dim,
00124 const _Cmp& __cmp, const _Acc& __acc,
00125 const _ValA& __a, const _ValB& __b)
00126 {
00127 return __cmp(__acc(__a, __dim), __acc(__b, __dim));
00128 }
00129
00135 template <typename _ValA, typename _ValB, typename _Dist,
00136 typename _Acc>
00137 inline
00138 typename _Dist::distance_type
00139 _S_node_distance (const size_t __dim,
00140 const _Dist& __dist, const _Acc& __acc,
00141 const _ValA& __a, const _ValB& __b)
00142 {
00143 return __dist(__acc(__a, __dim), __acc(__b, __dim));
00144 }
00145
00152 template <typename _ValA, typename _ValB, typename _Dist,
00153 typename _Acc>
00154 inline
00155 typename _Dist::distance_type
00156 _S_accumulate_node_distance (const size_t __dim,
00157 const _Dist& __dist, const _Acc& __acc,
00158 const _ValA& __a, const _ValB& __b)
00159 {
00160 typename _Dist::distance_type d = 0;
00161 for (size_t i=0; i<__dim; ++i)
00162 d += __dist(__acc(__a, i), __acc(__b, i));
00163 return d;
00164 }
00165
00171 template <typename _Val, typename _Cmp, typename _Acc>
00172 inline
00173 _Node_base*
00174 _S_node_descend (const size_t __dim,
00175 const _Cmp& __cmp, const _Acc& __acc,
00176 const _Val& __val, const _Node_base* __node)
00177 {
00178 if (_S_node_compare(__dim, __cmp, __acc, __val, static_cast<const _Node<_Val>* >(__node)->_M_value))
00179 return __node->_M_left;
00180 else
00181 return __node->_M_right;
00182 }
00183
00192 template <class SearchVal,
00193 typename _Val, typename _Cmp,
00194 typename _Acc, typename _Dist,
00195 typename _Predicate>
00196 inline
00197 std::pair<const _Node<_Val>*,
00198 std::pair<size_t, typename _Dist::distance_type> >
00199 _S_node_nearest (const size_t __k, size_t __dim, SearchVal const& __val,
00200 const _Node<_Val>* __node, const _Node_base* __end,
00201 const _Node<_Val>* __best, typename _Dist::distance_type __max,
00202 const _Cmp& __cmp, const _Acc& __acc, const _Dist& __dist,
00203 _Predicate __p)
00204 {
00205 const _Node_base* pcur = __node;
00206 const _Node_base* cur = _S_node_descend(__dim % __k, __cmp, __acc, __val, __node);
00207 size_t cur_dim = __dim+1;
00208
00209 while (cur)
00210 {
00211 if (__p(static_cast<const _Node<_Val>* >(cur)->_M_value))
00212 {
00213 typename _Dist::distance_type d = 0;
00214 for (size_t i=0; i != __k; ++i)
00215 d += _S_node_distance(i, __dist, __acc, __val, static_cast<const _Node<_Val>* >(cur)->_M_value);
00216 d = sqrt(d);
00217 if (d <= __max)
00218
00219
00220
00221
00222 {
00223 __best = static_cast<const _Node<_Val>* >(cur);
00224 __max = d;
00225 __dim = cur_dim;
00226 }
00227 }
00228 pcur = cur;
00229 cur = _S_node_descend(cur_dim % __k, __cmp, __acc, __val, cur);
00230 ++cur_dim;
00231 }
00232
00233 cur = pcur;
00234 --cur_dim;
00235 pcur = NULL;
00236
00237 const _Node_base* probe = cur;
00238 const _Node_base* pprobe = probe;
00239 const _Node_base* near_node;
00240 const _Node_base* far_node;
00241 size_t probe_dim = cur_dim;
00242 if (_S_node_compare(probe_dim % __k, __cmp, __acc, __val, static_cast<const _Node<_Val>* >(probe)->_M_value))
00243 near_node = probe->_M_right;
00244 else
00245 near_node = probe->_M_left;
00246 if (near_node
00247
00248 && (sqrt(_S_node_distance(probe_dim % __k, __dist, __acc, __val, static_cast<const _Node<_Val>* >(probe)->_M_value)) <= __max))
00249 {
00250 probe = near_node;
00251 ++probe_dim;
00252 }
00253 while (cur != __end)
00254 {
00255 while (probe != cur)
00256 {
00257 if (_S_node_compare(probe_dim % __k, __cmp, __acc, __val, static_cast<const _Node<_Val>* >(probe)->_M_value))
00258 {
00259 near_node = probe->_M_left;
00260 far_node = probe->_M_right;
00261 }
00262 else
00263 {
00264 near_node = probe->_M_right;
00265 far_node = probe->_M_left;
00266 }
00267 if (pprobe == probe->_M_parent)
00268 {
00269 if (__p(static_cast<const _Node<_Val>* >(probe)->_M_value))
00270 {
00271 typename _Dist::distance_type d = 0;
00272 for (size_t i=0; i < __k; ++i)
00273 d += _S_node_distance(i, __dist, __acc, __val, static_cast<const _Node<_Val>* >(probe)->_M_value);
00274 d = sqrt(d);
00275 if (d <= __max)
00276 {
00277 __best = static_cast<const _Node<_Val>* >(probe);
00278 __max = d;
00279 __dim = probe_dim;
00280 }
00281 }
00282 pprobe = probe;
00283 if (near_node)
00284 {
00285 probe = near_node;
00286 ++probe_dim;
00287 }
00288 else if (far_node &&
00289
00290 sqrt(_S_node_distance(probe_dim % __k, __dist, __acc, __val, static_cast<const _Node<_Val>* >(probe)->_M_value)) <= __max)
00291 {
00292 probe = far_node;
00293 ++probe_dim;
00294 }
00295 else
00296 {
00297 probe = probe->_M_parent;
00298 --probe_dim;
00299 }
00300 }
00301 else
00302 {
00303 if (pprobe == near_node && far_node
00304
00305 && sqrt(_S_node_distance(probe_dim % __k, __dist, __acc, __val, static_cast<const _Node<_Val>* >(probe)->_M_value)) <= __max)
00306 {
00307 pprobe = probe;
00308 probe = far_node;
00309 ++probe_dim;
00310 }
00311 else
00312 {
00313 pprobe = probe;
00314 probe = probe->_M_parent;
00315 --probe_dim;
00316 }
00317 }
00318 }
00319 pcur = cur;
00320 cur = cur->_M_parent;
00321 --cur_dim;
00322 pprobe = cur;
00323 probe = cur;
00324 probe_dim = cur_dim;
00325 if (cur != __end)
00326 {
00327 if (pcur == cur->_M_left)
00328 near_node = cur->_M_right;
00329 else
00330 near_node = cur->_M_left;
00331 if (near_node
00332
00333 && (sqrt(_S_node_distance(cur_dim % __k, __dist, __acc, __val, static_cast<const _Node<_Val>* >(cur)->_M_value)) <= __max))
00334 {
00335 probe = near_node;
00336 ++probe_dim;
00337 }
00338 }
00339 }
00340 return std::pair<const _Node<_Val>*,
00341 std::pair<size_t, typename _Dist::distance_type> >
00342 (__best, std::pair<size_t, typename _Dist::distance_type>
00343 (__dim, __max));
00344 }
00345
00346
00347 }
00348
00349 #endif // include guard
00350
00351
00352
00353
00354
00355
00356
00357
00358
00359
00360
00361
00362