├── .gitignore ├── Images ├── kdtree.pdn └── kdtree.png ├── KdTree.sln ├── KdTreeLib ├── HyperRect.cs ├── IKdTree.cs ├── IPriorityQueue.cs ├── KdTree.cs ├── KdTreeLib.csproj ├── KdTreeNode.cs ├── Math │ ├── DoubleMath.cs │ ├── FloatMath.cs │ ├── GeoMath.cs │ ├── GeoUtils.cs │ ├── ITypeMath.cs │ └── TypeMath.cs ├── NearestNeighbourList.cs └── PriorityQueue.cs ├── KdTreeTestsLib ├── KdTreeTests.cs ├── KdTreeTestsLib.csproj ├── NearestNeighbourListTests.cs └── PriorityQueueTests.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 | /.vs/ 117 | -------------------------------------------------------------------------------- /Images/kdtree.pdn: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/codeandcats/KdTree/075fb48442924cf0062371a4bfcbf03f7a93636a/Images/kdtree.pdn -------------------------------------------------------------------------------- /Images/kdtree.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/codeandcats/KdTree/075fb48442924cf0062371a4bfcbf03f7a93636a/Images/kdtree.png -------------------------------------------------------------------------------- /KdTree.sln: -------------------------------------------------------------------------------- 1 |  2 | Microsoft Visual Studio Solution File, Format Version 12.00 3 | # Visual Studio 15 4 | VisualStudioVersion = 15.0.27130.2036 5 | MinimumVisualStudioVersion = 10.0.40219.1 6 | Project("{9A19103F-16F7-4668-BE54-9A1E7A4F7556}") = "KdTreeLib", "KdTreeLib\KdTreeLib.csproj", "{68C8066D-7844-41C2-993D-3160798164D1}" 7 | EndProject 8 | Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "KdTreeTestsLib", "KdTreeTestsLib\KdTreeTestsLib.csproj", "{4FCBB1BD-C02B-4804-8BC9-84C6C3F146F9}" 9 | EndProject 10 | Project("{2150E333-8FDC-42A3-9474-1A3956D46DE8}") = "Solution Items", "Solution Items", "{880686F7-E229-4A10-B754-6A0893F681C4}" 11 | ProjectSection(SolutionItems) = preProject 12 | LICENSE = LICENSE 13 | README.md = README.md 14 | EndProjectSection 15 | EndProject 16 | Global 17 | GlobalSection(SolutionConfigurationPlatforms) = preSolution 18 | Debug|Any CPU = Debug|Any CPU 19 | Release|Any CPU = Release|Any CPU 20 | EndGlobalSection 21 | GlobalSection(ProjectConfigurationPlatforms) = postSolution 22 | {68C8066D-7844-41C2-993D-3160798164D1}.Debug|Any CPU.ActiveCfg = Debug|Any CPU 23 | {68C8066D-7844-41C2-993D-3160798164D1}.Debug|Any CPU.Build.0 = Debug|Any CPU 24 | {68C8066D-7844-41C2-993D-3160798164D1}.Release|Any CPU.ActiveCfg = Release|Any CPU 25 | {68C8066D-7844-41C2-993D-3160798164D1}.Release|Any CPU.Build.0 = Release|Any CPU 26 | {4FCBB1BD-C02B-4804-8BC9-84C6C3F146F9}.Debug|Any CPU.ActiveCfg = Debug|Any CPU 27 | {4FCBB1BD-C02B-4804-8BC9-84C6C3F146F9}.Debug|Any CPU.Build.0 = Debug|Any CPU 28 | {4FCBB1BD-C02B-4804-8BC9-84C6C3F146F9}.Release|Any CPU.ActiveCfg = Release|Any CPU 29 | {4FCBB1BD-C02B-4804-8BC9-84C6C3F146F9}.Release|Any CPU.Build.0 = Release|Any CPU 30 | EndGlobalSection 31 | GlobalSection(SolutionProperties) = preSolution 32 | HideSolutionNode = FALSE 33 | EndGlobalSection 34 | GlobalSection(ExtensibilityGlobals) = postSolution 35 | SolutionGuid = {B47FB0CA-E29B-4D23-9CC5-28BCA1254A63} 36 | EndGlobalSection 37 | EndGlobal 38 | -------------------------------------------------------------------------------- /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 | MinPoint = new T[dimensions], 38 | MaxPoint = new T[dimensions] 39 | }; 40 | 41 | for (var dimension = 0; dimension < dimensions; dimension++) 42 | { 43 | rect.MinPoint[dimension] = math.NegativeInfinity; 44 | rect.MaxPoint[dimension] = math.PositiveInfinity; 45 | } 46 | 47 | return rect; 48 | } 49 | 50 | public T[] GetClosestPoint(T[] toPoint, ITypeMath math) 51 | { 52 | T[] closest = new T[toPoint.Length]; 53 | 54 | for (var dimension = 0; dimension < toPoint.Length; dimension++) 55 | { 56 | if (math.Compare(minPoint[dimension], toPoint[dimension]) > 0) 57 | { 58 | closest[dimension] = minPoint[dimension]; 59 | } 60 | else if (math.Compare(maxPoint[dimension], toPoint[dimension]) < 0) 61 | { 62 | closest[dimension] = maxPoint[dimension]; 63 | } 64 | else 65 | // Point is within rectangle, at least on this dimension 66 | closest[dimension] = toPoint[dimension]; 67 | } 68 | 69 | return closest; 70 | } 71 | 72 | public HyperRect Clone() 73 | { 74 | var rect = new HyperRect 75 | { 76 | MinPoint = MinPoint, 77 | MaxPoint = MaxPoint 78 | }; 79 | return rect; 80 | } 81 | } 82 | } 83 | -------------------------------------------------------------------------------- /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 | } 17 | 18 | public class DuplicateNodeError : Exception 19 | { 20 | public DuplicateNodeError() 21 | : base("Cannot Add Node With Duplicate Coordinates") 22 | { 23 | } 24 | } 25 | 26 | [Serializable] 27 | public class KdTree : IKdTree 28 | { 29 | public KdTree(int dimensions, ITypeMath typeMath) 30 | { 31 | this.dimensions = dimensions; 32 | this.typeMath = typeMath; 33 | Count = 0; 34 | } 35 | 36 | public KdTree(int dimensions, ITypeMath typeMath, AddDuplicateBehavior addDuplicateBehavior) 37 | : this(dimensions, typeMath) 38 | { 39 | AddDuplicateBehavior = addDuplicateBehavior; 40 | } 41 | 42 | private int dimensions; 43 | 44 | private ITypeMath typeMath = null; 45 | 46 | private KdTreeNode root = null; 47 | 48 | public AddDuplicateBehavior AddDuplicateBehavior { get; private set; } 49 | 50 | public bool Add(TKey[] point, TValue value) 51 | { 52 | var nodeToAdd = new KdTreeNode(point, value); 53 | 54 | if (root == null) 55 | { 56 | root = new KdTreeNode(point, value); 57 | } 58 | else 59 | { 60 | int dimension = -1; 61 | KdTreeNode parent = root; 62 | 63 | do 64 | { 65 | // Increment the dimension we're searching in 66 | dimension = (dimension + 1) % dimensions; 67 | 68 | // Does the node we're adding have the same hyperpoint as this node? 69 | if (typeMath.AreEqual(point, parent.Point)) 70 | { 71 | switch (AddDuplicateBehavior) 72 | { 73 | case AddDuplicateBehavior.Skip: 74 | return false; 75 | 76 | case AddDuplicateBehavior.Error: 77 | throw new DuplicateNodeError(); 78 | 79 | case AddDuplicateBehavior.Update: 80 | parent.Value = value; 81 | return true; 82 | 83 | default: 84 | // Should never happen 85 | throw new Exception("Unexpected AddDuplicateBehavior"); 86 | } 87 | } 88 | 89 | // Which side does this node sit under in relation to it's parent at this level? 90 | int compare = typeMath.Compare(point[dimension], parent.Point[dimension]); 91 | 92 | if (parent[compare] == null) 93 | { 94 | parent[compare] = nodeToAdd; 95 | break; 96 | } 97 | else 98 | { 99 | parent = parent[compare]; 100 | } 101 | } 102 | while (true); 103 | } 104 | 105 | Count++; 106 | return true; 107 | } 108 | 109 | private void ReaddChildNodes(KdTreeNode removedNode) 110 | { 111 | if (removedNode.IsLeaf) 112 | return; 113 | 114 | // The folllowing code might seem a little redundant but we're using 115 | // 2 queues so we can add the child nodes back in, in (more or less) 116 | // the same order they were added in the first place 117 | var nodesToReadd = new Queue>(); 118 | 119 | var nodesToReaddQueue = new Queue>(); 120 | 121 | if (removedNode.LeftChild != null) 122 | nodesToReaddQueue.Enqueue(removedNode.LeftChild); 123 | 124 | if (removedNode.RightChild != null) 125 | nodesToReaddQueue.Enqueue(removedNode.RightChild); 126 | 127 | while (nodesToReaddQueue.Count > 0) 128 | { 129 | var nodeToReadd = nodesToReaddQueue.Dequeue(); 130 | 131 | nodesToReadd.Enqueue(nodeToReadd); 132 | 133 | for (int side = -1; side <= 1; side += 2) 134 | { 135 | if (nodeToReadd[side] != null) 136 | { 137 | nodesToReaddQueue.Enqueue(nodeToReadd[side]); 138 | 139 | nodeToReadd[side] = null; 140 | } 141 | } 142 | } 143 | 144 | while (nodesToReadd.Count > 0) 145 | { 146 | var nodeToReadd = nodesToReadd.Dequeue(); 147 | 148 | Count--; 149 | Add(nodeToReadd.Point, nodeToReadd.Value); 150 | } 151 | } 152 | 153 | public void RemoveAt(TKey[] point) 154 | { 155 | // Is tree empty? 156 | if (root == null) 157 | return; 158 | 159 | KdTreeNode node; 160 | 161 | if (typeMath.AreEqual(point, root.Point)) 162 | { 163 | node = root; 164 | root = null; 165 | Count--; 166 | ReaddChildNodes(node); 167 | return; 168 | } 169 | 170 | node = root; 171 | 172 | int dimension = -1; 173 | do 174 | { 175 | dimension = (dimension + 1) % dimensions; 176 | 177 | int compare = typeMath.Compare(point[dimension], node.Point[dimension]); 178 | 179 | if (node[compare] == null) 180 | // Can't find node 181 | return; 182 | 183 | if (typeMath.AreEqual(point, node[compare].Point)) 184 | { 185 | var nodeToRemove = node[compare]; 186 | node[compare] = null; 187 | Count--; 188 | 189 | ReaddChildNodes(nodeToRemove); 190 | } 191 | else 192 | node = node[compare]; 193 | } 194 | while (node != null); 195 | } 196 | 197 | public KdTreeNode[] GetNearestNeighbours(TKey[] point, int count) 198 | { 199 | if (count > Count) 200 | count = Count; 201 | 202 | if (count < 0) 203 | { 204 | throw new ArgumentException("Number of neighbors cannot be negative"); 205 | } 206 | 207 | if (count == 0) 208 | return new KdTreeNode[0]; 209 | 210 | var neighbours = new KdTreeNode[count]; 211 | 212 | var nearestNeighbours = new NearestNeighbourList, TKey>(count, typeMath); 213 | 214 | var rect = HyperRect.Infinite(dimensions, typeMath); 215 | 216 | AddNearestNeighbours(root, point, rect, 0, nearestNeighbours, typeMath.MaxValue); 217 | 218 | count = nearestNeighbours.Count; 219 | 220 | var neighbourArray = new KdTreeNode[count]; 221 | 222 | for (var index = 0; index < count; index++) 223 | neighbourArray[count - index - 1] = nearestNeighbours.RemoveFurtherest(); 224 | 225 | return neighbourArray; 226 | } 227 | 228 | /* 229 | * 1. Search for the target 230 | * 231 | * 1.1 Start by splitting the specified hyper rect 232 | * on the specified node's point along the current 233 | * dimension so that we end up with 2 sub hyper rects 234 | * (current dimension = depth % dimensions) 235 | * 236 | * 1.2 Check what sub rectangle the the target point resides in 237 | * under the current dimension 238 | * 239 | * 1.3 Set that rect to the nearer rect and also the corresponding 240 | * child node to the nearest rect and node and the other rect 241 | * and child node to the further rect and child node (for use later) 242 | * 243 | * 1.4 Travel into the nearer rect and node by calling function 244 | * recursively with nearer rect and node and incrementing 245 | * the depth 246 | * 247 | * 2. Add leaf to list of nearest neighbours 248 | * 249 | * 3. Walk back up tree and at each level: 250 | * 251 | * 3.1 Add node to nearest neighbours if 252 | * we haven't filled our nearest neighbour 253 | * list yet or if it has a distance to target less 254 | * than any of the distances in our current nearest 255 | * neighbours. 256 | * 257 | * 3.2 If there is any point in the further rectangle that is closer to 258 | * the target than our furtherest nearest neighbour then travel into 259 | * that rect and node 260 | * 261 | * That's it, when it finally finishes traversing the branches 262 | * it needs to we'll have our list! 263 | */ 264 | 265 | private void AddNearestNeighbours( 266 | KdTreeNode node, 267 | TKey[] target, 268 | HyperRect rect, 269 | int depth, 270 | NearestNeighbourList, TKey> nearestNeighbours, 271 | TKey maxSearchRadiusSquared) 272 | { 273 | if (node == null) 274 | return; 275 | 276 | // Work out the current dimension 277 | int dimension = depth % dimensions; 278 | 279 | // Split our hyper-rect into 2 sub rects along the current 280 | // node's point on the current dimension 281 | var leftRect = rect.Clone(); 282 | leftRect.MaxPoint[dimension] = node.Point[dimension]; 283 | 284 | var rightRect = rect.Clone(); 285 | rightRect.MinPoint[dimension] = node.Point[dimension]; 286 | 287 | // Which side does the target reside in? 288 | int compare = typeMath.Compare(target[dimension], node.Point[dimension]); 289 | 290 | var nearerRect = compare <= 0 ? leftRect : rightRect; 291 | var furtherRect = compare <= 0 ? rightRect : leftRect; 292 | 293 | var nearerNode = compare <= 0 ? node.LeftChild : node.RightChild; 294 | var furtherNode = compare <= 0 ? node.RightChild : node.LeftChild; 295 | 296 | // Let's walk down into the nearer branch 297 | if (nearerNode != null) 298 | { 299 | AddNearestNeighbours( 300 | nearerNode, 301 | target, 302 | nearerRect, 303 | depth + 1, 304 | nearestNeighbours, 305 | maxSearchRadiusSquared); 306 | } 307 | 308 | TKey distanceSquaredToTarget; 309 | 310 | // Walk down into the further branch but only if our capacity hasn't been reached 311 | // OR if there's a region in the further rect that's closer to the target than our 312 | // current furtherest nearest neighbour 313 | TKey[] closestPointInFurtherRect = furtherRect.GetClosestPoint(target, typeMath); 314 | distanceSquaredToTarget = typeMath.DistanceSquaredBetweenPoints(closestPointInFurtherRect, target); 315 | 316 | if (typeMath.Compare(distanceSquaredToTarget, maxSearchRadiusSquared) <= 0) 317 | { 318 | if (nearestNeighbours.IsCapacityReached) 319 | { 320 | if (typeMath.Compare(distanceSquaredToTarget, nearestNeighbours.GetFurtherestDistance()) < 0) 321 | AddNearestNeighbours( 322 | furtherNode, 323 | target, 324 | furtherRect, 325 | depth + 1, 326 | nearestNeighbours, 327 | maxSearchRadiusSquared); 328 | } 329 | else 330 | { 331 | AddNearestNeighbours( 332 | furtherNode, 333 | target, 334 | furtherRect, 335 | depth + 1, 336 | nearestNeighbours, 337 | maxSearchRadiusSquared); 338 | } 339 | } 340 | 341 | // Try to add the current node to our nearest neighbours list 342 | distanceSquaredToTarget = typeMath.DistanceSquaredBetweenPoints(node.Point, target); 343 | 344 | if (typeMath.Compare(distanceSquaredToTarget, maxSearchRadiusSquared) <= 0) 345 | nearestNeighbours.Add(node, distanceSquaredToTarget); 346 | } 347 | 348 | /// 349 | /// Performs a radial search. 350 | /// 351 | /// Center point 352 | /// Radius to find neighbours within 353 | public KdTreeNode[] RadialSearch(TKey[] center, TKey radius) 354 | { 355 | var nearestNeighbours = new NearestNeighbourList, TKey>(typeMath); 356 | return RadialSearch(center, radius, nearestNeighbours); 357 | } 358 | 359 | /// 360 | /// Performs a radial search up to a maximum count. 361 | /// 362 | /// Center point 363 | /// Radius to find neighbours within 364 | /// Maximum number of neighbours 365 | public KdTreeNode[] RadialSearch(TKey[] center, TKey radius, int count) 366 | { 367 | var nearestNeighbours = new NearestNeighbourList, TKey>(count, typeMath); 368 | return RadialSearch(center, radius, nearestNeighbours); 369 | } 370 | 371 | private KdTreeNode[] RadialSearch(TKey[] center, TKey radius, NearestNeighbourList, TKey> nearestNeighbours) 372 | { 373 | AddNearestNeighbours( 374 | root, 375 | center, 376 | HyperRect.Infinite(dimensions, typeMath), 377 | 0, 378 | nearestNeighbours, 379 | typeMath.Multiply(radius, radius)); 380 | 381 | var count = nearestNeighbours.Count; 382 | 383 | var neighbourArray = new KdTreeNode[count]; 384 | 385 | for (var index = 0; index < count; index++) 386 | neighbourArray[count - index - 1] = nearestNeighbours.RemoveFurtherest(); 387 | 388 | return neighbourArray; 389 | } 390 | 391 | public int Count { get; private set; } 392 | 393 | public bool TryFindValueAt(TKey[] point, out TValue value) 394 | { 395 | var parent = root; 396 | int dimension = -1; 397 | do 398 | { 399 | if (parent == null) 400 | { 401 | value = default(TValue); 402 | return false; 403 | } 404 | else if (typeMath.AreEqual(point, parent.Point)) 405 | { 406 | value = parent.Value; 407 | return true; 408 | } 409 | 410 | // Keep searching 411 | dimension = (dimension + 1) % dimensions; 412 | int compare = typeMath.Compare(point[dimension], parent.Point[dimension]); 413 | parent = parent[compare]; 414 | } 415 | while (true); 416 | } 417 | 418 | public TValue FindValueAt(TKey[] point) 419 | { 420 | if (TryFindValueAt(point, out TValue value)) 421 | return value; 422 | else 423 | return default(TValue); 424 | } 425 | 426 | public bool TryFindValue(TValue value, out TKey[] point) 427 | { 428 | if (root == null) 429 | { 430 | point = null; 431 | return false; 432 | } 433 | 434 | // First-in, First-out list of nodes to search 435 | var nodesToSearch = new Queue>(); 436 | 437 | nodesToSearch.Enqueue(root); 438 | 439 | while (nodesToSearch.Count > 0) 440 | { 441 | var nodeToSearch = nodesToSearch.Dequeue(); 442 | 443 | if (nodeToSearch.Value.Equals(value)) 444 | { 445 | point = nodeToSearch.Point; 446 | return true; 447 | } 448 | else 449 | { 450 | for (int side = -1; side <= 1; side += 2) 451 | { 452 | var childNode = nodeToSearch[side]; 453 | 454 | if (childNode != null) 455 | nodesToSearch.Enqueue(childNode); 456 | } 457 | } 458 | } 459 | 460 | point = null; 461 | return false; 462 | } 463 | 464 | public TKey[] FindValue(TValue value) 465 | { 466 | if (TryFindValue(value, out TKey[] point)) 467 | return point; 468 | else 469 | return null; 470 | } 471 | 472 | private void AddNodeToStringBuilder(KdTreeNode node, StringBuilder sb, int depth) 473 | { 474 | sb.AppendLine(node.ToString()); 475 | 476 | for (var side = -1; side <= 1; side += 2) 477 | { 478 | for (var index = 0; index <= depth; index++) 479 | sb.Append("\t"); 480 | 481 | sb.Append(side == -1 ? "L " : "R "); 482 | 483 | if (node[side] == null) 484 | sb.AppendLine(""); 485 | else 486 | AddNodeToStringBuilder(node[side], sb, depth + 1); 487 | } 488 | } 489 | 490 | public override string ToString() 491 | { 492 | if (root == null) 493 | return ""; 494 | 495 | var sb = new StringBuilder(); 496 | AddNodeToStringBuilder(root, sb, 0); 497 | return sb.ToString(); 498 | } 499 | 500 | private void AddNodesToList(KdTreeNode node, List> nodes) 501 | { 502 | if (node == null) 503 | return; 504 | 505 | nodes.Add(node); 506 | 507 | for (var side = -1; side <= 1; side += 2) 508 | { 509 | if (node[side] != null) 510 | { 511 | AddNodesToList(node[side], nodes); 512 | node[side] = null; 513 | } 514 | } 515 | } 516 | 517 | private void SortNodesArray(KdTreeNode[] nodes, int byDimension, int fromIndex, int toIndex) 518 | { 519 | for (var index = fromIndex + 1; index <= toIndex; index++) 520 | { 521 | var newIndex = index; 522 | 523 | while (true) 524 | { 525 | var a = nodes[newIndex - 1]; 526 | var b = nodes[newIndex]; 527 | if (typeMath.Compare(b.Point[byDimension], a.Point[byDimension]) < 0) 528 | { 529 | nodes[newIndex - 1] = b; 530 | nodes[newIndex] = a; 531 | } 532 | else 533 | break; 534 | } 535 | } 536 | } 537 | 538 | private void AddNodesBalanced(KdTreeNode[] nodes, int byDimension, int fromIndex, int toIndex) 539 | { 540 | if (fromIndex == toIndex) 541 | { 542 | Add(nodes[fromIndex].Point, nodes[fromIndex].Value); 543 | nodes[fromIndex] = null; 544 | return; 545 | } 546 | 547 | // Sort the array from the fromIndex to the toIndex 548 | SortNodesArray(nodes, byDimension, fromIndex, toIndex); 549 | 550 | // Find the splitting point 551 | int midIndex = fromIndex + (int)System.Math.Round((toIndex + 1 - fromIndex) / 2f) - 1; 552 | 553 | // Add the splitting point 554 | Add(nodes[midIndex].Point, nodes[midIndex].Value); 555 | nodes[midIndex] = null; 556 | 557 | // Recurse 558 | int nextDimension = (byDimension + 1) % dimensions; 559 | 560 | if (fromIndex < midIndex) 561 | AddNodesBalanced(nodes, nextDimension, fromIndex, midIndex - 1); 562 | 563 | if (toIndex > midIndex) 564 | AddNodesBalanced(nodes, nextDimension, midIndex + 1, toIndex); 565 | } 566 | 567 | public void Balance() 568 | { 569 | var nodeList = new List>(); 570 | AddNodesToList(root, nodeList); 571 | 572 | Clear(); 573 | 574 | AddNodesBalanced(nodeList.ToArray(), 0, 0, nodeList.Count - 1); 575 | } 576 | 577 | private void RemoveChildNodes(KdTreeNode node) 578 | { 579 | for (var side = -1; side <= 1; side += 2) 580 | { 581 | if (node[side] != null) 582 | { 583 | RemoveChildNodes(node[side]); 584 | node[side] = null; 585 | } 586 | } 587 | } 588 | 589 | public void Clear() 590 | { 591 | if (root != null) 592 | RemoveChildNodes(root); 593 | } 594 | 595 | public void SaveToFile(string filename) 596 | { 597 | BinaryFormatter formatter = new BinaryFormatter(); 598 | using (FileStream stream = File.Create(filename)) 599 | { 600 | formatter.Serialize(stream, this); 601 | stream.Flush(); 602 | } 603 | } 604 | 605 | public static KdTree LoadFromFile(string filename) 606 | { 607 | BinaryFormatter formatter = new BinaryFormatter(); 608 | using (FileStream stream = File.Open(filename, FileMode.Open)) 609 | { 610 | return (KdTree)formatter.Deserialize(stream); 611 | } 612 | 613 | } 614 | 615 | public IEnumerator> GetEnumerator() 616 | { 617 | var left = new Stack>(); 618 | var right = new Stack>(); 619 | 620 | void addLeft(KdTreeNode node) 621 | { 622 | if (node.LeftChild != null) 623 | { 624 | left.Push(node.LeftChild); 625 | } 626 | } 627 | 628 | void addRight(KdTreeNode node) 629 | { 630 | if (node.RightChild != null) 631 | { 632 | right.Push(node.RightChild); 633 | } 634 | } 635 | 636 | if (root != null) 637 | { 638 | yield return root; 639 | 640 | addLeft(root); 641 | addRight(root); 642 | 643 | while (true) 644 | { 645 | if (left.Any()) 646 | { 647 | var item = left.Pop(); 648 | 649 | addLeft(item); 650 | addRight(item); 651 | 652 | yield return item; 653 | } 654 | else if (right.Any()) 655 | { 656 | var item = right.Pop(); 657 | 658 | addLeft(item); 659 | addRight(item); 660 | 661 | yield return item; 662 | } 663 | else 664 | { 665 | break; 666 | } 667 | } 668 | } 669 | } 670 | 671 | IEnumerator IEnumerable.GetEnumerator() 672 | { 673 | return GetEnumerator(); 674 | } 675 | } 676 | } -------------------------------------------------------------------------------- /KdTreeLib/KdTreeLib.csproj: -------------------------------------------------------------------------------- 1 |  2 | 3 | 4 | net45;net451;net452;net46;net461;net462;net47;net471;netstandard2.0;netcoreapp2.0 5 | codeandcats@gmail.com, RobIII 6 | codeandcats@gmail.com 7 | KdTree 8 | true 9 | KdTree 10 | Copyright codeandcats@gmail.com 2013 11 | true 12 | https://raw.github.com/codeandcats/KdTree/master/LICENSE 13 | https://github.com/codeandcats/KdTree 14 | kdtree kd-tree binary search tree bst spatial 15 | * Remove requirement to limit number of result in radial search 16 | Generic multi-dimensional binary search tree. 17 | https://raw.githubusercontent.com/codeandcats/KdTree/master/Images/kdtree.png 18 | 1.4.1 19 | true 20 | 21 | 22 | 23 | TRACE;RELEASE;NETSTANDARD2_0 24 | 25 | -------------------------------------------------------------------------------- /KdTreeLib/KdTreeNode.cs: -------------------------------------------------------------------------------- 1 | using System; 2 | using System.Text; 3 | 4 | namespace KdTree 5 | { 6 | [Serializable] 7 | public class KdTreeNode 8 | { 9 | public KdTreeNode() 10 | { 11 | } 12 | 13 | public KdTreeNode(TKey[] point, TValue value) 14 | { 15 | Point = point; 16 | Value = value; 17 | } 18 | 19 | public TKey[] Point; 20 | public TValue Value = default(TValue); 21 | 22 | internal KdTreeNode LeftChild = null; 23 | internal KdTreeNode RightChild = null; 24 | 25 | internal KdTreeNode this[int compare] 26 | { 27 | get 28 | { 29 | if (compare <= 0) 30 | return LeftChild; 31 | else 32 | return RightChild; 33 | } 34 | set 35 | { 36 | if (compare <= 0) 37 | LeftChild = value; 38 | else 39 | RightChild = value; 40 | } 41 | } 42 | 43 | public bool IsLeaf 44 | { 45 | get 46 | { 47 | return (LeftChild == null) && (RightChild == null); 48 | } 49 | } 50 | 51 | public override string ToString() 52 | { 53 | var sb = new StringBuilder(); 54 | 55 | for (var dimension = 0; dimension < Point.Length; dimension++) 56 | { 57 | sb.Append(Point[dimension].ToString() + "\t"); 58 | } 59 | 60 | if (Value == null) 61 | sb.Append("null"); 62 | else 63 | sb.Append(Value.ToString()); 64 | 65 | return sb.ToString(); 66 | } 67 | } 68 | } -------------------------------------------------------------------------------- /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/GeoMath.cs: -------------------------------------------------------------------------------- 1 | using System; 2 | 3 | namespace KdTree.Math 4 | { 5 | [Serializable] 6 | public class GeoMath : FloatMath 7 | { 8 | public override float DistanceSquaredBetweenPoints(float[] a, float[] b) 9 | { 10 | double dst = GeoUtils.Distance(a[0], a[1], b[0], b[1], 'K'); 11 | return (float)(dst * dst); 12 | } 13 | } 14 | } 15 | -------------------------------------------------------------------------------- /KdTreeLib/Math/GeoUtils.cs: -------------------------------------------------------------------------------- 1 | namespace KdTree.Math 2 | { 3 | // Via http://www.geodatasource.com/developers/c-sharp 4 | // This code is licensed under LGPLv3. 5 | public class GeoUtils 6 | { 7 | public static double Distance(double lat1, double lon1, double lat2, double lon2, char unit) 8 | { 9 | double theta = lon1 - lon2; 10 | double dist = System.Math.Sin(Deg2rad(lat1)) * System.Math.Sin(Deg2rad(lat2)) + System.Math.Cos(Deg2rad(lat1)) * System.Math.Cos(Deg2rad(lat2)) * System.Math.Cos(Deg2rad(theta)); 11 | dist = System.Math.Acos(dist); 12 | dist = Rad2deg(dist); 13 | dist = dist * 60 * 1.1515; 14 | if (unit == 'K') 15 | { 16 | dist = dist * 1.609344; 17 | } 18 | else if (unit == 'N') 19 | { 20 | dist = dist * 0.8684; 21 | } 22 | return (dist); 23 | } 24 | 25 | //::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::: 26 | //:: This function converts decimal degrees to radians ::: 27 | //::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::: 28 | private static double Deg2rad(double deg) 29 | { 30 | return (deg * System.Math.PI / 180.0); 31 | } 32 | 33 | //::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::: 34 | //:: This function converts radians to decimal degrees ::: 35 | //::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::: 36 | private static double Rad2deg(double rad) 37 | { 38 | return (rad / System.Math.PI * 180.0); 39 | } 40 | 41 | } 42 | } 43 | -------------------------------------------------------------------------------- /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 | public NearestNeighbourList(ITypeMath distanceMath) 26 | { 27 | this.maxCapacity = int.MaxValue; 28 | this.distanceMath = distanceMath; 29 | 30 | queue = new PriorityQueue(distanceMath); 31 | } 32 | 33 | private PriorityQueue queue; 34 | 35 | private ITypeMath distanceMath; 36 | 37 | private int maxCapacity; 38 | public int MaxCapacity { get { return maxCapacity; } } 39 | 40 | public int Count { get { return queue.Count; } } 41 | 42 | public bool Add(TItem item, TDistance distance) 43 | { 44 | if (queue.Count >= maxCapacity) 45 | { 46 | // If the distance of this item is less than the distance of the last item 47 | // in our neighbour list then pop that neighbour off and push this one on 48 | // otherwise don't even bother adding this item 49 | if (distanceMath.Compare(distance, queue.GetHighestPriority()) < 0) 50 | { 51 | queue.Dequeue(); 52 | queue.Enqueue(item, distance); 53 | return true; 54 | } 55 | else 56 | return false; 57 | } 58 | else 59 | { 60 | queue.Enqueue(item, distance); 61 | return true; 62 | } 63 | } 64 | 65 | public TItem GetFurtherest() 66 | { 67 | if (Count == 0) 68 | throw new Exception("List is empty"); 69 | else 70 | return queue.GetHighest(); 71 | } 72 | 73 | public TDistance GetFurtherestDistance() 74 | { 75 | if (Count == 0) 76 | throw new Exception("List is empty"); 77 | else 78 | return queue.GetHighestPriority(); 79 | } 80 | 81 | public TItem RemoveFurtherest() 82 | { 83 | return queue.Dequeue(); 84 | } 85 | 86 | public bool IsCapacityReached 87 | { 88 | get { return Count == MaxCapacity; } 89 | } 90 | } 91 | } 92 | -------------------------------------------------------------------------------- /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 | /// 25 | ///This constructor will use a default capacity of 4. 26 | /// 27 | public PriorityQueue(ITypeMath priorityMath) 28 | { 29 | this.capacity = 4; 30 | queue = new ItemPriority[capacity]; 31 | 32 | this.priorityMath = priorityMath; 33 | } 34 | 35 | private ITypeMath priorityMath; 36 | 37 | private ItemPriority[] queue; 38 | 39 | private int capacity; 40 | 41 | private int count; 42 | public int Count { get { return count; } } 43 | 44 | // Try to avoid unnecessary slow memory reallocations by creating your queue with an ample capacity 45 | private void ExpandCapacity() 46 | { 47 | // Double our capacity 48 | capacity *= 2; 49 | 50 | // Create a new queue 51 | var newQueue = new ItemPriority[capacity]; 52 | 53 | // Copy the contents of the original queue to the new one 54 | Array.Copy(queue, newQueue, queue.Length); 55 | 56 | // Copy the new queue over the original one 57 | queue = newQueue; 58 | } 59 | 60 | public void Enqueue(TItem item, TPriority priority) 61 | { 62 | if (++count > capacity) 63 | ExpandCapacity(); 64 | 65 | int newItemIndex = count - 1; 66 | 67 | queue[newItemIndex] = new ItemPriority { Item = item, Priority = priority }; 68 | 69 | ReorderItem(newItemIndex, -1); 70 | } 71 | 72 | public TItem Dequeue() 73 | { 74 | TItem item = queue[0].Item; 75 | 76 | queue[0].Item = default(TItem); 77 | queue[0].Priority = priorityMath.MinValue; 78 | 79 | ReorderItem(0, 1); 80 | 81 | count--; 82 | 83 | return item; 84 | } 85 | 86 | private void ReorderItem(int index, int direction) 87 | { 88 | if ((direction != -1) && (direction != 1)) 89 | throw new ArgumentException("Invalid Direction"); 90 | 91 | var item = queue[index]; 92 | 93 | int nextIndex = index + direction; 94 | 95 | while ((nextIndex >= 0) && (nextIndex < count)) 96 | { 97 | var next = queue[nextIndex]; 98 | 99 | int compare = priorityMath.Compare(item.Priority, next.Priority); 100 | 101 | // If we're moving up and our priority is higher than the next priority then swap 102 | // Or if we're moving down and our priority is lower than the next priority then swap 103 | if ( 104 | ((direction == -1) && (compare > 0)) 105 | || 106 | ((direction == 1) && (compare < 0)) 107 | ) 108 | { 109 | queue[index] = next; 110 | queue[nextIndex] = item; 111 | 112 | index += direction; 113 | nextIndex += direction; 114 | } 115 | else 116 | break; 117 | } 118 | } 119 | 120 | public TItem GetHighest() 121 | { 122 | if (count == 0) 123 | throw new Exception("Queue is empty"); 124 | else 125 | return queue[0].Item; 126 | } 127 | 128 | public TPriority GetHighestPriority() 129 | { 130 | if (count == 0) 131 | throw new Exception("Queue is empty"); 132 | else 133 | return queue[0].Priority; 134 | } 135 | } 136 | } 137 | -------------------------------------------------------------------------------- /KdTreeTestsLib/KdTreeTests.cs: -------------------------------------------------------------------------------- 1 | using KdTree.Math; 2 | using Microsoft.VisualStudio.TestTools.UnitTesting; 3 | using System; 4 | using System.Collections.Generic; 5 | using System.Linq; 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 | var olcCount = tree.Count(); 121 | 122 | tree.Add(testNodes[0].Point, newValue); 123 | 124 | var actualValue = tree.FindValueAt(testNodes[0].Point); 125 | var newCount = tree.Count(); 126 | 127 | Assert.AreEqual(newValue, actualValue); 128 | Assert.AreEqual(olcCount, newCount); 129 | } 130 | 131 | [TestMethod] 132 | [TestCategory("KdTree")] 133 | public void TestTryFindValueAt() 134 | { 135 | AddTestNodes(); 136 | 137 | string actualValue; 138 | 139 | foreach (var node in testNodes) 140 | { 141 | if (tree.TryFindValueAt(node.Point, out actualValue)) 142 | Assert.AreEqual(node.Value, actualValue); 143 | else 144 | Assert.Fail("Could not find test node"); 145 | } 146 | 147 | if (!tree.TryFindValueAt(new float[] { 3.14f, 5 }, out actualValue)) 148 | Assert.IsNull(actualValue); 149 | else 150 | Assert.Fail("Reportedly found node it shouldn't have"); 151 | } 152 | 153 | [TestMethod] 154 | [TestCategory("KdTree")] 155 | public void TestFindValueAt() 156 | { 157 | AddTestNodes(); 158 | 159 | string actualValue; 160 | 161 | foreach (var node in testNodes) 162 | { 163 | actualValue = tree.FindValueAt(node.Point); 164 | 165 | Assert.AreEqual(node.Value, actualValue); 166 | } 167 | 168 | actualValue = tree.FindValueAt(new float[] { 3.15f, 5 }); 169 | 170 | Assert.IsNull(actualValue); 171 | } 172 | 173 | [TestMethod] 174 | [TestCategory("KdTree")] 175 | public void TestFindValue() 176 | { 177 | AddTestNodes(); 178 | 179 | float[] actualPoint; 180 | 181 | foreach (var node in testNodes) 182 | { 183 | actualPoint = tree.FindValue(node.Value); 184 | Assert.AreEqual(node.Point, actualPoint); 185 | } 186 | 187 | actualPoint = tree.FindValue("Your Mumma"); 188 | Assert.IsNull(actualPoint); 189 | } 190 | 191 | [TestMethod] 192 | [TestCategory("KdTree")] 193 | public void TestRemoveAt() 194 | { 195 | AddTestNodes(); 196 | 197 | var nodesToRemove = new KdTreeNode[] { 198 | testNodes[1], // Root-Left 199 | testNodes[0] // Root 200 | }; 201 | 202 | foreach (var nodeToRemove in nodesToRemove) 203 | { 204 | tree.RemoveAt(nodeToRemove.Point); 205 | testNodes.Remove(nodeToRemove); 206 | 207 | Assert.IsNull(tree.FindValue(nodeToRemove.Value)); 208 | Assert.IsNull(tree.FindValueAt(nodeToRemove.Point)); 209 | 210 | foreach (var testNode in testNodes) 211 | { 212 | Assert.AreEqual(testNode.Value, tree.FindValueAt(testNode.Point)); 213 | Assert.AreEqual(testNode.Point, tree.FindValue(testNode.Value)); 214 | } 215 | 216 | Assert.AreEqual(testNodes.Count, tree.Count); 217 | } 218 | } 219 | 220 | [TestMethod] 221 | [TestCategory("KdTree")] 222 | public void TestGetNearestNeighbours() 223 | { 224 | var toowoomba = new City() 225 | { 226 | Address = "Toowoomba, QLD, Australia", 227 | Lat = -27.5829487f, 228 | Long = 151.8643252f, 229 | DistanceFromToowoomba = 0 230 | }; 231 | 232 | City[] cities = new City[] 233 | { 234 | toowoomba, 235 | new City() 236 | { 237 | Address = "Brisbane, QLD, Australia", 238 | Lat = -27.4710107f, 239 | Long = 153.0234489f, 240 | DistanceFromToowoomba = 1.16451615177537f 241 | }, 242 | new City() 243 | { 244 | Address = "Goldcoast, QLD, Australia", 245 | Lat = -28.0172605f, 246 | Long = 153.4256987f, 247 | DistanceFromToowoomba = 1.6206523211724f 248 | }, 249 | new City() 250 | { 251 | Address = "Sunshine, QLD, Australia", 252 | Lat = -27.3748288f, 253 | Long = 153.0554193f, 254 | DistanceFromToowoomba = 1.20913979664506f 255 | }, 256 | new City() 257 | { 258 | Address = "Melbourne, VIC, Australia", 259 | Lat = -37.814107f, 260 | Long = 144.96328f, 261 | DistanceFromToowoomba = 12.3410301438779f 262 | }, 263 | new City() 264 | { 265 | Address = "Sydney, NSW, Australia", 266 | Lat = -33.8674869f, 267 | Long = 151.2069902f, 268 | DistanceFromToowoomba = 6.31882185929341f 269 | }, 270 | new City() 271 | { 272 | Address = "Perth, WA, Australia", 273 | Lat = -31.9530044f, 274 | Long = 115.8574693f, 275 | DistanceFromToowoomba = 36.2710774395312f 276 | }, 277 | new City() 278 | { 279 | Address = "Darwin, NT, Australia", 280 | Lat = -12.4628198f, 281 | Long = 130.8417694f, 282 | DistanceFromToowoomba = 25.895292049265f 283 | } 284 | /*, 285 | new City() 286 | { 287 | Address = "London, England", 288 | Lat = 51.5112139f, 289 | Long = -0.1198244f, 290 | DistanceFromToowoomba = 171.33320836029f 291 | 292 | }*/ 293 | }; 294 | 295 | foreach (var city in cities) 296 | { 297 | tree.Add(new float[] { city.Long, -city.Lat }, city.Address); 298 | } 299 | 300 | /* 301 | var sb = new System.Text.StringBuilder(); 302 | sb.AppendLine("Before Balance:"); 303 | sb.AppendLine(tree.ToString()); 304 | sb.AppendLine(""); 305 | sb.AppendLine(""); 306 | tree.Balance(); 307 | sb.AppendLine("After Balance:"); 308 | sb.AppendLine(tree.ToString()); 309 | System.Windows.Forms.Clipboard.SetText(sb.ToString()); 310 | */ 311 | 312 | for (var findLimit = 0; findLimit <= cities.Length; findLimit++) 313 | { 314 | var actualNeighbours = tree.GetNearestNeighbours( 315 | new float[] { toowoomba.Long, -toowoomba.Lat }, 316 | findLimit); 317 | 318 | var expectedNeighbours = cities 319 | .OrderBy(p => p.DistanceFromToowoomba) 320 | .Take(findLimit) 321 | .ToArray(); 322 | 323 | Assert.AreEqual(findLimit, actualNeighbours.Length); 324 | Assert.AreEqual(findLimit, expectedNeighbours.Length); 325 | 326 | for (var index = 0; index < actualNeighbours.Length; index++) 327 | { 328 | Assert.AreEqual(expectedNeighbours[index].Address, actualNeighbours[index].Value); 329 | } 330 | } 331 | } 332 | 333 | [TestMethod] 334 | [TestCategory("KdTree")] 335 | public void TestRadialSearch() 336 | { 337 | var toowoomba = new City() 338 | { 339 | Address = "Toowoomba, QLD, Australia", 340 | Lat = -27.5829487f, 341 | Long = 151.8643252f, 342 | DistanceFromToowoomba = 0 343 | }; 344 | 345 | City[] cities = new City[] 346 | { 347 | toowoomba, 348 | new City() 349 | { 350 | Address = "Brisbane, QLD, Australia", 351 | Lat = -27.4710107f, 352 | Long = 153.0234489f, 353 | DistanceFromToowoomba = 1.16451615177537f 354 | }, 355 | new City() 356 | { 357 | Address = "Goldcoast, QLD, Australia", 358 | Lat = -28.0172605f, 359 | Long = 153.4256987f, 360 | DistanceFromToowoomba = 1.6206523211724f 361 | }, 362 | new City() 363 | { 364 | Address = "Sunshine, QLD, Australia", 365 | Lat = -27.3748288f, 366 | Long = 153.0554193f, 367 | DistanceFromToowoomba = 1.20913979664506f 368 | }, 369 | new City() 370 | { 371 | Address = "Melbourne, VIC, Australia", 372 | Lat = -37.814107f, 373 | Long = 144.96328f, 374 | DistanceFromToowoomba = 12.3410301438779f 375 | }, 376 | new City() 377 | { 378 | Address = "Sydney, NSW, Australia", 379 | Lat = -33.8674869f, 380 | Long = 151.2069902f, 381 | DistanceFromToowoomba = 6.31882185929341f 382 | }, 383 | new City() 384 | { 385 | Address = "Perth, WA, Australia", 386 | Lat = -31.9530044f, 387 | Long = 115.8574693f, 388 | DistanceFromToowoomba = 36.2710774395312f 389 | }, 390 | new City() 391 | { 392 | Address = "Darwin, NT, Australia", 393 | Lat = -12.4628198f, 394 | Long = 130.8417694f, 395 | DistanceFromToowoomba = 25.895292049265f 396 | } 397 | }; 398 | 399 | foreach (var city in cities) 400 | { 401 | tree.Add(new float[] { city.Long, -city.Lat }, city.Address); 402 | } 403 | var expectedNeighbours = cities 404 | .OrderBy(p => p.DistanceFromToowoomba).ToList(); 405 | 406 | for (var i = 1; i < 100; i *= 2) 407 | { 408 | var actualNeighbours = tree.RadialSearch(new float[] { toowoomba.Long, -toowoomba.Lat }, i); 409 | 410 | for (var index = 0; index < actualNeighbours.Length; index++) 411 | { 412 | Assert.AreEqual(expectedNeighbours[index].Address, actualNeighbours[index].Value); 413 | } 414 | } 415 | } 416 | 417 | [TestMethod] 418 | [TestCategory("KdTree")] 419 | public void TestEnumerable() 420 | { 421 | AddTestNodes(); 422 | 423 | foreach (var node in tree) 424 | { 425 | var testNode = testNodes.FirstOrDefault(n => n.Point == node.Point && n.Value == node.Value); 426 | 427 | Assert.IsNotNull(testNode); 428 | 429 | testNodes.Remove(testNode); 430 | } 431 | 432 | Assert.AreEqual(0, testNodes.Count); 433 | } 434 | } 435 | } -------------------------------------------------------------------------------- /KdTreeTestsLib/KdTreeTestsLib.csproj: -------------------------------------------------------------------------------- 1 |  2 | 3 | 4 | netcoreapp2.0;net45;net451;net452;net46;net461;net462;net47;net471 5 | 1.4.0 6 | false 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /KdTreeTestsLib/NearestNeighbourListTests.cs: -------------------------------------------------------------------------------- 1 | using KdTree.Math; 2 | using Microsoft.VisualStudio.TestTools.UnitTesting; 3 | using System.Collections.Generic; 4 | using System.Linq; 5 | 6 | struct Planet 7 | { 8 | public string Name; 9 | public float DistanceFromEarth; 10 | } 11 | 12 | namespace KdTree.Tests 13 | { 14 | [TestClass] 15 | public class NearestNeighbourListTests 16 | { 17 | private NearestNeighbourList nearestNeighbours; 18 | private List neighbouringPlanets; 19 | 20 | [TestInitialize] 21 | public void Setup() 22 | { 23 | nearestNeighbours = new NearestNeighbourList(5, new FloatMath()); 24 | 25 | neighbouringPlanets = new List(); 26 | neighbouringPlanets.AddRange(new Planet[] 27 | { 28 | new Planet() { Name = "Mercury", DistanceFromEarth = 91700000f }, 29 | new Planet() { Name = "Venus", DistanceFromEarth = 41400000f }, 30 | new Planet() { Name = "Mars", DistanceFromEarth = 78300000f }, 31 | new Planet() { Name = "Jupiter", DistanceFromEarth = 624400000f }, 32 | new Planet() { Name = "Saturn", DistanceFromEarth = 1250000000f }, 33 | new Planet() { Name = "Uranus", DistanceFromEarth = 2720000000f }, 34 | new Planet() { Name = "Neptune", DistanceFromEarth = 4350000000f } 35 | }); 36 | } 37 | 38 | [TestCleanup] 39 | public void TearDown() 40 | { 41 | nearestNeighbours = null; 42 | } 43 | 44 | private void AddItems() 45 | { 46 | foreach (var planet in neighbouringPlanets) 47 | { 48 | nearestNeighbours.Add(planet, planet.DistanceFromEarth); 49 | } 50 | } 51 | 52 | [TestMethod] 53 | [TestCategory("NearestNeighbourList")] 54 | public void TestAddAndCount() 55 | { 56 | AddItems(); 57 | 58 | Assert.AreEqual(5, nearestNeighbours.Count); 59 | } 60 | 61 | [TestMethod] 62 | [TestCategory("NearestNeighbourList")] 63 | public void TestRemoveFurtherest() 64 | { 65 | AddItems(); 66 | 67 | var sortedPlanets = neighbouringPlanets 68 | .OrderBy(p => p.DistanceFromEarth) 69 | .Take(5) 70 | .OrderByDescending(p => p.DistanceFromEarth) 71 | .ToArray(); 72 | 73 | for (var index = 0; index < sortedPlanets.Length; index++) 74 | { 75 | Assert.AreEqual( 76 | sortedPlanets[index].Name, 77 | nearestNeighbours.RemoveFurtherest().Name); 78 | } 79 | 80 | Assert.AreEqual(0, nearestNeighbours.Count); 81 | } 82 | } 83 | } 84 | -------------------------------------------------------------------------------- /KdTreeTestsLib/PriorityQueueTests.cs: -------------------------------------------------------------------------------- 1 | using KdTree.Math; 2 | using Microsoft.VisualStudio.TestTools.UnitTesting; 3 | using System.Collections.Generic; 4 | using System.Linq; 5 | 6 | struct Person 7 | { 8 | public string Name; 9 | public int Age; 10 | } 11 | 12 | namespace KdTree.Tests 13 | { 14 | [TestClass] 15 | public class PriorityQueueTests 16 | { 17 | private PriorityQueue queue; 18 | private List people; 19 | 20 | [TestInitialize] 21 | public void Setup() 22 | { 23 | queue = new PriorityQueue(2, new FloatMath()); 24 | 25 | people = new List(); 26 | people.AddRange(new Person[] 27 | { 28 | new Person() { Name = "Chris", Age = 16 }, 29 | new Person() { Name = "Stewie", Age = 1 }, 30 | new Person() { Name = "Brian", Age = 10 }, 31 | new Person() { Name = "Meg", Age = 15 }, 32 | new Person() { Name = "Peter", Age = 41 }, 33 | new Person() { Name = "Lois", Age = 38 } 34 | }); 35 | } 36 | 37 | [TestCleanup] 38 | public void TearDown() 39 | { 40 | queue = null; 41 | } 42 | 43 | [TestMethod] 44 | [TestCategory("PriorityQueue")] 45 | public void TestQueue() 46 | { 47 | foreach (var person in people) 48 | queue.Enqueue(person.Name, person.Age); 49 | 50 | var peopleSortedByAgeDesc = people.OrderByDescending(p => p.Age).ToArray(); 51 | 52 | for (var index = 0; index < peopleSortedByAgeDesc.Length; index++) 53 | { 54 | var person = peopleSortedByAgeDesc[index]; 55 | 56 | Assert.AreEqual(person.Name, queue.Dequeue()); 57 | 58 | Assert.AreEqual(peopleSortedByAgeDesc.Length - index - 1, queue.Count); 59 | } 60 | 61 | Assert.AreEqual(0, queue.Count); 62 | } 63 | } 64 | } 65 | -------------------------------------------------------------------------------- /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, and multi-dimensional Binary Search Tree written in C#. 5 | 6 | # Install 7 | 8 | Install via NuGet Package Manager Console: 9 | 10 | ``` 11 | PM> Install-Package KdTree 12 | ``` 13 | 14 | # Examples 15 | 16 | ## Find nearest point in two dimensions 17 | 18 | ```cs 19 | var tree = new KdTree(2, new FloatMath()); 20 | tree.Add(new[] { 50.0f, 80.0f }, 100); 21 | tree.Add(new[] { 20.0f, 10.0f }, 200); 22 | 23 | var nodes = tree.GetNearestNeighbours(new[] { 30.0f, 20.0f }, 1); 24 | ``` 25 | --------------------------------------------------------------------------------