PointCluster.cs

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