Удалить близкие точки

struct Point {
 float x, y, z;
};
 
std::vector<Point> points;

#include <list>
#include <vector>
#include <unordered_map>
#include <limits>
#include <random>
#include <iostream>
 
struct Point
{
  float x, y, z;
 
  friend float distance2(const Point &a, const Point &b)
  { 
    float dx = a.x - b.x, dy = a.y - b.y, dz = a.z - b.z;
    return dx * dx + dy * dy + dz * dz; 
  }
};
 
struct MeshKey
{
  int ix, iy, iz;
 
  friend MeshKey operator +(const MeshKey &lhs, const MeshKey &rhs)
    { return { lhs.ix + rhs.ix, lhs.iy + rhs.iy, lhs.iz + rhs.iz }; }
 
  friend bool operator ==(const MeshKey &, const MeshKey &) = default;
 
  struct Hash
  {
    std::size_t operator ()(const MeshKey &key) const
      {  return ((std::hash<int>()(key.ix) << 1) ^ std::hash<int>()(key.iy) << 1) ^ std::hash<int>()(key.iz); }
  };
};
 
using Points = std::list<Point>;
using Mesh = std::unordered_map<MeshKey, std::vector<Points::iterator>, MeshKey::Hash>;
using MeshNode = Mesh::value_type;
 
void populate(Mesh &mesh, float step, Points &points)
{
  for (auto it = points.begin(); it != points.end(); ++it)
    mesh[{ (int) (it->x / step), (int) (it->y / step), (int) (it->z / step) }].push_back(it);
}
 
const MeshKey HALF_ADJ_DXYZ[] = 
{
  { -1, +1,  0 }, {  0, +1,  0 }, { +1, +1,  0 }, 
                                  { +1,  0,  0 },
 
  { -1, +1, +1 }, {  0, +1, +1 }, { +1, +1, +1 }, 
  { -1,  0, +1 },                 { +1,  0, +1 }, 
  { -1, -1, +1 }, {  0, -1, +1 }, { +1, -1, +1 }
};
 
std::size_t count_violations(const Mesh &mesh, const MeshKey &key, const Point &point, float min_dist2,
                             std::size_t n_violations = 0,
                             std::size_t n_limit = std::numeric_limits<std::size_t>::max())
{
  for (const MeshKey &dxyz : HALF_ADJ_DXYZ)
  {
    auto it_adj_node = mesh.find(key + dxyz);
    if (it_adj_node == mesh.end())
      continue;
 
    for (auto it_adj_point : it_adj_node->second)
    {
      float dist2 = distance2(point, *it_adj_point);
      if (dist2 < min_dist2)
        if (++n_violations > n_limit)
          return n_violations;
    }
  }
 
  return n_violations;
}
 
std::size_t count_violations(const Mesh &mesh, float min_dist)
{
  float min_dist2 = min_dist * min_dist;
 
  std::size_t n_violations = 0;
 
  for (const MeshNode &node : mesh)
    for (auto it_point : node.second)
      n_violations = count_violations(mesh, node.first, *it_point, min_dist2, n_violations);
 
  return n_violations;
}
 
void cleanup(Points &points, Mesh &mesh, float min_dist)
{
  float min_dist2 = min_dist * min_dist;
 
  for (auto it_node = mesh.begin(); it_node != mesh.end(); )
  {
    for (auto itit_point = it_node->second.begin(); itit_point != it_node->second.end(); )
    {
      auto it_point = *itit_point;
      if (count_violations(mesh, it_node->first, *it_point, min_dist2, 0, 0) > 0)
      {
        points.erase(it_point);
        itit_point = it_node->second.erase(itit_point);
      }
      else
        ++itit_point;
    }
 
    if (it_node->second.empty())
      it_node = mesh.erase(it_node);
    else
      ++it_node;
  }
}
 
int main()
{
  Points points(1000000);
 
  std::random_device rd;
  std::mt19937 gen(rd());
  std::uniform_real_distribution<float> dis(0.f, 1.f);
  for (Point &point : points)
    point = { dis(gen), dis(gen), dis(gen) };
 
  const float MIN_DIST = 1.e-2f;
 
  Mesh mesh;
  populate(mesh, MIN_DIST, points);
 
  std::size_t n_initial = points.size();
  std::cout << "Initial points: " << n_initial << std::endl;
  std::cout << "Violating pairs: " << count_violations(mesh, MIN_DIST) << std::endl;
 
  cleanup(points, mesh, MIN_DIST);
 
  std::cout << "Points after cleanup: " << points.size() << std::endl;
  std::cout << "Removed points: " << n_initial - points.size() << std::endl; 
 
  std::cout << "Violating pairs after cleanup: " << count_violations(mesh, MIN_DIST) << std::endl;
}