public List <PathFinderNode> FindPath(Point start, Point end, out int totalCost) { totalCost = 0; mFound = false; mCloseNodeCounter = 0; if (mOpenNodeValue > 250) { Array.Clear(mCalcGrid, 0, mCalcGrid.Length); mOpenNodeValue = 1; mCloseNodeValue = 2; } else { 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) { mLocation = mOpen.Pop(); //Is it in closed list? means this node was already processed if (mCalcGrid [mLocation].Status == mCloseNodeValue) { continue; } if (mLocation == mEndLocation) { mCalcGrid [mLocation].Status = mCloseNodeValue; mFound = true; break; } if (mCloseNodeCounter > mMaxSteps) { return(null); } mLocationX = (ushort)(mLocation & mGridXMinus1); mLocationY = (ushort)(mLocation >> mGridYLog2); //Lets calculate each successors int maxi = mDiagonals ? 8 : 4; for (int i = 0; i < maxi; i++) { mNewLocationX = (mLocationX == 0 && mDirection [i, 0] < 0) ? (ushort)(mGridX - 1) : (ushort)(mLocationX + mDirection [i, 0]); mNewLocationY = (ushort)(mLocationY + mDirection [i, 1]); if (mNewLocationY >= mGridY) { continue; } if (mNewLocationX >= mGridX) { mNewLocationX = 0; } // Unbreakeable? mNewLocation = (mNewLocationY << mGridYLog2) + mNewLocationX; int gridValue = (mGrid [mNewLocation] & mGridBit) > 0 ? 1 : 0; if (gridValue == 0) { continue; } // Check custom validator if (mCustomCosts != null) { int customValue = mCustomCosts [mNewLocation]; if (mOnCellCross != null && customValue < 0) { customValue = mOnCellCross(mNewLocation); } if (customValue == 0) { continue; } else if (customValue < 0) { customValue = 0; } gridValue += customValue; } if (mHeavyDiagonals && i > 3) { mNewG = mCalcGrid [mLocation].G + (int)(gridValue * 2.41f); } else { mNewG = mCalcGrid [mLocation].G + gridValue; } if (mNewG > mMaxSearchCost) { continue; } //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; int dist = Math.Abs(mNewLocationX - end.X); dist = Math.Min(dist, mGridX - dist); switch (mFormula) { default: case HeuristicFormula.Manhattan: mH = mHEstimate * (dist + Math.Abs(mNewLocationY - end.Y)); break; case HeuristicFormula.MaxDXDY: mH = mHEstimate * (Math.Max(dist, Math.Abs(mNewLocationY - end.Y))); break; case HeuristicFormula.DiagonalShortCut: int h_diagonal = Math.Min(dist, Math.Abs(mNewLocationY - end.Y)); int h_straight = (dist + Math.Abs(mNewLocationY - end.Y)); mH = (mHEstimate * 2) * h_diagonal + mHEstimate * (h_straight - 2 * h_diagonal); break; case HeuristicFormula.Euclidean: mH = (int)(mHEstimate * Math.Sqrt(Math.Pow(dist, 2) + Math.Pow((mNewLocationY - end.Y), 2))); break; case HeuristicFormula.EuclideanNoSQR: mH = (int)(mHEstimate * (Math.Pow(dist, 2) + Math.Pow((mNewLocationY - end.Y), 2))); break; case HeuristicFormula.Custom1: Point dxy = new Point(dist, 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; } mCalcGrid [mNewLocation].F = mNewG + mH; 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]; totalCost = fNodeTmp.G; 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); return(mClose); } return(null); }
public List <PathFinderNode> FindPath(Point start, Point end, out float totalCost) { totalCost = 0; mFound = false; mCloseNodeCounter = 0; if (mOpenNodeValue > 250) { Array.Clear(mCalcGrid, 0, mCalcGrid.Length); mOpenNodeValue = 1; mCloseNodeValue = 2; } else { mOpenNodeValue += 2; mCloseNodeValue += 2; } mOpen.Clear(); mClose.Clear(); callNumber++; mLocation = (start.Y * mGridX) + start.X; mEndLocation = (end.Y * mGridX) + 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) { mLocation = mOpen.Pop(); // Is it in closed list? means this node was already processed if (mCalcGrid[mLocation].Status == mCloseNodeValue) { continue; } if (mLocation == mEndLocation) { mCalcGrid[mLocation].Status = mCloseNodeValue; mFound = true; break; } if (mCloseNodeCounter > mMaxSteps) { return(null); } mLocationX = (ushort)(mLocation % mGridX); mLocationY = (ushort)(mLocation / mGridX); //Lets calculate each successors for (int i = 0; i < 6; i++) { int cellSide; if (mLocationX % 2 == 0) { mNewLocationX = (ushort)(mLocationX + mDirectionHex0[i, 0]); mNewLocationY = (ushort)(mLocationY + mDirectionHex0[i, 1]); cellSide = mCellSide0[i]; } else { mNewLocationX = (ushort)(mLocationX + mDirectionHex1[i, 0]); mNewLocationY = (ushort)(mLocationY + mDirectionHex1[i, 1]); cellSide = mCellSide1[i]; } if (mNewLocationY >= mGridY) { continue; } if (mNewLocationX >= mGridX) { continue; } // Unbreakeable? mNewLocation = (mNewLocationY * mGridX) + mNewLocationX; if (mGrid[mNewLocation].isBlocked) { continue; } if (mGrid[mNewLocation].altitude <mMinAltitude || mGrid[mNewLocation].altitude> mMaxAltitude) { continue; } if (mTerrainCapability != TERRAIN_CAPABILITY.Any) { bool isWater = mGrid[mNewLocation].isWater; if (mTerrainCapability == TERRAIN_CAPABILITY.OnlyGround) { if (isWater) { continue; } } else { if (!isWater) { continue; } } } float gridValue = 1; float[] sideCosts = mGrid[mLocation].crossCost; if (sideCosts != null) { gridValue = sideCosts[cellSide]; } // Check custom validator if (mOnCellCross != null) { if (mGrid[mNewLocation].cachedCallNumber != callNumber) { mGrid[mNewLocation].cachedCallNumber = callNumber; mGrid[mNewLocation].cachedEventCostValue = mOnCellCross(mNewLocation); } gridValue += mGrid[mNewLocation].cachedEventCostValue; } if (gridValue <= 0) { gridValue = 1; } mNewG = mCalcGrid[mLocation].G + gridValue; if (mNewG > mMaxSearchCost) { continue; } //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; int dist = Math.Abs(mNewLocationX - end.X); switch (mFormula) { default: case HeuristicFormula.Manhattan: mH = mHEstimate * (dist + Math.Abs(mNewLocationY - end.Y)); break; case HeuristicFormula.MaxDXDY: mH = mHEstimate * (Math.Max(dist, Math.Abs(mNewLocationY - end.Y))); break; case HeuristicFormula.DiagonalShortCut: float h_diagonal = Math.Min(dist, Math.Abs(mNewLocationY - end.Y)); float h_straight = (dist + Math.Abs(mNewLocationY - end.Y)); mH = (mHEstimate * 2) * h_diagonal + mHEstimate * (h_straight - 2 * h_diagonal); break; case HeuristicFormula.Euclidean: mH = mHEstimate * Mathf.Sqrt(Mathf.Pow(dist, 2) + Mathf.Pow((mNewLocationY - end.Y), 2)); break; case HeuristicFormula.EuclideanNoSQR: mH = mHEstimate * (Mathf.Pow(dist, 2) + Mathf.Pow((mNewLocationY - end.Y), 2)); break; case HeuristicFormula.Custom1: Point dxy = new Point(dist, Math.Abs(end.Y - mNewLocationY)); float Orthogonal = Math.Abs(dxy.X - dxy.Y); float Diagonal = Math.Abs(((dxy.X + dxy.Y) - Orthogonal) / 2); mH = mHEstimate * (Diagonal + Orthogonal + dxy.X + dxy.Y); break; } mCalcGrid[mNewLocation].F = mNewG + mH; 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[mEndLocation]; totalCost = fNodeTmp.G; mGrid[mEndLocation].lastPathFindingCost = totalCost; 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; int loc = (posY * mGridX) + posX; fNodeTmp = mCalcGrid[loc]; fNode.F = fNodeTmp.F; fNode.G = fNodeTmp.G; mGrid[loc].lastPathFindingCost = fNodeTmp.G; fNode.H = 0; fNode.PX = fNodeTmp.PX; fNode.PY = fNodeTmp.PY; fNode.X = posX; fNode.Y = posY; } mClose.Add(fNode); return(mClose); } return(null); }