Defines
#define	_ISOC99_SOURCE
#define	_XOPEN_SOURCE
Functions
int	getPathLength (int xstart, int ystart, int xgoal, int ygoal)
void	GraphCell2XY (GraphMapCell c, int x, int *y)
	Returns X and Y index of a graph cell identified by a pointer.
A* Implementation Functions
This functions have not a meaning in A* algorithm, but are needed for implementation.
void	initGraphStructure (void)
	Init Graph structure with default values.
A* related functions
This functions have a meaning in theorical A*.
float	cFunction (int x1, int y1, int x2, int y2)
	Calculates the lenght of edge connecting two points.
void	calculateMapHeuristic (int xgoal, int ygoal)
	Calculates Map Heuristic values.
float	calculateCost (int x1, int y1, int x2, int y2)
	Calculates the cost of moving from the first cell to the second cell.
A* Main function

int	aAlgorithm (double xstart_real, double ystart_real, double xgoal_real, double ygoal_real, GraphMapCell **original_path)
	Search the shortest path between two points.
Path simplifier functions

PathPoint *	newPathPoint (PathPoint *last, float x, float y)
	Put the REAL coordonates (X,Y) in a list of points.
void	calc_line (float x1, float y1, float x2, float y2, Line *l)
	Calc line parameters from two points.
float	calc_dist (Line *l, float x, float y)
	Calc the distance between a line and a point.
int	path_simplifier (PathPoint path, int nbpoints, PathPoint first_point, double *angle)
	Simplify a given path.
Queue functions
This functions implements a queue needed by other functions.
int	queueIsEmpty (NodeQueue *q)
	Tests if a queue is empty.
void	pushNode (NodeQueue *q, int x, int y)
	Push a node into the queue in a FIFO mode.
void	pushNodeLast (NodeQueue *q, int x, int y)
	Push a node into the queue in a LIFO mode.
int	popNode (NodeQueue q, Node n)
	Pop the first node of the queue.
void	printQueue (NodeQueue *q)
	Print the queue.
NodeQueue *	newQueue (void)
	Init queue values.
int	delQueue (NodeQueue *q)
	Free queue memory.
int	isInQueue (NodeQueue *q, int x, int y)
	Finds out if the node (X,Y) is in the queue.
void	drainQueue (NodeQueue *q)
	Free memory allocated in the queue.
A* Constants

#define	SQRT2 M_SQRT2
	Root square of two.
#define	WALL_COST 1000
	Cost of jumping a wall.

Detailed Description

Path Planer Architecture

PathPlaner is composed of three sublibraries or modules:

aalgorithm.c : is the implementation of A* Algorithm. More details in aalgorithm().
path_simplifier.c : The result path of A* is a set of discrete cells (or pixels) that should be simplify. More details in path_simplifier().
pathqueue.c : It is an implementation of a points queue used by the others modules.

Define Documentation

#define _ISOC99_SOURCE

#define _XOPEN_SOURCE

#define SQRT2 M_SQRT2

Root square of two.

#define WALL_COST 1000

Cost of jumping a wall.

Function Documentation

int aAlgorithm	(	double	xstart_real,
		double	ystart_real,
		double	xgoal_real,
		double	ygoal_real,
		GraphMapCell **	original_path
	)

Search the shortest path between two points.

Parameters:

	xstart_real	Coordonate X of the start point
	ystart_real	Coordonate Y of the start point
	xgoal_real	Coordonate X of the goal point
	ygoal_real	Coordonate Y of the goal point
	original_path	[out] Pointer to the first map cell of the path is stored here. Use GraphMapCell.next to traverse the found path.

See also:: graph

Returns:: -1 if the goal is not founded, the number of points to the goal is founded on success.

This is an implementation of A* Algorithm (Algorithm 24, p531) defined in the book "Principles of Robot Motion" by H. Choset&others

prioritySet : Set of nodes to process order by heuristic distance

nbest : Best node to reach the goal

Init graph structure with default values.
Calculate Heuristic distances.
Initial values : put start node in queue.
Expand nbest :for all x E Star(nbest) that are not in C
REPEAT until the queue is empty.
1. Pick nbest from prioritySet and remove
2. add to processed
3. IF nbest == goal ; EXIT
4. Expand nbest :for all x E Star(nbest) that are not in C
5. IF is not in Priorityset add to prioritySet.
6. ELSE update backpointer to point to nbest.

Here is the call graph for this function:

Here is the caller graph for this function:

float calc_dist	(	Line *	l,
		float	x,
		float	y
	)

Calc the distance between a line and a point.

Parameters:

	l	A pointer to line parameters
	x	Coordonate X of the point
	y	Coordonate Y of the point

Returns:: Cross cost

If it is a hotizontal line, the distance is the difference between y coordonate and b parameter.

If it is a vertical line, the distance is the difference between x coordonate and b parameter.

If it is a oblicuous line, the distance is $\frac{a_1 * x + a_2 * y - b} {sqrt{(a_1)^2+(a_2)^2}}$ .

Here is the caller graph for this function:

void calc_line	(	float	x1,
		float	y1,
		float	x2,
		float	y2,
		Line *	l
	)

Calc line parameters from two points.

Parameters:

	x1	Coordonate X of the first point
	y1	Coordonate Y of the first point
	x2	Coordonate X of the second point
	y2	Coordonate Y of the second point
	l	A pointer to line parameters

Returns parameters of a line : a1 * x + a2 * y = b

Here is the caller graph for this function:

float calculateCost	(	int	x1,
		int	y1,
		int	x2,
		int	y2
	)

Calculates the cost of moving from the first cell to the second cell.

Parameters:

	x1	Coordonate X of the first cell
	y1	Coordonate Y of the first cell
	x2	Coordonate X of the second cell
	y2	Coordonate Y of the second cell

Returns:: Cross cost

Warning:: Cells must be contiguous

Here is the caller graph for this function:

void calculateMapHeuristic	(	int	xgoal,
		int	ygoal
	)

Calculates Map Heuristic values.

Parameters:

	xgoal	Coordonate X of the goal
	ygoal	Coordonate Y of the goal

This function calculates the map heuristic values (the distance to the goal that is supoused to be the shortest).

The chosen method has been the euclidean distance. The distance between a point of the grid $(x,y)$ and the goal $(x_(goal),y_(goal))$ is $\sqrt{(x_(goal)-x)^2+(y_(goal)-y)^2}$ .

Another method explained in the book is coded but not used.It is called Brushfire algorithm. (section 4.3.2 of the book)

Here is the call graph for this function:

Here is the caller graph for this function:

float cFunction	(	int	x1,
		int	y1,
		int	x2,
		int	y2
	)

Calculates the lenght of edge connecting two points.

Parameters:

	x1	Coordonate X of the first cell
	y1	Coordonate Y of the first cell
	x2	Coordonate X of the second cell
	y2	Coordonate Y of the second cell

Returns:: Lenght of edge

Here is the call graph for this function:

Here is the caller graph for this function:

int delQueue ( NodeQueue * q )

Free queue memory.

Parameters:

q

Queue

Returns:: 1 if memory was succesfully free 0 if the queue is not empty

Here is the call graph for this function:

void drainQueue ( NodeQueue * q )

Free memory allocated in the queue.

Parameters:

q

Queue

Here is the call graph for this function:

int getPathLength	(	int	xstart,
		int	ystart,
		int	xgoal,
		int	ygoal
	)

void GraphCell2XY	(	GraphMapCell *	c,
		int *	x,
		int *	y
	)

Returns X and Y index of a graph cell identified by a pointer.

Parameters:

`[in]`	c	Pointer to the cell in question.
`[out]`	x	Cell's X index.
`[out]`	y	Cell's Y index.

Here is the caller graph for this function:

void initGraphStructure ( void )

Init Graph structure with default values.

See also:: graph

Here is the caller graph for this function:

int isInQueue	(	NodeQueue *	q,
		int	x,
		int	y
	)

Finds out if the node (X,Y) is in the queue.

Parameters:

	q	Queue
	x	Coordonate X of a cell
	y	Coordonate Y of a cell

Returns:: 1 if (X,Y) was founded, 0 otherwise

Here is the caller graph for this function:

PathPoint* newPathPoint	(	PathPoint *	last,
		float	x,
		float	y
	)

Put the REAL coordonates (X,Y) in a list of points.

Parameters:

	last	Last point of the points list
	x	Coordonate X of the first cell
	y	Coordonate Y of the first cell

Returns:: A pointer to allocated memory

Here is the caller graph for this function:

NodeQueue* newQueue ( void )

Init queue values.

Here is the caller graph for this function:

int path_simplifier	(	PathPoint *	path,
		int	nbpoints,
		PathPoint *	first_point,
		double *	angle
	)

Simplify a given path.

Parameters:

	path	A pointer to a list of points of the original path
	nbpoints	Number of points of the original path
	first_point	[out] A pointer to the list of points of simplest path
	angle	[out] Angle of the last line

Returns:: Number of point of the simplest path

This is an algorithm who tries to simplify the path given by A* Algorithm (

See also:: aalgorithm()). Basically, the algorithm works that follows: # Takes two points of the original path. # Calculates the line between them. # Calculates the distance between this line and all the points of the original path. If this distance is acceptable (less than PATH_ERROR), the line is part of the simply path. If not, goes to 1 taking two closer points . # The process finish when the last point is the final point.

In pseudo-code:

 first_index = 0;
 last_index = nbpoints;
 step = (last_index - first_index)/2
 WHILE (the first index less than nbpoints )
        Calc the line
        FOR all the points between path[first_index] and path[last_index]
                calc the distance betwen the point and the line
                Take the max_error
        END FOR
        IF max_error less than ACCEPTABLE_ERROR THEN
                The line is part of the simple path.
                first_index = last_index
                last_index = last_index + step
        ELSE reduce last_index
                step = (last_index - first_index)/2
                last_index = last_index - step;
        END IF
 END WHILE

Here is the call graph for this function:

Here is the caller graph for this function:

int popNode	(	NodeQueue *	q,
		Node *	n
	)

Pop the first node of the queue.

Parameters:

	q	Queue
	n	Pointer to a node

Returns:: 1 if node was succesfully poped 0 if the queue is empty

Here is the call graph for this function:

Here is the caller graph for this function:

void printQueue ( NodeQueue * q )

Print the queue.

Parameters:

q

Queue

Note:: Used only for debug

void pushNode	(	NodeQueue *	q,
		int	x,
		int	y
	)

Push a node into the queue in a FIFO mode.

Parameters:

	q	Queue
	x	Coordonate X of a cell
	y	Coordonate Y of a cell

Here is the call graph for this function:

Here is the caller graph for this function:

void pushNodeLast	(	NodeQueue *	q,
		int	x,
		int	y
	)

Push a node into the queue in a LIFO mode.

Parameters:

	q	Queue
	x	Coordonate X of a cell
	y	Coordonate Y of a cell

Here is the call graph for this function:

int queueIsEmpty ( NodeQueue * q )

Tests if a queue is empty.

Parameters:

q

A queue

Returns:: 1 if the queue is empty, 0 otherwise

Here is the caller graph for this function:

Path planer internal functions

Defines

Functions

A* Implementation Functions

A* related functions

A* Main function

Path simplifier functions

Queue functions

A* Constants

Detailed Description

Path Planer Architecture

Define Documentation

Function Documentation