private KDTreeNode _Recur_Build(Vector3[] pts, int from, int cnt, int depth)
{
if (cnt <= 0)
{
return(null);
}
int axis = depth % 3;
_SortPoints(pts, from, cnt, axis);
Vector3 median = pts[from + cnt / 2];
KDTreeNode newNode = _MakeNode(median);
newNode.left = _Recur_Build(pts, from, cnt / 2, depth + 1);
newNode.right = _Recur_Build(pts, from + cnt / 2 + 1, cnt - cnt / 2 - 1, depth + 1);
if (newNode.left != null)
{
newNode.left.parent = newNode;
}
if (newNode.right != null)
{
newNode.right.parent = newNode;
}
return(newNode);
}
private KDTreeNode _MakeNode(Vector3 median)
{
var node = new KDTreeNode();
node.pos = median;
return(node);
}
private int _Recur_Serialize(KDTreeNode nd)
{
KDTreeSerial s = new KDTreeSerial();
if (nd.left != null)
{
s.leftIdx = _Recur_Serialize(nd.left);
}
if (nd.right != null)
{
s.rightIdx = _Recur_Serialize(nd.right);
}
s.pos = nd.pos;
m_serials.Add(s);
int curId = m_serials.Count - 1;
if (s.leftIdx >= 0)
{
m_serials[s.leftIdx].parentIdx = curId;
}
if (s.rightIdx >= 0)
{
m_serials[s.rightIdx].parentIdx = curId;
}
return(curId);
}
public void OnAfterDeserialize()
{
if (m_serials == null || m_serials.Count == 0)
{
return;
}
List <KDTreeNode> nodes = new List <KDTreeNode>();
for (int i = 0; i < m_serials.Count; ++i)
{ //populate all KDTreeNode first
KDTreeSerial s = m_serials[i];
KDTreeNode node = new KDTreeNode();
node.pos = s.pos;
nodes.Add(node);
}
for (int i = 0; i < nodes.Count; ++i)
{ //recover the links
int l = m_serials[i].leftIdx;
nodes[i].left = l >= 0 ? nodes[l] : null;
int r = m_serials[i].rightIdx;
nodes[i].right = r >= 0 ? nodes[r] : null;
int p = m_serials[i].parentIdx;
nodes[i].parent = p >= 0 ? nodes[p] : null;
}
m_rootNode = nodes[nodes.Count - 1]; //post-order serialization
}
private void _Recur_CheckNearestDownward(Vector3 pt, KDTreeNode nd, ref float minDistSqr, ref KDTreeNode nearest, int depth)
{
//check nearest
float sqr = (pt - nd.pos).sqrMagnitude;
if (sqr < minDistSqr)
{
minDistSqr = sqr;
nearest = nd;
}
//++m_visitNodeCnt;
int axis = depth % 3;
float ptv = pt[axis];
float ndv = nd.pos[axis];
float sqr_ptv_ndv = (ptv - ndv) * (ptv - ndv);
if (nd.left != null && (ptv < ndv || sqr_ptv_ndv < minDistSqr))
{
_Recur_CheckNearestDownward(pt, nd.left, ref minDistSqr, ref nearest, depth + 1);
}
if (nd.right != null && (ptv > ndv || sqr_ptv_ndv < minDistSqr))
{
_Recur_CheckNearestDownward(pt, nd.right, ref minDistSqr, ref nearest, depth + 1);
}
}
/// <summary>
/// find the point nearest to 'pt'
/// </summary>
//public int m_visitNodeCnt = 0; //used to evaluate kdtree's GetNearest perf
public Vector3 GetNearest(Vector3 pt)
{
//m_visitNodeCnt = 0;
int depth = 0;
float minDistSqr = float.MaxValue;
KDTreeNode nearest = m_rootNode;
KDTreeNode curNode = _Recur_FindLeafNode(pt, m_rootNode, ref minDistSqr, ref nearest, ref depth);
_Recur_GetNearest(pt, curNode, ref minDistSqr, ref nearest, depth);
//Dbg.Log("m_visitNodeCnt = {0}", m_visitNodeCnt);
return(nearest.pos);
}
private KDTreeNode _NeedCheckOtherSide(Vector3 pt, KDTreeNode nd, float minDistSqr, int depth)
{
if (nd == null)
{
return(null);
}
int axis = depth % 3;
Vector3 ndPos = nd.pos;
if ((pt[axis] - ndPos[axis]) * (pt[axis] - ndPos[axis]) > minDistSqr)
{
return(null); //not crossing the separating plane, return false
}
if (pt[axis] < ndPos[axis])
{
return(nd.right);
}
else
{
return(nd.left);
}
}
private void _Recur_GetNearest(Vector3 pt, KDTreeNode nd, ref float minDistSqr, ref KDTreeNode nearest, int depth)
{
//check nearest
while (nd != null)
{
float sqr = (pt - nd.pos).sqrMagnitude;
if (sqr < minDistSqr)
{
minDistSqr = sqr;
nearest = nd;
}
//++m_visitNodeCnt;
KDTreeNode otherSideNode = _NeedCheckOtherSide(pt, nd.parent, minDistSqr, depth - 1);
if (otherSideNode != null)
{
_Recur_CheckNearestDownward(pt, nd.parent, ref minDistSqr, ref nearest, depth - 1);
}
nd = nd.parent;
--depth;
}
}
private KDTreeNode _Recur_FindLeafNode(Vector3 pt, KDTreeNode nd, ref float minDistSqr, ref KDTreeNode nearest, ref int depth)
{
KDTreeNode nextNd = null;
while (true)
{
//m_visitNodeCnt++;
int axis = depth % 3;
float sqr = (pt - nd.pos).sqrMagnitude;
if (sqr < minDistSqr)
{
minDistSqr = sqr;
nearest = nd;
}
if (pt[axis] < nd.pos[axis])
{
nextNd = nd.left;
}
else
{
nextNd = nd.right;
}
if (nextNd == null)
{
return(nd);
}
else
{
nd = nextNd;
}
++depth;
}
}
public void Build(Vector3[] lst)
{
m_rootNode = _Recur_Build(lst, 0, lst.Length, 0);
}
public void Invalidate()
{
m_serials.Clear();
m_rootNode = null;
}