Computing collisions

Computing collisions#

CollisionComputation#

#include "navground/core/collision_computation.h"
struct DiscCache#

An struct that holds pre-computed values about a disc-shaped obstacle to speed up collision checking. For collision checking, the agent is considered point-like, i.e, the radius of the agent should be added to the disc-cache margin.

Public Functions

DiscCache(Vector2 delta, ng_float_t margin, Vector2 velocity = Vector2::Zero(), ng_float_t visible_angle = HALF_PI)#

Construct a disc cache.

Parameters:

  • delta – [in] The difference of positions (obstacle - agent) in absolute frame

  • margin – [in] The margin (sum of the radii and safety margin)

  • velocity – [in] The disc velocity

  • visible_angle – [in] When colliding, directions outside the sector defined by [-visible_angle, visible_angle] are considered safe.

Public Members

Vector2 delta#

the difference of positions (obstacle - agent) in absolute frame

Vector2 velocity#

The disc velocity in absolute frame

ng_float_t distance#

The distance between disc and agent (computed)

class CollisionComputation#

This class compute collisions of moving points with lists of DiscCache and LineSegment.

Subclassed by navground::core::CachedCollisionComputation

Public Functions

inline CollisionComputation()#

Construct an instance.

std::valarray<ng_float_t> get_free_distance_for_sector(Radians from, Radians length, size_t resolution, ng_float_t max_distance, bool dynamic = false, ng_float_t speed = 0)#

Return the free distance to collision for an interval of headings.

Parameters:

  • from – [in] The interval lower bound

  • length – [in] The length of the interval

  • resolution – [in] The number of values in the interval

  • max_distance – [in] The maximum distance to consider (collision behind this distance are effectively ignored)

  • dynamic – [in] If the agent is moving

  • speed – [in] The speed at which the agent is moving

Returns:

The free distance for each angle in the interval [from, from + length].

std::valarray<ng_float_t> get_angles_for_sector(Radians from, Radians length, size_t resolution) const#

Return regularly sampled angles.

Parameters:

  • from – [in] The interval lower bound

  • length – [in] The length of the interval

  • resolution – [in] The number of values in the interval

Returns:

Angles regularly sampled in the interval [from, from + length].

std::tuple<std::valarray<ng_float_t>, std::valarray<ng_float_t>> get_contour_for_sector(Radians from, Radians length, size_t resolution, ng_float_t max_distance, bool dynamic = false, ng_float_t speed = 0)#

Return the polar contour for an interval of headings.

Parameters:

  • from – [in] The interval lower bound

  • length – [in] The length of the interval

  • resolution – [in] The number of values in the interval

  • max_distance – [in] The maximum distance to consider (collision behind this distance are effectively ignored)

  • dynamic – [in] If the agent is moving

  • speed – [in] The speed at which the agent is moving

Returns:

An arrays of angles sampled regularly in the interval [from, from + length] and one array with the free distance in their direction.

void setup(Pose2 pose_, ng_float_t margin_, const std::vector<LineSegment> &line_segments, std::vector<DiscCache> static_discs, std::vector<DiscCache> dynamic_discs)#

Set the state from collections of LineSegment and DiscCache.

Parameters:

  • pose_ – [in] The pose of the agent

  • margin_ – [in] The margin

  • line_segments – [in] The line segments

  • static_discs – [in] The static discs

  • dynamic_discs – [in] The dynamic discs

void setup(Pose2 pose_, ng_float_t margin_, const std::vector<LineSegment> &line_segments, const std::vector<Disc> &static_discs, const std::vector<Neighbor> &dynamic_discs)#

Set the state from collections of LineSegment, Disc, and Neighbor.

Parameters:

  • pose_ – [in] The pose

  • margin_ – [in] The margin

  • line_segments – [in] The line segments

  • static_discs – [in] The static discs

  • dynamic_discs – [in] The dynamic discs

ng_float_t static_free_distance(Radians angle, ng_float_t max_distance, bool include_neighbors = true)#

Returns the free distance if the agent will be static.

Parameters:

  • angle – [in] The angle (absolute)

  • max_distance – [in] The maximal distance to consider

  • include_neighbors – [in] Indicates if the neighbors should be included in the computation

Returns:

The distance in direction angle before possibly colliding

ng_float_t dynamic_free_distance(Radians angle, ng_float_t max_distance, ng_float_t speed)#

Returns the free distance if the agent will be move.

Parameters:

  • angle – [in] The angle (absolute)

  • max_distance – [in] The maximal distance to consider

  • speed – [in] The speed of the agent

Returns:

The distance in direction angle before possibly colliding

bool dynamic_may_collide(const DiscCache &obstacle, ng_float_t max_distance, ng_float_t speed)#

Tentatively checks whenever a moving disc-cache may collide with the agent within an horizon.

Parameters:

  • obstacle – [in] The obstacle

  • max_distance – [in] The maximal distance to consider

  • speed – [in] The speed of the agent

Returns:

False if it is impossible that the agent collides with the obstacle within max_distance.

bool static_may_collide(const DiscCache &obstacle, ng_float_t max_distance)#

Tentatively checks whenever a static disc-cache may collide with the agent within an horizon.

Parameters:

  • obstacle – [in] The obstacle

  • max_distance – [in] The maximal distance to consider

Returns:

False if it is impossible that the agent collides with the obstacle within max_distance.

CachedCollisionComputation#

#include "navground/core/cached_collision_computation.h"
class CachedCollisionComputation : public navground::core::CollisionComputation#

This class extend CollisionComputation to cache the results.

It assumes that the agents is only interested in possible collisions when moving in directions comprised in an interval \([\alpha, \alpha + \Delta]\), represented by in \(N\) points at regular steps, only up to a maximal distance \(D\), and that the agent moves at at given speed.

Public Functions

inline CachedCollisionComputation()#

Construct an instance

void set_resolution(size_t value)#

Sets the resolution: the number of discrete angles \(N\).

Parameters:

value – [in] A positive value

inline size_t get_resolution() const#

Gets the resolution: the number of discrete angles \(N\).

Returns:

The size of the cache.

void set_min_angle(Radians value)#

Sets the cache interval lower bound \(\alpha\).

Parameters:

value – [in] The lower bound

inline ng_float_t get_min_angle() const#

Gets the cache interval lower bound \(\alpha\).

Returns:

The lower bound.

void set_length(ng_float_t value)#

Sets the cache interval length \(\Delta\).

Parameters:

value – [in] The interval length

inline ng_float_t get_length() const#

Gets the cache interval length \(\Delta\).

Returns:

The interval length

void set_max_distance(ng_float_t value)#

Sets the maximal distance \(D\) to consider.

Parameters:

value – [in] The maximal distance

inline ng_float_t get_max_distance() const#

Gets the maximal distance \(D\) to consider.

Returns:

The maximal distance

void set_speed(ng_float_t value)#

Sets the agent speed.

Parameters:

value – [in] The speed

inline ng_float_t get_speed() const#

Gets the agent speed.

Returns:

The agent speed.

ng_float_t static_free_distance(Radians angle, bool include_neighbors = true)#

Returns the free distance if the agent will be static.

Parameters:

  • angle – [in] The angle (absolute)

  • include_neighbors – [in] Indicates if the neighbors should be included in the computation

Returns:

The distance in direction angle before possibly colliding

ng_float_t dynamic_free_distance(Radians angle)#

Returns the free distance if the agent will be move.

Parameters:

angle – [in] The angle (absolute)

Returns:

The distance in direction angle before possibly colliding

void reset()#

Resets the cache.

const std::valarray<ng_float_t> &get_cache(bool assuming_static = false, bool include_neighbors = true)#

Gets a pointer where collision distances are cached. Negative entries mean distance not computed (-2) or no collision (-1).

Parameters:

  • assuming_static – [in] The assuming static

  • include_neighbors – [in] Indicates if the neighbors should be included in the computation

Returns:

The collision distance cache.

std::valarray<ng_float_t> get_free_distance(bool dynamic = false)#

Returns the free distance to collision for the cached interval of headings.

Parameters:

dynamic – [in] If the agent is moving

Returns:

The distance before possibly colliding for each direction in the interval \([\alpha, \alpha + \Delta]\).