Hi everyone, I have been working on my version of an Auto Navigation Bot which takes in the start and the end location along with a grid map to find a path joining the Start and End.
Base on my reading I found that there are quiet a lot of algorithms out there but the simplest one was the Wavefront algorithm.
C# implementations is as follows :
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
/*
* This is an implementation of a modified form of the Wavefront Algorithm
* which can be found on http://pirate.shu.edu/~wachsmut/Teaching/CSAS-Robotics/Challenges/06_Wavefront/index.html
*
* NOTE:- Adjacent cells only represent the cells which can be reached from the the source by one hop
* in any primary direction (only E-W-N-S)
*/
namespace WavefromtMotion
{
/// <summary>
/// Implementation of the Wavefront Algorithm.
/// </summary>
public class Wavefront
{
/// <summary>
/// Enum representing the various states of the grid cells.
/// </summary>
public enum LocationType
{
/// <summary>
/// Represents a wall.
/// </summary>
Wall = 255,
/// <summary>
/// Represents an empty cell (i.e. allows pass through).
/// </summary>
Empty = 0,
/// <summary>
/// Represents destination cell.
/// </summary>
Destination = 254,
/// <summary>
/// Represents source cell.
/// </summary>
Source = 1
}
/// <summary>
/// Struct representing a location on the map grid.
/// </summary>
public struct Location
{
/// <summary>
/// Coordinates.
/// </summary>
public int X, Y;
public static Location NullType
{
get { return new Location(-1, -1); }
}
public Location(int pX, int pY)
{
X = pX;
Y = pY;
}
public override bool Equals(object obj)
{
if (obj == null)
return (this.Equals(Location.NullType));
else
{
return ((((Location)obj).X == this.X) && (((Location)obj).Y == this.Y));
}
}
}
int[,] objGrid;
bool hitSource, hitDest;
int recurCount = 0;
Location source, destination;
/// <summary>
/// Constructor.
/// </summary>
/// <param name="pMap">Grid representing the floorplan in a two dimentional array.</param>
public Wavefront(int[,] pMap)
{
objGrid = pMap;
source = Location.NullType;
destination = Location.NullType;
}
/// <summary>
/// This method will calculate the weights for all the cells on the grid.
/// </summary>
public void GeneratePathWeights()
{
while ((!(hitDest && hitSource)) || (recurCount < objGrid.GetLength(0)))
{
recurCount++;
for (int y = 0; y < objGrid.GetLength(1); y++)
{
for (int x = 0; x < objGrid.GetLength(0); x++)
{
if (objGrid[x, y] == (int)LocationType.Wall)
continue;
else if (objGrid[x, y] == (int)LocationType.Empty)
{
if (IsAdjacentLocOfType(x, y, LocationType.Source))
{
source = GetAdjacentLowestCell(x, y);
objGrid[x, y] = 2;
hitSource = true;
}
else
{
int adjCnt = GetAdjacentLowestCount(x, y);
if (adjCnt != (int)LocationType.Wall)
objGrid[x, y] = adjCnt + 1;
}
}
else if (objGrid[x, y] == (int)LocationType.Destination)
{
destination = new Location(x, y);
int adjCnt = GetAdjacentLowestCount(x, y);
if (adjCnt != (int)LocationType.Wall)
{
hitDest = true;
}
}
}
}
}
}
/// <summary>
/// Gets the path found between the Source and the Destination.
/// </summary>
/// <returns>An array of Location, representing the path found.</returns>
/// <remarks>Null returned if no path is found.</remarks>
public Location[] GetPath()
{
if (destination.Equals(Location.NullType) || source.Equals(Location.NullType))
return null;
Location[] path = new Location[10];
bool exitLoop = false;
int locPointer = 0;
while (!exitLoop)
{
if (locPointer == 0)
{
path[locPointer] = destination;
}
else
{
path[locPointer] = GetAdjacentLowestCell(path[locPointer - 1].X, path[locPointer - 1].Y);
if (path[locPointer].Equals(source))
{
exitLoop = true;
}
}
if (!exitLoop)
{
locPointer++;
if (locPointer == path.Length - 1)
{
//Resize the array by 10 if needed.
Array.Resize<Location>(ref path, path.Length + 10);
}
}
}
//Again resige the array to only contain the actual path.
Array.Resize<Location>(ref path, locPointer + 1);
return path;
}
/// <summary>
/// Gets the adjacent cell number which has the lowest count.
/// </summary>
/// <param name="pXCord">X coordinate.</param>
/// <param name="pYCord">Y coordinate.</param>
/// <returns>Location of the lowest cell.</returns>
private Location GetAdjacentLowestCell(int pXCord, int pYCord)
{
int lowestCnt = (int)LocationType.Wall;
Location lowestCell = Location.NullType;
if (pXCord < objGrid.GetLength(0) - 1)
{
if (objGrid[pXCord + 1, pYCord] != (int)LocationType.Empty && objGrid[pXCord + 1, pYCord] != (int)LocationType.Destination)
{
if (lowestCnt > objGrid[pXCord + 1, pYCord])
{
lowestCnt = objGrid[pXCord + 1, pYCord];
lowestCell = new Location(pXCord + 1, pYCord);
}
}
}
if (pXCord != 0 && pXCord - 1 >= 0)
{
if (objGrid[pXCord - 1, pYCord] != (int)LocationType.Empty && objGrid[pXCord - 1, pYCord] != (int)LocationType.Destination)
{
if (lowestCnt > objGrid[pXCord - 1, pYCord])
{
lowestCnt = objGrid[pXCord - 1, pYCord];
lowestCell = new Location(pXCord - 1, pYCord);
}
}
}
if (pYCord < objGrid.GetLength(1) - 1)
{
if (objGrid[pXCord, pYCord + 1] != (int)LocationType.Empty && objGrid[pXCord, pYCord + 1] != (int)LocationType.Destination)
{
if (lowestCnt > objGrid[pXCord, pYCord + 1])
{
lowestCnt = objGrid[pXCord, pYCord + 1];
lowestCell = new Location(pXCord, pYCord + 1);
}
}
}
if (pYCord != 0 && pYCord - 1 >= 0)
{
if (objGrid[pXCord, pYCord - 1] != (int)LocationType.Empty && objGrid[pXCord, pYCord - 1] != (int)LocationType.Destination)
{
if (lowestCnt > objGrid[pXCord, pYCord - 1])
{
lowestCnt = objGrid[pXCord, pYCord - 1];
lowestCell = new Location(pXCord, pYCord - 1);
}
}
}
return lowestCell;
}
/// <summary>
/// Gets the lowest weight of the surrounding cells.
/// </summary>
/// <param name="pXCord">X coordinate.</param>
/// <param name="pYCord">Y coordinate.</param>
/// <returns>Lowest weight.</returns>
private int GetAdjacentLowestCount(int pXCord, int pYCord)
{
Location lowestCell = GetAdjacentLowestCell(pXCord, pYCord);
if (lowestCell.Equals(Location.NullType))
return (int)LocationType.Wall;
else
return objGrid[lowestCell.X, lowestCell.Y];
}
/// <summary>
/// Checks if any of the adjacent cells of of the required type.
/// </summary>
/// <param name="pXCord">X coordinate.</param>
/// <param name="pYCord">Y coordinate.</param>
/// <param name="pType">Type to check for.</param>
/// <returns>Boolean result of the check.</returns>
private bool IsAdjacentLocOfType(int pXCord, int pYCord, LocationType pType)
{
if (pXCord < objGrid.GetLength(0) - 1)
{
if (objGrid[pXCord + 1, pYCord] == (int)pType)
{
return true;
}
if (pXCord - 1 >= 0)
{
if (objGrid[pXCord - 1, pYCord] == (int)pType)
{
return true;
}
}
}
if (pYCord < objGrid.GetLength(1) - 1)
{
if (objGrid[pXCord, pYCord + 1] == (int)pType)
{
return true;
}
if (pYCord - 1 >= 0)
{
if (objGrid[pXCord, pYCord - 1] == (int)pType)
{
return true;
}
}
}
return false;
}
}
}
[line]
Sample usage:
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
namespace WavefromtMotion
{
class Program
{
static void Main(string[] args)
{
int[,] map= new int[,] {{0,0,0,255,254,0},
{0,0,0,0,0,0},
{0,0,0,255,255,0},
{0,255,0,255,0,0},
{0,0,0,0,0,0},
{1,255,0,0,0,0}};
var objfront = new Wavefront(map);
objfront.GeneratePathWeights();
Wavefront.Location[] path = objfront.GetPath();
foreach (Wavefront.Location lc in path)
{
map[lc.X,lc.Y] = -1;
}
String disp = String.Empty;
for (int x1 = 0; x1 < map.GetLength(0); x1++)
{
for (int y1 = 0; y1 < map.GetLength(1); y1++)
{
if (map[x1, y1] == (int)Wavefront.LocationType.Source)
disp = "S";
else if (map[x1, y1] == (int)Wavefront.LocationType.Wall)
disp = "X";
else if (map[x1, y1] == (int)Wavefront.LocationType.Destination)
disp = "D";
else if (map[x1, y1] == -1)
disp = "-";
else
disp = map[x1, y1].ToString();
Console.Write(disp + "\t");
}
Console.WriteLine();
}
Console.ReadLine();
}
}
}
This algorithm can further be updated to use adapting routing (In case a new object is encountered on the floor), by scanning the environment at every cell and if a difference is found, updating the grid and re generating the path weights.
If anyone knows of a better algorithm, I would like to try it out 
