EBGeometry_PointCloud.hpp

Go to the documentation of this file.

// SPDX-FileCopyrightText: 2026 Robert Marskar <robert.marskar@sintef.no>
//
// SPDX-License-Identifier: GPL-3.0-or-later
 
#ifndef EBGEOMETRY_POINTCLOUD_HPP
#define EBGEOMETRY_POINTCLOUD_HPP
 
// Std includes
#include <cstddef>
#include <cstdint>
#include <vector>
 
// Our includes
#include "EBGeometry_GPU.hpp"
#include "EBGeometry_Macros.hpp"
#include "EBGeometry_Math.hpp"
#include "EBGeometry_Vec.hpp"
 
namespace EBGeometry {
 
namespace PointCloud {
 
inline constexpr uint32_t InvalidIndex = Math::Limits<uint32_t>::max();
 
template <class T>
struct Hit
{
  uint32_t index           = InvalidIndex;           
  T        distanceSquared = Math::Limits<T>::max(); 
 
  [[nodiscard]] EBGEOMETRY_HOST_DEVICE
  bool
  valid() const noexcept
  {
    return index != InvalidIndex;
  }
};
 
template <class T>
class KBest
{
public:
  EBGEOMETRY_HOST_DEVICE
  KBest(Hit<T>* a_out, const std::size_t a_k, const uint32_t a_exclude = InvalidIndex) noexcept
    : m_out(a_out), m_k(a_k), m_exclude(a_exclude)
  {
    EBGEOMETRY_EXPECT(a_out != nullptr);
    EBGEOMETRY_EXPECT(a_k > 0);
  }
 
  EBGEOMETRY_HOST_DEVICE
  void
  insert(const T a_distanceSquared, const uint32_t a_index) noexcept
  {
    if (a_distanceSquared >= m_bound || a_index == m_exclude) {
      return;
    }
 
    std::size_t slot = (m_found < m_k) ? m_found++ : (m_k - 1);
 
    for (; slot > 0 && m_out[slot - 1].distanceSquared > a_distanceSquared; slot--) {
      m_out[slot] = m_out[slot - 1];
    }
 
    m_out[slot] = Hit<T>{a_index, a_distanceSquared};
 
    if (m_found == m_k) {
      m_bound = m_out[m_k - 1].distanceSquared;
    }
  }
 
  [[nodiscard]] EBGEOMETRY_HOST_DEVICE
  T
  bound() const noexcept
  {
    return m_bound;
  }
 
  [[nodiscard]] EBGEOMETRY_HOST_DEVICE
  std::size_t
  found() const noexcept
  {
    return m_found;
  }
 
private:
  Hit<T>*     m_out;                            
  std::size_t m_k;                              
  uint32_t    m_exclude;                        
  std::size_t m_found = 0;                      
  T           m_bound = Math::Limits<T>::max(); 
};
 
template <class T>
EBGEOMETRY_HOST_DEVICE
inline std::size_t
closestPointsBruteForce(const Vec3T<T>*   a_positions,
                        const uint32_t    a_numPoints,
                        const Vec3T<T>&   a_query,
                        const std::size_t a_k,
                        Hit<T>*           a_out,
                        const uint32_t    a_exclude = InvalidIndex) noexcept
{
  EBGEOMETRY_EXPECT(a_positions != nullptr || a_numPoints == 0);
 
  KBest<T> best(a_out, a_k, a_exclude);
 
  for (uint32_t i = 0; i < a_numPoints; i++) {
    best.insert((a_positions[i] - a_query).length2(), i);
  }
 
  return best.found();
}
 
template <class T>
[[nodiscard]] EBGEOMETRY_HOST_DEVICE
inline Hit<T>
closestPointBruteForce(const Vec3T<T>* a_positions,
                       const uint32_t  a_numPoints,
                       const Vec3T<T>& a_query,
                       const uint32_t  a_exclude = InvalidIndex) noexcept
{
  Hit<T> hit;
 
  closestPointsBruteForce(a_positions, a_numPoints, a_query, 1, &hit, a_exclude);
 
  return hit;
}
 
} // namespace PointCloud
 
} // namespace EBGeometry
 
#endif