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
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
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
