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

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

#define SQR(x) ((x) * (x)) // x²
 
// расстояние между точками в 3D
double distance(Point &p1, Point &p2)
{
  return sqrt(SQR(p1.x - p2.x) + 
              SQR(p1.y - p2.y) + 
              SQR(p1.z - p2.z));
}
 
// удаление точек так, чтобы расстояние между любыми двумя 
// не превышало max_dist 
void del_pts(vector<Point> &pts, const double max_dist)
{
  for (int idx1 = 0; idx1 < pts.size(); idx1++)
    for (int idx2 = pts.size() - 1; idx2 > idx1; idx2--)
      if (distance(pts[idx1], pts[idx2]) > max_dist)
        pts.erase[idx2];
}

#include <iostream>
#include <vector>
#include <cmath>
 
struct Point {
    double x, y, z;
};
 
#define SQR(x) ((x) * (x))
 
// Расстояние между точками в 3D
double distance(const Point &p1, const Point &p2) { 
    return sqrt(SQR(p1.x - p2.x) + SQR(p1.y - p2.y) + SQR(p1.z - p2.z)); 
} 
 
// Удаление точек (исправленная версия жадного алгоритма)
void del_pts(std::vector<Point> &pts, const double max_dist) { 
    for (size_t idx1 = 0; idx1 < pts.size(); idx1++) {
        // Идем с конца. Если точка удаляется, pts.size() уменьшается автоматически
        for (size_t idx2 = pts.size() - 1; idx2 > idx1; idx2--) {
            if (distance(pts[idx1], pts[idx2]) > max_dist) {
                pts.erase(pts.begin() + idx2); 
            }
        }
    }

double distance(const Point &p1, const Point &p2)
{
    return std::hypot(p1.x - p2.x, p1.y - p2.y, p1.z - p2.z);
}

std::vector<int> dups(point.size(), -1);
 
#pragma omp parallel for
for (int i = 0; i < points.size(); ++i)
 dups[i] = find_dup(points, i);

// C++20
#include <cmath>
#include <list>
#include <vector>
#include <unordered_map>
#include <limits>
#include <random>
#include <iostream>
 
struct Point
{
  float x, y, z;
 
  friend float distance(const Point &a, const Point &b)
    { return std::hypot(a.x - b.x, a.y - b.y, a.z - b.z); }
};
 
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_dist,
                             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 dist = distance(point, *it_adj_point);
      if (dist < min_dist)
        if (++n_violations > n_limit)
          return n_violations;
    }
  }
 
  return n_violations;
}
 
std::size_t count_violations(const Mesh &mesh, float 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_dist, n_violations);
 
  return n_violations;
}
 
void cleanup(Points &points, Mesh &mesh, float 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_dist, 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;
}