// Method ins translated from 352.ins.c of Gonnet & Baeza-Yates
public static KDNode ins(HPoint key, Object val, KDNode t, int lev, int K)
{
if (t == null)
{
t = new KDNode(key, val);
}
else if (key.equals(t.k))
{
// "re-insert"
if (t.deleted)
{
t.deleted = false;
t.v = val;
}
else
{
throw (new KeyDuplicateException());
}
}
else if (key.coord[lev] > t.k.coord[lev])
{
t.right = ins(key, val, t.right, (lev + 1) % K, K);
}
else
{
t.left = ins(key, val, t.left, (lev + 1) % K, K);
}
return(t);
}
/**
* Range search in a KD-tree. Uses algorithm translated from
* 352.range.c of Gonnet & Baeza-Yates.
*
* @param lowk lower-bounds for key
* @param uppk upper-bounds for key
*
* @return array of Objects whose keys fall in range [lowk,uppk]
*
* @throws KeySizeException on mismatch among lowk.length, uppk.length, or K
*/
public Object[] range(double[] lowk, double[] uppk)
{
if (lowk.Length != uppk.Length)
{
throw new KeySizeException();
}
else if (lowk.Length != m_K)
{
throw new KeySizeException();
}
else
{
List <KDNode> v = new List <KDNode>();
KDNode.rsearch(new HPoint(lowk), new HPoint(uppk),
m_root, 0, m_K, v);
Object[] o = new Object[v.Count];
for (int i = 0; i < v.Count; ++i)
{
KDNode n = (KDNode)v[i];
o[i] = n.v;
}
return(o);
}
}
// Method rsearch translated from 352.range.c of Gonnet & Baeza-Yates
public static void rsearch(HPoint lowk, HPoint uppk, KDNode t, int lev,
int K, List <KDNode> v)
{
if (t == null)
{
return;
}
if (lowk.coord[lev] <= t.k.coord[lev])
{
rsearch(lowk, uppk, t.left, (lev + 1) % K, K, v);
}
int j;
for (j = 0; j < K && lowk.coord[j] <= t.k.coord[j] &&
uppk.coord[j] >= t.k.coord[j]; j++)
{
;
}
if (j == K && !t.deleted)
{
v.Add(t);
}
if (uppk.coord[lev] > t.k.coord[lev])
{
rsearch(lowk, uppk, t.right, (lev + 1) % K, K, v);
}
}
// ツリーを再帰的に探索
void Search(KDNode node, Vector3 pt, ref float minDist, ref int minIdx, int depth = 0)
{
var pivot = node.pivot;
float currentDist = (pivot - pt).sqrMagnitude;
var lr = node.lr;
if (currentDist < minDist)
{
minDist = currentDist;
minIdx = node.pivotIdx;
}
int axis = node.axis;
float axisDist = pt[axis] - pivot[axis];
int leftOrRight = axisDist <= 0 ? 0 : 1;
if (lr[leftOrRight] != null)
{
Search(lr[leftOrRight], pt, ref minDist, ref minIdx, depth + 1);
}
leftOrRight = (leftOrRight + 1) % 2;
if ((lr[leftOrRight] != null) && (minDist > axisDist * axisDist))
{
Search(lr[leftOrRight], pt, ref minDist, ref minIdx, depth + 1);
}
}
/// <summary>
/// Construct a new nearest neighbour iterator.
/// </summary>
/// <param name="pRoot">The root of the tree to begin searching from.</param>
/// <param name="tSearchPoint">The point in n-dimensional space to search.</param>
/// <param name="kDistance">The distance function used to evaluate the points.</param>
/// <param name="iMaxPoints">The max number of points which can be returned by this iterator. Capped to max in tree.</param>
/// <param name="fThreshold">Threshold to apply to the search space. Negative numbers indicate that no threshold is applied.</param>
public NearestNeighbour(KDNode <T> pRoot, double[] tSearchPoint, DistanceFunctions kDistance, int iMaxPoints, double fThreshold)
{
// Check the dimensionality of the search point.
if (tSearchPoint.Length != pRoot.iDimensions)
{
throw new Exception("Dimensionality of search point and kd-tree are not the same.");
}
// Store the search point.
this.tSearchPoint = new double[tSearchPoint.Length];
Array.Copy(tSearchPoint, this.tSearchPoint, tSearchPoint.Length);
// Store the point count, distance function and tree root.
iPointsRemaining = Math.Min(iMaxPoints, pRoot.Size);
this.fThreshold = fThreshold;
kDistanceFunction = kDistance;
this.pRoot = pRoot;
iMaxPointsReturned = iMaxPoints;
_CurrentDistance = -1;
// Create an interval heap for the points we check.
pEvaluated = new IntervalHeap <T>();
// Create a min heap for the things we need to check.
pPending = new MinHeap <KDNode <T> >();
pPending.Insert(0, pRoot);
}
// prekresli sa vrchol
private void repaintNodes(KDNode n)
{
System.Drawing.Font font = new System.Drawing.Font("Verdana", 10);
System.Drawing.SolidBrush myBrush = new System.Drawing.SolidBrush(System.Drawing.Color.Black);
System.Drawing.Pen myPen = new System.Drawing.Pen(System.Drawing.Color.Black);
if (n.highlight)
{
myBrush.Color = System.Drawing.Color.Green;
myPen.Color = System.Drawing.Color.Green;
}
if (n.highlightRed)
{
myBrush.Color = System.Drawing.Color.Red;
myPen.Color = System.Drawing.Color.Red;
}
if (n.highligthIn)
{
myBrush.Color = System.Drawing.Color.Red;
myPen.Color = System.Drawing.Color.Red;
g.DrawEllipse(myPen, n.getPoint().X - (RADIUS + 2), n.getPoint().Y - (RADIUS + 2), 2 * (RADIUS + 2), 2 * (RADIUS + 2));
myPen.Color = System.Drawing.Color.Black;
}
g.FillEllipse(myBrush, n.getPoint().X - RADIUS, n.getPoint().Y - RADIUS, 2*RADIUS, 2*RADIUS);
string s = "(" + n.getPoint().X.ToString() + "," + n.getPoint().Y.ToString() + ")";
g.DrawString(s, font, myBrush, n.getPoint().X - 35, n.getPoint().Y);
}
// Method ins translated from 352.ins.c of Gonnet & Baeza-Yates
public static KDNode ins(HPoint key, Object val, KDNode t, int lev, int K, out bool worked)
{
worked = true;
if (t == null)
{
t = new KDNode(key, val);
}
else if (key.equals(t.k))
{
// "re-insert"
if (t.deleted)
{
t.deleted = false;
t.v = val;
}
else
{
worked = false;
}
}
else if (key.coord[lev] > t.k.coord[lev])
{
t.right = ins(key, val, t.right, (lev + 1) % K, K, out worked);
}
else
{
t.left = ins(key, val, t.left, (lev + 1) % K, K, out worked);
}
return(t);
}
private void Insert(GameObject obj, KDNode currentNode)
{
if (IsLowerThan(obj, currentNode.obj, currentNode.splitDimension))
{
if (currentNode.leftChild == null)
{
currentNode.leftChild = CreateNode(obj, NextDimension(currentNode.splitDimension, _nbDimensions));
}
else
{
Insert(obj, currentNode.leftChild);
}
}
else
{
if (currentNode.rightChild == null)
{
currentNode.rightChild = CreateNode(obj, NextDimension(currentNode.splitDimension, _nbDimensions));
}
else
{
Insert(obj, currentNode.rightChild);
}
}
}
public void DrawTree(KDNode root)
{
if (root != null)
{
DrawTree(root.left);
DrawTree(root.right);
// Gizmos.DrawSphere(root.Component.transform.position, 1f);
Vector3[] corners = root.boundary.corners;
Gizmos.DrawLine(corners[0], corners[1]);
Gizmos.DrawLine(corners[1], corners[2]);
Gizmos.DrawLine(corners[2], corners[3]);
Gizmos.DrawLine(corners[3], corners[0]);
Gizmos.DrawLine(corners[4], corners[5]);
Gizmos.DrawLine(corners[5], corners[6]);
Gizmos.DrawLine(corners[6], corners[7]);
Gizmos.DrawLine(corners[7], corners[4]);
Gizmos.DrawLine(corners[0], corners[6]);
Gizmos.DrawLine(corners[1], corners[5]);
Gizmos.DrawLine(corners[2], corners[4]);
Gizmos.DrawLine(corners[3], corners[7]);
}
}
/// <summary>
/// Split this leaf node by creating left and right children, then moving all the children of
/// this node into the respective buckets.
/// </summary>
private void SplitLeafNode()
{
// Create the new children.
pRight = new KDNode <T>(iDimensions, iBucketCapacity);
pLeft = new KDNode <T>(iDimensions, iBucketCapacity);
// Move each item in this leaf into the children.
for (int i = 0; i < Size; ++i)
{
// Store.
double[] tOldPoint = tPoints[i];
T kOldData = tData[i];
// If larger, put it in the right.
if (tOldPoint[iSplitDimension] > fSplitValue)
{
pRight.AddLeafPoint(tOldPoint, kOldData);
}
// If smaller, put it in the left.
else
{
pLeft.AddLeafPoint(tOldPoint, kOldData);
}
}
// Wipe the data from this KDNode.
tPoints = null;
tData = null;
}
//public bool Insert(T item, KDNode root)
//{
// if (!Root.boundary.Contains(item.transform.position) && root is null)
// return false;
// root.components.Add(item);
// if(root.components.Count > CAPACITY)
// {
// Subdivide();
// }
// return false;
//}
//private void Subdivide()
//{
// KDNode left;
// KDNode right;
//}
public bool Insert(T data, int depth, KDNode node)
{
if (!Root.boundary.Contains(data.transform.position) && Root is null)
{
return(false);
}
int axis = node.depth % DIMENSION;
node.components.Add(data);
if (node.components.Count > CAPACITY)
{
switch (axis)
{
case 1:
XSubdivide(node);
break;
case 2:
YSubdivide(node);
break;
case 3:
ZSubdivide(node);
break;
default:
break;
}
}
return(false);
}
// Method rsearch translated from 352.range.c of Gonnet & Baeza-Yates
public static void rsearch(Point3 lowk, Point3 uppk, KDNode t, int lev, List <KDNode> v)
{
if (t == null)
{
return;
}
if (lowk.coord(lev) <= t.k.coord(lev))
{
rsearch(lowk, uppk, t.left, (lev + 1) % 3, v);
}
int j;
for (j = 0; j < 3 && lowk.coord(j) <= t.k.coord(j) &&
uppk.coord(j) >= t.k.coord(j); j++)
{
;
}
if (j == 3 && !t.deleted)
{
v.Add(t);
}
if (uppk.coord(lev) > t.k.coord(lev))
{
rsearch(lowk, uppk, t.right, (lev + 1) % 3, v);
}
}
// Try to delete the specified key from the tree. If successful,
// prunes the dead branches off. Returns the new KDNode at this
// location (possibly null). Reports success or failure in
// deleted.
public static KDNode delete(Point3 key, KDNode t, int lev, ref bool deleted)
{
deleted = false;
if (t == null)
{
return(null);
}
if (!t.deleted && key.equals(t.k))
{
t.deleted = true;
deleted = true;
}
else if (key.coord(lev) > t.k.coord(lev))
{
t.right = delete(key, t.right, (lev + 1) % 3, ref deleted);
}
else
{
t.left = delete(key, t.left, (lev + 1) % 3, ref deleted);
}
if (!t.deleted || t.left != null || t.right != null)
{
return(t);
}
else
{
return(null);
}
}
private void DrawTree(KDNode node)
{
if (node == null)
{
return;
}
DrawTree(node.Left);
DrawTree(node.Right);
if (node.TriangleKeys == null)
{
return;
}
foreach (Vector3 key in node.TriangleKeys)
{
Triangle t = triangleList[key];
var v = new VertexPositionColor[3];
v[0].Position = t.Point1;
v[1].Position = t.Point2;
v[2].Position = t.Point3;
for (int i = 0; i < 3; i++)
{
v[i].Color = Color.Black;
}
GraphicsDevice.DrawUserPrimitives <VertexPositionColor>(PrimitiveType.TriangleList, v, 0, 1);
}
}
/**
* Insert a node in a KD-tree. Uses algorithm translated from 352.ins.c of
*
* <PRE>
* @Book{GonnetBaezaYates1991,
* author = {G.H. Gonnet and R. Baeza-Yates},
* title = {Handbook of Algorithms and Data Structures},
* publisher = {Addison-Wesley},
* year = {1991}
* }
* </PRE>
*
* @param key key for KD-tree node
* @param value value at that key
*
* @throws KeySizeException if key.length mismatches K
* @throws KeyDuplicateException if key already in tree
*/
public void insert(double[] key, Object value)
{
if (key.Length != m_K)
{
throw new KeySizeException();
}
else
{
try
{
bool worked;
m_root = KDNode.ins(new HPoint(key), value, m_root, 0, m_K, out worked);
if (worked)
{
m_count++;
}
}
catch (KeyDuplicateException e)
{
throw e;
}
}
}
/**
* Find KD-tree nodes whose keys are <I>n</I> nearest neighbors to
* key. Uses algorithm above. Neighbors are returned in ascending
* order of distance to key.
*
* @param key key for KD-tree node
* @param n how many neighbors to find
*
* @return objects at node nearest to key, or null on failure
*
* @throws KeySizeException if key.length mismatches K
* @throws IllegalArgumentException if <I>n</I> is negative or
* exceeds tree size
*/
public Object[] nearest(double[] key, int n)
{
if (n < 0 || n > m_count)
{
throw new ArgumentException("Number of neighbors cannot be negative or greater than number of nodes");
}
if (key.Length != m_K)
{
throw new KeySizeException();
}
Object[] nbrs = new Object[n];
NearestNeighborList nnl = new NearestNeighborList(n);
// initial call is with infinite hyper-rectangle and max distance
HRect hr = HRect.infiniteHRect(key.Length);
double max_dist_sqd = Double.MaxValue;
HPoint keyp = new HPoint(key);
KDNode.nnbr(m_root, keyp, hr, max_dist_sqd, 0, m_K, nnl);
for (int i = 0; i < n; ++i)
{
KDNode kd = (KDNode)nnl.removeHighest();
nbrs[n - i - 1] = kd.v;
}
return(nbrs);
}
void NodePaintInterface.repaint(KDNode n)
{
//Console.WriteLine("(" + n.getPoint().X.ToString() + "," + n.getPoint().Y.ToString() + ")");
repaintBoxes(n);
repaintNodes(n);
}
// Recursively build a tree by separating points at plane boundaries.
private static KDNode makeFromPointsInner(int depth,
int start, int end,
Vector3[] points,
int[] indices)
{
KDNode root = new KDNode();
root.Axis = depth % DIMENSIONS;
int splitPoint = findPivotIndex(points, indices, start, end, root.Axis);
root.PivotIndex = indices[splitPoint];
root.Pivot = points[root.PivotIndex];
int leftEndIndex = splitPoint - 1;
if (leftEndIndex >= start)
{
root.Children[LEFT] = makeFromPointsInner(depth + 1,
start, leftEndIndex,
points,
indices);
}
int rightStartIndex = splitPoint + 1;
if (rightStartIndex <= end)
{
root.Children[RIGHT] = makeFromPointsInner(depth + 1,
rightStartIndex, end,
points,
indices);
}
return(root);
}
public static KDNode delete(HPoint key, KDNode t, int lev, int K, ref bool deleted)
{
if (t == null)
{
return(null);
}
if (!t.deleted && key.equals(t.k))
{
t.deleted = true;
deleted = true;
}
else if (key.coord[lev] > t.k.coord[lev])
{
t.right = delete(key, t.right, (lev + 1) % K, K, ref deleted);
}
else
{
t.left = delete(key, t.left, (lev + 1) % K, K, ref deleted);
}
if (!t.deleted || t.left != null || t.right != null)
{
return(t);
}
else
{
return(null);
}
}
// 再帰的にKDツリーの構築を行う
KDNode MakeFromPoints(Vector3[] points, int depth, int stIdx, int enIdx, int[] inds)
{
KDNode root = new KDNode();
root.axis = depth % dimension;
int splitPointIdx = SplitByAxis(points, inds, stIdx, enIdx, root.axis);
root.lr = new KDNode[2];
root.pivotIdx = inds[splitPointIdx];
root.pivot = points[root.pivotIdx];
int leftEndIdx = splitPointIdx - 1;
if (leftEndIdx >= stIdx)
{
root.lr[0] = MakeFromPoints(points, depth + 1, stIdx, leftEndIdx, inds);
}
int rightStartIdx = splitPointIdx + 1;
if (rightStartIdx <= enIdx)
{
root.lr[1] = MakeFromPoints(points, depth + 1, rightStartIdx, enIdx, inds);
}
return(root);
}
internal void InitEmptyLeaf()
{
Left = new KDNode();
Right = new KDNode();
Left.Triangles = new List <Triangle>();
Right.Triangles = new List <Triangle>();
}
public KDNode FindParent(Point2D x0)
{
KDNode parent = null;
KDNode next = this;
int split;
while (next != null)
{
split = next.axis;
parent = next;
if (split == 0)
{
if (x0.X > next.x.X)
{
next = next.Right;
}
else
{
next = next.Left;
}
}
else
{
if (x0.Y > next.x.Y)
{
next = next.Right;
}
else
{
next = next.Left;
}
}
}
return(parent);
}
void NodePaintInterface.repaint(KDNode n)
{
/*
System.Drawing.Font font = new System.Drawing.Font("Verdana", 10);
System.Drawing.SolidBrush myBrush = new System.Drawing.SolidBrush(System.Drawing.Color.Black);
System.Drawing.Pen myPen = new System.Drawing.Pen(System.Drawing.Color.Black);
if (n.highlight)
{
myBrush.Color = System.Drawing.Color.Green;
myPen.Color = System.Drawing.Color.Green;
}
if (n.getDim() == 0)
{
g.DrawLine(myPen, n.getSplit(), n.from, n.getSplit(), n.to);
} else {
g.DrawLine(myPen, n.from, n.getSplit(), n.to, n.getSplit());
}
string s = "(" + n.getPoint().X.ToString() + "," + n.getPoint().Y.ToString() + ")";
g.DrawString(s, font, myBrush, n.getPoint().X - 35, n.getPoint().Y);
*/
}
public KDNode(Point2D x0, int axis0)
{
x = x0;
axis = axis0;
Left = Right = Parent = null;
isChecked = false;
id = 0;
}
// constructor is used only by class; other methods are static
private KDNode(HPoint key, Object val)
{
k = key;
v = val;
left = null;
right = null;
deleted = false;
}
int numNearest; // = 'k' pri hladani 'k' najblizsich vrcholov
public KDTree(System.Drawing.Graphics g)
{
root = null;
this.g = g;
width = 900;
height = 600;
numNearest = 1;
}
private KDNode CreateNode(GameObject obj, KDDimension splitDimension)
{
KDNode res = new KDNode();
res.obj = obj;
res.splitDimension = splitDimension;
return(res);
}
internal static KDNode Build(List <Triangle> pTriangles, int pDepth, Axis pPreviousAxis)
{
KDNode node = new KDNode();
node.Triangles = pTriangles;
float tris_count = node.Triangles.Count;
if (tris_count == 0)
{
return(node);
}
node.Box = new Bounds(node.Triangles[0].Box.center, node.Triangles[0].Box.size);
if (tris_count == 1 || pDepth >= MaxDepth)
{
node.InitEmptyLeaf();
return(node);
}
Vector3 mid_point = Vector3.zero;
for (int i = 1; i < tris_count; ++i)
{
node.Box.Encapsulate(node.Triangles[i].Box);
}
for (int i = 0; i < tris_count; ++i)
{
mid_point += node.Triangles[0].GetMidPoint() * (1 / tris_count);
}
node.MidPoint = mid_point;
Axis long_axis = node.GetLongestAxis(pPreviousAxis);
node.SelectionAxis = long_axis;
List <Triangle> left_triangles = new List <Triangle>();
List <Triangle> right_triangles = new List <Triangle>();
bool should_split = node.DivideLeftRightTriangles(ref left_triangles, ref right_triangles);
if (should_split)
{
node.Left = Build(left_triangles, pDepth + 1, long_axis);
node.Right = Build(right_triangles, pDepth + 1, long_axis);
}
else
{
node.InitEmptyLeaf();
}
return(node);
}
public KDTree(Game game, Dictionary <Vector3, Triangle> triangleList) : base(game)
{
this.triangleList = triangleList;
root = BuildKdTree(triangleList.Keys.ToList(), new List <Vector3>());
Initialize();
effect = new BasicEffect(GraphicsDevice);
}
void Start()
{
nn = null;
glowTime = 0;
descAudio.Stop();
explainAudio.Stop();
introAudio.Play();
}
/* Insert funguje tak, ze ked osetrime okrajove pripady,
* rekurzivne hladame podstrom (porovnavanim n.point so subtree.split),
* do ktoreho sa ma n vlozit. No nejdeme az po List, ale skoncime v takom
* koreni podstromu, v ktorom by bola narusena rovnovaha, keby sa do jeho syna
* vlozil n (zistime to porovnanim left.numLeafs a right.numLeafs).
* Vieme, ze n patri do tohto podstromu, ale kedze jeho vlozenim by
* bola narusena rovnovaha tak cely tento podstrom prebudujeme.
*/
public void insertLeaf(int x, int y)
{
// vkladame vrchol n
KDNode n = new KDNode(x, y, null, g);
// pripad, ked je strom prazdny
if (root == null)
{
root = n;
} else {
// neda sa pridat vrchol na to iste miesto
if (alreadyIn(n)) {
return;
}
// najde sa miesto pre n -> k je koren podstromu, ktoreho rovnovaha by bola narusena
KDNode k = findPlace(root, n);
//Console.WriteLine(k.getSplit());
if (k.getParent() != null) {
//Console.WriteLine(k.getParent().getNumLeafs());
}
// nastavi sa, ci je k lavym alebo pravym synom svojho otca
Boolean isLeftUp = false;
if (!k.Equals(root))
{
isLeftUp = k.Equals(k.getParent().getLeft());
}
List<KDNode> l = new List<KDNode>();
// najdu sa vsetky listy v prebudovavanom podstrome -
// z nich a z n sa vytvori novy vyvazeny podstrom
l.AddRange(findLeafs(k));
l.Add(n);
// rebuilt() vracia root noveho podstromu. uz je pripojeny k parent.
KDNode m = rebuilt(l, k.getDim(), k.getParent(), isLeftUp);
// a ohranicenia buniek
updateFromTo(m);
// ak sa prebudoval cely strom, meni sa root.
if (k.Equals(root))
{
root = m;
updateNumLeafsDown(root);
//updateNumLeafsUp(root);
} else {
updateNumLeafsDown(m);
if (isLeftUp)
{
m.getParent().setLeft(m);
} else {
m.getParent().setRight(m);
}
updateNumLeafsUp(m);
}
// pocet listov sa musi preratat aj pre rodicov nad prebudovanym podstromom
// pre kazdy vrchol z noveho podstromu sa updatene pocet listov, ktori su jeho potomkami
}
}
public KDTree(Game game, Dictionary<Vector3, Triangle> triangleList)
: base(game)
{
this.triangleList = triangleList;
root = BuildKdTree(triangleList.Keys.ToList(), new List<Vector3>());
Initialize();
effect = new BasicEffect(GraphicsDevice);
}
private void DumpNode(KDNode node, string prefix, bool isLeft)
{
if (node != null && node.obj != null)
{
Debug.Log(prefix + (isLeft ? "|-- " : "\\-- ") + node.obj.transform.position.ToString() + " - " + node.splitDimension.ToString());
string addedPrefix = isLeft ? "| " : " ";
DumpNode(node.leftChild, prefix + addedPrefix, true);
DumpNode(node.rightChild, prefix + addedPrefix, false);
}
}
public KDNode <Vector2> CreateTree(IEnumerable <Vector2> points)
{
Debug.WriteLine("Creating a KD tree for " + points.Count() + " values.");
IList <Vector2> vectorList = new List <Vector2>(points);
KDNode <Vector2> treeRoot = CreateNode(null, vectorList, Dimension.X);
return(treeRoot);
}
public MeshData(Mesh pMesh)
{
TriMesh = pMesh;
RawVertices = TriMesh.vertices.ToList <Vector3>();
RawNormals = TriMesh.normals.ToList <Vector3>();
RawTriangles = TriMesh.triangles.ToList <int>();
GenerateTriangleList();
Root = KDNode.Build(Triangles, 0, Axis.NO_AXIS);
}
/**
* Find KD-tree node whose key is identical to key. Uses algorithm
* translated from 352.srch.c of Gonnet & Baeza-Yates.
*
* @param key key for KD-tree node
*
* @return object at key, or null if not found
*
* @throws KeySizeException if key.length mismatches K
*/
public Object search(double[] key)
{
if (key.Length != m_K)
{
throw new KeySizeException();
}
KDNode kd = KDNode.srch(new HPoint(key), m_root, m_K);
return(kd == null ? null : kd.v);
}
protected void PerformKDRegionSearch(KDNode<TriangleGraph> node, ref BoundingBox region, List<TriangleGraph> triangleCollection, bool isLandmark)
{
if (node != null && (isLandmark || !usedLandmarkTriangles.ContainsKey(node.element.ID)))
{
if (region.Contains(node.element.Centroid) != ContainmentType.Disjoint
|| region.Contains(node.element.GetVertex0()) != ContainmentType.Disjoint
|| region.Contains(node.element.GetVertex1()) != ContainmentType.Disjoint
|| region.Contains(node.element.GetVertex2()) != ContainmentType.Disjoint)
{
triangleCollection.Add(node.element);
}
PerformKDRegionSearch(node.leftChild, ref region, triangleCollection, isLandmark);
PerformKDRegionSearch(node.rightChild, ref region, triangleCollection, isLandmark);
}
}
protected void PerformKDRegionSearch(KDNode<TriangleGraph> node, ref BoundingBox region, List<TriangleGraph> triangleCollection)
{
if (node != null)
{
if (region.Contains(node.element.Centroid) != ContainmentType.Disjoint
|| region.Contains(node.element.GetVertex0()) != ContainmentType.Disjoint
|| region.Contains(node.element.GetVertex1()) != ContainmentType.Disjoint
|| region.Contains(node.element.GetVertex2()) != ContainmentType.Disjoint)
{
triangleCollection.Add(node.element);
}
PerformKDRegionSearch(node.leftChild, ref region, triangleCollection);
PerformKDRegionSearch(node.rightChild, ref region, triangleCollection);
}
}
/**
* Delete a node from a KD-tree. Instead of actually deleting node and
* rebuilding tree, marks node as deleted. Hence, it is up to the caller
* to rebuild the tree as needed for efficiency.
*
* @param key key for KD-tree node
*
* @throws KeySizeException if key.length mismatches K
* @throws KeyMissingException if no node in tree has key
*/
public void delete(double[] key)
{
if (key.Length != 3)
{
throw new KeySizeException();
}
else
{
bool deleted = false;
m_root = KDNode.delete(new Point3(key), m_root, 0, ref deleted);
if (deleted == false) {
throw new KeyNotFoundException();
}
m_count--;
}
}
/**
* Delete a node from a KD-tree. Instead of actually deleting node and
* rebuilding tree, marks node as deleted. Hence, it is up to the caller
* to rebuild the tree as needed for efficiency.
*
* @param key key for KD-tree node
*
* @throws KeySizeException if key.length mismatches K
* @throws KeyMissingException if no node in tree has key
*/
public void delete(double[] key)
{
if (key.Length != m_K)
{
throw new KeySizeException();
}
else
{
bool deleted = false;
m_root = KDNode.delete(new HPoint(key), m_root, 0, m_K, ref deleted);
if (deleted == false) {
throw new KeyMissingException();
}
m_count--;
}
}
// prekresli sa bunka vrchola
private void repaintBoxes(KDNode n)
{
if (n.isLeaf)
{
System.Drawing.SolidBrush myBrush = new System.Drawing.SolidBrush(System.Drawing.Color.Black);
System.Drawing.Pen myPen = new System.Drawing.Pen(System.Drawing.Color.Black);
if (n.highligthBox)
{
myBrush.Color = System.Drawing.Color.LightSteelBlue;
g.FillRectangle(myBrush, n.from.X, n.from.Y, n.to.X - n.from.X, n.to.Y - n.from.Y);
}
//Console.WriteLine("vrchol: " + n.getPoint() + " from: " + n.from + " to: " + n.to);
g.DrawRectangle(myPen, n.from.X, n.from.Y, n.to.X - n.from.X, n.to.Y - n.from.Y);
} else {
//Console.WriteLine("vrchol: (" + n.getSplit() + " from: " + n.from + " to: " + n.to);
}
}
void RecursivelyBuildBounds(KDNode<ForestElement> node)
{
if (node == null)
return;
RecursivelyBuildBounds(node.leftChild);
RecursivelyBuildBounds(node.rightChild);
node.bounds = node.element.Transform.TransformBounds(node.element.Mesh.GetBounds());
if (node.leftChild != null)
{
node.bounds.Min = Vector3.Min(node.leftChild.bounds.Min, node.bounds.Min);
node.bounds.Max = Vector3.Max(node.leftChild.bounds.Max, node.bounds.Max);
}
if (node.rightChild != null)
{
node.bounds.Min = Vector3.Min(node.rightChild.bounds.Min, node.bounds.Min);
node.bounds.Max = Vector3.Max(node.rightChild.bounds.Max, node.bounds.Max);
}
}
// Method rsearch translated from 352.range.c of Gonnet & Baeza-Yates
public static void rsearch(Point3 lowk, Point3 uppk, KDNode t, int lev, List<KDNode> v)
{
if (t == null) return;
if (lowk.coord(lev) <= t.k.coord(lev))
{
rsearch(lowk, uppk, t.left, (lev + 1) % 3, v);
}
int j;
for (j = 0; j < 3 && lowk.coord(j) <= t.k.coord(j) &&
uppk.coord(j) >= t.k.coord(j); j++)
;
if (j == 3 && !t.deleted) v.Add(t);
if (uppk.coord(lev) > t.k.coord(lev))
{
rsearch(lowk, uppk, t.right, (lev + 1) % 3, v);
}
}
public KDTree()
{
m_root = null;
}
// Method srch translated from 352.srch.c of Gonnet & Baeza-Yates
public static KDNode srch(HPoint key, KDNode t, int K)
{
for (int lev = 0; t != null; lev = (lev + 1) % K)
{
if (!t.deleted && key.equals(t.k))
{
return t;
}
else if (key.coord[lev] > t.k.coord[lev])
{
t = t.right;
}
else
{
t = t.left;
}
}
return null;
}
// Method rsearch translated from 352.range.c of Gonnet & Baeza-Yates
public static void rsearch(HPoint lowk, HPoint uppk, KDNode t, int lev,
int K, List<KDNode> v)
{
if (t == null) return;
if (lowk.coord[lev] <= t.k.coord[lev])
{
rsearch(lowk, uppk, t.left, (lev + 1) % K, K, v);
}
int j;
for (j = 0; j < K && lowk.coord[j] <= t.k.coord[j] &&
uppk.coord[j] >= t.k.coord[j]; j++)
;
if (j == K && !t.deleted) v.Add(t);
if (uppk.coord[lev] > t.k.coord[lev])
{
rsearch(lowk, uppk, t.right, (lev + 1) % K, K, v);
}
}
/**
* Creates a KD-tree with specified number of dimensions.
*
* @param k number of dimensions
*/
public KDTree(int k)
{
m_K = k;
m_root = null;
}
// Method Nearest Neighbor from Andrew Moore's thesis. Numbered
// comments are direct quotes from there. Step "SDL" is added to
// make the algorithm work correctly. NearestNeighborList solution
// courtesy of Bjoern Heckel.
public static void nnbr(KDNode kd, HPoint target, HRect hr,
double max_dist_sqd, int lev, int K,
NearestNeighborList nnl)
{
// 1. if kd is empty then set dist-sqd to infinity and exit.
if (kd == null)
{
return;
}
// 2. s := split field of kd
int s = lev % K;
// 3. pivot := dom-elt field of kd
HPoint pivot = kd.k;
double pivot_to_target = HPoint.sqrdist(pivot, target);
// 4. Cut hr into to sub-hyperrectangles left-hr and right-hr.
// The cut plane is through pivot and perpendicular to the s
// dimension.
HRect left_hr = hr; // optimize by not cloning
HRect right_hr = (HRect)hr.clone();
left_hr.max.coord[s] = pivot.coord[s];
right_hr.min.coord[s] = pivot.coord[s];
// 5. target-in-left := target_s <= pivot_s
bool target_in_left = target.coord[s] < pivot.coord[s];
KDNode nearer_kd;
HRect nearer_hr;
KDNode further_kd;
HRect further_hr;
// 6. if target-in-left then
// 6.1. nearer-kd := left field of kd and nearer-hr := left-hr
// 6.2. further-kd := right field of kd and further-hr := right-hr
if (target_in_left)
{
nearer_kd = kd.left;
nearer_hr = left_hr;
further_kd = kd.right;
further_hr = right_hr;
}
//
// 7. if not target-in-left then
// 7.1. nearer-kd := right field of kd and nearer-hr := right-hr
// 7.2. further-kd := left field of kd and further-hr := left-hr
else
{
nearer_kd = kd.right;
nearer_hr = right_hr;
further_kd = kd.left;
further_hr = left_hr;
}
// 8. Recursively call Nearest Neighbor with paramters
// (nearer-kd, target, nearer-hr, max-dist-sqd), storing the
// results in nearest and dist-sqd
nnbr(nearer_kd, target, nearer_hr, max_dist_sqd, lev + 1, K, nnl);
KDNode nearest = (KDNode)nnl.getHighest();
double dist_sqd;
if (!nnl.isCapacityReached())
{
dist_sqd = Double.MaxValue;
}
else
{
dist_sqd = nnl.getMaxPriority();
}
// 9. max-dist-sqd := minimum of max-dist-sqd and dist-sqd
max_dist_sqd = Math.Min(max_dist_sqd, dist_sqd);
// 10. A nearer point could only lie in further-kd if there were some
// part of further-hr within distance sqrt(max-dist-sqd) of
// target. If this is the case then
HPoint closest = further_hr.closest(target);
if (HPoint.eucdist(closest, target) < Math.Sqrt(max_dist_sqd))
{
// 10.1 if (pivot-target)^2 < dist-sqd then
if (pivot_to_target < dist_sqd)
{
// 10.1.1 nearest := (pivot, range-elt field of kd)
nearest = kd;
// 10.1.2 dist-sqd = (pivot-target)^2
dist_sqd = pivot_to_target;
// add to nnl
if (!kd.deleted)
{
nnl.insert(kd, dist_sqd);
}
// 10.1.3 max-dist-sqd = dist-sqd
// max_dist_sqd = dist_sqd;
if (nnl.isCapacityReached())
{
max_dist_sqd = nnl.getMaxPriority();
}
else
{
max_dist_sqd = Double.MaxValue;
}
}
// 10.2 Recursively call Nearest Neighbor with parameters
// (further-kd, target, further-hr, max-dist_sqd),
// storing results in temp-nearest and temp-dist-sqd
nnbr(further_kd, target, further_hr, max_dist_sqd, lev + 1, K, nnl);
KDNode temp_nearest = (KDNode)nnl.getHighest();
double temp_dist_sqd = nnl.getMaxPriority();
// 10.3 If tmp-dist-sqd < dist-sqd then
if (temp_dist_sqd < dist_sqd)
{
// 10.3.1 nearest := temp_nearest and dist_sqd := temp_dist_sqd
nearest = temp_nearest;
dist_sqd = temp_dist_sqd;
}
}
// SDL: otherwise, current point is nearest
else if (pivot_to_target < max_dist_sqd)
{
nearest = kd;
dist_sqd = pivot_to_target;
}
}
// Method ins translated from 352.ins.c of Gonnet & Baeza-Yates
public static KDNode ins(HPoint key, Object val, KDNode t, int lev, int K)
{
if (t == null)
{
t = new KDNode(key, val);
}
else if (key.equals(t.k))
{
// "re-insert"
if (t.deleted)
{
t.deleted = false;
t.v = val;
}
else
{
throw (new KeyDuplicateException());
}
}
else if (key.coord[lev] > t.k.coord[lev])
{
t.right = ins(key, val, t.right, (lev + 1) % K, K);
}
else
{
t.left = ins(key, val, t.left, (lev + 1) % K, K);
}
return t;
}
private Boolean alreadyIn(KDNode n)
{
KDNode k = findBlockIn(n);
if ((k.getPoint().X == n.getPoint().X) && (k.getPoint().Y == n.getPoint().Y))
{
return true;
}
return false;
}
// Method srch translated from 352.srch.c of Gonnet & Baeza-Yates
public static KDNode srch(Point3 key, KDNode t)
{
for (int lev = 0; t != null; lev = (lev + 1) % 3)
{
if (!t.deleted && key.equals(t.k))
{
return t;
}
else if (key.coord(lev) > t.k.coord(lev))
{
t = t.right;
}
else
{
t = t.left;
}
}
return null;
}
// Try to delete the specified key from the tree. If successful,
// prunes the dead branches off. Returns the new KDNode at this
// location (possibly null). Reports success or failure in
// deleted.
public static KDNode delete(Point3 key, KDNode t, int lev, ref bool deleted)
{
deleted = false;
if (t == null) return null;
if (!t.deleted && key.equals(t.k)) {
t.deleted = true;
deleted = true;
} else if (key.coord(lev) > t.k.coord(lev)) {
t.right = delete(key, t.right, (lev + 1) % 3, ref deleted);
} else {
t.left = delete(key, t.left, (lev + 1) % 3, ref deleted);
}
if (!t.deleted || t.left != null || t.right != null) {
return t;
} else {
return null;
}
}
private void repaint(KDNode n)
{
n.repaint();
if (n.getLeft() != null)
{
repaint(n.getLeft());
}
if (n.getRight() != null)
{
repaint(n.getRight());
}
}
// Method Nearest Neighbor from Andrew Moore's thesis. Numbered
// comments are direct quotes from there. Step "SDL" is added to
// make the algorithm work correctly. NearestNeighborList solution
// courtesy of Bjoern Heckel.
// The nearest neighbor is returned in best, with distance
// sqrt(best_dist_sq). Tmp is a temporary point, passed around
// as an optimization so it doesn't need to be recreated all the
// time. Can be passed in as null by callers.
public static void nnbr(KDNode kd, Point3 target, Rect3 hr,
double max_dist_sqd, int lev,
ref KDNode best, ref double best_dist_sq,
Point3 tmp)
{
// 1. if kd is empty then set dist-sqd to infinity and exit.
if (kd == null)
{
return;
}
if (tmp == null) {
tmp = new Point3();
}
// 2. s := split field of kd
int s = lev % 3;
// 3. pivot := dom-elt field of kd
Point3 pivot = kd.k;
double pivot_to_target = Point3.sqrDist(pivot, target);
// 4. Cut hr into to sub-hyperrectangles left-hr and right-hr.
// The cut plane is through pivot and perpendicular to the s
// dimension.
Rect3 left_hr = hr; // optimize by not cloning
Rect3 right_hr = (Rect3)hr.clone();
left_hr.max.setCoord(s, pivot.coord(s));
right_hr.min.setCoord(s, pivot.coord(s));
// 5. target-in-left := target_s <= pivot_s
bool target_in_left = target.coord(s) < pivot.coord(s);
KDNode nearer_kd;
Rect3 nearer_hr;
KDNode further_kd;
Rect3 further_hr;
// 6. if target-in-left then
// 6.1. nearer-kd := left field of kd and nearer-hr := left-hr
// 6.2. further-kd := right field of kd and further-hr := right-hr
if (target_in_left)
{
nearer_kd = kd.left;
nearer_hr = left_hr;
further_kd = kd.right;
further_hr = right_hr;
}
//
// 7. if not target-in-left then
// 7.1. nearer-kd := right field of kd and nearer-hr := right-hr
// 7.2. further-kd := left field of kd and further-hr := left-hr
else
{
nearer_kd = kd.right;
nearer_hr = right_hr;
further_kd = kd.left;
further_hr = left_hr;
}
right_hr = null;
// 8. Recursively call Nearest Neighbor with paramters
// (nearer-kd, target, nearer-hr, max-dist-sqd), storing the
// results in nearest and dist-sqd
nnbr(nearer_kd, target, nearer_hr, max_dist_sqd, lev + 1, ref best, ref best_dist_sq, tmp);
nearer_hr = null;
KDNode nearest = best;
double dist_sqd;
dist_sqd = best_dist_sq;
// 9. max-dist-sqd := minimum of max-dist-sqd and dist-sqd
max_dist_sqd = Math.Min(max_dist_sqd, dist_sqd);
// 10. A nearer point could only lie in further-kd if there were some
// part of further-hr within distance sqrt(max-dist-sqd) of
// target. If this is the case then
Point3 closest = further_hr.closest(target, tmp);
if (Point3.sqrDist(closest, target) < max_dist_sqd)
{
// 10.1 if (pivot-target)^2 < dist-sqd then
if (pivot_to_target < dist_sqd)
{
// 10.1.1 nearest := (pivot, range-elt field of kd)
nearest = kd;
// 10.1.2 dist-sqd = (pivot-target)^2
dist_sqd = pivot_to_target;
// add to nnl
if (!kd.deleted)
{
best = kd;
best_dist_sq = dist_sqd;
}
max_dist_sqd = best_dist_sq;
}
// 10.2 Recursively call Nearest Neighbor with parameters
// (further-kd, target, further-hr, max-dist_sqd),
// storing results in temp-nearest and temp-dist-sqd
nnbr(further_kd, target, further_hr, max_dist_sqd, lev + 1, ref best, ref best_dist_sq, tmp);
}
}
// zistuje, ci je vrchol n vo vnutri obdlznika from, to
public void findInside(KDNode n, Point from, Point to)
{
if (n.isLeaf)
{
// je vrchol vnutri obdlznika?
if (pointInsideRect(n.getPoint(), from, to))
{
inside.Add(n);
}
return;
}
// ak ma obdlznik s bunkou nejaky prienik vnori sa
if (intersects(n, from, to))
{
findInside(n.getLeft(), from, to);
findInside(n.getRight(), from, to);
} else {
return;
}
}
/**
* Insert a node in a KD-tree. Uses algorithm translated from 352.ins.c of
*
* <PRE>
* @Book{GonnetBaezaYates1991,
* author = {G.H. Gonnet and R. Baeza-Yates},
* title = {Handbook of Algorithms and Data Structures},
* publisher = {Addison-Wesley},
* year = {1991}
* }
* </PRE>
*
* @param key key for KD-tree node
* @param value value at that key
*
* @throws KeySizeException if key.length mismatches K
* @throws KeyDuplicateException if key already in tree
*/
public void insert(double[] key, Object value)
{
if (key.Length != m_K)
{
throw new KeySizeException();
}
else try
{
m_root = KDNode.ins(new HPoint(key), value, m_root, 0, m_K);
}
catch (KeyDuplicateException)
{
//throw e;
}
m_count++;
}
// constructor is used only by class; other methods are static
private KDNode(HPoint key, Object val)
{
k = key;
v = val;
left = null;
right = null;
deleted = false;
}
public static KDNode delete(HPoint key, KDNode t, int lev, int K, ref bool deleted)
{
if (t == null) return null;
if (!t.deleted && key.equals(t.k))
{
t.deleted = true;
deleted = true;
}
else if (key.coord[lev] > t.k.coord[lev])
{
t.right = delete(key, t.right, (lev + 1) % K, K, ref deleted);
}
else
{
t.left = delete(key, t.left, (lev + 1) % K, K, ref deleted);
}
if (!t.deleted || t.left != null || t.right != null)
{
return t;
}
else {
return null;
}
}
// prienik bunky n s obdlznikom
private Boolean intersects(KDNode n, Point from, Point to)
{
// prienik je nie je vtedy, ak je cela bunka nad, pod, vlavo alebo vpravo voci obd.
Point nodeF = n.getFrom();
Point nodeT = n.getTo();
//Console.WriteLine("ma, ci nema prienik?");
//cely hore
if(nodeT.Y < from.Y) {
return false;
}
//cely dole
if (nodeF.Y > to.Y)
{
return false;
}
//cely vlavo
if (nodeF.X > to.X)
{
return false;
}
//cely vpravo
if (nodeT.X < from.X)
{
return false;
}
// Console.WriteLine("ma prienik");
return true;
}
public void notHighlight(KDNode n)
{
if (n == null) return;
n.highlight = false;
n.highlightRed = false;
n.highligthBox = false;
n.highligthIn = false;
notHighlight(n.getLeft());
notHighlight(n.getRight());
}
