/// <summary>
/// Expert's constructor.
/// </summary>
/// <param name="_points"></param>
/// <param name="BB">predefined bounding box, must contain all points in <paramref name="_points"/></param>
/// <param name="Permutation">
/// output; permutation of <paramref name="_points"/> by sorting in tree; if null, an internal buffer is allocated
/// </param>
public PointLocalization(MultidimensionalArray _points, BoundingBox BB, int[] Permutation)
{
using (new FuncTrace()) {
int D = _points.GetLength(1);
int N = _points.GetLength(0);
PointsBB = BB;
// find code
// =========
double[] pt = new double[D];
GeomBinTreeBranchCode[] _Codes = new GeomBinTreeBranchCode[N];
for (int n = 0; n < N; n++)
{
_points.GetRow(n, pt);
_Codes[n] = GeomBinTreeBranchCode.CreateFormPoint(PointsBB, pt);
}
// sort
// ====
int[] IndexIntoQuadNodes;
if (Permutation == null)
{
IndexIntoQuadNodes = new int[N]; // we only sort an index list into the arrays 'QuadNodes', 'quadweights' and 'Locations'
}
else
{
if (Permutation.Length != N)
{
throw new ArgumentException("false Länge", "Permutation");
}
IndexIntoQuadNodes = Permutation;
}
for (int i = 0; i < N; i++)
{
IndexIntoQuadNodes[i] = i;
}
Array.Sort <int>(IndexIntoQuadNodes, delegate(int a, int b) {
GeomBinTreeBranchCode _a = _Codes[a];
GeomBinTreeBranchCode _b = _Codes[b];
return(_a.CompareTo(_b));
}); // radix sort would be better here, because it would have linear runtime
// store sorted
// ============
this.Codes = new GeomBinTreeBranchCode[N];
this.Points = MultidimensionalArray.Create(N, D);
for (int n = 0; n < N; n++)
{
int it = IndexIntoQuadNodes[n];
for (int d = 0; d < D; d++)
{
this.Points[n, d] = _points[it, d];
}
this.Codes[n] = _Codes[it];
}
}
}
/// <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="code"/>;
/// The output is not returned (via return value) to avoid heap allocation.
/// </param>
/// <param name="code">
/// branch code
/// </param>
public void LeaveBoxFromCode(BoundingBox subBB, GeomBinTreeBranchCode code)
{
BoundingBoxCode cd;
cd.Branch = code;
cd.SignificantBits = 32;
SubBoxFromCode(subBB, cd);
}
/// <summary>
///
/// </summary>
/// <param name="branchCode">the branch code; only the bits 32 to 32-<paramref name="BranchDepth"/> are taken into account</param>
/// <param name="BranchDepth">num</param>
/// <param name="i0"></param>
/// <param name="Len"></param>
public void GetPointsInBranch(GeomBinTreeBranchCode branchCode, int BranchDepth, out int i0, out int Len)
{
if (BranchDepth < 0 || BranchDepth > 32)
{
throw new ArgumentException("must be between 0 (including) and 32 (including).", "BranchDepth");
}
{
// ancient implementation
//i0 = 0;
//Len = Codes.Length;
//GetPointsInBranchRecursive(branchCode, BranchDepth, ref i0, ref Len, 0x80000000, 0);
}
{
// faster ????
uint mask = 0;
uint m = 0x80000000;
for (int i = 0; i < BranchDepth; i++)
{
mask += m >> i;
}
GeomBinTreeBranchCode loCode;
loCode.Code = branchCode.Code & mask;
GeomBinTreeBranchCode hiCode;
hiCode.Code = branchCode.Code | (~mask);
//int _i0, _Len;
if (hiCode < this.Codes[0] || loCode > this.Codes[this.Codes.Length - 1])
{
i0 = 0;
Len = 0;
}
else
{
int loi = Array.BinarySearch(this.Codes, loCode); // use the .NET binary search ...
if (loi < 0)
{
loi = ~loi;
}
int hii = Array.BinarySearch(this.Codes, hiCode); // use the .NET binary search ...
if (hii < 0)
{
hii = ~hii;
hii--;
}
i0 = loi;
Len = hii - loi + 1;
}
}
Test_GetPointsInBranch(i0, Len, branchCode, BranchDepth);
}
/// <summary>
/// old implementation of <see cref="GetPointsInBranch"/>
/// </summary>
void GetPointsInBranchRecursive(GeomBinTreeBranchCode branchCode, int BranchDepth, ref int i0, ref int Len, uint mask, int idxIntoTree)
{
if (BranchDepth <= 0)
{
return;
}
BranchDepth--;
TreeNode[] tree = InitTree();
int iMid = tree[idxIntoTree].iMid;
//if (iMid < 0) {
// iMid = Array.BinarySearch<GeomBinGeomTreeCode>(this.Codes, 0, this.Codes.Length, ???);
// if (iMid < 0) iMid = ~iMid;
//}
if ((branchCode.Code & mask) == 0)
{
// left
Len = iMid - i0;
if (Len > 0)
{
GetPointsInBranchRecursive(branchCode, BranchDepth, ref i0, ref Len, mask >> 1, tree[idxIntoTree].Left);
}
}
else
{
// right
Len = i0 + Len - iMid;
i0 = iMid;
if (Len > 0)
{
GetPointsInBranchRecursive(branchCode, BranchDepth, ref i0, ref Len, mask >> 1, tree[idxIntoTree].Right);
}
}
}
///// <summary>
/////
///// </summary>
///// <param name="container"></param>
///// <param name="bbout"></param>
//public void GetBox(BoundingBox container, BoundingBox bbout) {
// container.SubBoxFromCode
//}
/// <summary>
/// true, if the branch notated by <paramref name="code"/> is contained in this box
/// </summary>
public bool IsInside(GeomBinTreeBranchCode code)
{
uint mask = GetBitMask();
return((this.Branch.Code & mask) == (code.Code & mask));
}
/*
* static int MyBinarySearch(GeomBinTreeBranchCode[] array, int i0, int Length, GeomBinTreeBranchCode searchValue) {
* if(Length <= 0)
* return -1;
*
* if (array[i0] >= searchValue)
* return i0;
* if (array[i0 + Length - 1] < searchValue)
* return i0 + Length;
*
* if (Length >= 2) {
* int ifnd = int.MinValue;
* MyBinarySearchRec(array, i0, Length, i0, Length, ref searchValue, out ifnd);
* return ifnd;
* } else {
* if (array[i0] >= searchValue)
* return i0;
* else
* return i0 + 1;
* }
* }
*
* static void MyBinarySearchRec(GeomBinTreeBranchCode[] array, int i0, int Length, int _i0, int _Len, ref GeomBinTreeBranchCode searchValue, out int ifnd) {
* if( Length < 2)
* throw new ApplicationException();
* int iMid = i0 + Length / 2;
*
* if (array[iMid] >= searchValue && array[iMid - 1] < searchValue) {
* ifnd = iMid;
* } else {
* if (array[iMid] >= searchValue) {
* MyBinarySearchRec(array, i0, Length, iMid, i0 + _Len - iMid, ref searchValue, out ifnd);
* } else {
* MyBinarySearchRec(array, i0, Length, i0, iMid - i0, ref searchValue, out ifnd);
* }
*
* }
*
* //if( iMid == i0 && array[iMid] >= searchValue && ) {
* // return i0;
* //}
* //if(
* }*/
public void InitRecursive(GeomBinTreeBranchCode BoxCode, uint shifti, PointLocalization owner, int i0, int Len, uint RecDepth)
{
GeomBinTreeBranchCode BoxB = BoxCode; BoxB.Code |= shifti;
int __iMid;
for (__iMid = i0; __iMid < (i0 + Len); __iMid++)
{
if (owner.Codes[__iMid].Code >= BoxB.Code)
{
break;
}
// if there is a performance problem, this linear search should be exchanged by a binary one
}
this.iMid = __iMid;
//{
// GeomBinTreeBranchCode search = BoxB;
// search.Code -= 1;
// int iMid2 = MyBinarySearch(owner.Codes, 0, owner.Codes.Length, search);
// if (iMid2 != iMid)
// throw new ApplicationException();
//}
//if (iMid < 0) iMid = ~iMid;
int LenLeft = iMid - i0;
int LenRigt = Len + i0 - iMid;
{
/*
* // test left
* {
* BoundingBoxCode cdLeft;
* cdLeft.Branch = BoxCode;
* cdLeft.SignificantBits = RecDepth + 1;
* Test(i0, iMid, owner, cdLeft);
* }
*
* // test right
* {
* BoundingBoxCode cdRight;
* cdRight.Branch = BoxB;
* cdRight.SignificantBits = RecDepth + 1;
* Test(iMid, i0 + Len, owner, cdRight);
* }
*/
}
if (shifti == 1)
{
return; // reached max. supported tree depth
}
shifti = shifti >> 1;
if (LenLeft > 0)
{
Left = new TreeNodeTmp();
Left.InitRecursive(BoxCode, shifti, owner, i0, LenLeft, RecDepth + 1);
}
if (LenRigt > 0)
{
Right = new TreeNodeTmp();
Right.InitRecursive(BoxB, shifti, owner, iMid, LenRigt, RecDepth + 1);
}
}
void Test_GetPointsInBranch(int i0, int Len, GeomBinTreeBranchCode branchCode, int BranchDepth)
{
// logischer test
{
/*
* uint mask = 0;
* uint m = 0x80000000;
* for (int i = 0; i < BranchDepth; i++) {
* mask += m >> i;
* }
*
*
* GeomBinTreeBranchCode loCode;
* loCode.Code = branchCode.Code & mask;
*
* GeomBinTreeBranchCode hiCode;
* hiCode.Code = branchCode.Code | (~mask);
*
*
* int L = Codes.Length;
* for (int l = 0; l < L; l++) {
* var Code = Codes[l].Code;
*
* bool inside = (l >= i0 && l < (i0 + Len));
* bool gt = (Code >= loCode.Code) && (Code <= hiCode.Code);
*
* if (inside != gt)
* Console.WriteLine("schas");
* }
*
*
* /*
* int InsideErr = 0, OutsideErr = 0;
*
* int L = Codes.Length;
* for (int l = 0; l < L; l++) {
* var Code = Codes[l].Code;
*
* if ((Code & mask) == (branchCode.Code & mask)) {
* // inside
* if (l < i0 || l >= (i0 + Len))
* InsideErr++;
* } else {
* if (l >= i0 && l < (i0 + Len))
* OutsideErr++;
* }
* }
*
* if (InsideErr != 0 || OutsideErr != 0)
* throw new ApplicationException("internal error - should not happen.");
*/
}
// geometrischer test
{
BoundingBox bb = new BoundingBox(this.Points.GetLength(1));
BoundingBoxCode bbcode;
bbcode.SignificantBits = (uint)BranchDepth;
bbcode.Branch = branchCode;
this.PointsBB.SubBoxFromCode(bb, bbcode);
int D = this.Points.GetLength(1);
double[] pt = new double[D];
for (int i = i0; i < (i0 + Len); i++)
{
for (int d = 0; d < D; d++)
{
pt[d] = this.Points[i, d];
}
if (!bb.Contains(pt))
{
throw new ApplicationException("test failed.");
}
}
}
}
