private int FindHelper(kdnode node, Point3D pt)
{
if (node == null)
{
return -1;
}
else if (Math.Abs(pt.X - node.Pt.X) < mEpsilon &&
Math.Abs(pt.Y - node.Pt.Y) < mEpsilon &&
Math.Abs(pt.Z - node.Pt.Z) < mEpsilon)
{
return node.Idx;
}
else if (pt.X < node.Pt.X - mEpsilon)
{
return FindHelper(node.Left, new Point3D(pt.Y, pt.Z, pt.X));
}
else if (pt.X > node.Pt.X + mEpsilon)
{
return FindHelper(node.Right, new Point3D(pt.Y, pt.Z, pt.X));
}
else
{
int leftresult = FindHelper(node.Left, new Point3D(pt.Y, pt.Z, pt.X));
if (leftresult != -1)
return leftresult;
return FindHelper(node.Right, new Point3D(pt.Y, pt.Z, pt.X));
}
}
private bool InsertHelper(ref kdnode node, Point3D pt, int idx)
{
if (node == null)
{
node = new kdnode(pt, idx);
return true;
}
else if( Math.Abs(pt.X - node.Pt.X) < mEpsilon &&
Math.Abs(pt.Y - node.Pt.Y) < mEpsilon &&
Math.Abs(pt.Z - node.Pt.Z) < mEpsilon )
{
return false;
}
else
{
if (pt.X < node.Pt.X)
return InsertHelper(ref node.Left, new Point3D(pt.Y, pt.Z, pt.X), idx);
else
return InsertHelper(ref node.Right, new Point3D(pt.Y, pt.Z, pt.X), idx);
}
}
public kdtree( double epsilon)
{
root = null;
mEpsilon = epsilon;
}
public kdnode(Point3D pt, int idx)
{
Pt = pt;
Idx = idx;
Left = null;
Right = null;
}