aalgorithm.c File Reference

#include "aalgorithm.h"
#include "pathqueue.h"
#include "map.h"
#include <math.h>
#include <stdlib.h>

Include dependency graph for aalgorithm.c:

Go to the source code of this file.

#define	SQRT2   1.41421356
#define	WALL_COST   1000
	Cost fo jumping a wall.
GraphMapCell	graph [MAP_HEIGHT][MAP_WIDTH]
void	pushNodeInOrder (NodeQueue *q, int x, int y)
float	cFunction (int x1, int y1, int x2, int y2)
Functions
int	aAlgorithm (double xstart_real, double ystart_real, double xgoal_real, double ygoal_real, double **original_path)
void	calculateMapHeuristic (int xgoal, int ygoal)
float	calculateCost (int x1, int y1, int x2, int y2)
A* Implementation Functions
This functions have not a meaning in A* algorithm, but are needed for implementation.
void	initGraphStructure (void)
A* Other Functions
int	getPathLength (int xstart, int ystart, int xgoal, int ygoal)

---

Detailed Description

Implementation of the A* algorithm. Main functions.

Author:

Jose Maria Martin Laguna <jmmartin@etud.insa-toulouse.fr>

Note:

The author asumes that the reader knows how A* algorithm works. In this documentation only the implementation is explained. More information "Principles of Robot Motion" by H. Choset&others (Algorithm 24, p531)

Definition in file aalgorithm.c.

---

Function Documentation

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

Search the shortest path between two points

Parameters:

	xstart	Coordonate X of the start point
	ystart	Coordonate Y of the start point
	xgoal	Coordonate X of the goal point
	ygoal	Coordonate Y of the goal point
	original_path	[out] The original path. See details bellow.

See also:

graph

Returns:

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

The original path will be return as a list of points as follows: {xstart,ystart, x[1], y[1], x[2], y[2], ..., x[nbpoints-2], y[nbpoints-2], xgoal, ygoal}

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

1. Init graph structure with default values.

2. Calculate Heuristic distances.

3. Initial values : put start node in queue.

4. 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.

5. Deallocates memory of the queue

Definition at line 45 of file aalgorithm.c.

Referenced by path_planner().

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

Definition at line 272 of file aalgorithm.c.

Referenced by calculateMapHeuristic().

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 and the goal is .

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

Definition at line 201 of file aalgorithm.c.

Referenced by aAlgorithm().

Here is the caller graph for this function:

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

Returns the number of points of original path

Parameters:

	xstart	Coordonate X of the first cell
	ystart	Coordonate Y of the first cell
	xgoal	Coordonate X of the second cell
	ygoal	Coordonate Y of the second cell

Returns:

number of points of original path This function set also the path cell MAP_PATH, MAP_GOAL, MAP_START attributes into the map.

Definition at line 370 of file aalgorithm.c.

Referenced by aAlgorithm().

Here is the caller graph for this function:

void initGraphStructure

(

void

)

Init Graph structure with default values

See also:

graph

Definition at line 291 of file aalgorithm.c.

Referenced by aAlgorithm().

Here is the caller graph for this function:

---