--- /dev/null
+using System;\r
+using System.Collections.Generic;\r
+using Microsoft.DirectX;\r
+using Microsoft.DirectX.Direct3D;\r
+\r
+namespace tso2mqo\r
+{\r
+ public class Cell : LinkedList<int>\r
+ {\r
+ /// nodeをプッシュ\r
+ public bool Push(LinkedListNode<int> node)\r
+ {\r
+ if (node == null)\r
+ return false; // 無効オブジェクトは登録しない\r
+ if (node.List == this)\r
+ return false; // 2重登録チェック\r
+ AddFirst(node);\r
+ return true;\r
+ }\r
+ }\r
+\r
+ /// 線形8分木空間管理クラス\r
+ public class LinerOctreeManager\r
+ {\r
+ public const int MaxLevel = 7;\r
+\r
+ /// 線形空間ポインタ配列\r
+ public Cell[] cells;\r
+ /// べき乗数値配列\r
+ protected UInt32[] pow = new UInt32[MaxLevel + 1];\r
+ /// 領域の幅\r
+ protected Vector3 size;\r
+ /// 領域の最小値\r
+ protected Vector3 rgn_min;\r
+ /// 領域の最大値\r
+ protected Vector3 rgn_max;\r
+ /// 最小領域の辺の長さ\r
+ protected Vector3 unit;\r
+ /// 空間の数\r
+ public UInt32 ncell;\r
+ /// 最下位レベル\r
+ protected int level;\r
+\r
+ /// 線形8分木配列を構築する\r
+ public bool Init(int level, ref Vector3 min, ref Vector3 max)\r
+ {\r
+ // 各レベルでの空間数を算出\r
+ pow[0] = 1;\r
+ for (int i = 1; i < MaxLevel + 1; i++)\r
+ pow[i] = pow[i - 1] * 8;\r
+\r
+ // levelレベル（0基点）の配列作成\r
+ ncell = (pow[level + 1] - 1) / 3;\r
+ cells = new Cell[ncell];\r
+\r
+ // 領域を登録\r
+ rgn_min = min;\r
+ rgn_max = max;\r
+ size = rgn_max - rgn_min;\r
+ float div = (float)(1 << level);\r
+ Console.WriteLine("div {0}", div);\r
+ unit = size;\r
+ unit.X /= div;\r
+ unit.Y /= div;\r
+ unit.Z /= div;\r
+\r
+ this.level = level;\r
+\r
+ return true;\r
+ }\r
+\r
+ /// オブジェクトを登録する\r
+ public bool Regist(ref Vector3 min, ref Vector3 max, LinkedListNode<int> node)\r
+ {\r
+ // オブジェクトの境界範囲から登録モートン番号を算出\r
+ UInt32 num = GetMortonNumber(ref min, ref max);\r
+ if (num < ncell)\r
+ {\r
+ // 空間が無い場合は新規作成\r
+ if (cells[num] == null)\r
+ CreateNewCell(num);\r
+ return cells[num].Push(node);\r
+ }\r
+ return false; // 登録失敗\r
+ }\r
+\r
+ /// 空間を生成\r
+ public bool CreateNewCell(UInt32 num)\r
+ {\r
+ // 引数の要素番号\r
+ while (num < ncell && cells[num] == null)\r
+ {\r
+ // 指定の要素番号に空間を新規作成\r
+ cells[num] = new Cell();\r
+\r
+ // 親空間にジャンプ\r
+ num = (num - 1) >> 2;\r
+ }\r
+ return true;\r
+ }\r
+\r
+ /// 座標から空間番号を算出\r
+ public UInt32 GetMortonNumber(ref Vector3 min, ref Vector3 max)\r
+ {\r
+ // 最小レベルにおける各軸位置を算出\r
+ UInt32 LT = GetPointElem(ref min);\r
+ UInt32 RB = GetPointElem(ref max);\r
+\r
+ // 空間番号を引き算して\r
+ // 最上位区切りから所属レベルを算出\r
+ UInt32 Def = RB ^ LT;\r
+ int HiLevel = 1;\r
+ int i;\r
+ for (i = 0; i < level; i++)\r
+ {\r
+ UInt32 Check = (Def >> (i * 3)) & 0x7;\r
+ if (Check != 0)\r
+ HiLevel = i + 1;\r
+ }\r
+ UInt32 SpaceNum = RB >> (HiLevel * 3);\r
+ UInt32 AddNum = (pow[level - HiLevel] - 1) / 7;\r
+ SpaceNum += AddNum;\r
+\r
+ if (SpaceNum > ncell)\r
+ return UInt32.MaxValue;\r
+\r
+ return SpaceNum;\r
+ }\r
+\r
+ /// ビット分割関数\r
+ public static UInt32 BitSeparateFor3D(Byte n)\r
+ {\r
+ UInt32 s = n;\r
+ s = (s | s << 8) & 0x0000f00f;\r
+ s = (s | s << 4) & 0x000c30c3;\r
+ s = (s | s << 2) & 0x00249249;\r
+ return s;\r
+ }\r
+\r
+ /// 3Dモートン空間番号算出関数\r
+ public static UInt32 Get3DMortonNumber(Byte x, Byte y, Byte z)\r
+ {\r
+ return BitSeparateFor3D(x) | BitSeparateFor3D(y) << 1 | BitSeparateFor3D(z) << 2;\r
+ }\r
+\r
+ /// 座標→線形8分木要素番号変換関数\r
+ public UInt32 GetPointElem(ref Vector3 p)\r
+ {\r
+ return Get3DMortonNumber(\r
+ (Byte)((p.X - rgn_min.X) / unit.X),\r
+ (Byte)((p.Y - rgn_min.Y) / unit.Y),\r
+ (Byte)((p.Z - rgn_min.Z) / unit.Z)\r
+ );\r
+ }\r
+ }\r
+}\r
\r
namespace tso2mqo\r
{\r
- public class Array3D<T>\r
- {\r
- public T[] data;\r
- public int xx, yy, zz;\r
-\r
- public Array3D(int x, int y, int z)\r
- {\r
- data = new T[x*y*z];\r
- xx = x;\r
- yy = y;\r
- zz = z;\r
- }\r
-\r
- public T Get(int x, int y, int z)\r
- {\r
- return data[x+y*xx+z*xx*yy];\r
- }\r
-\r
- public void Set(int x, int y, int z, T v)\r
- {\r
- data[x+y*xx+z*xx*yy] = v;\r
- }\r
- }\r
-\r
public class PointCluster\r
{\r
+ LinerOctreeManager octree = new LinerOctreeManager();\r
public List<Vector3> points;\r
- public int div;\r
- public float divu;\r
- public Array3D<List<int>> clusters;\r
public Vector3 min = new Vector3(float.MaxValue, float.MaxValue, float.MaxValue);\r
public Vector3 max = new Vector3(float.MinValue, float.MinValue, float.MinValue);\r
\r
public void Add(Vector3 p)\r
{\r
points.Add(p);\r
- if(p.X < min.X) min.X= p.X; else if(p.X > max.X) max.X= p.X;\r
- if(p.Y < min.Y) min.Y= p.Y; else if(p.Y > max.Y) max.Y= p.Y;\r
- if(p.Z < min.Z) min.Z= p.Z; else if(p.Z > max.Z) max.Z= p.Z;\r
- }\r
-\r
- public void Add(float x, float y, float z)\r
- {\r
- Add(new Vector3(x, y, z));\r
+ if (p.X < min.X) min.X = p.X; else if (p.X > max.X) max.X = p.X;\r
+ if (p.Y < min.Y) min.Y = p.Y; else if (p.Y > max.Y) max.Y = p.Y;\r
+ if (p.Z < min.Z) min.Z = p.Z; else if (p.Z > max.Z) max.Z = p.Z;\r
}\r
\r
public void Clustering()\r
{\r
- float x = max.X - min.X;\r
- float y = max.Y - min.Y;\r
- float z = max.Z - min.Z;\r
- div = (int)Math.Ceiling((float)Math.Sqrt(Math.Sqrt(points.Count)));\r
-\r
- if(x >= y && x >= z) divu= x / div;\r
- else if(y >= x && y >= z) divu= y / div;\r
- else if(z >= x && z >= y) divu= z / div;\r
-\r
- clusters = new Array3D<List<int>>\r
- (Math.Max(1, (int)(x / divu)),\r
- Math.Max(1, (int)(y / divu)),\r
- Math.Max(1, (int)(z / divu)));\r
-\r
- for(int i= 0, n= points.Count; i < n; ++i)\r
- AddCluster(i, points[i].X, points[i].Y, points[i].Z);\r
- }\r
-\r
- public int Clump(int a, int min, int max)\r
- {\r
- return a < min ? min : a > max ? max : a;\r
- }\r
-\r
- public int IndexX(float x) { return Clump((int)((x-min.X) / divu), 0, clusters.xx-1); }\r
- public int IndexY(float y) { return Clump((int)((y-min.Y) / divu), 0, clusters.yy-1); }\r
- public int IndexZ(float z) { return Clump((int)((z-min.Z) / divu), 0, clusters.zz-1); }\r
-\r
- public void AddCluster(int i, float x, float y, float z)\r
- {\r
- int a = IndexX(x), b= IndexY(y), c= IndexZ(z);\r
- List<int> l;\r
- \r
- try\r
+ octree.Init(5, ref min, ref max);\r
+ for (int i = 0; i < points.Count; i++)\r
{\r
- l = clusters.Get(a, b, c);\r
-\r
- if(l == null)\r
- clusters.Set(a, b, c, l= new List<int>());\r
-\r
- l.Add(i);\r
- } catch(Exception e)\r
- {\r
- System.Diagnostics.Debug.WriteLine(e);\r
+ Vector3 p = points[i];\r
+ if (!octree.Regist(ref p, ref p, new LinkedListNode<int>(i)))\r
+ {\r
+ Console.WriteLine("failed to regist node");\r
+ }\r
}\r
}\r
\r
public int NearestIndex(Vector3 p)\r
{\r
- int limit = 99;\r
- int near = -1;\r
- float distsq = float.MaxValue;\r
- int a = IndexX(p.X);\r
- int b = IndexY(p.Y);\r
- int c = IndexZ(p.Z);\r
-\r
- for(int i= 0; i <= limit; ++i)\r
+ int near = -1;\r
+ float nearest_lensq = float.MaxValue;\r
+ UInt32 num = octree.GetMortonNumber(ref p, ref p);\r
+ if (num < octree.ncell)\r
{\r
- for(int xx= a-i; xx <= a+i; ++xx)\r
- for(int yy= b-i; yy <= b+i; ++yy)\r
- for(int zz= c-i; zz <= c+i; ++zz)\r
+ Cell cell = octree.cells[num];\r
+ //Console.WriteLine("cell {0} {1}", num, cell.Count);\r
+ foreach (int i in cell)\r
{\r
- if(xx < 0 || xx >= clusters.xx) continue;\r
- if(yy < 0 || yy >= clusters.yy) continue;\r
- if(zz < 0 || zz >= clusters.zz) continue;\r
-\r
- List<int> l = clusters.Get(xx, yy, zz);\r
-\r
- if(l == null)\r
- continue;\r
-\r
- foreach(int j in l)\r
+ float lensq = Vector3.LengthSq(points[i] - p);\r
+ if (lensq < nearest_lensq)\r
{\r
- float d = Vector3.LengthSq(points[j] - p);\r
- if(d >= distsq)\r
- continue;\r
-\r
- if(limit == 99)\r
- limit = i + 1;\r
- distsq = d;\r
- near = j;\r
+ nearest_lensq = lensq;\r
+ near = i;\r
}\r
}\r
}\r
+ else\r
+ Console.WriteLine("near index not found");\r
return near;\r
}\r
}\r
OSDN Git Service
