PointCluster.cs

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