PointCluster.cs

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