├── .gitignore ├── Images ├── kdtree.pdn └── kdtree.png ├── KdTree.nuspec ├── KdTree.sln ├── KdTreeLib ├── HyperRect.cs ├── IKdTree.cs ├── IPriorityQueue.cs ├── KdTree.cs ├── KdTreeLib.csproj ├── KdTreeNode.cs ├── Math │ ├── DoubleMath.cs │ ├── FloatMath.cs │ ├── ITypeMath.cs │ └── TypeMath.cs ├── NearestNeighbourList.cs ├── PriorityQueue.cs ├── PriorityQueue2.cs └── Properties │ └── AssemblyInfo.cs ├── KdTreeTestsLib ├── KdTreeTests.cs ├── KdTreeTestsLib.csproj ├── NearestNeighbourListTests.cs ├── PriorityQueueTests.cs └── Properties │ └── AssemblyInfo.cs ├── LICENSE └── README.md /.gitignore: -------------------------------------------------------------------------------- 1 | # Build Folders (you can keep bin if you'd like, to store dlls and pdbs) 2 | [Bb]in/ 3 | [Oo]bj/ 4 | 5 | # mstest test results 6 | TestResults 7 | 8 | ## Ignore Visual Studio temporary files, build results, and 9 | ## files generated by popular Visual Studio add-ons. 10 | 11 | # User-specific files 12 | *.suo 13 | *.user 14 | *.sln.docstates 15 | 16 | # Build results 17 | [Dd]ebug/ 18 | [Rr]elease/ 19 | x64/ 20 | *_i.c 21 | *_p.c 22 | *.ilk 23 | *.meta 24 | *.obj 25 | *.pch 26 | *.pdb 27 | *.pgc 28 | *.pgd 29 | *.rsp 30 | *.sbr 31 | *.tlb 32 | *.tli 33 | *.tlh 34 | *.tmp 35 | *.log 36 | *.vspscc 37 | *.vssscc 38 | .builds 39 | 40 | # Ignore output files 41 | *.dll 42 | *.exe 43 | 44 | # Visual C++ cache files 45 | ipch/ 46 | *.aps 47 | *.ncb 48 | *.opensdf 49 | *.sdf 50 | 51 | # Visual Studio profiler 52 | *.psess 53 | *.vsp 54 | *.vspx 55 | 56 | # Guidance Automation Toolkit 57 | *.gpState 58 | 59 | # ReSharper is a .NET coding add-in 60 | _ReSharper* 61 | 62 | # NCrunch 63 | *.ncrunch* 64 | .*crunch*.local.xml 65 | 66 | # Installshield output folder 67 | [Ee]xpress 68 | 69 | # DocProject is a documentation generator add-in 70 | DocProject/buildhelp/ 71 | DocProject/Help/*.HxT 72 | DocProject/Help/*.HxC 73 | DocProject/Help/*.hhc 74 | DocProject/Help/*.hhk 75 | DocProject/Help/*.hhp 76 | DocProject/Help/Html2 77 | DocProject/Help/html 78 | 79 | # Click-Once directory 80 | publish 81 | 82 | # Publish Web Output 83 | *.Publish.xml 84 | 85 | # NuGet Packages Directory 86 | packages 87 | 88 | # NuGet Packages 89 | *.nupkg 90 | 91 | # Windows Azure Build Output 92 | csx 93 | *.build.csdef 94 | 95 | # Windows Store app package directory 96 | AppPackages/ 97 | 98 | # Others 99 | [Bb]in 100 | [Oo]bj 101 | sql 102 | TestResults 103 | [Tt]est[Rr]esult* 104 | *.Cache 105 | ClientBin 106 | [Ss]tyle[Cc]op.* 107 | ~$* 108 | *.dbmdl 109 | Generated_Code #added for RIA/Silverlight projects 110 | 111 | # Backup & report files from converting an old project file to a newer 112 | # Visual Studio version. Backup files are not needed, because we have git ;-) 113 | _UpgradeReport_Files/ 114 | Backup*/ 115 | UpgradeLog*.XML 116 | -------------------------------------------------------------------------------- /Images/kdtree.pdn: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Unity-Technologies/KdTree/9c45cfe352909685b8248a887505a18f7b3573db/Images/kdtree.pdn -------------------------------------------------------------------------------- /Images/kdtree.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Unity-Technologies/KdTree/9c45cfe352909685b8248a887505a18f7b3573db/Images/kdtree.png -------------------------------------------------------------------------------- /KdTree.nuspec: -------------------------------------------------------------------------------- 1 |  2 | 3 | 4 | KdTree 5 | 1.3.0 6 | Kd-Tree 7 | codeandcats@gmail.com 8 | codeandcats@gmail.com 9 | https://raw.github.com/codeandcats/KdTree/master/LICENSE 10 | https://raw.githubusercontent.com/codeandcats/KdTree/master/Images/kdtree.png 11 | true 12 | Generic multi-dimensional binary search tree. 13 | Generic multi-dimensional binary search tree. 14 | Copyright codeandcats@gmail.com 2013 15 | en-AU 16 | kdtree kd-tree binary search tree bst spatial 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /KdTree.sln: -------------------------------------------------------------------------------- 1 |  2 | Microsoft Visual Studio Solution File, Format Version 12.00 3 | # Visual Studio 2012 4 | Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "KdTreeLib", "KdTreeLib\KdTreeLib.csproj", "{68C8066D-7844-41C2-993D-3160798164D1}" 5 | EndProject 6 | Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "KdTreeTestsLib", "KdTreeTestsLib\KdTreeTestsLib.csproj", "{4FCBB1BD-C02B-4804-8BC9-84C6C3F146F9}" 7 | EndProject 8 | Global 9 | GlobalSection(SolutionConfigurationPlatforms) = preSolution 10 | Debug|Any CPU = Debug|Any CPU 11 | Release|Any CPU = Release|Any CPU 12 | EndGlobalSection 13 | GlobalSection(ProjectConfigurationPlatforms) = postSolution 14 | {68C8066D-7844-41C2-993D-3160798164D1}.Debug|Any CPU.ActiveCfg = Debug|Any CPU 15 | {68C8066D-7844-41C2-993D-3160798164D1}.Debug|Any CPU.Build.0 = Debug|Any CPU 16 | {68C8066D-7844-41C2-993D-3160798164D1}.Release|Any CPU.ActiveCfg = Release|Any CPU 17 | {68C8066D-7844-41C2-993D-3160798164D1}.Release|Any CPU.Build.0 = Release|Any CPU 18 | {4FCBB1BD-C02B-4804-8BC9-84C6C3F146F9}.Debug|Any CPU.ActiveCfg = Debug|Any CPU 19 | {4FCBB1BD-C02B-4804-8BC9-84C6C3F146F9}.Debug|Any CPU.Build.0 = Debug|Any CPU 20 | {4FCBB1BD-C02B-4804-8BC9-84C6C3F146F9}.Release|Any CPU.ActiveCfg = Release|Any CPU 21 | {4FCBB1BD-C02B-4804-8BC9-84C6C3F146F9}.Release|Any CPU.Build.0 = Release|Any CPU 22 | EndGlobalSection 23 | GlobalSection(SolutionProperties) = preSolution 24 | HideSolutionNode = FALSE 25 | EndGlobalSection 26 | EndGlobal 27 | -------------------------------------------------------------------------------- /KdTreeLib/HyperRect.cs: -------------------------------------------------------------------------------- 1 | namespace KdTree 2 | { 3 | public struct HyperRect 4 | { 5 | private T[] minPoint; 6 | public T[] MinPoint 7 | { 8 | get 9 | { 10 | return minPoint; 11 | } 12 | set 13 | { 14 | minPoint = new T[value.Length]; 15 | value.CopyTo(minPoint, 0); 16 | } 17 | } 18 | 19 | private T[] maxPoint; 20 | public T[] MaxPoint 21 | { 22 | get 23 | { 24 | return maxPoint; 25 | } 26 | set 27 | { 28 | maxPoint = new T[value.Length]; 29 | value.CopyTo(maxPoint, 0); 30 | } 31 | } 32 | 33 | public static HyperRect Infinite(int dimensions, ITypeMath math) 34 | { 35 | var rect = new HyperRect(); 36 | 37 | rect.MinPoint = new T[dimensions]; 38 | rect.MaxPoint = new T[dimensions]; 39 | 40 | for (var dimension = 0; dimension < dimensions; dimension++) 41 | { 42 | rect.MinPoint[dimension] = math.NegativeInfinity; 43 | rect.MaxPoint[dimension] = math.PositiveInfinity; 44 | } 45 | 46 | return rect; 47 | } 48 | 49 | public T[] GetClosestPoint(T[] toPoint, ITypeMath math) 50 | { 51 | T[] closest = new T[toPoint.Length]; 52 | 53 | for (var dimension = 0; dimension < toPoint.Length; dimension++) 54 | { 55 | if (math.Compare(minPoint[dimension], toPoint[dimension]) > 0) 56 | { 57 | closest[dimension] = minPoint[dimension]; 58 | } 59 | else if (math.Compare(maxPoint[dimension], toPoint[dimension]) < 0) 60 | { 61 | closest[dimension] = maxPoint[dimension]; 62 | } 63 | else 64 | // Point is within rectangle, at least on this dimension 65 | closest[dimension] = toPoint[dimension]; 66 | } 67 | 68 | return closest; 69 | } 70 | 71 | public HyperRect Clone() 72 | { 73 | var rect = new HyperRect(); 74 | rect.MinPoint = MinPoint; 75 | rect.MaxPoint = MaxPoint; 76 | return rect; 77 | } 78 | } 79 | } 80 | -------------------------------------------------------------------------------- /KdTreeLib/IKdTree.cs: -------------------------------------------------------------------------------- 1 | using System.Collections.Generic; 2 | 3 | namespace KdTree 4 | { 5 | public interface IKdTree : IEnumerable> 6 | { 7 | bool Add(TKey[] point, TValue value); 8 | 9 | bool TryFindValueAt(TKey[] point, out TValue value); 10 | 11 | TValue FindValueAt(TKey[] point); 12 | 13 | bool TryFindValue(TValue value, out TKey[] point); 14 | 15 | TKey[] FindValue(TValue value); 16 | 17 | KdTreeNode[] RadialSearch(TKey[] center, TKey radius, int count); 18 | 19 | void RemoveAt(TKey[] point); 20 | 21 | void Clear(); 22 | 23 | KdTreeNode[] GetNearestNeighbours(TKey[] point, int count = int.MaxValue); 24 | 25 | int Count { get; } 26 | } 27 | } 28 | -------------------------------------------------------------------------------- /KdTreeLib/IPriorityQueue.cs: -------------------------------------------------------------------------------- 1 | namespace KdTree 2 | { 3 | public interface IPriorityQueue 4 | { 5 | void Enqueue(TItem item, TPriority priority); 6 | 7 | TItem Dequeue(); 8 | 9 | int Count { get; } 10 | } 11 | } 12 | -------------------------------------------------------------------------------- /KdTreeLib/KdTree.cs: -------------------------------------------------------------------------------- 1 | using System; 2 | using System.Collections; 3 | using System.Collections.Generic; 4 | using System.IO; 5 | using System.Linq; 6 | using System.Runtime.Serialization.Formatters.Binary; 7 | using System.Text; 8 | 9 | namespace KdTree 10 | { 11 | public enum AddDuplicateBehavior 12 | { 13 | Skip, 14 | Error, 15 | Update, 16 | Collect 17 | } 18 | 19 | public class DuplicateNodeError : Exception 20 | { 21 | public DuplicateNodeError() 22 | : base("Cannot Add Node With Duplicate Coordinates") 23 | { 24 | } 25 | } 26 | 27 | [Serializable] 28 | public class KdTree : IKdTree 29 | { 30 | public KdTree(int dimensions, ITypeMath typeMath) 31 | { 32 | this.dimensions = dimensions; 33 | this.typeMath = typeMath; 34 | Count = 0; 35 | } 36 | 37 | public KdTree(int dimensions, ITypeMath typeMath, AddDuplicateBehavior addDuplicateBehavior) 38 | : this(dimensions, typeMath) 39 | { 40 | AddDuplicateBehavior = addDuplicateBehavior; 41 | } 42 | 43 | private int dimensions; 44 | 45 | private ITypeMath typeMath = null; 46 | 47 | private KdTreeNode root = null; 48 | 49 | public AddDuplicateBehavior AddDuplicateBehavior { get; private set; } 50 | 51 | public bool Add(TKey[] point, TValue value) 52 | { 53 | var nodeToAdd = new KdTreeNode(point, value); 54 | 55 | if (root == null) 56 | { 57 | root = new KdTreeNode(point, value); 58 | } 59 | else 60 | { 61 | int dimension = -1; 62 | KdTreeNode parent = root; 63 | 64 | do 65 | { 66 | // Increment the dimension we're searching in 67 | dimension = (dimension + 1) % dimensions; 68 | 69 | // Does the node we're adding have the same hyperpoint as this node? 70 | if (typeMath.AreEqual(point, parent.Point)) 71 | { 72 | switch (AddDuplicateBehavior) 73 | { 74 | case AddDuplicateBehavior.Skip: 75 | return false; 76 | 77 | case AddDuplicateBehavior.Error: 78 | throw new DuplicateNodeError(); 79 | 80 | case AddDuplicateBehavior.Update: 81 | parent.Value = value; 82 | break; 83 | 84 | case AddDuplicateBehavior.Collect: 85 | parent.AddDuplicate(value); 86 | return false; 87 | 88 | default: 89 | // Should never happen 90 | throw new Exception("Unexpected AddDuplicateBehavior"); 91 | } 92 | } 93 | 94 | // Which side does this node sit under in relation to it's parent at this level? 95 | int compare = typeMath.Compare(point[dimension], parent.Point[dimension]); 96 | 97 | if (parent[compare] == null) 98 | { 99 | parent[compare] = nodeToAdd; 100 | break; 101 | } 102 | else 103 | { 104 | parent = parent[compare]; 105 | } 106 | } 107 | while (true); 108 | } 109 | 110 | Count++; 111 | return true; 112 | } 113 | 114 | private void ReadChildNodes(KdTreeNode removedNode) 115 | { 116 | if (removedNode.IsLeaf) 117 | return; 118 | 119 | // The folllowing code might seem a little redundant but we're using 120 | // 2 queues so we can add the child nodes back in, in (more or less) 121 | // the same order they were added in the first place 122 | var nodesToReadd = new Queue>(); 123 | 124 | var nodesToReaddQueue = new Queue>(); 125 | 126 | if (removedNode.LeftChild != null) 127 | nodesToReaddQueue.Enqueue(removedNode.LeftChild); 128 | 129 | if (removedNode.RightChild != null) 130 | nodesToReaddQueue.Enqueue(removedNode.RightChild); 131 | 132 | while (nodesToReaddQueue.Count > 0) 133 | { 134 | var nodeToReadd = nodesToReaddQueue.Dequeue(); 135 | 136 | nodesToReadd.Enqueue(nodeToReadd); 137 | 138 | for (int side = -1; side <= 1; side += 2) 139 | { 140 | if (nodeToReadd[side] != null) 141 | { 142 | nodesToReaddQueue.Enqueue(nodeToReadd[side]); 143 | 144 | nodeToReadd[side] = null; 145 | } 146 | } 147 | } 148 | 149 | while (nodesToReadd.Count > 0) 150 | { 151 | var nodeToReadd = nodesToReadd.Dequeue(); 152 | 153 | Count--; 154 | Add(nodeToReadd.Point, nodeToReadd.Value); 155 | } 156 | } 157 | 158 | public void RemoveAt(TKey[] point) 159 | { 160 | // Is tree empty? 161 | if (root == null) 162 | return; 163 | 164 | KdTreeNode node; 165 | 166 | if (typeMath.AreEqual(point, root.Point)) 167 | { 168 | node = root; 169 | root = null; 170 | Count--; 171 | ReadChildNodes(node); 172 | return; 173 | } 174 | 175 | node = root; 176 | 177 | int dimension = -1; 178 | do 179 | { 180 | dimension = (dimension + 1) % dimensions; 181 | 182 | int compare = typeMath.Compare(point[dimension], node.Point[dimension]); 183 | 184 | if (node[compare] == null) 185 | // Can't find node 186 | return; 187 | 188 | if (typeMath.AreEqual(point, node[compare].Point)) 189 | { 190 | var nodeToRemove = node[compare]; 191 | node[compare] = null; 192 | Count--; 193 | 194 | ReadChildNodes(nodeToRemove); 195 | } 196 | else 197 | node = node[compare]; 198 | } 199 | while (node != null); 200 | } 201 | 202 | public KdTreeNode[] GetNearestNeighbours(TKey[] point, int count) 203 | { 204 | if (count > Count) 205 | count = Count; 206 | 207 | if (count < 0) 208 | { 209 | throw new ArgumentException("Number of neighbors cannot be negative"); 210 | } 211 | 212 | if (count == 0) 213 | return new KdTreeNode[0]; 214 | 215 | var neighbours = new KdTreeNode[count]; 216 | 217 | var nearestNeighbours = new NearestNeighbourList, TKey>(count, typeMath); 218 | 219 | var rect = HyperRect.Infinite(dimensions, typeMath); 220 | 221 | AddNearestNeighbours(root, point, rect, 0, nearestNeighbours, typeMath.MaxValue); 222 | 223 | count = nearestNeighbours.Count; 224 | 225 | var neighbourArray = new KdTreeNode[count]; 226 | 227 | for (var index = 0; index < count; index++) 228 | neighbourArray[count - index - 1] = nearestNeighbours.RemoveFurtherest(); 229 | 230 | return neighbourArray; 231 | } 232 | 233 | /* 234 | * 1. Search for the target 235 | * 236 | * 1.1 Start by splitting the specified hyper rect 237 | * on the specified node's point along the current 238 | * dimension so that we end up with 2 sub hyper rects 239 | * (current dimension = depth % dimensions) 240 | * 241 | * 1.2 Check what sub rectangle the the target point resides in 242 | * under the current dimension 243 | * 244 | * 1.3 Set that rect to the nearer rect and also the corresponding 245 | * child node to the nearest rect and node and the other rect 246 | * and child node to the further rect and child node (for use later) 247 | * 248 | * 1.4 Travel into the nearer rect and node by calling function 249 | * recursively with nearer rect and node and incrementing 250 | * the depth 251 | * 252 | * 2. Add leaf to list of nearest neighbours 253 | * 254 | * 3. Walk back up tree and at each level: 255 | * 256 | * 3.1 Add node to nearest neighbours if 257 | * we haven't filled our nearest neighbour 258 | * list yet or if it has a distance to target less 259 | * than any of the distances in our current nearest 260 | * neighbours. 261 | * 262 | * 3.2 If there is any point in the further rectangle that is closer to 263 | * the target than our furtherest nearest neighbour then travel into 264 | * that rect and node 265 | * 266 | * That's it, when it finally finishes traversing the branches 267 | * it needs to we'll have our list! 268 | */ 269 | 270 | private void AddNearestNeighbours( 271 | KdTreeNode node, 272 | TKey[] target, 273 | HyperRect rect, 274 | int depth, 275 | NearestNeighbourList, TKey> nearestNeighbours, 276 | TKey maxSearchRadiusSquared) 277 | { 278 | if (node == null) 279 | return; 280 | 281 | // Work out the current dimension 282 | int dimension = depth % dimensions; 283 | 284 | // Split our hyper-rect into 2 sub rects along the current 285 | // node's point on the current dimension 286 | var leftRect = rect.Clone(); 287 | leftRect.MaxPoint[dimension] = node.Point[dimension]; 288 | 289 | var rightRect = rect.Clone(); 290 | rightRect.MinPoint[dimension] = node.Point[dimension]; 291 | 292 | // Which side does the target reside in? 293 | int compare = typeMath.Compare(target[dimension], node.Point[dimension]); 294 | 295 | var nearerRect = compare <= 0 ? leftRect : rightRect; 296 | var furtherRect = compare <= 0 ? rightRect : leftRect; 297 | 298 | var nearerNode = compare <= 0 ? node.LeftChild : node.RightChild; 299 | var furtherNode = compare <= 0 ? node.RightChild : node.LeftChild; 300 | 301 | // Let's walk down into the nearer branch 302 | if (nearerNode != null) 303 | { 304 | AddNearestNeighbours( 305 | nearerNode, 306 | target, 307 | nearerRect, 308 | depth + 1, 309 | nearestNeighbours, 310 | maxSearchRadiusSquared); 311 | } 312 | 313 | TKey distanceSquaredToTarget; 314 | 315 | // Walk down into the further branch but only if our capacity hasn't been reached 316 | // OR if there's a region in the further rect that's closer to the target than our 317 | // current furtherest nearest neighbour 318 | TKey[] closestPointInFurtherRect = furtherRect.GetClosestPoint(target, typeMath); 319 | distanceSquaredToTarget = typeMath.DistanceSquaredBetweenPoints(closestPointInFurtherRect, target); 320 | 321 | if (typeMath.Compare(distanceSquaredToTarget, maxSearchRadiusSquared) <= 0) 322 | { 323 | if (nearestNeighbours.IsCapacityReached) 324 | { 325 | if (typeMath.Compare(distanceSquaredToTarget, nearestNeighbours.GetFurtherestDistance()) < 0) 326 | AddNearestNeighbours( 327 | furtherNode, 328 | target, 329 | furtherRect, 330 | depth + 1, 331 | nearestNeighbours, 332 | maxSearchRadiusSquared); 333 | } 334 | else 335 | { 336 | AddNearestNeighbours( 337 | furtherNode, 338 | target, 339 | furtherRect, 340 | depth + 1, 341 | nearestNeighbours, 342 | maxSearchRadiusSquared); 343 | } 344 | } 345 | 346 | // Try to add the current node to our nearest neighbours list 347 | distanceSquaredToTarget = typeMath.DistanceSquaredBetweenPoints(node.Point, target); 348 | 349 | if (typeMath.Compare(distanceSquaredToTarget, maxSearchRadiusSquared) <= 0) 350 | nearestNeighbours.Add(node, distanceSquaredToTarget); 351 | } 352 | 353 | public KdTreeNode[] RadialSearch(TKey[] center, TKey radius, int count) 354 | { 355 | var nearestNeighbours = new NearestNeighbourList, TKey>(count, typeMath); 356 | 357 | AddNearestNeighbours( 358 | root, 359 | center, 360 | HyperRect.Infinite(dimensions, typeMath), 361 | 0, 362 | nearestNeighbours, 363 | typeMath.Multiply(radius, radius)); 364 | 365 | count = nearestNeighbours.Count; 366 | 367 | var neighbourArray = new KdTreeNode[count]; 368 | 369 | for (var index = 0; index < count; index++) 370 | neighbourArray[count - index - 1] = nearestNeighbours.RemoveFurtherest(); 371 | 372 | return neighbourArray; 373 | } 374 | 375 | public int Count { get; private set; } 376 | 377 | public bool TryFindValueAt(TKey[] point, out TValue value) 378 | { 379 | var parent = root; 380 | int dimension = -1; 381 | do 382 | { 383 | if (parent == null) 384 | { 385 | value = default(TValue); 386 | return false; 387 | } 388 | else if (typeMath.AreEqual(point, parent.Point)) 389 | { 390 | value = parent.Value; 391 | return true; 392 | } 393 | 394 | // Keep searching 395 | dimension = (dimension + 1) % dimensions; 396 | int compare = typeMath.Compare(point[dimension], parent.Point[dimension]); 397 | parent = parent[compare]; 398 | } 399 | while (true); 400 | } 401 | 402 | public TValue FindValueAt(TKey[] point) 403 | { 404 | TValue value; 405 | if (TryFindValueAt(point, out value)) 406 | return value; 407 | else 408 | return default(TValue); 409 | } 410 | 411 | public bool TryFindValue(TValue value, out TKey[] point) 412 | { 413 | if (root == null) 414 | { 415 | point = null; 416 | return false; 417 | } 418 | 419 | // First-in, First-out list of nodes to search 420 | var nodesToSearch = new Queue>(); 421 | 422 | nodesToSearch.Enqueue(root); 423 | 424 | while (nodesToSearch.Count > 0) 425 | { 426 | var nodeToSearch = nodesToSearch.Dequeue(); 427 | 428 | if (nodeToSearch.Value.Equals(value)) 429 | { 430 | point = nodeToSearch.Point; 431 | return true; 432 | } 433 | else 434 | { 435 | for (int side = -1; side <= 1; side += 2) 436 | { 437 | var childNode = nodeToSearch[side]; 438 | 439 | if (childNode != null) 440 | nodesToSearch.Enqueue(childNode); 441 | } 442 | } 443 | } 444 | 445 | point = null; 446 | return false; 447 | } 448 | 449 | public TKey[] FindValue(TValue value) 450 | { 451 | TKey[] point; 452 | if (TryFindValue(value, out point)) 453 | return point; 454 | else 455 | return null; 456 | } 457 | 458 | private void AddNodeToStringBuilder(KdTreeNode node, StringBuilder sb, int depth) 459 | { 460 | sb.AppendLine(node.ToString()); 461 | 462 | for (var side = -1; side <= 1; side += 2) 463 | { 464 | for (var index = 0; index <= depth; index++) 465 | sb.Append("\t"); 466 | 467 | sb.Append(side == -1 ? "L " : "R "); 468 | 469 | if (node[side] == null) 470 | sb.AppendLine(""); 471 | else 472 | AddNodeToStringBuilder(node[side], sb, depth + 1); 473 | } 474 | } 475 | 476 | public override string ToString() 477 | { 478 | if (root == null) 479 | return ""; 480 | 481 | var sb = new StringBuilder(); 482 | AddNodeToStringBuilder(root, sb, 0); 483 | return sb.ToString(); 484 | } 485 | 486 | private void AddNodesToList(KdTreeNode node, List> nodes) 487 | { 488 | if (node == null) 489 | return; 490 | 491 | nodes.Add(node); 492 | 493 | for (var side = -1; side <= 1; side += 2) 494 | { 495 | if (node[side] != null) 496 | { 497 | AddNodesToList(node[side], nodes); 498 | node[side] = null; 499 | } 500 | } 501 | } 502 | 503 | private void SortNodesArray(KdTreeNode[] nodes, int byDimension, int fromIndex, int toIndex) 504 | { 505 | for (var index = fromIndex + 1; index <= toIndex; index++) 506 | { 507 | var newIndex = index; 508 | 509 | while (true) 510 | { 511 | var a = nodes[newIndex - 1]; 512 | var b = nodes[newIndex]; 513 | if (typeMath.Compare(b.Point[byDimension], a.Point[byDimension]) < 0) 514 | { 515 | nodes[newIndex - 1] = b; 516 | nodes[newIndex] = a; 517 | } 518 | else 519 | break; 520 | } 521 | } 522 | } 523 | 524 | private void AddNodesBalanced(KdTreeNode[] nodes, int byDimension, int fromIndex, int toIndex) 525 | { 526 | if (fromIndex == toIndex) 527 | { 528 | Add(nodes[fromIndex].Point, nodes[fromIndex].Value); 529 | nodes[fromIndex] = null; 530 | return; 531 | } 532 | 533 | // Sort the array from the fromIndex to the toIndex 534 | SortNodesArray(nodes, byDimension, fromIndex, toIndex); 535 | 536 | // Find the splitting point 537 | int midIndex = fromIndex + (int)System.Math.Round((toIndex + 1 - fromIndex) / 2f) - 1; 538 | 539 | // Add the splitting point 540 | Add(nodes[midIndex].Point, nodes[midIndex].Value); 541 | nodes[midIndex] = null; 542 | 543 | // Recurse 544 | int nextDimension = (byDimension + 1) % dimensions; 545 | 546 | if (fromIndex < midIndex) 547 | AddNodesBalanced(nodes, nextDimension, fromIndex, midIndex - 1); 548 | 549 | if (toIndex > midIndex) 550 | AddNodesBalanced(nodes, nextDimension, midIndex + 1, toIndex); 551 | } 552 | 553 | public void Balance() 554 | { 555 | var nodeList = new List>(); 556 | AddNodesToList(root, nodeList); 557 | 558 | Clear(); 559 | 560 | AddNodesBalanced(nodeList.ToArray(), 0, 0, nodeList.Count - 1); 561 | } 562 | 563 | private void RemoveChildNodes(KdTreeNode node) 564 | { 565 | for (var side = -1; side <= 1; side += 2) 566 | { 567 | if (node[side] != null) 568 | { 569 | RemoveChildNodes(node[side]); 570 | node[side] = null; 571 | } 572 | } 573 | } 574 | 575 | public void Clear() 576 | { 577 | if (root != null) 578 | RemoveChildNodes(root); 579 | } 580 | 581 | public void SaveToFile(string filename) 582 | { 583 | BinaryFormatter formatter = new BinaryFormatter(); 584 | using (FileStream stream = File.Create(filename)) 585 | { 586 | formatter.Serialize(stream, this); 587 | stream.Flush(); 588 | } 589 | } 590 | 591 | public static KdTree LoadFromFile(string filename) 592 | { 593 | BinaryFormatter formatter = new BinaryFormatter(); 594 | using (FileStream stream = File.Open(filename, FileMode.Open)) 595 | { 596 | return (KdTree)formatter.Deserialize(stream); 597 | } 598 | 599 | } 600 | 601 | public IEnumerator> GetEnumerator() 602 | { 603 | var left = new Stack>(); 604 | var right = new Stack>(); 605 | 606 | Action> addLeft = node => 607 | { 608 | if (node.LeftChild != null) 609 | { 610 | left.Push(node.LeftChild); 611 | } 612 | }; 613 | 614 | Action> addRight = node => 615 | { 616 | if (node.RightChild != null) 617 | { 618 | right.Push(node.RightChild); 619 | } 620 | }; 621 | 622 | if (root != null) 623 | { 624 | yield return root; 625 | 626 | addLeft(root); 627 | addRight(root); 628 | 629 | while (true) 630 | { 631 | if (left.Any()) 632 | { 633 | var item = left.Pop(); 634 | 635 | addLeft(item); 636 | addRight(item); 637 | 638 | yield return item; 639 | } 640 | else if (right.Any()) 641 | { 642 | var item = right.Pop(); 643 | 644 | addLeft(item); 645 | addRight(item); 646 | 647 | yield return item; 648 | } 649 | else 650 | { 651 | break; 652 | } 653 | } 654 | } 655 | } 656 | 657 | IEnumerator IEnumerable.GetEnumerator() 658 | { 659 | return GetEnumerator(); 660 | } 661 | } 662 | } -------------------------------------------------------------------------------- /KdTreeLib/KdTreeLib.csproj: -------------------------------------------------------------------------------- 1 |  2 | 3 | 4 | 5 | Debug 6 | AnyCPU 7 | {68C8066D-7844-41C2-993D-3160798164D1} 8 | Library 9 | Properties 10 | KdTree 11 | KdTreeLib 12 | v3.5 13 | 512 14 | 15 | 16 | 17 | true 18 | full 19 | false 20 | bin\Debug\ 21 | DEBUG;TRACE 22 | prompt 23 | 4 24 | 25 | 26 | pdbonly 27 | true 28 | bin\Release\ 29 | TRACE 30 | prompt 31 | 4 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 64 | -------------------------------------------------------------------------------- /KdTreeLib/KdTreeNode.cs: -------------------------------------------------------------------------------- 1 | using System; 2 | using System.Text; 3 | using System.Linq; 4 | using System.Collections.Generic; 5 | 6 | namespace KdTree 7 | { 8 | [Serializable] 9 | public class KdTreeNode 10 | { 11 | public KdTreeNode() 12 | { 13 | } 14 | 15 | public KdTreeNode(TKey[] point, TValue value) 16 | { 17 | Point = point; 18 | Value = value; 19 | } 20 | 21 | public TKey[] Point; 22 | public TValue Value = default(TValue); 23 | public List Duplicates = null; 24 | 25 | internal KdTreeNode LeftChild = null; 26 | internal KdTreeNode RightChild = null; 27 | 28 | internal KdTreeNode this[int compare] 29 | { 30 | get 31 | { 32 | if (compare <= 0) 33 | return LeftChild; 34 | else 35 | return RightChild; 36 | } 37 | set 38 | { 39 | if (compare <= 0) 40 | LeftChild = value; 41 | else 42 | RightChild = value; 43 | } 44 | } 45 | 46 | public bool IsLeaf 47 | { 48 | get 49 | { 50 | return (LeftChild == null) && (RightChild == null); 51 | } 52 | } 53 | 54 | public void AddDuplicate(TValue value) 55 | { 56 | if (Duplicates == null) 57 | Duplicates = new List() { value }; 58 | else 59 | Duplicates.Add(value); 60 | } 61 | 62 | public override string ToString() 63 | { 64 | var sb = new StringBuilder(); 65 | 66 | for (var dimension = 0; dimension < Point.Length; dimension++) 67 | { 68 | sb.Append(Point[dimension].ToString()); 69 | } 70 | 71 | if (Value == null) 72 | sb.Append("null"); 73 | else 74 | sb.Append(Value.ToString()); 75 | 76 | return sb.ToString(); 77 | } 78 | } 79 | } -------------------------------------------------------------------------------- /KdTreeLib/Math/DoubleMath.cs: -------------------------------------------------------------------------------- 1 | using System; 2 | 3 | namespace KdTree.Math 4 | { 5 | [Serializable] 6 | public class DoubleMath : TypeMath 7 | { 8 | public override int Compare(double a, double b) 9 | { 10 | return a.CompareTo(b); 11 | } 12 | 13 | public override bool AreEqual(double a, double b) 14 | { 15 | return a == b; 16 | } 17 | 18 | public override double MinValue 19 | { 20 | get { return double.MinValue; } 21 | } 22 | 23 | public override double MaxValue 24 | { 25 | get { return double.MaxValue; } 26 | } 27 | 28 | public override double Zero 29 | { 30 | get { return 0; } 31 | } 32 | 33 | public override double NegativeInfinity { get { return double.NegativeInfinity; } } 34 | 35 | public override double PositiveInfinity { get { return double.PositiveInfinity; } } 36 | 37 | public override double Add(double a, double b) 38 | { 39 | return a + b; 40 | } 41 | 42 | public override double Subtract(double a, double b) 43 | { 44 | return a - b; 45 | } 46 | 47 | public override double Multiply(double a, double b) 48 | { 49 | return a * b; 50 | } 51 | 52 | public override double DistanceSquaredBetweenPoints(double[] a, double[] b) 53 | { 54 | double distance = Zero; 55 | int dimensions = a.Length; 56 | 57 | // Return the absolute distance bewteen 2 hyper points 58 | for (var dimension = 0; dimension < dimensions; dimension++) 59 | { 60 | double distOnThisAxis = Subtract(a[dimension], b[dimension]); 61 | double distOnThisAxisSquared = Multiply(distOnThisAxis, distOnThisAxis); 62 | 63 | distance = Add(distance, distOnThisAxisSquared); 64 | } 65 | 66 | return distance; 67 | } 68 | } 69 | } 70 | -------------------------------------------------------------------------------- /KdTreeLib/Math/FloatMath.cs: -------------------------------------------------------------------------------- 1 | using System; 2 | 3 | namespace KdTree.Math 4 | { 5 | [Serializable] 6 | public class FloatMath : TypeMath 7 | { 8 | public override int Compare(float a, float b) 9 | { 10 | return a.CompareTo(b); 11 | } 12 | 13 | public override bool AreEqual(float a, float b) 14 | { 15 | return a == b; 16 | } 17 | 18 | public override float MinValue 19 | { 20 | get { return float.MinValue; } 21 | } 22 | 23 | public override float MaxValue 24 | { 25 | get { return float.MaxValue; } 26 | } 27 | 28 | public override float Zero 29 | { 30 | get { return 0; } 31 | } 32 | 33 | public override float NegativeInfinity { get { return float.NegativeInfinity; } } 34 | 35 | public override float PositiveInfinity { get { return float.PositiveInfinity; } } 36 | 37 | public override float Add(float a, float b) 38 | { 39 | return a + b; 40 | } 41 | 42 | public override float Subtract(float a, float b) 43 | { 44 | return a - b; 45 | } 46 | 47 | public override float Multiply(float a, float b) 48 | { 49 | return a * b; 50 | } 51 | 52 | public override float DistanceSquaredBetweenPoints(float[] a, float[] b) 53 | { 54 | float distance = Zero; 55 | int dimensions = a.Length; 56 | 57 | // Return the absolute distance bewteen 2 hyper points 58 | for (var dimension = 0; dimension < dimensions; dimension++) 59 | { 60 | float distOnThisAxis = Subtract(a[dimension], b[dimension]); 61 | float distOnThisAxisSquared = Multiply(distOnThisAxis, distOnThisAxis); 62 | 63 | distance = Add(distance, distOnThisAxisSquared); 64 | } 65 | 66 | return distance; 67 | } 68 | } 69 | } 70 | -------------------------------------------------------------------------------- /KdTreeLib/Math/ITypeMath.cs: -------------------------------------------------------------------------------- 1 | namespace KdTree 2 | { 3 | public interface ITypeMath 4 | { 5 | int Compare(T a, T b); 6 | 7 | T MinValue { get; } 8 | 9 | T MaxValue { get; } 10 | 11 | T Min(T a, T b); 12 | 13 | T Max(T a, T b); 14 | 15 | bool AreEqual(T a, T b); 16 | 17 | bool AreEqual(T[] a, T[] b); 18 | 19 | T Add(T a, T b); 20 | 21 | T Subtract(T a, T b); 22 | 23 | T Multiply(T a, T b); 24 | 25 | T Zero { get; } 26 | 27 | T NegativeInfinity { get; } 28 | 29 | T PositiveInfinity { get; } 30 | 31 | T DistanceSquaredBetweenPoints(T[] a, T[] b); 32 | } 33 | } 34 | -------------------------------------------------------------------------------- /KdTreeLib/Math/TypeMath.cs: -------------------------------------------------------------------------------- 1 | using System; 2 | 3 | namespace KdTree.Math 4 | { 5 | // Algebraic! 6 | [Serializable] 7 | public abstract class TypeMath : ITypeMath 8 | { 9 | #region ITypeMath members 10 | 11 | public abstract int Compare(T a, T b); 12 | 13 | public abstract bool AreEqual(T a, T b); 14 | 15 | public virtual bool AreEqual(T[] a, T[] b) 16 | { 17 | if (a.Length != b.Length) 18 | return false; 19 | 20 | for (var index = 0; index < a.Length; index++) 21 | { 22 | if (!AreEqual(a[index], b[index])) 23 | return false; 24 | } 25 | 26 | return true; 27 | } 28 | 29 | public abstract T MinValue { get; } 30 | 31 | public abstract T MaxValue { get; } 32 | 33 | public T Min(T a, T b) 34 | { 35 | if (Compare(a, b) < 0) 36 | return a; 37 | else 38 | return b; 39 | } 40 | 41 | public T Max(T a, T b) 42 | { 43 | if (Compare(a, b) > 0) 44 | return a; 45 | else 46 | return b; 47 | } 48 | 49 | public abstract T Zero { get; } 50 | 51 | public abstract T NegativeInfinity { get; } 52 | 53 | public abstract T PositiveInfinity { get; } 54 | 55 | public abstract T Add(T a, T b); 56 | 57 | public abstract T Subtract(T a, T b); 58 | 59 | public abstract T Multiply(T a, T b); 60 | 61 | public abstract T DistanceSquaredBetweenPoints(T[] a, T[] b); 62 | 63 | #endregion 64 | } 65 | } 66 | -------------------------------------------------------------------------------- /KdTreeLib/NearestNeighbourList.cs: -------------------------------------------------------------------------------- 1 | using System; 2 | 3 | namespace KdTree 4 | { 5 | public interface INearestNeighbourList 6 | { 7 | bool Add(TItem item, TDistance distance); 8 | TItem GetFurtherest(); 9 | TItem RemoveFurtherest(); 10 | 11 | int MaxCapacity { get; } 12 | int Count { get; } 13 | } 14 | 15 | public class NearestNeighbourList : INearestNeighbourList 16 | { 17 | public NearestNeighbourList(int maxCapacity, ITypeMath distanceMath) 18 | { 19 | this.maxCapacity = maxCapacity; 20 | this.distanceMath = distanceMath; 21 | 22 | queue = new PriorityQueue(maxCapacity, distanceMath); 23 | } 24 | 25 | private PriorityQueue queue; 26 | 27 | private ITypeMath distanceMath; 28 | 29 | private int maxCapacity; 30 | public int MaxCapacity { get { return maxCapacity; } } 31 | 32 | public int Count { get { return queue.Count; } } 33 | 34 | public bool Add(TItem item, TDistance distance) 35 | { 36 | if (queue.Count >= maxCapacity) 37 | { 38 | // If the distance of this item is less than the distance of the last item 39 | // in our neighbour list then pop that neighbour off and push this one on 40 | // otherwise don't even bother adding this item 41 | if (distanceMath.Compare(distance, queue.GetHighestPriority()) < 0) 42 | { 43 | queue.Dequeue(); 44 | queue.Enqueue(item, distance); 45 | return true; 46 | } 47 | else 48 | return false; 49 | } 50 | else 51 | { 52 | queue.Enqueue(item, distance); 53 | return true; 54 | } 55 | } 56 | 57 | public TItem GetFurtherest() 58 | { 59 | if (Count == 0) 60 | throw new Exception("List is empty"); 61 | else 62 | return queue.GetHighest(); 63 | } 64 | 65 | public TDistance GetFurtherestDistance() 66 | { 67 | if (Count == 0) 68 | throw new Exception("List is empty"); 69 | else 70 | return queue.GetHighestPriority(); 71 | } 72 | 73 | public TItem RemoveFurtherest() 74 | { 75 | return queue.Dequeue(); 76 | } 77 | 78 | public bool IsCapacityReached 79 | { 80 | get { return Count == MaxCapacity; } 81 | } 82 | } 83 | } 84 | -------------------------------------------------------------------------------- /KdTreeLib/PriorityQueue.cs: -------------------------------------------------------------------------------- 1 | using System; 2 | 3 | struct ItemPriority 4 | { 5 | public TItem Item; 6 | public TPriority Priority; 7 | } 8 | 9 | namespace KdTree 10 | { 11 | public class PriorityQueue : IPriorityQueue 12 | { 13 | public PriorityQueue(int capacity, ITypeMath priorityMath) 14 | { 15 | if (capacity <= 0) 16 | throw new ArgumentException("Capacity must be greater than zero"); 17 | 18 | this.capacity = capacity; 19 | queue = new ItemPriority[capacity]; 20 | 21 | this.priorityMath = priorityMath; 22 | } 23 | 24 | private ITypeMath priorityMath; 25 | 26 | private ItemPriority[] queue; 27 | 28 | private int capacity; 29 | 30 | private int count; 31 | public int Count { get { return count; } } 32 | 33 | // Try to avoid unnecessary slow memory reallocations by creating your queue with an ample capacity 34 | private void ExpandCapacity() 35 | { 36 | // Double our capacity 37 | capacity *= 2; 38 | 39 | // Create a new queue 40 | var newQueue = new ItemPriority[capacity]; 41 | 42 | // Copy the contents of the original queue to the new one 43 | Array.Copy(queue, newQueue, queue.Length); 44 | 45 | // Copy the new queue over the original one 46 | queue = newQueue; 47 | } 48 | 49 | public void Enqueue(TItem item, TPriority priority) 50 | { 51 | if (++count > capacity) 52 | ExpandCapacity(); 53 | 54 | int newItemIndex = count - 1; 55 | 56 | queue[newItemIndex] = new ItemPriority { Item = item, Priority = priority }; 57 | 58 | ReorderItem(newItemIndex, -1); 59 | } 60 | 61 | public TItem Dequeue() 62 | { 63 | TItem item = queue[0].Item; 64 | 65 | queue[0].Item = default(TItem); 66 | queue[0].Priority = priorityMath.MinValue; 67 | 68 | ReorderItem(0, 1); 69 | 70 | count--; 71 | 72 | return item; 73 | } 74 | 75 | private void ReorderItem(int index, int direction) 76 | { 77 | if ((direction != -1) && (direction != 1)) 78 | throw new ArgumentException("Invalid Direction"); 79 | 80 | var item = queue[index]; 81 | 82 | int nextIndex = index + direction; 83 | 84 | while ((nextIndex >= 0) && (nextIndex < count)) 85 | { 86 | var next = queue[nextIndex]; 87 | 88 | int compare = priorityMath.Compare(item.Priority, next.Priority); 89 | 90 | // If we're moving up and our priority is higher than the next priority then swap 91 | // Or if we're moving down and our priority is lower than the next priority then swap 92 | if ( 93 | ((direction == -1) && (compare > 0)) 94 | || 95 | ((direction == 1) && (compare < 0)) 96 | ) 97 | { 98 | queue[index] = next; 99 | queue[nextIndex] = item; 100 | 101 | index += direction; 102 | nextIndex += direction; 103 | } 104 | else 105 | break; 106 | } 107 | } 108 | 109 | public TItem GetHighest() 110 | { 111 | if (count == 0) 112 | throw new Exception("Queue is empty"); 113 | else 114 | return queue[0].Item; 115 | } 116 | 117 | public TPriority GetHighestPriority() 118 | { 119 | if (count == 0) 120 | throw new Exception("Queue is empty"); 121 | else 122 | return queue[0].Priority; 123 | } 124 | } 125 | } 126 | -------------------------------------------------------------------------------- /KdTreeLib/PriorityQueue2.cs: -------------------------------------------------------------------------------- 1 | using System; 2 | using System.Collections.Generic; 3 | using System.Linq; 4 | using KdTree.Math; 5 | 6 | namespace KdTree 7 | { 8 | internal class PriorityQueue2 9 | { 10 | public PriorityQueue2(int capacity = 512) 11 | { 12 | priorityMath = TypeMath.GetMath(); 13 | maxPriority = priorityMath.Max; 14 | Initialize(capacity); 15 | } 16 | 17 | public PriorityQueue2(ITypeMath priorityMath, int capacity = 512) 18 | { 19 | this.priorityMath = priorityMath; 20 | maxPriority = priorityMath.Max; 21 | Initialize(capacity); 22 | } 23 | 24 | private ITypeMath priorityMath; 25 | 26 | private TPriority maxPriority; 27 | 28 | private TItem[] items; 29 | private TPriority[] priorities; 30 | 31 | public int Count { get; private set; } 32 | 33 | private int capacity; 34 | 35 | private void Initialize(int capacity) 36 | { 37 | this.capacity = capacity; 38 | items = new TItem[capacity + 1]; 39 | priorities = new TPriority[capacity + 1]; 40 | priorities[0] = maxPriority; 41 | items[0] = default(TItem); 42 | } 43 | 44 | public void Add(TItem item, TPriority priority) 45 | { 46 | if (Count++ >= capacity) 47 | { 48 | ExpandCapacity(); 49 | } 50 | 51 | priorities[Count] = priority; 52 | items[Count] = item; 53 | BubbleUp(Count); 54 | } 55 | 56 | public TItem Remove() 57 | { 58 | if (Count == 0) 59 | return default(TItem); 60 | 61 | TItem element = items[1]; 62 | items[1] = items[Count]; 63 | priorities[1] = priorities[Count]; 64 | items[Count] = default(TItem); 65 | priorities[Count] = priorityMath.Zero; 66 | Count--; 67 | BubbleDown(1); 68 | 69 | return element; 70 | } 71 | 72 | public TItem GetFront() 73 | { 74 | return items[1]; 75 | } 76 | 77 | public TPriority GetMaxPriority() 78 | { 79 | return priorities[1]; 80 | } 81 | 82 | private void BubbleDown(int index) 83 | { 84 | TItem element = items[index]; 85 | TPriority priority = priorities[index]; 86 | int child; 87 | 88 | for (; index * 2 <= Count; index = child) 89 | { 90 | child = index * 2; 91 | 92 | if (child != Count) 93 | if (priorityMath.Compare(priorities[child], priorities[child + 1]) < 0) 94 | child++; 95 | 96 | if (priorityMath.Compare(priority, priorities[child]) < 0) 97 | { 98 | priorities[index] = priorities[child]; 99 | items[index] = items[child]; 100 | } 101 | else 102 | { 103 | break; 104 | } 105 | } 106 | priorities[index] = priority; 107 | items[index] = element; 108 | } 109 | 110 | private void BubbleUp(int index) 111 | { 112 | TItem element = items[index]; 113 | TPriority priority = priorities[index]; 114 | 115 | while (priorityMath.Compare(priorities[index / 2], priority) < 0) 116 | { 117 | priorities[index] = priorities[index / 2]; 118 | items[index] = items[index / 2]; 119 | index /= 2; 120 | } 121 | 122 | priorities[index] = priority; 123 | items[index] = element; 124 | } 125 | 126 | private void ExpandCapacity() 127 | { 128 | capacity = Count * 2; 129 | 130 | TItem[] newItems = new TItem[capacity + 1]; 131 | TPriority[] newPriorities = new TPriority[capacity + 1]; 132 | 133 | Array.Copy(items, 0, newItems, 0, items.Length); 134 | Array.Copy(priorities, 0, newPriorities, 0, priorities.Length); 135 | 136 | items = newItems; 137 | priorities = newPriorities; 138 | } 139 | 140 | public void Clear() 141 | { 142 | for (int i = 1; i < Count; i++) 143 | items[i] = default(TItem); 144 | 145 | Count = 0; 146 | } 147 | } 148 | } 149 | -------------------------------------------------------------------------------- /KdTreeLib/Properties/AssemblyInfo.cs: -------------------------------------------------------------------------------- 1 | using System.Reflection; 2 | using System.Runtime.CompilerServices; 3 | using System.Runtime.InteropServices; 4 | 5 | // General Information about an assembly is controlled through the following 6 | // set of attributes. Change these attribute values to modify the information 7 | // associated with an assembly. 8 | [assembly: AssemblyTitle("KdTreeLib")] 9 | [assembly: AssemblyDescription("Generic multi-dimensional binary search tree")] 10 | [assembly: AssemblyConfiguration("")] 11 | [assembly: AssemblyCompany("codeandcats@gmail.com")] 12 | [assembly: AssemblyProduct("KdTreeLib")] 13 | [assembly: AssemblyCopyright("Copyright © 2016")] 14 | [assembly: AssemblyTrademark("")] 15 | [assembly: AssemblyCulture("")] 16 | 17 | // Setting ComVisible to false makes the types in this assembly not visible 18 | // to COM components. If you need to access a type in this assembly from 19 | // COM, set the ComVisible attribute to true on that type. 20 | [assembly: ComVisible(false)] 21 | 22 | // The following GUID is for the ID of the typelib if this project is exposed to COM 23 | [assembly: Guid("40b103e3-41e5-4a82-b9ba-384af1391862")] 24 | 25 | // Version information for an assembly consists of the following four values: 26 | // 27 | // Major Version 28 | // Minor Version 29 | // Build Number 30 | // Revision 31 | // 32 | // You can specify all the values or you can default the Build and Revision Numbers 33 | // by using the '*' as shown below: 34 | // [assembly: AssemblyVersion("1.0.*")] 35 | [assembly: AssemblyVersion("1.3.0.0")] 36 | [assembly: AssemblyFileVersion("1.3.0.0")] 37 | -------------------------------------------------------------------------------- /KdTreeTestsLib/KdTreeTests.cs: -------------------------------------------------------------------------------- 1 | using System; 2 | using System.Collections.Generic; 3 | using System.Linq; 4 | using Microsoft.VisualStudio.TestTools.UnitTesting; 5 | using KdTree.Math; 6 | 7 | struct City 8 | { 9 | public string Address; 10 | public float Lat; 11 | public float Long; 12 | public float DistanceFromToowoomba; 13 | } 14 | 15 | namespace KdTree.Tests 16 | { 17 | [TestClass] 18 | public class KdTreeTests 19 | { 20 | private KdTree tree; 21 | 22 | [TestInitialize] 23 | public void Setup() 24 | { 25 | tree = new KdTree(2, new FloatMath()); 26 | 27 | testNodes = new List>(); 28 | testNodes.AddRange(new KdTreeNode[] 29 | { 30 | new KdTreeNode(new float[] { 5, 5 }, "Root"), 31 | 32 | new KdTreeNode(new float[] { 2.5f, 2.5f }, "Root-Left"), 33 | new KdTreeNode(new float[] { 7.5f, 7.5f }, "Root-Right"), 34 | 35 | new KdTreeNode(new float[] { 1, 10 }, "Root-Left-Left"), 36 | 37 | new KdTreeNode(new float[] { 10, 10 }, "Root-Right-Right") 38 | }); 39 | } 40 | 41 | [TestCleanup] 42 | public void TearDown() 43 | { 44 | tree = null; 45 | } 46 | 47 | private List> testNodes; 48 | 49 | private void AddTestNodes() 50 | { 51 | foreach (var node in testNodes) 52 | if (!tree.Add(node.Point, node.Value)) 53 | throw new Exception("Failed to add node to tree"); 54 | } 55 | 56 | [TestMethod] 57 | [TestCategory("KdTree")] 58 | public void TestAdd() 59 | { 60 | // Add nodes to tree 61 | AddTestNodes(); 62 | 63 | // Check count of nodes is right 64 | Assert.AreEqual(testNodes.Count, tree.Count); 65 | } 66 | 67 | [TestMethod] 68 | [TestCategory("KdTree")] 69 | public void TestAddDuplicateInSkipMode() 70 | { 71 | tree = new KdTree(2, new FloatMath()); 72 | 73 | Assert.AreEqual(AddDuplicateBehavior.Skip, tree.AddDuplicateBehavior); 74 | 75 | AddTestNodes(); 76 | 77 | var count = tree.Count; 78 | 79 | var added = tree.Add(testNodes[0].Point, "Some other value"); 80 | 81 | Assert.AreEqual(false, added); 82 | Assert.AreEqual(count, tree.Count); 83 | } 84 | 85 | [TestMethod] 86 | [TestCategory("KdTree")] 87 | public void TestAddDuplicateInErrorMode() 88 | { 89 | tree = new KdTree(2, new FloatMath(), AddDuplicateBehavior.Error); 90 | 91 | AddTestNodes(); 92 | 93 | var count = tree.Count; 94 | Exception error = null; 95 | 96 | try 97 | { 98 | tree.Add(testNodes[0].Point, "Some other value"); 99 | } 100 | catch (Exception e) 101 | { 102 | error = e; 103 | } 104 | 105 | Assert.AreEqual(count, tree.Count); 106 | Assert.IsNotNull(error); 107 | Assert.IsInstanceOfType(error, typeof(DuplicateNodeError)); 108 | } 109 | 110 | [TestMethod] 111 | [TestCategory("KdTree")] 112 | public void TestAddDuplicateInUpdateMode() 113 | { 114 | tree = new KdTree(2, new FloatMath(), AddDuplicateBehavior.Update); 115 | 116 | AddTestNodes(); 117 | 118 | var newValue = "I love chicken, I love liver, Meow Mix Meow Mix please deliver"; 119 | 120 | tree.Add(testNodes[0].Point, newValue); 121 | 122 | var actualValue = tree.FindValueAt(testNodes[0].Point); 123 | 124 | Assert.AreEqual(newValue, actualValue); 125 | } 126 | 127 | [TestMethod] 128 | [TestCategory("KdTree")] 129 | public void TestTryFindValueAt() 130 | { 131 | AddTestNodes(); 132 | 133 | string actualValue; 134 | 135 | foreach (var node in testNodes) 136 | { 137 | if (tree.TryFindValueAt(node.Point, out actualValue)) 138 | Assert.AreEqual(node.Value, actualValue); 139 | else 140 | Assert.Fail("Could not find test node"); 141 | } 142 | 143 | if (!tree.TryFindValueAt(new float[] { 3.14f, 5 }, out actualValue)) 144 | Assert.IsNull(actualValue); 145 | else 146 | Assert.Fail("Reportedly found node it shouldn't have"); 147 | } 148 | 149 | [TestMethod] 150 | [TestCategory("KdTree")] 151 | public void TestFindValueAt() 152 | { 153 | AddTestNodes(); 154 | 155 | string actualValue; 156 | 157 | foreach (var node in testNodes) 158 | { 159 | actualValue = tree.FindValueAt(node.Point); 160 | 161 | Assert.AreEqual(node.Value, actualValue); 162 | } 163 | 164 | actualValue = tree.FindValueAt(new float[] { 3.15f, 5 }); 165 | 166 | Assert.IsNull(actualValue); 167 | } 168 | 169 | [TestMethod] 170 | [TestCategory("KdTree")] 171 | public void TestFindValue() 172 | { 173 | AddTestNodes(); 174 | 175 | float[] actualPoint; 176 | 177 | foreach (var node in testNodes) 178 | { 179 | actualPoint = tree.FindValue(node.Value); 180 | Assert.AreEqual(node.Point, actualPoint); 181 | } 182 | 183 | actualPoint = tree.FindValue("Your Mumma"); 184 | Assert.IsNull(actualPoint); 185 | } 186 | 187 | [TestMethod] 188 | [TestCategory("KdTree")] 189 | public void TestRemoveAt() 190 | { 191 | AddTestNodes(); 192 | 193 | var nodesToRemove = new KdTreeNode[] { 194 | testNodes[1], // Root-Left 195 | testNodes[0] // Root 196 | }; 197 | 198 | foreach (var nodeToRemove in nodesToRemove) 199 | { 200 | tree.RemoveAt(nodeToRemove.Point); 201 | testNodes.Remove(nodeToRemove); 202 | 203 | Assert.IsNull(tree.FindValue(nodeToRemove.Value)); 204 | Assert.IsNull(tree.FindValueAt(nodeToRemove.Point)); 205 | 206 | foreach (var testNode in testNodes) 207 | { 208 | Assert.AreEqual(testNode.Value, tree.FindValueAt(testNode.Point)); 209 | Assert.AreEqual(testNode.Point, tree.FindValue(testNode.Value)); 210 | } 211 | 212 | Assert.AreEqual(testNodes.Count, tree.Count); 213 | } 214 | } 215 | 216 | [TestMethod] 217 | [TestCategory("KdTree")] 218 | public void TestGetNearestNeighbours() 219 | { 220 | var toowoomba = new City() 221 | { 222 | Address = "Toowoomba, QLD, Australia", 223 | Lat = -27.5829487f, 224 | Long = 151.8643252f, 225 | DistanceFromToowoomba = 0 226 | }; 227 | 228 | City[] cities = new City[] 229 | { 230 | toowoomba, 231 | new City() 232 | { 233 | Address = "Brisbane, QLD, Australia", 234 | Lat = -27.4710107f, 235 | Long = 153.0234489f, 236 | DistanceFromToowoomba = 1.16451615177537f 237 | }, 238 | new City() 239 | { 240 | Address = "Goldcoast, QLD, Australia", 241 | Lat = -28.0172605f, 242 | Long = 153.4256987f, 243 | DistanceFromToowoomba = 1.6206523211724f 244 | }, 245 | new City() 246 | { 247 | Address = "Sunshine, QLD, Australia", 248 | Lat = -27.3748288f, 249 | Long = 153.0554193f, 250 | DistanceFromToowoomba = 1.20913979664506f 251 | }, 252 | new City() 253 | { 254 | Address = "Melbourne, VIC, Australia", 255 | Lat = -37.814107f, 256 | Long = 144.96328f, 257 | DistanceFromToowoomba = 12.3410301438779f 258 | }, 259 | new City() 260 | { 261 | Address = "Sydney, NSW, Australia", 262 | Lat = -33.8674869f, 263 | Long = 151.2069902f, 264 | DistanceFromToowoomba = 6.31882185929341f 265 | }, 266 | new City() 267 | { 268 | Address = "Perth, WA, Australia", 269 | Lat = -31.9530044f, 270 | Long = 115.8574693f, 271 | DistanceFromToowoomba = 36.2710774395312f 272 | }, 273 | new City() 274 | { 275 | Address = "Darwin, NT, Australia", 276 | Lat = -12.4628198f, 277 | Long = 130.8417694f, 278 | DistanceFromToowoomba = 25.895292049265f 279 | } 280 | /*, 281 | new City() 282 | { 283 | Address = "London, England", 284 | Lat = 51.5112139f, 285 | Long = -0.1198244f, 286 | DistanceFromToowoomba = 171.33320836029f 287 | 288 | }*/ 289 | }; 290 | 291 | foreach (var city in cities) 292 | { 293 | tree.Add(new float[] { city.Long, -city.Lat }, city.Address); 294 | } 295 | 296 | /* 297 | var sb = new System.Text.StringBuilder(); 298 | sb.AppendLine("Before Balance:"); 299 | sb.AppendLine(tree.ToString()); 300 | sb.AppendLine(""); 301 | sb.AppendLine(""); 302 | tree.Balance(); 303 | sb.AppendLine("After Balance:"); 304 | sb.AppendLine(tree.ToString()); 305 | System.Windows.Forms.Clipboard.SetText(sb.ToString()); 306 | */ 307 | 308 | for (var findLimit = 0; findLimit <= cities.Length; findLimit++) 309 | { 310 | var actualNeighbours = tree.GetNearestNeighbours( 311 | new float[] { toowoomba.Long, -toowoomba.Lat }, 312 | findLimit); 313 | 314 | var expectedNeighbours = cities 315 | .OrderBy(p => p.DistanceFromToowoomba) 316 | .Take(findLimit) 317 | .ToArray(); 318 | 319 | Assert.AreEqual(findLimit, actualNeighbours.Length); 320 | Assert.AreEqual(findLimit, expectedNeighbours.Length); 321 | 322 | for (var index = 0; index < actualNeighbours.Length; index++) 323 | { 324 | Assert.AreEqual(expectedNeighbours[index].Address, actualNeighbours[index].Value); 325 | } 326 | } 327 | } 328 | 329 | [TestMethod] 330 | [TestCategory("KdTree")] 331 | public void TestEnumerable() 332 | { 333 | AddTestNodes(); 334 | 335 | foreach (var node in tree) 336 | { 337 | var testNode = testNodes.FirstOrDefault(n => n.Point == node.Point && n.Value == node.Value); 338 | 339 | Assert.IsNotNull(testNode); 340 | 341 | testNodes.Remove(testNode); 342 | } 343 | 344 | Assert.AreEqual(0, testNodes.Count); 345 | } 346 | } 347 | } -------------------------------------------------------------------------------- /KdTreeTestsLib/KdTreeTestsLib.csproj: -------------------------------------------------------------------------------- 1 |  2 | 3 | 4 | Debug 5 | AnyCPU 6 | {4FCBB1BD-C02B-4804-8BC9-84C6C3F146F9} 7 | Library 8 | Properties 9 | KdTree.Tests 10 | KdTreeTestsLib 11 | v4.5 12 | 512 13 | {3AC096D0-A1C2-E12C-1390-A8335801FDAB};{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC} 14 | 10.0 15 | $(MSBuildExtensionsPath32)\Microsoft\VisualStudio\v$(VisualStudioVersion) 16 | $(ProgramFiles)\Common Files\microsoft shared\VSTT\$(VisualStudioVersion)\UITestExtensionPackages 17 | False 18 | UnitTest 19 | 20 | 21 | true 22 | full 23 | false 24 | bin\Debug\ 25 | DEBUG;TRACE 26 | prompt 27 | 4 28 | 29 | 30 | pdbonly 31 | true 32 | bin\Release\ 33 | TRACE 34 | prompt 35 | 4 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | {68c8066d-7844-41c2-993d-3160798164d1} 62 | KdTreeLib 63 | 64 | 65 | 66 | 67 | 68 | 69 | False 70 | 71 | 72 | False 73 | 74 | 75 | False 76 | 77 | 78 | False 79 | 80 | 81 | 82 | 83 | 84 | 85 | 92 | -------------------------------------------------------------------------------- /KdTreeTestsLib/NearestNeighbourListTests.cs: -------------------------------------------------------------------------------- 1 | using System; 2 | using System.Collections.Generic; 3 | using System.Linq; 4 | using Microsoft.VisualStudio.TestTools.UnitTesting; 5 | using KdTree; 6 | using KdTree.Math; 7 | 8 | struct Planet 9 | { 10 | public string Name; 11 | public float DistanceFromEarth; 12 | } 13 | 14 | namespace KdTree.Tests 15 | { 16 | [TestClass] 17 | public class NearestNeighbourListTests 18 | { 19 | private NearestNeighbourList nearestNeighbours; 20 | private List neighbouringPlanets; 21 | 22 | [TestInitialize] 23 | public void Setup() 24 | { 25 | nearestNeighbours = new NearestNeighbourList(5, new FloatMath()); 26 | 27 | neighbouringPlanets = new List(); 28 | neighbouringPlanets.AddRange(new Planet[] 29 | { 30 | new Planet() { Name = "Mercury", DistanceFromEarth = 91700000f }, 31 | new Planet() { Name = "Venus", DistanceFromEarth = 41400000f }, 32 | new Planet() { Name = "Mars", DistanceFromEarth = 78300000f }, 33 | new Planet() { Name = "Jupiter", DistanceFromEarth = 624400000f }, 34 | new Planet() { Name = "Saturn", DistanceFromEarth = 1250000000f }, 35 | new Planet() { Name = "Uranus", DistanceFromEarth = 2720000000f }, 36 | new Planet() { Name = "Neptune", DistanceFromEarth = 4350000000f } 37 | }); 38 | } 39 | 40 | [TestCleanup] 41 | public void TearDown() 42 | { 43 | nearestNeighbours = null; 44 | } 45 | 46 | private void AddItems() 47 | { 48 | foreach (var planet in neighbouringPlanets) 49 | { 50 | nearestNeighbours.Add(planet, planet.DistanceFromEarth); 51 | } 52 | } 53 | 54 | [TestMethod] 55 | [TestCategory("NearestNeighbourList")] 56 | public void TestAddAndCount() 57 | { 58 | AddItems(); 59 | 60 | Assert.AreEqual(5, nearestNeighbours.Count); 61 | } 62 | 63 | [TestMethod] 64 | [TestCategory("NearestNeighbourList")] 65 | public void TestRemoveFurtherest() 66 | { 67 | AddItems(); 68 | 69 | var sortedPlanets = neighbouringPlanets 70 | .OrderBy(p => p.DistanceFromEarth) 71 | .Take(5) 72 | .OrderByDescending(p => p.DistanceFromEarth) 73 | .ToArray(); 74 | 75 | for (var index = 0; index < sortedPlanets.Length; index++) 76 | { 77 | Assert.AreEqual( 78 | sortedPlanets[index].Name, 79 | nearestNeighbours.RemoveFurtherest().Name); 80 | } 81 | 82 | Assert.AreEqual(0, nearestNeighbours.Count); 83 | } 84 | } 85 | } 86 | -------------------------------------------------------------------------------- /KdTreeTestsLib/PriorityQueueTests.cs: -------------------------------------------------------------------------------- 1 | using System; 2 | using Microsoft.VisualStudio.TestTools.UnitTesting; 3 | using System.Collections.Generic; 4 | using System.Linq; 5 | using KdTree; 6 | using KdTree.Math; 7 | 8 | struct Person 9 | { 10 | public string Name; 11 | public int Age; 12 | } 13 | 14 | namespace KdTree.Tests 15 | { 16 | [TestClass] 17 | public class PriorityQueueTests 18 | { 19 | private PriorityQueue queue; 20 | private List people; 21 | 22 | [TestInitialize] 23 | public void Setup() 24 | { 25 | queue = new PriorityQueue(2, new FloatMath()); 26 | 27 | people = new List(); 28 | people.AddRange(new Person[] 29 | { 30 | new Person() { Name = "Chris", Age = 16 }, 31 | new Person() { Name = "Stewie", Age = 1 }, 32 | new Person() { Name = "Brian", Age = 10 }, 33 | new Person() { Name = "Meg", Age = 15 }, 34 | new Person() { Name = "Peter", Age = 41 }, 35 | new Person() { Name = "Lois", Age = 38 } 36 | }); 37 | } 38 | 39 | [TestCleanup] 40 | public void TearDown() 41 | { 42 | queue = null; 43 | } 44 | 45 | [TestMethod] 46 | [TestCategory("PriorityQueue")] 47 | public void TestQueue() 48 | { 49 | foreach (var person in people) 50 | queue.Enqueue(person.Name, person.Age); 51 | 52 | var peopleSortedByAgeDesc = people.OrderByDescending(p => p.Age).ToArray(); 53 | 54 | for (var index = 0; index < peopleSortedByAgeDesc.Length; index++) 55 | { 56 | var person = peopleSortedByAgeDesc[index]; 57 | 58 | Assert.AreEqual(person.Name, queue.Dequeue()); 59 | 60 | Assert.AreEqual(peopleSortedByAgeDesc.Length - index - 1, queue.Count); 61 | } 62 | 63 | Assert.AreEqual(0, queue.Count); 64 | } 65 | } 66 | } 67 | -------------------------------------------------------------------------------- /KdTreeTestsLib/Properties/AssemblyInfo.cs: -------------------------------------------------------------------------------- 1 | using System.Reflection; 2 | using System.Runtime.CompilerServices; 3 | using System.Runtime.InteropServices; 4 | 5 | // General Information about an assembly is controlled through the following 6 | // set of attributes. Change these attribute values to modify the information 7 | // associated with an assembly. 8 | [assembly: AssemblyTitle("KdTreeTestsLib")] 9 | [assembly: AssemblyDescription("")] 10 | [assembly: AssemblyConfiguration("")] 11 | [assembly: AssemblyCompany("")] 12 | [assembly: AssemblyProduct("KdTreeTestsLib")] 13 | [assembly: AssemblyCopyright("Copyright © 2016")] 14 | [assembly: AssemblyTrademark("")] 15 | [assembly: AssemblyCulture("")] 16 | 17 | // Setting ComVisible to false makes the types in this assembly not visible 18 | // to COM components. If you need to access a type in this assembly from 19 | // COM, set the ComVisible attribute to true on that type. 20 | [assembly: ComVisible(false)] 21 | 22 | // The following GUID is for the ID of the typelib if this project is exposed to COM 23 | [assembly: Guid("24f267bf-8c4e-48ad-ad22-f176528ef69e")] 24 | 25 | // Version information for an assembly consists of the following four values: 26 | // 27 | // Major Version 28 | // Minor Version 29 | // Build Number 30 | // Revision 31 | // 32 | // You can specify all the values or you can default the Build and Revision Numbers 33 | // by using the '*' as shown below: 34 | // [assembly: AssemblyVersion("1.0.*")] 35 | [assembly: AssemblyVersion("1.3.0.0")] 36 | [assembly: AssemblyFileVersion("1.3.0.0")] 37 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | The MIT License (MIT) 2 | 3 | Copyright (c) 2013 codeandcats 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy of 6 | this software and associated documentation files (the "Software"), to deal in 7 | the Software without restriction, including without limitation the rights to 8 | use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of 9 | the Software, and to permit persons to whom the Software is furnished to do so, 10 | subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 17 | FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 18 | COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 19 | IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 20 | CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 21 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | KdTree 2 | ====== 3 | 4 | A fast, generic, multi-dimensional Binary Search Tree written in C# 5 | 6 | Forked from [codeandcats KdTree](https://github.com/codeandcats/KdTree). 7 | 8 | ## Changes from KdTree 9 | 10 | This branch is modified to compile for .NET Framework 3.5, and removes the non-MIT licensed GeoUtils class. 11 | 12 | --------------------------------------------------------------------------------