distances.h

Go to the documentation of this file.

#ifndef DISTANCES_H
#define DISTANCES_H
 
#include <box2d/box2d.h>
#include <unordered_map>
#include <vector>
#include <cmath>
 
#include "utils.h"
#include "spogobot.h"
#include "robot.h"

float euclidean_distance(const b2Vec2& a, const b2Vec2& b);

struct GridCell {
    int x, y; 

    bool operator==(const GridCell& other) const {
        return x == other.x && y == other.y;
    }
};

struct GridCellHash {
    std::size_t operator()(const GridCell& cell) const {
        // Simple hash combining for x and y.
        return std::hash<int>()(cell.x) ^ (std::hash<int>()(cell.y) << 1);
    }
};

constexpr std::array<GridCell,9> precomputed_neighbor_cells{
    GridCell{-1,-1}, GridCell{-1,0}, GridCell{-1,1},
    GridCell{ 0,-1}, GridCell{ 0,0}, GridCell{ 0,1},
    GridCell{ 1,-1}, GridCell{ 1,0}, GridCell{ 1,1}
};

inline GridCell get_grid_cell(float x, float y, float cell_size) {
    return {static_cast<int>(std::floor(x / cell_size)),
            static_cast<int>(std::floor(y / cell_size))};
}
 
 
 
 
struct Candidate {
    std::size_t idx;   /* index inside robots[]                               */
    float       dist_sq;
    float       angle; /* atan2(dy,dx)                                        */
    float       half_ap; /* asin(r_body / dist)                               */
};
 
/* ------------------------------------------------------------------------ */
/* Angular-interval utilities (LOS test)                                    */
/* ------------------------------------------------------------------------ */
namespace angles {
    inline float wrap(float a) {
        while (a <= -M_PI) a += 2 * M_PI;
        while (a >   M_PI) a -= 2 * M_PI;
        return a;
    }
 
    struct Interval { float a, b; };  /* half-open, assume a < b in (-π, π]  */
 
    /* insert [a,b) into sorted union without overlaps                       */
    void add_interval(float a, float b, std::vector<Interval>& ivs);
 
    /* true iff [a,b) completely covered by ivs                              */
    bool fully_covered(float a, float b, const std::vector<Interval>& ivs);
 
    inline bool in_fov(float bearing,   /* atan2(dy,dx)                 */
            float center,    /* LED direction                 */
            float half_ap)   /* half-FOV (e.g. π/3)           */
    {
        return std::fabs(wrap(bearing - center)) <= half_ap;
    }
} // namespace angles
 
/* ------------------------------------------------------------------------ */
/* Building blocks for find_neighbors                                       */
/* ------------------------------------------------------------------------ */
 
/* Build a spatial hash of LED positions. */
std::unordered_map<GridCell,std::vector<std::size_t>,GridCellHash>
build_spatial_hash(span_t<float> xs,
                   span_t<float> ys,
                   float cell_size);
 
/* Collect robots that lie                                           *
 *   – in the 3×3 grid block around emitter i                        *
 *   – inside emitter i’s communication radius                       */
std::vector<Candidate>
collect_candidates(std::size_t                  i,
                   span_t<float>       xs,
                   span_t<float>       ys,
                   span_t<float>       cx,
                   span_t<float>       cy,
                   span_t<float>       body_rad,
                   span_t<float>       comm_rad,
                   span_t<float>       led_dir,
                   const std::unordered_map<GridCell,
                                            std::vector<std::size_t>,
                                            GridCellHash>& hash,
                   float cell_size,
                   bool  clip_fov);
 
/* Given distance-sorted candidates, keep only those not shadowed. */
std::vector<std::size_t>
filter_visible(const std::vector<Candidate>& cand);
 

void find_neighbors(ir_direction dir, std::vector<std::shared_ptr<PogobotObject>>& robots,
        float maxDistance,
        bool  enable_occlusion = true);
 

std::vector<float>
compute_wall_distances(ir_direction                           dir,
                       const std::vector<std::shared_ptr<PogobotObject>>& robots,
                       const arena_polygons_t&                arena_polygons);
 

void find_neighbors_to_pogowalls(std::vector<std::shared_ptr<Pogowall>>& pogowalls, ir_direction dir, std::vector<std::shared_ptr<PogobotObject>>& robots);
 
 
 
/************************* Period boundary conditions *************************/
  
inline float wrap01(float x, float L) {
    float k = std::floor(x / L);
    return x - k * L;
}
 
inline float delta_periodic(float d, float L) {
    d = std::fmod(d, L);
    if (d >  L * 0.5f) d -= L;
    if (d <= -L * 0.5f) d += L;
    return d;
}
 
inline b2Vec2 torus_delta(b2Vec2 a, b2Vec2 b, b2Vec2 dom_min, float Lx, float Ly) {
    float ax = a.x - dom_min.x, ay = a.y - dom_min.y;
    float bx = b.x - dom_min.x, by = b.y - dom_min.y;
    return { delta_periodic(bx - ax, Lx), delta_periodic(by - ay, Ly) };
}
 
// Periodic-boundary variant (toroidal domain).
void find_neighbors_periodic(
    ir_direction dir,
    std::vector<std::shared_ptr<PogobotObject>>& robots,
    float max_distance,
    b2Vec2 domain_min,
    float domain_w,
    float domain_h,
    bool enable_occlusion = true);
 
 
// Build a spatial hash with ghost insertions across edges for PBC.
std::unordered_map<GridCell,std::vector<std::size_t>,GridCellHash>
build_spatial_hash_periodic(span_t<float> xs,
                            span_t<float> ys,
                            float cell_size,
                            b2Vec2 domain_min,
                            float domain_w,
                            float domain_h);
 
 
#endif // DISTANCES_H
 
// MODELINE "{{{1
// vim:expandtab:softtabstop=4:shiftwidth=4:fileencoding=utf-8
// vim:foldmethod=marker