public List <AStar_PathFinderNode> FindPath(PathPoint start, PathPoint end)
{
AStar_PathFinderNode parentNode;
bool found = false;
int gridX = mGrid.GetUpperBound(0);
int gridY = mGrid.GetUpperBound(1);
mStop = false;
mStopped = false;
mOpen.Clear();
mClose.Clear();
sbyte[,] direction;
if (mDiagonals)
{
direction = new sbyte[8, 2] {
{ 0, -1 }, { 1, 0 }, { 0, 1 }, { -1, 0 }, { 1, -1 }, { 1, 1 }, { -1, 1 }, { -1, -1 }
}
}
;
else
{
direction = new sbyte[4, 2] {
{ 0, -1 }, { 1, 0 }, { 0, 1 }, { -1, 0 }
}
};
parentNode.G = 0;
parentNode.H = mHEstimate;
parentNode.F = parentNode.G + parentNode.H;
parentNode.X = start.Z;
parentNode.Y = start.Y;
parentNode.PX = parentNode.X;
parentNode.PY = parentNode.Y;
mOpen.Push(parentNode);
while (mOpen.Count > 0 && !mStop)
{
parentNode = mOpen.Pop();
if (parentNode.X == end.X && parentNode.Y == end.Y)
{
mClose.Add(parentNode);
found = true;
break;
}
if (mClose.Count > mSearchLimit)
{
mStopped = true;
return(null);
}
if (mPunishChangeDirection)
{
mHoriz = (parentNode.X - parentNode.PX);
}
//Lets calculate each successors
for (int i = 0; i < (mDiagonals ? 8 : 4); i++)
{
AStar_PathFinderNode newNode;
newNode.X = parentNode.X + direction[i, 0];
newNode.Y = parentNode.Y + direction[i, 1];
if (newNode.X < 0 || newNode.Y < 0 || newNode.X >= gridX || newNode.Y >= gridY)
{
continue;
}
int newG;
if (mHeavyDiagonals && i > 3)
{
newG = parentNode.G + (int)(mGrid[newNode.X, newNode.Y] * 2.41);
}
else
{
newG = parentNode.G + mGrid[newNode.X, newNode.Y];
}
if (newG == parentNode.G)
{
//Unbrekeable
continue;
}
if (mPunishChangeDirection)
{
if ((newNode.X - parentNode.X) != 0)
{
if (mHoriz == 0)
{
newG += 20;
}
}
if ((newNode.Y - parentNode.Y) != 0)
{
if (mHoriz != 0)
{
newG += 20;
}
}
}
int foundInOpenIndex = -1;
for (int j = 0; j < mOpen.Count; j++)
{
if (mOpen[j].X == newNode.X && mOpen[j].Y == newNode.Y)
{
foundInOpenIndex = j;
break;
}
}
if (foundInOpenIndex != -1 && mOpen[foundInOpenIndex].G <= newG)
{
continue;
}
int foundInCloseIndex = -1;
for (int j = 0; j < mClose.Count; j++)
{
if (mClose[j].X == newNode.X && mClose[j].Y == newNode.Y)
{
foundInCloseIndex = j;
break;
}
}
if (foundInCloseIndex != -1 && (mReopenCloseNodes || mClose[foundInCloseIndex].G <= newG))
{
continue;
}
newNode.PX = parentNode.X;
newNode.PY = parentNode.Y;
newNode.G = newG;
switch (mFormula)
{
default:
case AStar_HeuristicFormula.Manhattan:
newNode.H = mHEstimate * (Math.Abs(newNode.X - end.X) + Math.Abs(newNode.Y - end.Y));
break;
case AStar_HeuristicFormula.MaxDXDY:
newNode.H = mHEstimate * (Math.Max(Math.Abs(newNode.X - end.X), Math.Abs(newNode.Y - end.Y)));
break;
case AStar_HeuristicFormula.DiagonalShortCut:
int h_diagonal = Math.Min(Math.Abs(newNode.X - end.X), Math.Abs(newNode.Y - end.Y));
int h_straight = (Math.Abs(newNode.X - end.X) + Math.Abs(newNode.Y - end.Y));
newNode.H = (mHEstimate * 2) * h_diagonal + mHEstimate * (h_straight - 2 * h_diagonal);
break;
case AStar_HeuristicFormula.Euclidean:
newNode.H = (int)(mHEstimate * Math.Sqrt(Math.Pow((newNode.X - end.X), 2) + Math.Pow((newNode.Y - end.Y), 2)));
break;
case AStar_HeuristicFormula.EuclideanNoSQR:
newNode.H = (int)(mHEstimate * (Math.Pow((newNode.X - end.X), 2) + Math.Pow((newNode.Y - end.Y), 2)));
break;
case AStar_HeuristicFormula.Custom1:
PathPoint dxy = new PathPoint(Math.Abs(end.X - newNode.X), Math.Abs(end.Y - newNode.Y));
int Orthogonal = Math.Abs(dxy.X - dxy.Y);
int Diagonal = Math.Abs(((dxy.X + dxy.Y) - Orthogonal) / 2);
newNode.H = mHEstimate * (Diagonal + Orthogonal + dxy.X + dxy.Y);
break;
}
if (mTieBreaker)
{
int dx1 = parentNode.X - end.X;
int dy1 = parentNode.Y - end.Y;
int dx2 = start.X - end.X;
int dy2 = start.Y - end.Y;
int cross = Math.Abs(dx1 * dy2 - dx2 * dy1);
newNode.H = (int)(newNode.H + cross * 0.001);
}
newNode.F = newNode.G + newNode.H;
//It is faster if we leave the open node in the priority queue
//When it is removed, all nodes around will be closed, it will be ignored automatically
//if (foundInOpenIndex != -1)
// mOpen.RemoveAt(foundInOpenIndex);
//if (foundInOpenIndex == -1)
mOpen.Push(newNode);
}
mClose.Add(parentNode);
}
if (found)
{
AStar_PathFinderNode fNode = mClose[mClose.Count - 1];
for (int i = mClose.Count - 1; i >= 0; i--)
{
if (fNode.PX == mClose[i].X && fNode.PY == mClose[i].Y || i == mClose.Count - 1)
{
fNode = mClose[i];
}
else
{
mClose.RemoveAt(i);
}
}
mStopped = true;
return(mClose);
}
mStopped = true;
return(null);
}
/// <summary>
/// plan a safe route through the terrain, using the given start and end locations
/// </summary>
/// <param name="start_x_mm">starting x coordinate in mm</param>
/// <param name="start_y_mm">starting y coordinate in mm</param>
/// <param name="finish_x_mm">finishing x coordinate in mm</param>
/// <param name="finish_y_mm">finishing y coordinate in mm</param>
/// <returns>the path positions in millimetres</returns>
public List <float> CreatePlan(float start_x_mm, float start_y_mm,
float finish_x_mm, float finish_y_mm)
{
// get the dimension of the occupancy grid
int dimension = navigable_space.GetLength(0);
// calculate the start and end points on the safe navigation grid
// which may differ from the original grid size due to the power of 2 constraint
PathPoint PathStart = new PathPoint((int)((start_x_mm - OccupancyGridCentre_x_mm) / cellSize_mm) + (dimension / 2) + safety_offset,
(int)((start_y_mm - OccupancyGridCentre_y_mm) / cellSize_mm) + (dimension / 2) + safety_offset);
PathPoint PathEnd = new PathPoint((int)((finish_x_mm - OccupancyGridCentre_x_mm) / cellSize_mm) + (dimension / 2) + safety_offset,
(int)((finish_y_mm - OccupancyGridCentre_y_mm) / cellSize_mm) + (dimension / 2) + safety_offset);
// calculate a path, if one exists
List <AStar_PathFinderNode> path = pathfinder.FindPath(PathStart, PathEnd);
// convert the path into millimetres and store it in an array list
List <float> result = new List <float>();
if (path != null)
{
for (int i = 0; i < path.Count; i++)
{
AStar_PathFinderNode node = path[i];
float x = (float)((((node.X - safety_offset) - (dimension / 2)) * cellSize_mm) + OccupancyGridCentre_x_mm);
float y = (float)((((node.Y - safety_offset) - (dimension / 2)) * cellSize_mm) + OccupancyGridCentre_y_mm);
result.Add(x);
result.Add(y);
}
if (pathSmoothing)
{
ArrayList spline_x = new ArrayList();
ArrayList spline_y = new ArrayList();
float[] pts = new float[4 * 2];
int interval = 8;
for (int i = 0; i < result.Count; i += interval * 2)
{
for (int j = 0; j < 4; j++)
{
int idx = i - (2 * interval * 2) + (j * interval * 2);
if (idx < 0)
{
idx = 0;
}
if (idx >= result.Count - 1)
{
idx = result.Count - 2;
}
pts[j * 2] = (float)result[idx];
pts[(j * 2) + 1] = (float)result[idx + 1];
}
double temp = Math.Sqrt(Math.Pow(pts[2 * 2] - pts[1 * 2], 2F) + Math.Pow(pts[(2 * 2) + 1] - pts[(1 * 2) + 1], 2F));
int interpol = System.Convert.ToInt32(temp);
SplinePoint(pts, interpol, spline_x, spline_y);
}
if (spline_x.Count > 1)
{
List <float> smoothed_result = new List <float>();
smoothed_result.Add((float)result[0]);
smoothed_result.Add((float)result[1]);
for (int i = 1; i < spline_x.Count; i++)
{
float x = Convert.ToSingle((double)spline_x[i]);
float y = Convert.ToSingle((double)spline_y[i]);
smoothed_result.Add(x);
smoothed_result.Add(y);
}
smoothed_result.Add((float)result[result.Count - 2]);
smoothed_result.Add((float)result[result.Count - 1]);
result = smoothed_result;
}
}
}
return(result);
}
