/// <summary> ///Check if first angle is start point /// </summary> /// <param name="start"></param> /// <param name="parentNode"></param> /// <returns></returns> public static bool IsAngleStartPoint(Location start, PathFinderNode parentNode) { if (start.X == parentNode.ANX && start.Y == parentNode.ANY && start.Z == parentNode.ANZ) { return(true); } return(false); }
public static bool IfMinLength(byte length, PathFinderNode parentNode, int newNodeX, int newNodeY, int newNodeZ) { if ((newNodeY - parentNode.ANY) != 0 && (newNodeX - parentNode.ANX) != 0) { if (Math.Abs(parentNode.Y - parentNode.ANY) == 0 && Math.Abs(parentNode.X - parentNode.ANX) < length) { return(false); } if (Math.Abs(parentNode.X - parentNode.ANX) == 0 && Math.Abs(parentNode.Y - parentNode.ANY) < length) { return(false); } WriteNodeAN(parentNode); } else if ((newNodeY - parentNode.ANY) != 0 && (newNodeZ - parentNode.ANZ) != 0) { if (Math.Abs(parentNode.Y - parentNode.ANY) == 0 && Math.Abs(parentNode.Z - parentNode.ANZ) < length) { return(false); } if (Math.Abs(parentNode.Z - parentNode.ANZ) == 0 && Math.Abs(parentNode.Y - parentNode.ANY) < length) { return(false); } WriteNodeAN(parentNode); } else if ((newNodeX - parentNode.ANX) != 0 && (newNodeZ - parentNode.ANZ) != 0) { if (Math.Abs(parentNode.Z - parentNode.ANZ) == 0 && Math.Abs(parentNode.X - parentNode.ANX) < length) { return(false); } if (Math.Abs(parentNode.X - parentNode.ANX) == 0 && Math.Abs(parentNode.Z - parentNode.ANZ) < length) { return(false); } WriteNodeAN(parentNode); } else { newNodeANX = parentNode.ANX; newNodeANY = parentNode.ANY; newNodeANZ = parentNode.ANZ; } return(true); }
public static int GetNewG(int newNodeX, int newNodeY, int newNodeZ, PathFinderNode parentNode, Location start, int NodeG) { if (PointChecker.IsAngleStartPoint(start, parentNode)) { return(NodeG); } //Check if direction was changed int k = 0; if ((newNodeX - parentNode.PX) != 0) { k++; } if ((newNodeY - parentNode.PY) != 0) { k++; } if ((newNodeZ - parentNode.PZ) != 0) { k++; } if (k >= 2) { NodeG += 5; //Check distance from parent node to angle //int maxPointsToAngle = 8; //int dist = Math.Abs(parentNode.X - parentNode.ANX) + // Math.Abs(parentNode.Y - parentNode.ANY) + // Math.Abs(parentNode.Z - parentNode.ANZ); //if (dist < maxPointsToAngle) //{ // //Hard punish // NodeG += 30; //} //else //{ // NodeG += 5; //} } return(NodeG); }
public List <PathFinderNode> FindPath(Location start, Location end, sbyte[,] direction) { PathFinderNode parentNode; bool found = false; int gridX = mGrid.GetUpperBound(0) + 1; int gridY = mGrid.GetUpperBound(1) + 1; int gridZ = mGrid.GetUpperBound(2) + 1; mStop = false; mStopped = false; mOpen.Clear(); mClose.Clear(); parentNode.G = 0; parentNode.H = mHEstimate; parentNode.F = parentNode.G + parentNode.H; parentNode.X = start.X; parentNode.Y = start.Y; parentNode.Z = start.Z; parentNode.PX = parentNode.X; parentNode.PY = parentNode.Y; parentNode.PZ = parentNode.Z; parentNode.Id = 0; parentNode.ANX = start.X; parentNode.ANY = start.Y; parentNode.ANZ = start.Z; //Clearance zone creation CLZChecker cLZChecker = null; if (mCLZdist != 0) { //Clearance zone creation RefCLZCreator refCLZCreator = new RefCLZCreator(mCLZdist); mClearanceZone = refCLZCreator.Create(); cLZChecker = new CLZChecker(mGrid, mClearanceZone); } mOpen.Push(parentNode); while (mOpen.Count > 0 && !mStop) { parentNode = mOpen.Pop(); if (parentNode.X == end.X && parentNode.Y == end.Y && parentNode.Z == end.Z) { 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 : direction.GetUpperBound(0)); i++) { PathFinderNode newNode; newNode.X = parentNode.X + direction[i, 0]; newNode.Y = parentNode.Y + direction[i, 1]; newNode.Z = parentNode.Z + direction[i, 2]; newNode.Id = 0; if (newNode.X < 0 || newNode.Y < 0 || newNode.Z < 0 || newNode.X >= gridX || newNode.Y >= gridY || newNode.Z >= gridZ) { continue; } int newG; if (mHeavyDiagonals && i > 3) { newG = parentNode.G + (int)(mGrid[newNode.X, newNode.Y, newNode.Z] * 2.41); } else { newG = parentNode.G + mGrid[newNode.X, newNode.Y, newNode.Z]; } //check point by obstacles and clerance zone if (cLZChecker != null) { if (!cLZChecker.CheckNode(newNode.X, newNode.Y, newNode.Z, gridX, gridY, gridZ)) { continue; } } else { if (mGrid[newNode.X, newNode.Y, newNode.Z] == 1) { continue; } } if (mCompactPath) { int cost = 2; if ((start.X == end.X) && (newNode.X - start.X) != 0) { if ((newNode.Y - parentNode.Y) != 0) { newG += cost; } } if (start.Y == end.Y && (newNode.Y - start.Y) != 0) { if ((newNode.X - parentNode.X) != 0) { newG += cost; } } if (start.Z == end.Z && (newNode.Z - start.Z) != 0) { if ((newNode.X - parentNode.X) != 0 || (newNode.Y - parentNode.Y) != 0) { newG += cost; } } } if (mPunishChangeDirection) { newG += PunishChangeDirectionChecker.GetNewG(newNode.X, newNode.Y, newNode.Z, parentNode, start, newG); } int foundInOpenIndex = -1; for (int j = 0; j < mOpen.Count; j++) { if (mOpen[j].X == newNode.X && mOpen[j].Y == newNode.Y && mOpen[j].Z == newNode.Z) { 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 && mClose[j].Z == newNode.Z) { foundInCloseIndex = j; break; } } if (foundInCloseIndex != -1 && (mReopenCloseNodes || mClose[foundInCloseIndex].G <= newG)) { continue; } newNode.PX = parentNode.X; newNode.PY = parentNode.Y; newNode.PZ = parentNode.Z; newNode.G = newG; newNode.ANX = 0; newNode.ANY = 0; newNode.ANZ = 0; if (mANGlength != 0) { if (!NodesCheker.IfMinLength(mANGlength, parentNode, newNode.X, newNode.Y, newNode.Z)) { continue; } else { newNode.ANX = NodesCheker.newNodeANX; newNode.ANY = NodesCheker.newNodeANY; newNode.ANZ = NodesCheker.newNodeANZ; } } switch (mFormula) { default: case HeuristicFormula.Manhattan: newNode.H = mHEstimate * (Math.Abs(newNode.X - end.X) + Math.Abs(newNode.Y - end.Y) + Math.Abs(newNode.Z - end.Z)); break; case HeuristicFormula.MaxDXDY: newNode.H = mHEstimate * (Math.Max(Math.Abs(newNode.X - end.X), Math.Abs(newNode.Y - end.Y))); break; case 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 HeuristicFormula.Euclidean: newNode.H = (int)(mHEstimate * Math.Sqrt(Math.Pow((newNode.X - end.X), 2) + Math.Pow((newNode.Y - end.Y), 2))); break; case HeuristicFormula.EuclideanNoSQR: newNode.H = (int)(mHEstimate * (Math.Pow((newNode.X - end.X), 2) + Math.Pow((newNode.Y - end.Y), 2))); break; case HeuristicFormula.Custom1: Point dxy = new Point(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 dz1 = parentNode.Z - end.Z; int dx2 = start.X - end.X; int dy2 = start.Y - end.Y; int dz2 = start.Z - end.Z; int cross = Math.Abs(dx1 * dy2 - dx2 * dy1); //fix z here newNode.H = (int)(newNode.H + cross * 0.001); } newNode.F = newNode.G + newNode.H; mOpen.Push(newNode); } mClose.Add(parentNode); } if (found) { 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 && fNode.PZ == mClose[i].Z || i == mClose.Count - 1) { fNode = mClose[i]; } else { mClose.RemoveAt(i); } } mStopped = true; return(mClose); } mStopped = true; return(null); }
private static void WriteNodeAN(PathFinderNode parentNode) { newNodeANX = parentNode.X; newNodeANY = parentNode.Y; newNodeANZ = parentNode.Z; }
public List <PathFinderNode> FindPath(Location start, Location end) { PathFinderNode parentNode; bool found = false; int gridX = mGrid.GetUpperBound(0) + 1; int gridY = mGrid.GetUpperBound(1) + 1; int gridZ = mGrid.GetUpperBound(2) + 1; mStop = false; mStopped = false; mOpen.Clear(); mClose.Clear(); parentNode.G = 0; parentNode.H = mHEstimate; parentNode.F = parentNode.G + parentNode.H; parentNode.X = start.X; parentNode.Y = start.Y; parentNode.Z = start.Z; parentNode.PX = parentNode.X; parentNode.PY = parentNode.Y; parentNode.PZ = parentNode.Z; parentNode.PZ = parentNode.Z; parentNode.Id = 1; mOpen.Push(parentNode); while (mOpen.Count > 0 && !mStop) { parentNode = mOpen.Pop(); if (parentNode.X == end.X && parentNode.Y == end.Y && parentNode.Z == end.Z) { mClose.Add(parentNode); found = true; break; } if (mClose.Count > mSearchLimit) { mStopped = true; return(null); } if (mPunishChangeDirection) { mHoriz = (parentNode.X - parentNode.PX); } Neighbor neighb = new Neighbor(Graph.Matrix[parentNode.X, parentNode.Y, parentNode.Z], Graph); List <Node> neighbors = neighb.GetNeighbors(); //Lets calculate each successors foreach (Node neighbor in neighbors) { PathFinderNode newNode; newNode.X = neighbor.Location.X; newNode.Y = neighbor.Location.Y; newNode.Z = neighbor.Location.Z; newNode.Id = Graph.Matrix[newNode.X, newNode.Y, newNode.Z]; if (newNode.X < 0 || newNode.Y < 0 || newNode.Z < 0 || newNode.X >= gridX || newNode.Y >= gridY || newNode.Z >= gridZ) { continue; } int newG; //if (mHeavyDiagonals && i > 3) // newG = parentNode.G + (int)(mGrid[newNode.X, newNode.Y, newNode.Z] * 2.41); //else //newG = parentNode.G + mGrid[newNode.X, newNode.Y, newNode.Z]; newG = parentNode.G + neighbor.LengthToBase; //check obstacles if (newG == 1) { 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 && mOpen[j].Z == newNode.Z) { 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 && mClose[j].Z == newNode.Z) { foundInCloseIndex = j; break; } } if (foundInCloseIndex != -1 && (mReopenCloseNodes || mClose[foundInCloseIndex].G <= newG)) { continue; } newNode.PX = parentNode.X; newNode.PY = parentNode.Y; newNode.PZ = parentNode.Z; newNode.G = newG; switch (mFormula) { default: case HeuristicFormula.Manhattan: newNode.H = mHEstimate * (Math.Abs(newNode.X - end.X) + Math.Abs(newNode.Y - end.Y) + Math.Abs(newNode.Z - end.Z)); break; case HeuristicFormula.MaxDXDY: newNode.H = mHEstimate * (Math.Max(Math.Abs(newNode.X - end.X), Math.Abs(newNode.Y - end.Y))); break; case 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 HeuristicFormula.Euclidean: newNode.H = (int)(mHEstimate * Math.Sqrt(Math.Pow((newNode.X - end.X), 2) + Math.Pow((newNode.Y - end.Y), 2))); break; case HeuristicFormula.EuclideanNoSQR: newNode.H = (int)(mHEstimate * (Math.Pow((newNode.X - end.X), 2) + Math.Pow((newNode.Y - end.Y), 2))); break; case HeuristicFormula.Custom1: Point dxy = new Point(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 dz1 = parentNode.Z - end.Z; int dx2 = start.X - end.X; int dy2 = start.Y - end.Y; int dz2 = start.Z - end.Z; int cross = Math.Abs(dx1 * dy2 - dx2 * dy1); //fix z here newNode.H = (int)(newNode.H + cross * 0.001); } newNode.F = newNode.G + newNode.H; mOpen.Push(newNode); } mClose.Add(parentNode); } if (found) { 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 && fNode.PZ == mClose[i].Z || i == mClose.Count - 1) { fNode = mClose[i]; } else { mClose.RemoveAt(i); } } mStopped = true; return(mClose); } mStopped = true; return(null); }