distances.cpp File Reference

#include "distances.h"
#include <box2d/box2d.h>
#include <unordered_map>
#include <vector>
#include <cmath>
#include <set>
#include <iostream>

Classes
struct	Interval

Functions
float	euclidean_distance (const b2Vec2 &a, const b2Vec2 &b)
	Computes the Euclidean distance between two 2D points.
std::unordered_map< GridCell, std::vector< std::size_t >, GridCellHash >	build_spatial_hash (span_t< float > xs, span_t< float > ys, float cell_size)
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)
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 max_distance, bool enable_occlusion)
	Finds neighboring robots within a specified maximum distance.
std::vector< float >	compute_wall_distances (ir_direction dir, const std::vector< std::shared_ptr< PogobotObject > > &robots, const arena_polygons_t &arena_polygons)
	Compute, for every robot, the distance from its IR–emitter dir to the nearest arena wall.
void	find_neighbors_to_pogowalls (std::vector< std::shared_ptr< Pogowall > > &pogowalls, ir_direction dir, std::vector< std::shared_ptr< PogobotObject > > &robots)
	Finds robots that are close to given pogowalls.
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)
std::vector< Candidate >	collect_candidates_periodic (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, b2Vec2 domain_min, float domain_w, float domain_h)
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)

Function Documentation

build_spatial_hash()

std::unordered_map< GridCell, std::vector< std::size_t >, GridCellHash > build_spatial_hash	(	span_t< float >	xs,
		span_t< float >	ys,
		float	cell_size )

build_spatial_hash_periodic()

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 )

collect_candidates()

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 )

collect_candidates_periodic()

std::vector< Candidate > collect_candidates_periodic	(	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,
		b2Vec2	domain_min,
		float	domain_w,
		float	domain_h )

compute_wall_distances()

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

Compute, for every robot, the distance from its IR–emitter dir to the nearest arena wall.

The function returns a vector<double> whose i‑th element is the distance for robots[i]. The caller decides what to do with the result (store it in the robot, feed a sensor model, etc.).

Parameters

dir	Which IR emitter of the robot is queried.
robots	Robots for which the distance is required.
arena_polygons	Arena walls described as polygons (see arena_polygons_t).

Returns

std::vector<float> A vector of distances (same ordering as robots).

euclidean_distance()

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

Computes the Euclidean distance between two 2D points.

This function calculates the Euclidean distance between the points represented by the Box2D vectors a and b.

Parameters

a	The first point as a b2Vec2.
b	The second point as a b2Vec2.

Returns

float The Euclidean distance between a and b.

filter_visible()

std::vector< std::size_t > filter_visible

(

const std::vector< Candidate > &

cand

)

find_neighbors()

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

Finds neighboring robots within a specified maximum distance.

This function uses spatial hashing to efficiently determine the neighbors for each robot. It partitions the 2D space into grid cells of size maxDistance and assigns each robot to a cell. For every robot, the function then checks the same cell and all adjacent cells for other robots, and updates the robot's neighbor list if the Euclidean distance (squared) is within the allowed maximum distance.

Parameters

dir	Direction in which messages are sent (i.e. the ID number of the IR emitter)
robots	A vector of Robot objects. Each Robot must implement a method get_position() returning a b2Vec2, and contain a public member "neighbors" (e.g., a vector) that can store pointers to neighboring Robot objects.
maxDistance	The maximum distance within which two robots are considered neighbors.
enable_occlusion	When true, use the angular-shadow-map LOS filter; when false, accept every range-culled candidate.

find_neighbors_periodic()

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 )

find_neighbors_to_pogowalls()

void find_neighbors_to_pogowalls	(	std::vector< std::shared_ptr< Pogowall > > &	pogowalls,
		ir_direction	dir,
		std::vector< std::shared_ptr< PogobotObject > > &	robots )

Finds robots that are close to given pogowalls.

Parameters

pogowalls	A vector of Pogowall objects.
dir	Direction in which messages are sent (i.e. the ID number of the IR emitter)
robots	A vector of Robot objects.
max_distance	The maximum distance within which a robot and a pogowalls are considered neighbors.