/// <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); }
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); }
public List <AStar_PathFinderNode> FindPath(PathPoint start, PathPoint end) { lock (this) { // Is faster if we don't clear the matrix, just assign different values for open and close and ignore the rest // I could have user Array.Clear() but using unsafe code is faster, no much but it is. //fixed (PathFinderNodeFast* pGrid = tmpGrid) // ZeroMemory((byte*) pGrid, sizeof(PathFinderNodeFast) * 1000000); mFound = false; mStop = false; mStopped = false; mCloseNodeCounter = 0; mOpenNodeValue += 2; mCloseNodeValue += 2; mOpen.Clear(); mClose.Clear(); mLocation = (start.Y << mGridYLog2) + start.X; mEndLocation = (end.Y << mGridYLog2) + end.X; mCalcGrid[mLocation].G = 0; mCalcGrid[mLocation].F = mHEstimate; mCalcGrid[mLocation].PX = (ushort)start.X; mCalcGrid[mLocation].PY = (ushort)start.Y; mCalcGrid[mLocation].Status = mOpenNodeValue; mOpen.Push(mLocation); while (mOpen.Count > 0 && !mStop) { mLocation = mOpen.Pop(); //Is it in closed list? means this node was already processed if (mCalcGrid[mLocation].Status == mCloseNodeValue) { continue; } mLocationX = (ushort)(mLocation & mGridXMinus1); mLocationY = (ushort)(mLocation >> mGridYLog2); if (mLocation == mEndLocation) { mCalcGrid[mLocation].Status = mCloseNodeValue; mFound = true; break; } if (mCloseNodeCounter > mSearchLimit) { mStopped = true; return(null); } if (mPunishChangeDirection) { mHoriz = (mLocationX - mCalcGrid[mLocation].PX); } //Lets calculate each successors for (int i = 0; i < (mDiagonals ? 8 : 4); i++) { mNewLocationX = (ushort)(mLocationX + mDirection[i, 0]); mNewLocationY = (ushort)(mLocationY + mDirection[i, 1]); mNewLocation = (mNewLocationY << mGridYLog2) + mNewLocationX; if (mNewLocationX >= mGridX || mNewLocationY >= mGridY) { continue; } if (mCalcGrid[mNewLocation].Status == mCloseNodeValue && !mReopenCloseNodes) { continue; } Byte gridValue = (Byte)(255 - mGrid[mNewLocationX][mNewLocationY]); if (gridValue < 1) { gridValue = 1; } // Unbreakeable? if (gridValue == 0) { continue; } if (mHeavyDiagonals && i > 3) { mNewG = mCalcGrid[mLocation].G + (int)(gridValue * 2.41); } else { mNewG = mCalcGrid[mLocation].G + gridValue; } if (mPunishChangeDirection) { if ((mNewLocationX - mLocationX) != 0) { if (mHoriz == 0) { mNewG += Math.Abs(mNewLocationX - end.X) + Math.Abs(mNewLocationY - end.Y); } } if ((mNewLocationY - mLocationY) != 0) { if (mHoriz != 0) { mNewG += Math.Abs(mNewLocationX - end.X) + Math.Abs(mNewLocationY - end.Y); } } } //Is it open or closed? if (mCalcGrid[mNewLocation].Status == mOpenNodeValue || mCalcGrid[mNewLocation].Status == mCloseNodeValue) { // The current node has less code than the previous? then skip this node if (mCalcGrid[mNewLocation].G <= mNewG) { continue; } } mCalcGrid[mNewLocation].PX = mLocationX; mCalcGrid[mNewLocation].PY = mLocationY; mCalcGrid[mNewLocation].G = mNewG; switch (mFormula) { default: case AStar_HeuristicFormula.Manhattan: mH = mHEstimate * (Math.Abs(mNewLocationX - end.X) + Math.Abs(mNewLocationY - end.Y)); break; case AStar_HeuristicFormula.MaxDXDY: mH = mHEstimate * (Math.Max(Math.Abs(mNewLocationX - end.X), Math.Abs(mNewLocationY - end.Y))); break; case AStar_HeuristicFormula.DiagonalShortCut: int h_diagonal = Math.Min(Math.Abs(mNewLocationX - end.X), Math.Abs(mNewLocationY - end.Y)); int h_straight = (Math.Abs(mNewLocationX - end.X) + Math.Abs(mNewLocationY - end.Y)); mH = (mHEstimate * 2) * h_diagonal + mHEstimate * (h_straight - 2 * h_diagonal); break; case AStar_HeuristicFormula.Euclidean: mH = (int)(mHEstimate * Math.Sqrt(Math.Pow((mNewLocationY - end.X), 2) + Math.Pow((mNewLocationY - end.Y), 2))); break; case AStar_HeuristicFormula.EuclideanNoSQR: mH = (int)(mHEstimate * (Math.Pow((mNewLocationX - end.X), 2) + Math.Pow((mNewLocationY - end.Y), 2))); break; case AStar_HeuristicFormula.Custom1: PathPoint dxy = new PathPoint(Math.Abs(end.X - mNewLocationX), Math.Abs(end.Y - mNewLocationY)); int Orthogonal = Math.Abs(dxy.X - dxy.Y); int Diagonal = Math.Abs(((dxy.X + dxy.Y) - Orthogonal) / 2); mH = mHEstimate * (Diagonal + Orthogonal + dxy.X + dxy.Y); break; } if (mTieBreaker) { int dx1 = mLocationX - end.X; int dy1 = mLocationY - end.Y; int dx2 = start.X - end.X; int dy2 = start.Y - end.Y; int cross = Math.Abs(dx1 * dy2 - dx2 * dy1); mH = (int)(mH + cross * 0.001); } mCalcGrid[mNewLocation].F = mNewG + mH; //It is faster if we leave the open node in the priority queue //When it is removed, it will be already closed, it will be ignored automatically //if (tmpGrid[newLocation].Status == 1) //{ // //int removeX = newLocation & gridXMinus1; // //int removeY = newLocation >> gridYLog2; // mOpen.RemoveLocation(newLocation); //} //if (tmpGrid[newLocation].Status != 1) //{ mOpen.Push(mNewLocation); //} mCalcGrid[mNewLocation].Status = mOpenNodeValue; } mCloseNodeCounter++; mCalcGrid[mLocation].Status = mCloseNodeValue; } if (mFound) { mClose.Clear(); int posX = end.X; int posY = end.Y; PathFinderNodeFast fNodeTmp = mCalcGrid[(end.Y << mGridYLog2) + end.X]; AStar_PathFinderNode fNode; fNode.F = fNodeTmp.F; fNode.G = fNodeTmp.G; fNode.H = 0; fNode.PX = fNodeTmp.PX; fNode.PY = fNodeTmp.PY; fNode.X = end.X; fNode.Y = end.Y; while (fNode.X != fNode.PX || fNode.Y != fNode.PY) { mClose.Add(fNode); posX = fNode.PX; posY = fNode.PY; fNodeTmp = mCalcGrid[(posY << mGridYLog2) + posX]; fNode.F = fNodeTmp.F; fNode.G = fNodeTmp.G; fNode.H = 0; fNode.PX = fNodeTmp.PX; fNode.PY = fNodeTmp.PY; fNode.X = posX; fNode.Y = posY; } mClose.Add(fNode); 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); }