A common task in game code is finding which of a stored set of points is nearest to another arbitrary point. For example, you might want to find which waypoint or monster generator is nearest to the player's current position. However, the search is time-consuming when there are a lot of stored points and the search is just looking through all of them to find the nearest. I've had a go at implementing a kd-tree, a data structure that speeds up the nearest-neighbour search dramatically (see the attached script file). It is currently a very basic implementation, but I'd be happy to hear suggestions for improvements of functionality or efficiency.

Hey Andeeee, Thanks for posting this code. I'd like to use it to learn more about KDTrees as well as Unity. Could you give me any basic instructions how to implement your code? I tried dropping the script on a empty GO but I just get the error message - Can't add script behaviour KDTree. The script needs to derive from MonoBehaviour! Any advice? Thanks. Mitch

Hi Andeeee, I was wondering if your script could be used to find the nearest player to the current position of the ball as it moves around the field? I thought it might be used it to auto select the closest player? iByte

I made some progress - I don't know c# at all - I barely know unityscript, but I did manage to convert the KDTRee from c# to unityscript. It runs without errors and does seem to compute a KDTree from an array of points. But when I perform a FindNearest call on a point, it always returns an index of -1. It never finds the nearest point. I'm sure I screwed up the code somewhat when I took a sledgehammer to it and converted to unityscript. Here's the code - if anyone has any advice for me I'd appreciate it. I put both of these scripts on and empty GO: KDTree.js Code (csharp): var lr : KDTree[]; var pivot : Vector3; var pivotIndex : int; var axis : int; // Change this value to 2 if you only need two-dimensional X,Y points. The search will // be quicker in two dimensions. var numDims : int = 3; function KDTree() { lr = new KDTree[2]; } // Make a new tree from a list of points. function MakeFromPoints(points : Vector3[]) : KDTree { var indices : int[] = Iota(points.length); return MakeFromPointsInner(0, 0, points.length - 1, points, indices); } // Recursively build a tree by separating points at plane boundaries. function MakeFromPointsInner(depth : int, stIndex : int, enIndex : int, points : Vector3[], inds : int[]) : KDTree { var root : KDTree = this; root.KDTree(); root.axis = depth % numDims; var splitPoint : int = FindPivotIndex(points, inds, stIndex, enIndex, root.axis); root.pivotIndex = inds[splitPoint]; root.pivot = points[root.pivotIndex]; var leftEndIndex : int = splitPoint - 1; if (leftEndIndex >= stIndex) { root.lr[0] = MakeFromPointsInner(depth + 1, stIndex, leftEndIndex, points, inds); } var rightStartIndex : int = splitPoint + 1; if (rightStartIndex <= enIndex) { root.lr[1] = MakeFromPointsInner(depth + 1, rightStartIndex, enIndex, points, inds); } return root; } // Find a new pivot index from the range by splitting the points that fall either side // of its plane. function FindPivotIndex(points : Vector3[], inds : int[], stIndex : int, enIndex : int, axis : int) : int { var splitPoint : int = FindSplitPoint(points, inds, stIndex, enIndex, axis); // int splitPoint = Random.Range(stIndex, enIndex); var pivot : Vector3 = points[inds[splitPoint]]; SwapElements(inds, stIndex, splitPoint); var currPt : int = stIndex + 1; var endPt : int = enIndex; while (currPt <= endPt) { var curr : Vector3 = points[inds[currPt]]; if ((curr[axis] > pivot[axis])) { SwapElements(inds, currPt, endPt); endPt--; } else { SwapElements(inds, currPt - 1, currPt); currPt++; } } return currPt - 1; } function SwapElements(arr : int[], a : int, b : int) : void { var temp : int = arr[a]; arr[a] = arr[b]; arr[b] = temp; } // Simple "median of three" heuristic to find a reasonable splitting plane. function FindSplitPoint(points : Vector3[], inds : int[], stIndex : int, enIndex : int, axis : int) : int { var a : float = points[inds[stIndex]][axis]; var b : float = points[inds[enIndex]][axis]; var midIndex : int = (stIndex + enIndex) / 2; var m : float = points[inds[midIndex]][axis]; if (a > b) { if (m > a) { return stIndex; } if (b > m) { return enIndex; } return midIndex; } else { if (a > m) { return stIndex; } if (m > b) { return enIndex; } return midIndex; } } function Iota(num : int) : int[] { var result : int[] = new int[num]; for (var i : int = 0; i < num; i++) { result[i] = i; } return result; } // Find the nearest point in the set to the supplied point. function FindNearest(pt : Vector3) : int { var bestSqDist : float = Mathf.Infinity; var bestIndex : int = -1; Search(pt, bestSqDist, bestIndex); return bestIndex; } // Recursively search the tree. function Search(pt : Vector3, bestSqSoFar : float, bestIndex : int) : void { var mySqDist : float = (pivot - pt).sqrMagnitude; if (mySqDist < bestSqSoFar) { bestSqSoFar = mySqDist; bestIndex = pivotIndex; } var planeDist : float = pt[axis] - pivot[axis]; //DistFromSplitPlane(pt, pivot, axis); var selector : int = planeDist <= 0 ? 0 : 1; if (lr[selector] != null) { lr[selector].Search(pt, bestSqSoFar, bestIndex); } selector = (selector + 1) % 2; var sqPlaneDist : float = planeDist * planeDist; if ((lr[selector] != null) (bestSqSoFar > sqPlaneDist)) { lr[selector].Search(pt, bestSqSoFar, bestIndex); } } // Get a point's distance from an axis-aligned plane. function DistFromSplitPlane(pt : Vector3, planePt : Vector3, axis : int) : float { return pt[axis] - planePt[axis]; } and WayPoints.js Code (csharp): var index = 10; var scale = 50.0; var wayPoints : Vector3[]; var kd : KDTree; function Start () { wayPoints = new Vector3[index]; for (var wayPoint : Vector3 in wayPoints) { wayPoint.x = Random.Range(-scale, scale); wayPoint.y = Random.Range(-scale, scale); wayPoint.z = Random.Range(-scale, scale); } kd.MakeFromPoints(wayPoints); Debug.Log(kd.FindNearest(Vector3.zero)); } function OnDrawGizmos() { Gizmos.color = Color.white; for (var wayPoint : Vector3 in wayPoints) { Gizmos.DrawSphere(wayPoint, .5); } } In the Inspector I connect the KDTree Component to the kd variable in Waypoints.js. Thanks in advance for any bits of wisdom. Mitch

iByte, Not to long ago I was playing around with players chasing a soccer ball. You probably already know this but the closest player to the ball is not always the best player to go get the ball. It's the player that can intercept the ball in the fastest time. And that's a tricky task to compute. Not only do you have to factor in the velocity of the ball and the players, you also need to take into account player acceleration. And then there's the issue of the ball's trajectory when it takes the ball over the player's heads. I had quite a fun time playing with all that last year. I had some moderate success but quite a few times the wrong player would go get the ball so my math was not quite right. Mitch

Sorry it's taken me a bit of time to get onto this... @iByte: The kd-tree isn't suitable for finding the nearest player to the ball in a sports game. The reason is that there is an overhead in preprocessing the data to build the tree at first - this takes longer than a simple search through the points to check which is closest. The kd-tree is only suitable when the set of points is fixed. @MitchStan: You don't actually need to reimplement the code in JS unless you really want to. You can just use the class from JS - place the kdtree.cs file in the project's standard assets folder and it will be accessible to a JS script. You can't attach the class to an object directly because, as noted, it doesn't derive from MonoBehaviour. You should declare a variable of type KDTree in your code and initialise it with the MakeFromPoints static function:- Code (csharp): var tree: KDTree; var pointsArray: Vector3[]; function Start() { tree = KDTree.MakeFromPoints(pointsArray); } Having done that, you can search for the point nearest any arbitrary target point (a player's position, say) using the FindNearest function:- Code (csharp): var nearest: int = tree.FindNearest(targetPoint); This function returns an integer, which is an index into the original points array.

Thanks Andeeee. Works like a charm. I was having fun trying to convert it to JS but I couldn't get it to work. I think I'll try for a few more days - just to learn. Again, thanks! Mitch

Hi Andeee, When I type in the script you posted it comes up with the error: The name KDTree does not denote a valid type. I've placed the kdtree.cs in the assets folder so I dunno what's wrong.

Try placing the KDTree script file in the Standard Assets folder. You will need to do this if you are accessing the class from JavaScript.

Hi, andeeeee, very impressive work. By the way, how it would be easiest way to add or delete individual elements in this tree? I am interested to apply this algorithm for my RTS game. However, as game is dynamic (some warriors die, some are created new), vector3 array size would be always changing and I was interested if it would work correctly?

Hi all. A lot of time i'm searching a way to find dynamic mooving nearest object. I try to represent my map with zones. But this idea doesn't like me, because i use planes with triggers. I have a lot of objects. They move every time in different directions. And every frame they should find the nearest object to them. My algorithm work good, but it has some bugs. I learned little about kd-trees, but how does the work with dynamic objects? Is it fast? Thx and sorry for my english.

I managed to apply this algorithm for now, but I am facing some difficulties with finding not the first nearest neighbour, but k-th nearest neighbour. I modified the code like that: Code (CSharp): public void FindNearestR(int suitIndex, Vector3 pt, ref float sqrRmin) { float bestSqDist = 1000000000f; int bestIndex = -1; SearchR(pt, ref bestSqDist, ref bestIndex, ref sqrRmin); // int i = bestIndex; // int ii = i-1; sqrRmin = bestSqDist; suitIndex = bestIndex-1; } // Recursively search the tree. void SearchR(Vector3 pt, ref float bestSqSoFar, ref int bestIndex, ref float sqrRmin) { float mySqDist = (pivot - pt).sqrMagnitude; if(mySqDist > sqrRmin){ if (mySqDist < bestSqSoFar) { bestSqSoFar = mySqDist; bestIndex = pivotIndex; // else{ // Debug.Log(mySqDist); // } } float planeDist = pt[axis] - pivot[axis]; //DistFromSplitPlane(pt, pivot, axis); int selector = planeDist <= 0 ? 0 : 1; if (lr[selector] != null) { lr[selector].SearchR(pt, ref bestSqSoFar, ref bestIndex, ref sqrRmin); } selector = (selector + 1) % 2; float sqPlaneDist = planeDist * planeDist; if ((lr[selector] != null) && (bestSqSoFar > sqPlaneDist)) { lr[selector].SearchR(pt, ref bestSqSoFar, ref bestIndex, ref sqrRmin); } } } Here I used sqrRmin as minimum distance to allow NN to be used. For first NN sqrRmin=0, for 2nd NN it is becoming equal first NN sqrRmin and when function is called it should avoid counting first NN due to this condition. Unfortunately something wrong is going on with mySqDist > sqrRmin and I am always ending up with infinite loop. Does anyone see how it would be possible to fix this?

Great resource thank you. I am using it to place a collider on nearest tree as I have anywhere from 200k trees and up, however I am running into this issue when I put all of the trees into one array: maxVertices < 65536 && maxIndices < 65536*3 I checked to ensure my array could handle the amount of entities I was feeding it and that didn't break until I put in about 10Mil Vector3's, all I need is about 200-500k Vector3's. If you can respond to the reasoning for this error and whether or not there may be a fix for it or if thats just the max it can do that would be great thank you.

Just want to point out that the code snippet quoted above is Java and not C Sharp, for any nube (like me) that come by this thread. This bit below. Code (JavaScript): var tree: KDTree; var pointsArray: Vector3[]; function Start() { tree = KDTree.MakeFromPoints(pointsArray); }

Here I updated this class with additional functions, which allows to search for k-th nearest neighbour(s) or return distances to them instead of indices. P.S. I checked upon Unity 5 and it allows to build very large trees (I tried up to 1.5 million Vector3's and it's working fine).

You can keep calling: Code (CSharp): FindNearestK(Vector3 pt, int k) function with larger k orders until the distance to the found point becomes larger than your critical radius. This should run with O(m*n log n) performance, where m would be number of points inside your radius. It's going towards O(N*N) if your cut-off radius is getting close to the point where all dataset points are being added (so keep your cut-off radius reasonable). I should add soon that function into the class when I will be looking there next time.

This is exactly a functionality I need at the moment. I guess the common usecase here is to find neighbors to certain points and establish a connection to each point in a radius. A proper method for that would be great, but right now i am going to do it the way you described

I think it depends what exactly you are doing. If you need to find just the nearest one (i.e. find the closest tree for worker to be chopped in the forest to collect logs) or to get the environment properties (i.e. find how big is the sphere for 6 neighbours, calculate densities, etc.). It is good tip to use fixed number of points, as the loop will take always the same time to calculate rather than the fixed radius (i.e. in radius = 5 you can find sometimes 3 neighbours, while other time you can find 3000 neighbours, and if you get very large number of neighbours, the loops will become very heavy and your game could freeze).

I just tested it for a grid that is a 20x20 units area with one point at each point (x, y). so (0,0),(1,0),(2,0),...,(0,1),(0,2),...,(i,j),...(19,19). When I now try to find the neighbors within a 1 unit radius i get this as a result. Am i doing something wrong here? I use node.position (e.g. (14,6)) as an input position for the nearest k search UPDATE: I think i found the reason for this to happen. Basically the nearest k-th algorithm is not safe for an environment where two points have the exact same distance to a certain position. Code (CSharp): if ( mySqDist < bestSqSoFar ) { if ( mySqDist > minSqDist ) { bestSqSoFar = mySqDist; bestIndex = pivotIndex; } } This part ignores the second point with the same distance to the point as "minSqDist" and therefore will not be noted. This means that you might miss proper points here and the result of the k search will be false, since there is one (or more) point(s) that are nearer to the given location than the returned point UPDATE 2: Fixed the issue with the following changes: Code (CSharp): void SearchK( Vector3 pt, HashSet<int> p_pivotIndexSet, ref float bestSqSoFar, ref float minSqDist, ref int bestIndex ) { float mySqDist = ( pivot - pt ).sqrMagnitude; if ( mySqDist < bestSqSoFar ) { if ( mySqDist >= minSqDist && !p_pivotIndexSet.Contains( pivotIndex ) ) { bestSqSoFar = mySqDist; bestIndex = pivotIndex; } } Code (CSharp): public int FindNearestK( Vector3 pt, int k ) { // Find and returns k-th nearest neighbour float bestSqDist = 1000000000f; float minSqDist = -1.0f; int bestIndex = -1; HashSet<int> _pivotIndexSet = new HashSet<int>(); for ( int i = 0; i < k - 1; i++ ) { SearchK( pt, _pivotIndexSet, ref bestSqDist, ref minSqDist, ref bestIndex ); _pivotIndexSet.Add( bestIndex ); minSqDist = bestSqDist; bestSqDist = 1000000000f; bestIndex = -1; } SearchK( pt, _pivotIndexSet, ref bestSqDist, ref minSqDist, ref bestIndex ); return bestIndex; }

Hmm, I wouldn't use KDTree for the grid if points are exactly on the grid with fixed cell size. KDTree is probably best if you have completely randomly distributed points in space. If you have them in the grid, you can use x and y indices to find nearest point. For example if you have cell (8,7) the nearest points would be (7,7), (9,7), (8,6) and (8,8). You can use internal loop between (x-1,y-1) and (x+1,y+1) as for a grid to get neighbours. It might be faster and better defined that way.

You're absolutly right about a pure grid. I should have added that I use the grid just as a starting point. The final cluster is nothing that can be put into a pattern since you can drag points around, add new ones and pretty much organize them in an unpredictable pattern. However, the KD Tree is sufficient for this as well. You should be aware though that the current implemention of the k search is flawed. So as mentioned, for two points with the exact same distance to a point only one point will be returned. Using the hashset fixes that and does not produce a noticable overhead

I think you was using FindNearestK function, which returns exact k-th neighbour. For example if you call ind = FindNearest(pt, 6), you will always get 6-th nearest neighbour (there won't be 5th, 4th, 7th and other neighbours as this returns only a single integer). You can try using FindNearestsK, which is just a bit below in the class (lines 228 - 249 in my last version). This one should return an array of all neighbours up to k-th, i.e. in previous example it would return all 6 neighbours. You can check if in the array points are added correctly when using it with grid distributed points. P.S. Did you tried to check how much performance is drained by using hashtags vs not using them? I.e. you can check by looping over several thousand points to find their neighbours while switching between one version and another. I suspect there could be significant FPS drop on Hashtags version.

I did use the method with return value int[] (FindNearests). The thing is that you WILL NOT get the k-th nearest neighbor if there are two points with the same distance to a certain point. This may be a question of definition, but take a look at this example: You have 3 points: (1, 0), (0, 1) and (1, 1). You now want to find the second nearest neighbor to the point (0, 0). So you would do "FindNearest( Vector3.zero, 2);". This will return the point (1, 1), but not (0, 1). If you use "FindNearests" this becomes even worse, since you will get an indices array of the points [ (1, 0), (1, 1)]. But this are clearly NOT the two nearest points to (0, 0), but (1, 0) and (0, 1) are. So you will have a false result when you query the index of the second nearest point, since this is the index of (1, 1) instead of (0, 1). I have another question: Is there a way to do a "Nearest Point to ray" query? So that you shoot a ray and will the point with the closest distance to the ray will be returned? I can only imagine a numerical approach where you check the distance of the closest point for a fixed step distance and a maximum distance and return the point with the lowest distance to the ray. Any other ideas?

Oh, I see your point. It's probably that FindNearests are finding always the same point (first or last) if distances are exactly the same. Masking already found points, like you did with hashsets helps of course, but I am worried a bit about performance - did you checked in a way I described in previous post "P.S." ? I am interested in "Contains" function (this one acts as a loop), as it is in the deepest SearchK function, which itself does not have any loops. That could make search much slower. If you don't have performance checking class, let me know, I can post mine. Distance to the ray seems to be a different approach, where KDTree doesn't suppose to find it. If you have just a single ray, you can try something like this: http://answers.unity3d.com/questions/62644/distance-between-a-ray-and-a-point.html If you have many hundreds of thousands of rays, it may need some tweaking how to select best possible candidates list to avoid N^2 calculations. There are also different functions for line segments, planes and so on, i.e. check this page: http://wiki.unity3d.com/index.php/3d_Math_functions [Update] I just noticed that these HashSets has O(1) performance on checking containment, while Lists has O(N) performance, so they might be not making big impact. However, it might be worthwhile to keep in mind that if there is some noticeable performance issues, it could be good to keep a new function as a separate one, as KDTrees are mostly beneficial when there are no exactly equal distances.

So I was recently playing with the new job system in Unity 2018.1.0b8 and managed to jobify this kdtree. I placed the project with examples on GitHub: https://github.com/chanfort/kdtree-jobified-unity In order to jobify, I converted kdtree class into struct and also set 4 arrays to store kdtree data instead of recursive tree of objects. The content includes the old kdtree class, the new kdtree struct and the example in KDtest.cs . The example test is set to compare performance between the old kdtree and the new jobified one. The test runs with 5000 points kdtree build at startup. 5000 other query points are searching for their nearest neighbours in the kdtree on every update. The options changes by pressing A (build in arrays and original kdtree class), B (new kdtree struct and native arrays) and C (jobified (multithreaded) kdtree with IJobParallelFor usage over neighbour search) keys in the keyboard. The performance is as follows: Editor: A - 28 FPS, B - 12 FPS, C - 23 FPS When build: A - 64 FPS, B - 58 FPS, C - 125 FPS So at the moment, improvements are only visible when running the build project (not running in Editor). I will update these numbers when Unity will release Burst compiler (likely 2018.2). So stay tuned - looks very promising.

Is B) and C) done with the IL2CPP using the preview build which contains optimizations for NativeArray? I'll be very surprised if builtin arrays are faster than native arrays in il2cpp with the preview build. If not I'd be really curious to see what framerate is like with the preview il2cpp build. (https://forum.unity.com/threads/job-system-not-as-fast-as-mine-why-not.518374/#post-3397587) As a general advice. I really recommend measuring not FPS but the actual time spent on the job in ms. It gives a much more accurate picture of speedup / slow downs.

So just installed 2018.1.0b9 and tested for three cases: Editor: A - 34 ms, B - 80 ms, C - 51 ms Mono build: A - 15 ms, B - 17 ms, C - 7.1 ms IL2CPP build: A - 6.8 ms, B - 6.1 ms, C - 3.7 ms This time I used the mean deltaTime in ms. So, indeed IL2CPP speeds up native arrays. Now all points make sense .

Hey guys, since this topic is up to date/active since 2009: I made a k-d Tree for moving objects: https://gist.github.com/ditzel/194ec800053ce7083b73faa1be9101b0#file-kdtree-cs Video:

Nice. Would be interesting to see how your kdtree compares with andeee made kd-tree in terms of performance, i.e. measuring stress test FPS or ms with thousands of points every update.

Thanks for this, really useful! I did find a mistake in there though. When using RemoveAll or RemoveAt, it was possible that the Next reference was still pointing to the removed item, which caused weird problems ofcourse. Fixed it with this: Code (CSharp): /// <summary> /// Remove at position i (position in list or loop) /// </summary> public void RemoveAt(int i) { var list = new List<KdNode>(_getNodes()); list.RemoveAt(i); Clear(); foreach (var node in list) { node.next = null; _add(node); } } /// <summary> /// remove all objects that matches the given predicate /// </summary> /// <param name="match">lamda expression</param> public void RemoveAll(Predicate<T> match) { var list = new List<KdNode>(_getNodes()); list.RemoveAll(n => match(n.component)); Clear(); foreach (var node in list) { node.next = null; _add(node); } }