#include <SDL2/SDL.h>
#include <SDL2/SDL_image.h>
#include <box2d/box2d.h>
#include <vector>
#include <string>
#include <tuple>
#include <utility>
#include <cstdint>

Typedefs
typedef std::vector< std::vector< b2Vec2 > >	arena_polygons_t

Functions
void	adjust_mm_to_pixels (float delta)
	Adjusts the global mm_to_pixels scaling factor.
b2Vec2	visualization_position (float x, float y)
	Calculates the visualization position for given x and y coordinates.
b2Vec2	visualization_position (b2Vec2 pos)
	Calculates the visualization position for a given point.
std::vector< std::vector< b2Vec2 > >	read_poly_from_csv (const std::string &filename, float total_surface)
	Reads polygons from a CSV file and scales them to match a specified surface area.
b2Vec2	generate_random_point_within_polygon (const std::vector< b2Vec2 > &polygon)
	Generates a random point within the specified polygon.
bool	is_point_within_polygon (const std::vector< b2Vec2 > &polygon, float x, float y)
	Determines whether a point is within a polygon.
std::vector< b2Vec2 >	offset_polygon (const std::vector< b2Vec2 > &polygon, float offset)
	Computes an offset polygon.
std::vector< b2Vec2 >	generate_random_points_within_polygon_safe (const std::vector< std::vector< b2Vec2 > > &polygons, const std::vector< float > &reserve_radii, float max_neighbor_distance=std::numeric_limits< float >::infinity(), std::uint32_t attempts_per_point=100U, std::uint32_t max_restarts=100U)
std::vector< b2Vec2 >	generate_points_voronoi_lloyd (const std::vector< std::vector< b2Vec2 > > &polygons, std::size_t k, std::size_t n_samples=20 '000, std::size_t kmeans_iterations=20, std::uint32_t max_restarts=3)
std::vector< b2Vec2 >	generate_random_points_power_lloyd (const std::vector< std::vector< b2Vec2 > > &polygons, const std::vector< float > &reserve_radii, std::size_t n_samples=25 '000, std::size_t kmeans_iterations=25, float convergence_eps=1e-3f, std::uint32_t max_restarts=3)
	Uniformly sample approximately equi-spaced points in a (possibly holed) polygonal domain, while respecting per-point exclusion radii.
std::vector< b2Vec2 >	generate_random_points_layered (const std::vector< std::vector< b2Vec2 > > &polygons, const std::vector< float > &reserve_radii, std::uint32_t attempts_per_point=1 '000U, std::uint32_t max_restarts=25U)
std::vector< b2Vec2 >	generate_chessboard_points (const std::vector< std::vector< b2Vec2 > > &polygons, std::size_t n_points, float pitch, bool cluster_center=false)
	Generate a square-grid ("checkerboard") layout with exactly `n_points` nodes inside a (possibly holed) polygonal arena.
std::pair< float, float >	compute_polygon_dimensions (const std::vector< b2Vec2 > &polygon)
	Computes the width and height of a polygon.
float	compute_polygon_area (const std::vector< b2Vec2 > &poly)
	Computes the area of a polygon using the shoelace formula.
b2Vec2	polygon_centroid (const std::vector< b2Vec2 > &polygon)
	Computes the centroid of a polygon.
float	point_to_line_segment_distance (const b2Vec2 &p, const b2Vec2 &a, const b2Vec2 &b)
	Calculates the distance from a point to a line segment.
std::vector< b2Vec2 >	generate_regular_disk_points_in_polygon (const std::vector< std::vector< b2Vec2 > > &polygons, const std::vector< float > &reserve_radii)
	Place points on (approximate) concentric rings inside polygons[0] so that: • point i stays ≥ reserve_radii[i] from every polygon edge, • point i stays ≥ reserve_radii[i] + reserve_radii[j] from every previously accepted point j, • no point falls inside a hole (polygons[1…]).
std::tuple< std::vector< b2Vec2 >, std::vector< float > >	import_points_from_file (const arena_polygons_t &scaled_arena_polygons, const size_t nb_objects, const std::string &formation_filename, const std::pair< float, float > &imported_formation_min_coords, const std::pair< float, float > &imported_formation_max_coords)
	Import and rescale robot formation points.

Variables
float const	VISUALIZATION_SCALE = 100.0f
float	mm_to_pixels
	Global scaling factor from millimeters to pixels.
float	visualization_x
	Global visualization offset for the x-coordinate.
float	visualization_y
	Global visualization offset for the y-coordinate.

Typedef Documentation

◆ arena_polygons_t

typedef std::vector<std::vector<b2Vec2> > arena_polygons_t

Function Documentation

◆ adjust_mm_to_pixels()

void adjust_mm_to_pixels ( float delta )

Adjusts the global mm_to_pixels scaling factor.

This function modifies the conversion factor from millimeters to pixels by adding the provided delta. The resulting value is clamped between 0.10 and 10.0.

Parameters

delta The value to add to mm_to_pixels.

◆ compute_polygon_area()

float compute_polygon_area ( const std::vector< b2Vec2 > & poly )

Computes the area of a polygon using the shoelace formula.

This function calculates the area of a polygon defined by a sequence of points (b2Vec2) using the shoelace algorithm. The formula sums the cross products of consecutive vertices and returns half of the absolute value.

Parameters

poly	A vector of b2Vec2 points representing the vertices of the polygon.

Returns: float The computed area of the polygon.

◆ compute_polygon_dimensions()

std::pair< float, float > compute_polygon_dimensions ( const std::vector< b2Vec2 > & polygon )

Computes the width and height of a polygon.

This function calculates the dimensions of a polygon by determining the minimum and maximum x and y coordinates of its vertices. The width is computed as the difference between the maximum and minimum x values, and the height is the difference between the maximum and minimum y values.

Parameters

polygon A vector of b2Vec2 points representing the vertices of the polygon.

Returns: std::pair<float, float> A pair where the first element is the width and the second element is the height of the polygon.

◆ generate_chessboard_points()

std::vector< b2Vec2 > generate_chessboard_points	(	const std::vector< std::vector< b2Vec2 > > &	polygons,
		std::size_t	n_points,
		float	pitch,
		bool	cluster_center = false )

Generate a square-grid ("checkerboard") layout with **exactly n_points** nodes inside a (possibly holed) polygonal arena.

The grid is axis-aligned with spacing approximately pitch. The function will try multiple strategies to achieve exactly the requested number of points:

Try exact grid with the given pitch
Try grids with slightly adjusted pitch values
Use grid points as base and add/remove points strategically
For small numbers, use optimized placement

Parameters

[in]	polygons	0 = outer boundary (≥ 3 vertices), 1…N = holes
[in]	n_points	Exact number of nodes requested (≥ 1)
[in]	pitch	Target grid spacing in metres (> 0)
[in]	cluster_center	If true, creates compact formation centered in arena without holes. If false, uses distributed placement.

Returns: std::vector<b2Vec2> Exactly n_points valid nodes

Exceptions

std::runtime_error • if pitch ≤ 0 or n_points == 0
• if the outer polygon is missing / degenerate
• if it is impossible to place n_points nodes in the arena

Note: Requires is_point_within_polygon(poly,x,y) treating boundary points as inside. Uses Box2D's b2Vec2 for coordinates.

◆ generate_points_voronoi_lloyd()

std::vector< b2Vec2 > generate_points_voronoi_lloyd	(	const std::vector< std::vector< b2Vec2 > > &	polygons,
		std::size_t	k,
		std::size_t	n_samples = 20 '000,
		std::size_t	kmeans_iterations = 20,
		std::uint32_t	max_restarts = 3 )

Generate approximately equi-spaced random points inside a (possibly holed) polygonal domain by running a few iterations of Lloyd’s relaxation (a.k.a. “K-means” on a dense uniform sample).

Draw n_samples uniform random points inside the domain.
Initialise k cluster centres by picking k of those samples at random.
Repeat kmeans_iterations times (or until convergence): • Assign every sample to its closest centre. • Replace each centre by the arithmetic mean of the samples in its cluster. If the mean falls outside the domain, snap it to the in-domain sample that is nearest to the mean.
Return the final centres.

In practice after ~15–25 iterations you already get a centroidal Voronoi tessellation good enough for swarm-robot initialisation.

Parameters

polygons	outer polygon first, holes afterwards
k	number of points to generate
n_samples	size of the uniform background sample
kmeans_iterations	maximum number of Lloyd relaxations
max_restarts	how many times we may restart if a step fails

Exceptions

std::runtime_error if the domain is invalid or no solution is found

◆ generate_random_point_within_polygon()

b2Vec2 generate_random_point_within_polygon ( const std::vector< b2Vec2 > & polygon )

Generates a random point within the specified polygon.

This function calculates the bounding box of the provided polygon and repeatedly generates random points within that box until one is found that lies inside the polygon.

Parameters

polygon A vector of b2Vec2 points defining the polygon.

Returns: b2Vec2 A randomly generated point within the polygon.

Exceptions

std::runtime_error If the polygon has fewer than 3 points.

◆ generate_random_points_layered()

std::vector< b2Vec2 > generate_random_points_layered	(	const std::vector< std::vector< b2Vec2 > > &	polygons,
		const std::vector< float > &	reserve_radii,
		std::uint32_t	attempts_per_point = 1 '000U,
		std::uint32_t	max_restarts = 25U )

Priority-wall layered sampler.

Stage 0 : fill the boundary (outer wall or hole walls) greedily; Stage 1+ : build concentric layers around the already-accepted points.

A point i always keeps reserve_radii[i] clearance from walls and from every other point. NaN radii → return {NaN,NaN}.

Throws std::runtime_error after max_restarts failed global attempts.

◆ generate_random_points_power_lloyd()

std::vector< b2Vec2 > generate_random_points_power_lloyd	(	const std::vector< std::vector< b2Vec2 > > &	polygons,
		const std::vector< float > &	reserve_radii,
		std::size_t	n_samples = 25 '000,
		std::size_t	kmeans_iterations = 25,
		float	convergence_eps = 1e-3f,
		std::uint32_t	max_restarts = 3 )

Uniformly sample approximately equi-spaced points in a (possibly holed) polygonal domain, while respecting per-point exclusion radii.

The routine performs Lloyd relaxation in power-distance space:

Draw n_samples uniform random points inside the domain.
Initialise k = reserve_radii.size() centres with random samples.
Iterate kmeans_iterations times (or until all moves < convergence_eps): • Assign every sample to the centre that minimises the power distance ‖p − c‖² − rᵢ². • Replace each centre by the arithmetic mean of its samples. If that mean falls outside the domain, snap it back to the closest in-domain sample in the same cluster.
Run a lightweight “push-apart” pass so that no two centres end up closer than rᵢ + rⱼ (handles the rare residual overlaps).

The power metric naturally enlarges Voronoi cells for large radii, giving a blue-noise layout where big robots get more space.

Parameters

polygons	0-th entry = outer boundary, 1…N = holes
reserve_radii	desired exclusion radius per point (size k)
n_samples	size of the background uniform sample cloud
kmeans_iterations	maximum Lloyd iterations (≈15–30 is plenty)
convergence_eps	stop early if every shift < this (world units)
max_restarts	number of times we may restart on hard failure

Returns: std::vector<b2Vec2> the k centre positions

Exceptions

std::runtime_error if the domain is invalid or the algorithm fails after max_restarts attempts.

◆ generate_random_points_within_polygon_safe()

std::vector< b2Vec2 > generate_random_points_within_polygon_safe	(	const std::vector< std::vector< b2Vec2 > > &	polygons,
		const std::vector< float > &	reserve_radii,
		float	max_neighbor_distance,
		std::uint32_t	attempts_per_point,
		std::uint32_t	max_restarts )

Generate random points inside a (possibly holed) polygonal domain while respecting a per‑point exclusion radius and a global connectivity limit.

A candidate is accepted only if it is:

Inside the outer polygon and outside every “hole” polygon.
At a distance ≥ r_i + r_j from every previously accepted point j.
Within max_neighbor_distance of at least one previously accepted point (unless it is the very first point or max_neighbor_distance is +∞).

If radius is NaN for a point, it will be set to {NaN, NaN} in the result.

If it fails to build the whole set after attempts_per_point rejected candidates, the algorithm discards all progress and restarts. It will attempt the whole sampling process up to max_restarts times before throwing.

Generate random points inside a (possibly holed) polygonal domain while respecting a per‑point exclusion radius and a global connectivity limit.

A candidate is accepted only if it is:

Inside the outer polygon and outside every "hole" polygon.
At a distance ≥ r_i + r_j from every previously accepted point j.
Within max_neighbor_distance of at least one previously accepted point (unless it is the very first point or max_neighbor_distance is +∞).

If radius is NaN for a point, it will be set to {NaN, NaN} in the result.

If it fails to build the whole set after attempts_per_point rejected candidates, the algorithm discards all progress and restarts. It will attempt the whole sampling process up to max_restarts times before throwing.

◆ generate_regular_disk_points_in_polygon()

std::vector< b2Vec2 > generate_regular_disk_points_in_polygon	(	const std::vector< std::vector< b2Vec2 > > &	polygons,
		const std::vector< float > &	reserve_radii )

Place points on (approximate) concentric rings inside polygons[0] so that: • point i stays ≥ reserve_radii[i] from every polygon edge, • point i stays ≥ reserve_radii[i] + reserve_radii[j] from every previously accepted point j, • no point falls inside a hole (polygons[1…]).

Parameters

polygons	polygons[0] is the main area; polygons[1…] are holes.
reserve_radii	exclusion radii for each requested point (size N).

Returns: vector<b2Vec2> the generated points (size == reserve_radii.size()).

◆ import_points_from_file()

std::tuple< std::vector< b2Vec2 >, std::vector< float > > import_points_from_file	(	const arena_polygons_t &	scaled_arena_polygons,
		const size_t	nb_objects,
		const std::string &	formation_filename,
		const std::pair< float, float > &	imported_formation_min_coords,
		const std::pair< float, float > &	imported_formation_max_coords )

Import and rescale robot formation points.

This routine loads a formation description from formation_filename, rescales every (x, y) so that the entire formation fits inside the bounding box of scaled_arena_polygons, and preserves θ exactly as provided. The caller must supply the minimum and maximum (x, y) that were present in the file (these can be pre‑computed with a simple pass over the input).

Parameters

scaled_arena_polygons	Destination geometry (already scaled).
nb_objects	Number of objects.
formation_filename	Path to a .csv or .feather file with three numeric columns: x, y, theta.
imported_formation_min_coords	Minimum (x, y) encountered in the source file.
imported_formation_max_coords	Maximum (x, y) encountered in the source file.

Returns

std::tuple containing:

std::vector<b2Vec2> – positions mapped to arena space.
std::vector<float> – corresponding headings (in radians).

Exceptions

std::runtime_error if the file cannot be read or the extension is unsupported.

Note: No attempt is made to normalise θ; it is copied verbatim.

◆ is_point_within_polygon()

bool is_point_within_polygon	(	const std::vector< b2Vec2 > &	polygon,
		float	x,
		float	y )

Determines whether a point is within a polygon.

This function uses a ray-casting algorithm to test whether the point (x, y) lies inside the given polygon.

Parameters

polygon	A vector of b2Vec2 points defining the polygon.
x	The x-coordinate of the point.
y	The y-coordinate of the point.

Returns: true If the point is inside the polygon.; false Otherwise.

◆ offset_polygon()

std::vector< b2Vec2 > offset_polygon	(	const std::vector< b2Vec2 > &	polygon,
		float	offset )

Computes an offset polygon.

This function generates a new polygon by offsetting the original polygon inward or outward by a specified distance. It calculates normals at each vertex and computes new offset points accordingly.

Parameters

polygon	A vector of b2Vec2 points defining the original polygon.
offset	The offset distance to apply.

Returns: std::vector<b2Vec2> The resulting offset polygon.

Exceptions

std::runtime_error If the polygon has fewer than 3 points.

◆ point_to_line_segment_distance()

float point_to_line_segment_distance	(	const b2Vec2 &	p,
		const b2Vec2 &	a,
		const b2Vec2 &	b )

Calculates the distance from a point to a line segment.

This function computes the shortest distance from point p to the line segment defined by endpoints a and b.

Parameters

p	The point from which the distance is measured.
a	The first endpoint of the line segment.
b	The second endpoint of the line segment.

Returns: float The distance from point p to the line segment.

◆ polygon_centroid()

b2Vec2 polygon_centroid ( const std::vector< b2Vec2 > & polygon )

Computes the centroid of a polygon.

This function calculates the geometric center of the polygon using the shoelace formula.

Parameters

polygon A vector of b2Vec2 points defining the polygon.

Returns: b2Vec2 The centroid of the polygon.

◆ read_poly_from_csv()

std::vector< std::vector< b2Vec2 > > read_poly_from_csv	(	const std::string &	filename,
		float	total_surface )

Reads polygons from a CSV file and scales them to match a specified surface area.

This function loads polygons from a CSV file where each line contains the x and y coordinates separated by a comma. An empty line indicates the end of a polygon. Once the polygons are loaded, the function normalizes all points into the [0,1] range using the overall bounding box of the data. The effective area is then computed as the area of the first (main) polygon minus the sum of the areas of any subsequent polygons (considered holes). A uniform scaling factor is determined so that when applied to the normalized polygons, the effective area becomes equal to the specified total_surface.

Parameters

filename	The path to the CSV file containing the polygon vertices.
total_surface	The desired surface area for the main polygon after subtracting the holes.

Returns: std::vector<std::vector<b2Vec2>> A vector containing the scaled polygons, where each polygon is represented as a vector of b2Vec2 points.

Exceptions

std::runtime_error If the file cannot be opened, no polygons are loaded, or the effective area (main polygon area minus holes area) is non-positive.

◆ visualization_position() [1/2]

b2Vec2 visualization_position ( b2Vec2 pos )

Calculates the visualization position for a given point.

This overloaded function converts a b2Vec2 point to visualization coordinates using the global mm_to_pixels scaling factor and visualization offsets.

Parameters

pos	The original position as a b2Vec2.

Returns: b2Vec2 The computed visualization position.

◆ visualization_position() [2/2]

b2Vec2 visualization_position	(	float	x,
		float	y )

Calculates the visualization position for given x and y coordinates.

This function converts physical coordinates to visualization coordinates using the global mm_to_pixels scaling factor and visualization offsets.

Parameters

x	The x-coordinate in the original space.
y	The y-coordinate in the original space.

Returns: b2Vec2 The computed visualization position.

Variable Documentation

◆ mm_to_pixels

float mm_to_pixels

extern

Global scaling factor from millimeters to pixels.

◆ VISUALIZATION_SCALE

float const VISUALIZATION_SCALE = 100.0f

◆ visualization_x

float visualization_x

extern

Global visualization offset for the x-coordinate.

◆ visualization_y

float visualization_y

extern

Global visualization offset for the y-coordinate.

Typedefs

Functions

Variables

Typedef Documentation

◆ arena_polygons_t

Function Documentation

◆ adjust_mm_to_pixels()

◆ compute_polygon_area()

◆ compute_polygon_dimensions()

◆ generate_chessboard_points()

◆ generate_points_voronoi_lloyd()

◆ generate_random_point_within_polygon()

◆ generate_random_points_layered()

◆ generate_random_points_power_lloyd()

◆ generate_random_points_within_polygon_safe()

◆ generate_regular_disk_points_in_polygon()

◆ import_points_from_file()

◆ is_point_within_polygon()

◆ offset_polygon()

◆ point_to_line_segment_distance()

◆ polygon_centroid()

◆ read_poly_from_csv()

◆ visualization_position() [1/2]

◆ visualization_position() [2/2]

Variable Documentation

◆ mm_to_pixels

◆ VISUALIZATION_SCALE

◆ visualization_x

◆ visualization_y