PointCluster.cs

   1 using System;\r
   2 using System.Collections.Generic;\r
   3 using System.Text;\r
   4 using Microsoft.DirectX;\r
   5 using Microsoft.DirectX.Direct3D;\r
   6 \r
   7 namespace tso2mqo\r
   8 {\r
   9     public class Array3D<T>\r
  10     {\r
  11         public T[]  data;\r
  12         public int  xx, yy, zz;\r
  13 \r
  14         public Array3D(int x, int y, int z)\r
  15         {\r
  16             data    = new T[x*y*z];\r
  17             xx      = x;\r
  18             yy      = y;\r
  19             zz      = z;\r
  20         }\r
  21 \r
  22         public T Get(int x, int y, int z)\r
  23         {\r
  24             return data[x+y*xx+z*xx*yy];\r
  25         }\r
  26 \r
  27         public void Set(int x, int y, int z, T v)\r
  28         {\r
  29             data[x+y*xx+z*xx*yy]    = v;\r
  30         }\r
  31     }\r
  32 \r
  33     public class PointCluster\r
  34     {\r
  35         public List<Vector3> points;\r
  36         public int                  div;\r
  37         public float                divu;\r
  38         public Array3D<List<int>>   clusters;\r
  39         public Vector3 min = new Vector3(float.MaxValue, float.MaxValue, float.MaxValue);\r
  40         public Vector3 max = new Vector3(float.MinValue, float.MinValue, float.MinValue);\r
  41 \r
  42         public PointCluster(int n)\r
  43         {\r
  44             points = new List<Vector3>(n);\r
  45         }\r
  46 \r
  47         public Vector3 GetPoint(int i)\r
  48         {\r
  49             return points[i];\r
  50         }\r
  51 \r
  52         public void Add(Vector3 p)\r
  53         {\r
  54             points.Add(p);\r
  55             if(p.X < min.X) min.X= p.X; else if(p.X > max.X) max.X= p.X;\r
  56             if(p.Y < min.Y) min.Y= p.Y; else if(p.Y > max.Y) max.Y= p.Y;\r
  57             if(p.Z < min.Z) min.Z= p.Z; else if(p.Z > max.Z) max.Z= p.Z;\r
  58         }\r
  59 \r
  60         public void Add(float x, float y, float z)\r
  61         {\r
  62             Add(new Vector3(x, y, z));\r
  63         }\r
  64 \r
  65         public void Clustering()\r
  66         {\r
  67             float   x   = max.X - min.X;\r
  68             float   y   = max.Y - min.Y;\r
  69             float   z   = max.Z - min.Z;\r
  70             div         = (int)Math.Ceiling((float)Math.Sqrt(Math.Sqrt(points.Count)));\r
  71 \r
  72                  if(x >= y && x >= z)   divu= x / div;\r
  73             else if(y >= x && y >= z)   divu= y / div;\r
  74             else if(z >= x && z >= y)   divu= z / div;\r
  75 \r
  76             clusters    = new Array3D<List<int>>\r
  77                 (Math.Max(1, (int)(x / divu)),\r
  78                  Math.Max(1, (int)(y / divu)),\r
  79                  Math.Max(1, (int)(z / divu)));\r
  80 \r
  81             for(int i= 0, n= points.Count; i < n; ++i)\r
  82                 AddCluster(i, points[i].X, points[i].Y, points[i].Z);\r
  83         }\r
  84 \r
  85         public int Clump(int a, int min, int max)\r
  86         {\r
  87             return a < min ? min : a > max ? max : a;\r
  88         }\r
  89 \r
  90         public int  IndexX(float x) { return Clump((int)((x-min.X) / divu), 0, clusters.xx-1); }\r
  91         public int  IndexY(float y) { return Clump((int)((y-min.Y) / divu), 0, clusters.yy-1); }\r
  92         public int  IndexZ(float z) { return Clump((int)((z-min.Z) / divu), 0, clusters.zz-1); }\r
  93 \r
  94         public void AddCluster(int i, float x, float y, float z)\r
  95         {\r
  96             int         a   = IndexX(x), b= IndexY(y), c= IndexZ(z);\r
  97             List<int>   l;\r
  98             \r
  99             try\r
 100             {\r
 101                 l       = clusters.Get(a, b, c);\r
 102 \r
 103                 if(l == null)\r
 104                     clusters.Set(a, b, c, l= new List<int>());\r
 105 \r
 106                 l.Add(i);\r
 107             } catch(Exception e)\r
 108             {\r
 109                 System.Diagnostics.Debug.WriteLine(e);\r
 110             }\r
 111         }\r
 112 \r
 113         public int NearestIndex(Vector3 p)\r
 114         {\r
 115             int     limit   = 99;\r
 116             int     near    = -1;\r
 117             float   distsq  = float.MaxValue;\r
 118             int     a       = IndexX(p.X);\r
 119             int     b       = IndexY(p.Y);\r
 120             int     c       = IndexZ(p.Z);\r
 121 \r
 122             for(int i= 0; i <= limit; ++i)\r
 123             {\r
 124                 for(int xx= a-i; xx <= a+i; ++xx)\r
 125                 for(int yy= b-i; yy <= b+i; ++yy)\r
 126                 for(int zz= c-i; zz <= c+i; ++zz)\r
 127                 {\r
 128                     if(xx < 0 || xx >= clusters.xx) continue;\r
 129                     if(yy < 0 || yy >= clusters.yy) continue;\r
 130                     if(zz < 0 || zz >= clusters.zz) continue;\r
 131 \r
 132                     List<int>   l   = clusters.Get(xx, yy, zz);\r
 133 \r
 134                     if(l == null)\r
 135                         continue;\r
 136 \r
 137                     foreach(int j in l)\r
 138                     {\r
 139                         float   d   = Vector3.LengthSq(points[j] - p);\r
 140                         if(d >= distsq)\r
 141                             continue;\r
 142 \r
 143                         if(limit == 99)\r
 144                             limit   = i + 1;\r
 145                         distsq      = d;\r
 146                         near        = j;\r
 147                     }\r
 148                 }\r
 149             }\r
 150             return near;\r
 151         }\r
 152     }\r
 153 }\r