public static IKdTreeNode ReadNode(ModelLoadContext ctx)
{
var typeNode = ctx.Reader.ReadByte();
IKdTreeNode res;
switch (typeNode)
{
case 0:
res = null;
break;
case 1:
res = new KdTreeNode(ctx);
break;
case 2:
res = new KdTreeLeaf(ctx);
break;
default:
throw Contracts.Except("Bad value for type: {0}", typeNode);
}
byte b = ctx.Reader.ReadByte();
if (b != 169)
{
throw Contracts.Except("Detected inconsistency in deserializing.");
}
return(res);
}
public void TestRemoveAt()
{
AddTestNodes();
var nodesToRemove = new KdTreeNode <float, string>[] {
testNodes[1], // Root-Left
testNodes[0] // Root
};
foreach (var nodeToRemove in nodesToRemove)
{
tree.RemoveAt(nodeToRemove.Point);
testNodes.Remove(nodeToRemove);
Assert.IsNull(tree.FindValue(nodeToRemove.Value));
Assert.IsNull(tree.FindValueAt(nodeToRemove.Point));
foreach (var testNode in testNodes)
{
Assert.AreEqual(testNode.Value, tree.FindValueAt(testNode.Point));
Assert.AreEqual(testNode.Point, tree.FindValue(testNode.Value));
}
Assert.AreEqual(testNodes.Count, tree.Count);
}
}
public void Build(IList <Vector3> pts)
{
if (null == m_Points || m_Points.Length < pts.Count)
{
m_PointNum = 0;
m_Points = new Vector3[pts.Count * 2];
foreach (Vector3 pt in pts)
{
m_Points[m_PointNum++] = pt;
}
}
else
{
m_PointNum = 0;
foreach (Vector3 pt in pts)
{
m_Points[m_PointNum] = pt;
++m_PointNum;
}
}
if (m_PointNum > 0)
{
if (null == m_KdTree || m_KdTree.Length < 3 * m_PointNum)
{
m_KdTree = new KdTreeNode[3 * m_PointNum];
for (int i = 0; i < m_KdTree.Length; ++i)
{
m_KdTree[i] = new KdTreeNode();
}
}
m_MaxNodeNum = 2 * m_PointNum;
BuildImpl();
}
}
private static List <int> SelectPointIndices(PointCloud cloud, int numPoints, float minRange)
{
List <int> indices = new List <int>();
int maxIter = (int)(cloud.Points.Count * Registration.Settings.Default.MaxSamplingIterationProportion);
Random rand = new Random();
KdTreeNode kdRoot = new KdTreeNode(cloud.Points[rand.Next(0, cloud.Points.Count - 1)]);
var kd = new KdTree(3, kdRoot);
for (int i = 1; indices.Count < numPoints; i++)
{
if (!(i < maxIter))
{
throw new SamplingException("Please relax your sampling threshold parameters!");
}
int randomIndex = rand.Next(0, cloud.Points.Count - 1);
if (indices.Contains(randomIndex))
{
continue;
}
Point3D p = cloud.Points[randomIndex];
var nn = new List <Vector>(kd.FindInRange(p, minRange));
if (nn.Count == 0)
{
indices.Add(randomIndex);
kd.Add(p);
}
}
return(indices);
}
private void DrawKdRect <T>(KdTreeNode <T> node)
{
if (node.bounds.With(out var bounds))
{
DrawRect(Pens.DarkGreen, bounds);
DrawKdRectSplits(node, bounds);
}
}
private void Draw(Canvas canvas, KdTreeNode <spatial.Point> node, int level, double y1, double y2, double x1, double x2, ref int height)
{
height = Math.Max(height, level);
bool xBased = (level % 2 == 0);
if (node.LeftChild != null)
{
if (xBased)
{
Draw(canvas, node.LeftChild, level + 1, y1, y2, x1, node.Point.X, ref height);
}
else
{
Draw(canvas, node.LeftChild, level + 1, y1, node.Point.Y, x1, x2, ref height);
}
}
if (node.RigthChild != null)
{
if (xBased)
{
Draw(canvas, node.RigthChild, level + 1, y1, y2, node.Point.X, x2, ref height);
}
else
{
Draw(canvas, node.RigthChild, level + 1, node.Point.Y, y2, x1, x2, ref height);
}
}
Line line = new Line()
{
StrokeThickness = 2, Stroke = new SolidColorBrush(Colors.Green)
};
double x = node.Point.X; double y = node.Point.Y;
if (xBased)
{
line.X1 = x; line.X2 = x; line.Y1 = y1; line.Y2 = y2;
}
else
{
line.X1 = x1; line.X2 = x2; line.Y1 = y; line.Y2 = y;
}
EllipseGeometry geometry = new EllipseGeometry(new Point(x, y), 3, 3);
Path rectangle = new Path()
{
Data = geometry, Fill = new SolidColorBrush(Colors.Red)
};
canvas.Children.Add(line);
canvas.Children.Add(rectangle);
}
// TODO: exceptions
public void BuildIndex(IEnumerable <Vector2D> points)
{
ArrayList <IdxDat <Vector2D> > indexedPoints = new ArrayList <IdxDat <Vector2D> >();
int idx = 0;
foreach (Vector2D point in points)
{
indexedPoints.Add(new IdxDat <Vector2D>(idx++, point));
}
mRootNode = BuildSubTree(null, indexedPoints, 0);
mNumChanges = 0;
}
private HyperRect <int> LargestConnectedHyperrect(KdTreeNode <int, int> node)
{
if (node.IsLeaf)
{
return(area(node.LeftRect) > area(node.RightRect) ? node.LeftRect : node.RightRect);
}
else
{
var leftSide = node.LeftChild == null ? node.LeftRect : LargestConnectedHyperrect(node.LeftChild);
var rightSide = node.RightChild == null ? node.RightRect : LargestConnectedHyperrect(node.RightChild);
return(area(leftSide) > area(rightSide) ? leftSide : rightSide);
}
}
public static int GetNumberPictInRangeInNextTurns(Satellite satellite, KdTreeNode<float, PicCollection> n, KdTree<float, PicCollection> tree)
{
var possiblePict = new Coords((int)n.Point[0], (int)n.Point[1]);
var copySatellite = satellite.Clone();
copySatellite.TakePicture(possiblePict);
copySatellite.NextTurn();
var inRange = tree.RadialSearch(new float[] { copySatellite.Pos.Lat, copySatellite.Pos.Lon }, copySatellite.MaxRot, 150)
.Where(no => no.Value.PictureCanBeTaken(copySatellite.CurrentTurn))
.Where(k => copySatellite.CanTakePicture((int)k.Point[0], (int)k.Point[1])).Count() + GetNumberPictInRangeFixTrajectory(copySatellite, tree, 500);
//Console.WriteLine("Number in range for {0} {1}: {2}", possiblePict.Lat, possiblePict.Lon, inRange);
return inRange;
}
public void EndBuild()
{
if (m_ObjectNum > 0) {
if (null == m_KdTree || m_KdTree.Length < 3 * m_ObjectNum) {
m_KdTree = new KdTreeNode[3 * m_ObjectNum];
for (int i = 0; i < m_KdTree.Length; ++i) {
m_KdTree[i] = new KdTreeNode();
}
}
m_MaxNodeNum = 2 * m_ObjectNum;
BuildImpl();
}
}
public void EndBuild()
{
if (m_ObjectNum > 0)
{
if (null == m_KdTree || m_KdTree.Length < 3 * m_ObjectNum)
{
m_KdTree = new KdTreeNode[3 * m_ObjectNum];
for (int i = 0; i < m_KdTree.Length; ++i)
{
m_KdTree[i] = new KdTreeNode();
}
}
m_MaxNodeNum = 2 * m_ObjectNum;
BuildImpl();
}
}
private KdTreeNode BuildSubTree(KdTreeNode parentNode, ArrayList <IdxDat <Vector2D> > points, int level)
{
if (points.Count <= mMaxPtsPerLeaf) // terminal node
{
KdTreeNodeTerminal terminalNode = new KdTreeNodeTerminal(points);
terminalNode.ParentNode = parentNode;
return(terminalNode);
}
else // non-terminal node
{
ArrayList <IdxDat <Vector2D> > pointsRight = null;
double bound = ComputeBound(points, ref pointsRight, level % 2);
KdTreeNodeNonTerminal node = new KdTreeNodeNonTerminal(bound);
node.ParentNode = parentNode;
node.LeftNode = BuildSubTree(node, points, level + 1);
node.RightNode = BuildSubTree(node, pointsRight, level + 1);
return(node);
}
}
private void DrawKdRectSplits <T>(KdTreeNode <T> node, AaRect bounds)
{
Graphics g = _graphics;
if (node.type == NodeType.Inner)
{
Pen pen = Pens.DarkGray;
if (node.inner.axis == Axis.X)
{
g.DrawLine(pen, node.inner.pivot, bounds.min.y, node.inner.pivot, bounds.max.y);
DrawKdRectSplits(
node.inner.nodeLess,
AaRect.mm(bounds.min, node.inner.pivot, bounds.max.y)
);
DrawKdRectSplits(
node.inner.nodeMore,
AaRect.mm(node.inner.pivot, bounds.min.y, bounds.max)
);
}
if (node.inner.axis == Axis.Y)
{
g.DrawLine(pen, bounds.min.x, node.inner.pivot, bounds.max.x, node.inner.pivot);
DrawKdRectSplits(
node.inner.nodeLess,
AaRect.mm(bounds.min, bounds.max.x, node.inner.pivot)
);
DrawKdRectSplits(
node.inner.nodeMore,
AaRect.mm(bounds.min.x, node.inner.pivot, bounds.max)
);
}
DrawKdRect(node.inner.nodeBoth);
}
if (node.type == NodeType.Leaf)
{
var holder = node.leaf.holder;
while (holder != null)
{
//DrawRect(Pens.DarkRed, holder.fatRect.ExtendSides(V.fill(5)));
holder = holder.sibling.next;
}
}
}
public void Build(IList <ISpaceObject> objs)
{
if (null == m_Objects || m_Objects.Length < objs.Count)
{
m_ObjectNum = 0;
m_Objects = new KdTreeObject[objs.Count * 2];
foreach (ISpaceObject obj in objs)
{
m_Objects[m_ObjectNum++] = new KdTreeObject(obj);
}
}
else
{
m_ObjectNum = 0;
foreach (ISpaceObject obj in objs)
{
if (null == m_Objects[m_ObjectNum])
{
m_Objects[m_ObjectNum] = new KdTreeObject(obj);
}
else
{
m_Objects[m_ObjectNum].CopyFrom(obj);
}
++m_ObjectNum;
}
}
if (m_ObjectNum > 0)
{
if (null == m_KdTree || m_KdTree.Length < 3 * m_ObjectNum)
{
m_KdTree = new KdTreeNode[3 * m_ObjectNum];
for (int i = 0; i < m_KdTree.Length; ++i)
{
m_KdTree[i] = new KdTreeNode();
}
}
m_MaxNodeNum = 2 * m_ObjectNum;
BuildImpl();
}
}
/// <summary>
/// See interface docs.
/// </summary>
/// <param name="airPressures"></param>
/// <param name="fetchTimeUtc"></param>
public void LoadAirPressures(IEnumerable <AirPressure> airPressures, DateTime fetchTimeUtc)
{
if (airPressures == null)
{
throw new ArgumentNullException("airPressures");
}
FetchTimeUtc = fetchTimeUtc;
CountAirPressuresLoaded = 0;
var kdTree = new KdTree <float, AirPressure>(2, new GeoMath());
foreach (var airPressure in airPressures)
{
var node = new KdTreeNode <float, AirPressure>(new float[] { airPressure.Latitude, airPressure.Longitude }, airPressure);
kdTree.Add(node.Point, node.Value);
}
lock (_SyncLock) {
_AirPressures = kdTree;
}
CountAirPressuresLoaded = kdTree.Count;
}
public void Add(double key, KdTreeNode <T> value)
{
if (key > LastDistance && IsFull)
{
return;
}
if (!ContainsKey(key))
{
base.Add(key, new HashSet <KdTreeNode <T> >());
if (Count > K)
{
RemoveAt(Count - 1);
}
}
if (this[key].Add(value))
{
var last = this.Last();
LastElement = last.Value.Last();
LastDistance = last.Key;
}
}
//public bool RemovePoint(int idx, Vector2D point)
//{
// ArrayList<KdTreeNode> terminalNodes = new ArrayList<KdTreeNode>();
// GetTreeNodes(point, new Vector2D(), mRootNode, 0, terminalNodes);
// bool success = false;
// foreach (KdTreeNodeTerminal terminalNode in terminalNodes)
// {
// int oldCount = terminalNode.Points.Count;
// terminalNode.Points.Remove(new IdxDat<Vector2D>(idx));
// success = oldCount > terminalNode.Points.Count;
// if (success)
// {
// mNumChanges++;
// // *** the index is not updated here; it should be rebuilt manually after enough points have been removed/inserted
// break;
// }
// }
// return success;
//}
//public void InsertPoint(int idx, Vector2D point)
//{
// ArrayList<KdTreeNode> terminalNodes = new ArrayList<KdTreeNode>();
// GetTreeNodes(point, new Vector2D(), mRootNode, 0, terminalNodes);
// int min = int.MaxValue;
// int minIdx = -1;
// int i = 0;
// foreach (KdTreeNodeTerminal terminalNode in terminalNodes)
// {
// if (terminalNode.Points.Count < min) { minIdx = i; min = terminalNode.Points.Count; }
// i++;
// }
// ((KdTreeNodeTerminal)terminalNodes[minIdx]).Points.Add(new IdxDat<Vector2D>(idx, point));
// mNumChanges++;
// // *** the index is not updated here; it should be rebuilt manually after enough points have been removed/inserted
//}
private void GetTreeNodes(Vector2D refPoint, Vector2D size, KdTreeNode node, int level, ArrayList <KdTreeNode> treeNodes) // refPoint and size define a rectangle
{
int dim = level % 2;
if (node is KdTreeNodeNonTerminal)
{
KdTreeNodeNonTerminal nonTerminalNode = (KdTreeNodeNonTerminal)node;
double bound = nonTerminalNode.Bound;
double refPointCoord = GetVectCoord(refPoint, dim);
double sizeCoord = GetVectCoord(size, dim);
if (bound < refPointCoord)
{
GetTreeNodes(refPoint, size, nonTerminalNode.RightNode, level + 1, treeNodes);
}
else if (bound > refPointCoord + sizeCoord)
{
GetTreeNodes(refPoint, size, nonTerminalNode.LeftNode, level + 1, treeNodes);
}
else
{
// split the rectangle at the bound
Vector2D rightRefPoint = refPoint;
Vector2D rightSize = size;
SetVectCoord(ref size, dim, bound - refPointCoord);
GetTreeNodes(refPoint, size, nonTerminalNode.LeftNode, level + 1, treeNodes);
double rightRefPointCoord = GetVectCoord(rightRefPoint, dim);
double rightSizeCoord = GetVectCoord(rightSize, dim);
SetVectCoord(ref rightSize, dim, rightRefPointCoord + rightSizeCoord - bound);
SetVectCoord(ref rightRefPoint, dim, bound);
GetTreeNodes(rightRefPoint, rightSize, nonTerminalNode.RightNode, level + 1, treeNodes);
}
}
else
{
treeNodes.Add(node);
}
}
public void Build(IList <GeometryT> objs)
{
if (null == m_Objects || m_Objects.Length < objs.Count)
{
m_ObjectNum = 0;
m_Objects = new GeometryT[objs.Count * 2];
foreach (GeometryT obj in objs)
{
m_Objects[m_ObjectNum++] = obj;
obj.Indexed = false;
}
}
else
{
m_ObjectNum = 0;
foreach (GeometryT obj in objs)
{
m_Objects[m_ObjectNum] = obj;
obj.Indexed = false;
++m_ObjectNum;
}
}
if (m_ObjectNum > 0)
{
if (null == m_KdTree || m_KdTree.Length < 4 * m_ObjectNum)
{
m_KdTree = new KdTreeNode[4 * m_ObjectNum];
for (int i = 0; i < m_KdTree.Length; ++i)
{
m_KdTree[i] = new KdTreeNode();
}
}
m_MaxNodeNum = 4 * m_ObjectNum;
BuildImpl();
}
}
public void TestRemoveAt()
{
AddTestNodes();
var nodesToRemove = new KdTreeNode<float, string>[] {
testNodes[1], // Root-Left
testNodes[0] // Root
};
foreach (var nodeToRemove in nodesToRemove)
{
tree.RemoveAt(nodeToRemove.Point);
testNodes.Remove(nodeToRemove);
Assert.IsNull(tree.FindValue(nodeToRemove.Value));
Assert.IsNull(tree.FindValueAt(nodeToRemove.Point));
foreach (var testNode in testNodes)
{
Assert.AreEqual(testNode.Value, tree.FindValueAt(testNode.Point));
Assert.AreEqual(testNode.Point, tree.FindValue(testNode.Value));
}
Assert.AreEqual(testNodes.Count, tree.Count);
}
}
internal KdTreeNode(KdTreeNode <Q> leftChild, KdTreeNode <Q> rightChild, (int, PdfPoint, Q) point, int depth)
/// <summary>
/// Creates a new <see cref="KdTree{T}"/>.
/// </summary>
///
/// <param name="dimension">The number of dimensions in the tree.</param>
/// <param name="root">The root node, if already existent.</param>
///
public KDTree(int dimension, KdTreeNode root)
: base(dimension, root)
{
}
/// <summary>
/// Creates a new <see cref="KdTree{T}"/>.
/// </summary>
///
/// <param name="dimension">The number of dimensions in the tree.</param>
/// <param name="root">The root node, if already existent.</param>
/// <param name="count">The number of elements in the root node.</param>
/// <param name="leaves">The number of leaves linked through the root node.</param>
///
public KDTree(int dimension, KdTreeNode root, int count, int leaves)
: base(dimension, root, count, leaves)
{
}
internal static IPointIdFloat RandomSelect(
IList <IPointIdFloat> points,
int which,
ref IList <IPointIdFloat> left,
ref IList <IPointIdFloat> right,
int depth,
Random rnd)
{
int n = points.Count();
switch (n)
{
case 0:
throw new IndexOutOfRangeException();
case 1:
return(points.First());
default:
IPointIdFloat pivot;
int i;
//lock (syncLock)
//{ // synchronize
i = rnd.Next(n);
List <IPointIdFloat> localMiddle = new List <IPointIdFloat>();
List <IPointIdFloat> localLeft = new List <IPointIdFloat>();
List <IPointIdFloat> localRight = new List <IPointIdFloat>();
float pivotKey = KdTreeNode.KeyByDepth(points[i], depth);
foreach (IPointIdFloat p in points)
{
float pKey = KdTreeNode.KeyByDepth(p, depth);
if (pKey < pivotKey)
{
localLeft.Add(p);
}
else if (pKey > pivotKey)
{
localRight.Add(p);
}
else
{
localMiddle.Add(p);
}
}
int sizeLeft = localLeft.Count();
int sizeMiddle = localMiddle.Count();
if (which < sizeLeft)
{
foreach (IPointIdFloat p in localMiddle.Concat(localRight))
{
right.Add(p);
}
pivot = RandomSelect(localLeft, which, ref left, ref right, depth, rnd);
}
else if (which < sizeLeft + sizeMiddle)
{
pivot = localMiddle.First();
//All points with the same key as this node's, must go left, to make search faster
foreach (IPointIdFloat p in localLeft.Concat(localMiddle.Skip(1)))
{
left.Add(p);
}
foreach (IPointIdFloat p in localRight)
{
right.Add(p);
}
}
else
{
foreach (IPointIdFloat p in localLeft.Concat(localMiddle))
{
left.Add(p);
}
pivot = RandomSelect(localRight, which - sizeLeft - sizeMiddle, ref left, ref right, depth, rnd);
}
return(pivot);
}
}
