public void VerifyRec(int i0, int Len, BoundingBoxCode cd, int idx)
{
TreeNode[] tree = InitTree();
TreeNode nt = tree[idx];
for (int n = 0; n < Codes.Length; n++)
{
bool innen = cd.IsInside(this.Codes[n]);
bool innen2 = n >= i0 && n < (i0 + Len);
if (innen != innen2)
{
throw new ApplicationException();
}
}
if (nt.Left >= 0)
{
int i0L = i0;
int LenL = nt.iMid - i0L;
VerifyRec(i0L, LenL, cd.GetSubBox(false), nt.Left);
}
if (nt.Right >= 0)
{
int i0R = nt.iMid;
int LenR = i0 + Len - i0R;
VerifyRec(i0R, LenR, cd.GetSubBox(true), nt.Right);
}
}
/// <summary>
/// true, if the box notated by <paramref name="code"/> is fully contained in this box
/// </summary>
public bool IsInside(BoundingBoxCode code)
{
if (code.SignificantBits < this.SignificantBits)
{
return(false);
}
return(IsInside(code.Branch));
}
/// <summary>
/// computes the leave bounding-box of a geometric binary tree
/// </summary>
/// <param name="subBB">
/// on exit, the bounding box associated with the branch code <paramref name="bbCode"/>;
/// The output is not returned (via return value) to avoid heap allocation.
/// </param>
/// <param name="bbCode">
/// bounding box code
/// </param>
public void SubBoxFromCode(BoundingBox subBB, BoundingBoxCode bbCode)
{
if (subBB.D != this.D)
{
throw new ArgumentException("must have the same dimension as this bounding box.", "subBB");
}
int _D = D;
uint TreeDepth = bbCode.SignificantBits;
if (TreeDepth > 32)
{
throw new ArgumentException("tree Depth is limited to 32.");
}
Array.Copy(this.Max, subBB.Max, _D);
Array.Copy(this.Min, subBB.Min, _D);
uint cd = bbCode.Branch.Code;
uint mxx = 0x80000000;
//for (int i = 0; i < 30; i++)
// mxx *= 2;
int cur_d = 0;
for (int i = (int)TreeDepth - 1; i >= 0; i--)
{
double mean = (subBB.Min[cur_d] + subBB.Max[cur_d]) * 0.5;
// split bb;
//uint kaes = cd / mxx;
//switch (kaes) {
// case 0: subBB.Max[cur_d] = mean; break;
// case 1: subBB.Min[cur_d] = mean; break;
// default: throw new ApplicationException("error in algorithm");
//}
if ((cd & mxx) != 0)
{
subBB.Min[cur_d] = mean;
}
else
{
subBB.Max[cur_d] = mean;
}
// increment
mxx = mxx >> 1;
//mxx = Math.Max(1, mxx / 2);
//cd = cd % mxx;
cur_d++;
if (cur_d >= _D)
{
cur_d = 0;
}
}
}
public void Test(int i0, int iMid, PointLocalization owner, BoundingBoxCode b)
{
for (int i = 0; i < owner.Codes.Length; i++)
{
bool innen = b.IsInside(owner.Codes[i]);
bool innen2 = i >= i0 && i < iMid;
if (innen != innen2)
{
throw new ApplicationException();
}
}
}
/// <summary>
/// gets the code of either the 'left' or the 'right' sub-box of this box
/// </summary>
/// <param name="leftOrRight">
/// if false, the 'left' branch, i.e. the box with lower coordinate values;<br/>
/// if false, the 'right' branch, i.e. the box with higher coordinate values;<br/>
/// </param>
/// <returns></returns>
public BoundingBoxCode GetSubBox(bool leftOrRight)
{
if (this.SignificantBits >= 32)
{
throw new ApplicationException("maximum tree depth reached.");
}
BoundingBoxCode Ret = this;
Ret.SignificantBits++;
if (leftOrRight)
{
// right branch
uint upper = 0x80000000 >> ((int)this.SignificantBits);
Ret.Branch.Code |= upper;
}
return(Ret);
}
void FindNearPointsRecursice(ICollection <int> IndicesIntoPoints, double epsPow2, double[] pt, int i0, int Len, int _D, BoundingBoxCode BoxCode, uint shifti, ref uint MaxDepth, bool forceTst, int treeNodeIdx)
{
TreeNode[] tree = __tree;
uint Depth = BoxCode.SignificantBits;
{
/*/ test and debug:
* VerifyTree l = vt.Get(BoxCode, (int)Depth);
* if (l != null) {
* if (l.I0 != i0)
* throw new ApplicationException();
* if (l.Len != Len)
* throw new ApplicationException();
* }
*
* PointsBB.SubBoxFromCode(helper, BoxCode, Depth);
* for (int n = 0; n < Codes.Length; n++) {
* double[] _pt = new double[Points.GetLength(1)];
* for (int d = 0; d < pt.Length; d++) _pt[d] = Points[n, d];
*
* bool loc = helper.IsInsideStrict(_pt);
*
* if (n >= i0 && n < (i0 + Len)) {
* if (!loc)
* throw new ApplicationException("error in alg");
* } else {
* if (loc)
* throw new ApplicationException("error in alg 2");
* }
* }
* // end */
if (Len <= 0)
{
// no points in branch => terminate recursion
return;
}
PointsBB.SubBoxFromCode(helper, BoxCode);
//helper.ExtendByFactor(0.01);
double di = helper.Distance(pt);
if (di * di > epsPow2)
{
//if (helper.IsInside(pt))
// throw new ApplicationException("ohne Worte");
//for (int n = i0; n < (i0 + Len); n++) {
// double distPow2 = 0;
// for (int d = _D - 1; d >= 0; d--) {
// double delta = pt[d] - this.Points[n, d];
// distPow2 += delta * delta;
// }
// if (distPow2 <= epsPow2)
// throw new ApplicationException("ohne Worte");
//}
return; // no interesting points in box => terminate recursion
}
}
MaxDepth = Math.Max(MaxDepth, Depth);
if (Len <= 2 || Depth == 32 || forceTst)
{
// only a few points remaining, or maximum tree depth reached - test all of them
for (int n = i0; n < (i0 + Len); n++)
{
double distPow2 = 0;
for (int d = _D - 1; d >= 0; d--)
{
double delta = pt[d] - this.Points[n, d];
distPow2 += delta * delta;
}
if (distPow2 <= epsPow2)
{
IndicesIntoPoints.Add(n);
}
}
}
else
{
// do recursion;
BoundingBoxCode BoxB = BoxCode; BoxB.Branch.Code += shifti;
shifti = shifti >> 1;
Depth++;
BoxB.SignificantBits++;
BoxCode.SignificantBits++;
int idxMid = tree[treeNodeIdx].iMid;
int LenLeft = idxMid - i0;
int LenRigt = Len + i0 - idxMid;
bool forceLeft = (LenLeft == Len);
bool forceRigt = (LenRigt == Len);
FindNearPointsRecursice(IndicesIntoPoints, epsPow2, pt, i0, LenLeft, _D, BoxCode, shifti, ref MaxDepth, forceLeft, tree[treeNodeIdx].Left);
FindNearPointsRecursice(IndicesIntoPoints, epsPow2, pt, idxMid, LenRigt, _D, BoxB, shifti, ref MaxDepth, forceRigt, tree[treeNodeIdx].Right);
}
}
void FindNextPointsRecursice(Vector pt, ref double MinDistSoFarPow2, ref int iMin,
int i0, int Len, int _D, BoundingBoxCode BoxCode, uint shifti, ref uint MaxDepth, bool forceTst, int treeNodeIdx)
{
TreeNode[] tree = __tree;
uint Depth = BoxCode.SignificantBits;
{
if (Len <= 0)
{
// no points in branch => terminate recursion
return;
}
var __helper = new BoundingBox(_D);
PointsBB.SubBoxFromCode(__helper, BoxCode);
//helper.ExtendByFactor(0.01);
double di = __helper.Distance(pt);
if (di * di > MinDistSoFarPow2)
{
return; // no interesting points in box => terminate recursion
}
}
MaxDepth = Math.Max(MaxDepth, Depth);
if (Len <= 2 || Depth == 32 || forceTst)
{
// only a few points remaining, or maximum tree depth reached - test all of them
for (int n = i0; n < (i0 + Len); n++)
{
double distPow2 = 0;
for (int d = _D - 1; d >= 0; d--)
{
double delta = pt[d] - this.Points[n, d];
distPow2 += delta * delta;
}
if (distPow2 < MinDistSoFarPow2)
{
MinDistSoFarPow2 = distPow2;
iMin = n;
}
}
}
else
{
// do recursion;
BoundingBoxCode BoxB = BoxCode; BoxB.Branch.Code += shifti;
shifti = shifti >> 1;
Depth++;
BoxB.SignificantBits++;
BoxCode.SignificantBits++;
int idxMid = tree[treeNodeIdx].iMid;
int LenLeft = idxMid - i0;
int LenRigt = Len + i0 - idxMid;
bool forceLeft = (LenLeft == Len);
bool forceRigt = (LenRigt == Len);
FindNextPointsRecursice(pt, ref MinDistSoFarPow2, ref iMin, i0, LenLeft, _D, BoxCode, shifti, ref MaxDepth, forceLeft, tree[treeNodeIdx].Left);
FindNextPointsRecursice(pt, ref MinDistSoFarPow2, ref iMin, idxMid, LenRigt, _D, BoxB, shifti, ref MaxDepth, forceRigt, tree[treeNodeIdx].Right);
}
}
