private static GridPos? jumpLoop(JumpPointParam iParam, int iX, int iY, int iPx, int iPy) { GridPos? retVal = null; Stack<JumpSnapshot> stack = new Stack<JumpSnapshot>(); JumpSnapshot currentSnapshot = new JumpSnapshot(); JumpSnapshot newSnapshot = null; currentSnapshot.iX = iX; currentSnapshot.iY = iY; currentSnapshot.iPx = iPx; currentSnapshot.iPy = iPy; currentSnapshot.stage = 0; stack.Push(currentSnapshot); while (stack.Count != 0) { currentSnapshot = stack.Pop(); switch (currentSnapshot.stage) { case 0: if (!iParam.SearchGrid.IsWalkableAt(currentSnapshot.iX, currentSnapshot.iY)) { retVal = null; continue; } else if (iParam.SearchGrid.GetNodeAt(currentSnapshot.iX, currentSnapshot.iY).Equals(iParam.EndNode)) { retVal = new GridPos(currentSnapshot.iX, currentSnapshot.iY); continue; } currentSnapshot.tDx = currentSnapshot.iX - currentSnapshot.iPx; currentSnapshot.tDy = currentSnapshot.iY - currentSnapshot.iPy; currentSnapshot.jx = null; currentSnapshot.jy = null; if (iParam.CrossCorner) { // check for forced neighbors // along the diagonal if (currentSnapshot.tDx != 0 && currentSnapshot.tDy != 0) { if ((iParam.SearchGrid.IsWalkableAt(currentSnapshot.iX - currentSnapshot.tDx, currentSnapshot.iY + currentSnapshot.tDy) && !iParam.SearchGrid.IsWalkableAt(currentSnapshot.iX - currentSnapshot.tDx, currentSnapshot.iY)) || (iParam.SearchGrid.IsWalkableAt(currentSnapshot.iX + currentSnapshot.tDx, currentSnapshot.iY - currentSnapshot.tDy) && !iParam.SearchGrid.IsWalkableAt(currentSnapshot.iX, currentSnapshot.iY - currentSnapshot.tDy))) { retVal= new GridPos(currentSnapshot.iX, currentSnapshot.iY); continue; } } // horizontally/vertically else { if (currentSnapshot.tDx != 0) { // moving along x if ((iParam.SearchGrid.IsWalkableAt(currentSnapshot.iX + currentSnapshot.tDx, currentSnapshot.iY + 1) && !iParam.SearchGrid.IsWalkableAt(currentSnapshot.iX, currentSnapshot.iY + 1)) || (iParam.SearchGrid.IsWalkableAt(currentSnapshot.iX + currentSnapshot.tDx, currentSnapshot.iY - 1) && !iParam.SearchGrid.IsWalkableAt(currentSnapshot.iX, currentSnapshot.iY - 1))) { retVal= new GridPos(currentSnapshot.iX, currentSnapshot.iY); continue; } } else { if ((iParam.SearchGrid.IsWalkableAt(currentSnapshot.iX + 1, currentSnapshot.iY + currentSnapshot.tDy) && !iParam.SearchGrid.IsWalkableAt(currentSnapshot.iX + 1, currentSnapshot.iY)) || (iParam.SearchGrid.IsWalkableAt(currentSnapshot.iX - 1, currentSnapshot.iY + currentSnapshot.tDy) && !iParam.SearchGrid.IsWalkableAt(currentSnapshot.iX - 1, currentSnapshot.iY))) { retVal= new GridPos(currentSnapshot.iX, currentSnapshot.iY); continue; } } } // when moving diagonally, must check for vertical/horizontal jump points if (currentSnapshot.tDx != 0 && currentSnapshot.tDy != 0) { currentSnapshot.stage = 1; stack.Push(currentSnapshot); newSnapshot = new JumpSnapshot(); newSnapshot.iX = currentSnapshot.iX + currentSnapshot.tDx; newSnapshot.iY = currentSnapshot.iY; newSnapshot.iPx = currentSnapshot.iX; newSnapshot.iPy = currentSnapshot.iY; newSnapshot.stage = 0; stack.Push(newSnapshot); continue; } // moving diagonally, must make sure one of the vertical/horizontal // neighbors is open to allow the path // moving diagonally, must make sure one of the vertical/horizontal // neighbors is open to allow the path if (iParam.SearchGrid.IsWalkableAt(currentSnapshot.iX + currentSnapshot.tDx, currentSnapshot.iY) || iParam.SearchGrid.IsWalkableAt(currentSnapshot.iX, currentSnapshot.iY + currentSnapshot.tDy)) { newSnapshot = new JumpSnapshot(); newSnapshot.iX = currentSnapshot.iX + currentSnapshot.tDx; newSnapshot.iY = currentSnapshot.iY + currentSnapshot.tDy; newSnapshot.iPx = currentSnapshot.iX; newSnapshot.iPy = currentSnapshot.iY; newSnapshot.stage = 0; stack.Push(newSnapshot); continue; } else if (iParam.CrossAdjacentPoint) { newSnapshot = new JumpSnapshot(); newSnapshot.iX = currentSnapshot.iX + currentSnapshot.tDx; newSnapshot.iY = currentSnapshot.iY + currentSnapshot.tDy; newSnapshot.iPx = currentSnapshot.iX; newSnapshot.iPy = currentSnapshot.iY; newSnapshot.stage = 0; stack.Push(newSnapshot); continue; } } else //if (!iParam.CrossCorner) { // check for forced neighbors // along the diagonal if (currentSnapshot.tDx != 0 && currentSnapshot.tDy != 0) { if ((iParam.SearchGrid.IsWalkableAt(currentSnapshot.iX + currentSnapshot.tDx, currentSnapshot.iY + currentSnapshot.tDy) && iParam.SearchGrid.IsWalkableAt(currentSnapshot.iX, currentSnapshot.iY + currentSnapshot.tDy) && !iParam.SearchGrid.IsWalkableAt(currentSnapshot.iX + currentSnapshot.tDx, currentSnapshot.iY)) || (iParam.SearchGrid.IsWalkableAt(currentSnapshot.iX + currentSnapshot.tDx, currentSnapshot.iY + currentSnapshot.tDy) && iParam.SearchGrid.IsWalkableAt(currentSnapshot.iX + currentSnapshot.tDx, currentSnapshot.iY) && !iParam.SearchGrid.IsWalkableAt(currentSnapshot.iX, currentSnapshot.iY + currentSnapshot.tDy))) { retVal = new GridPos(currentSnapshot.iX, currentSnapshot.iY); continue; } } // horizontally/vertically else { if (currentSnapshot.tDx != 0) { // moving along x if ((iParam.SearchGrid.IsWalkableAt(currentSnapshot.iX, currentSnapshot.iY + 1) && !iParam.SearchGrid.IsWalkableAt(currentSnapshot.iX - currentSnapshot.tDx, currentSnapshot.iY + 1)) || (iParam.SearchGrid.IsWalkableAt(currentSnapshot.iX, currentSnapshot.iY - 1) && !iParam.SearchGrid.IsWalkableAt(currentSnapshot.iX - currentSnapshot.tDx, currentSnapshot.iY - 1))) { retVal= new GridPos(currentSnapshot.iX, currentSnapshot.iY); continue; } } else { if ((iParam.SearchGrid.IsWalkableAt(currentSnapshot.iX + 1, currentSnapshot.iY) && !iParam.SearchGrid.IsWalkableAt(currentSnapshot.iX + 1, currentSnapshot.iY - currentSnapshot.tDy)) || (iParam.SearchGrid.IsWalkableAt(currentSnapshot.iX - 1, currentSnapshot.iY) && !iParam.SearchGrid.IsWalkableAt(currentSnapshot.iX - 1, currentSnapshot.iY - currentSnapshot.tDy))) { retVal= new GridPos(currentSnapshot.iX, currentSnapshot.iY); continue; } } } // when moving diagonally, must check for vertical/horizontal jump points if (currentSnapshot.tDx != 0 && currentSnapshot.tDy != 0) { currentSnapshot.stage = 3; stack.Push(currentSnapshot); newSnapshot = new JumpSnapshot(); newSnapshot.iX = currentSnapshot.iX + currentSnapshot.tDx; newSnapshot.iY = currentSnapshot.iY; newSnapshot.iPx = currentSnapshot.iX; newSnapshot.iPy = currentSnapshot.iY; newSnapshot.stage = 0; stack.Push(newSnapshot); continue; } // moving diagonally, must make sure both of the vertical/horizontal // neighbors is open to allow the path if (iParam.SearchGrid.IsWalkableAt(currentSnapshot.iX + currentSnapshot.tDx, currentSnapshot.iY) && iParam.SearchGrid.IsWalkableAt(currentSnapshot.iX, currentSnapshot.iY + currentSnapshot.tDy)) { newSnapshot = new JumpSnapshot(); newSnapshot.iX = currentSnapshot.iX + currentSnapshot.tDx; newSnapshot.iY = currentSnapshot.iY + currentSnapshot.tDy; newSnapshot.iPx = currentSnapshot.iX; newSnapshot.iPy = currentSnapshot.iY; newSnapshot.stage = 0; stack.Push(newSnapshot); continue; } } retVal = null; break; case 1: currentSnapshot.jx = retVal; currentSnapshot.stage = 2; stack.Push(currentSnapshot); newSnapshot = new JumpSnapshot(); newSnapshot.iX = currentSnapshot.iX; newSnapshot.iY = currentSnapshot.iY + currentSnapshot.tDy; newSnapshot.iPx = currentSnapshot.iX; newSnapshot.iPy = currentSnapshot.iY; newSnapshot.stage = 0; stack.Push(newSnapshot); break; case 2: currentSnapshot.jy = retVal; if (currentSnapshot.jx != null || currentSnapshot.jy != null) { retVal = new GridPos(currentSnapshot.iX, currentSnapshot.iY); continue; } // moving diagonally, must make sure one of the vertical/horizontal // neighbors is open to allow the path if (iParam.SearchGrid.IsWalkableAt(currentSnapshot.iX + currentSnapshot.tDx, currentSnapshot.iY) || iParam.SearchGrid.IsWalkableAt(currentSnapshot.iX, currentSnapshot.iY + currentSnapshot.tDy)) { newSnapshot = new JumpSnapshot(); newSnapshot.iX = currentSnapshot.iX + currentSnapshot.tDx; newSnapshot.iY = currentSnapshot.iY + currentSnapshot.tDy; newSnapshot.iPx = currentSnapshot.iX; newSnapshot.iPy = currentSnapshot.iY; newSnapshot.stage = 0; stack.Push(newSnapshot); continue; } else if (iParam.CrossAdjacentPoint) { newSnapshot = new JumpSnapshot(); newSnapshot.iX = currentSnapshot.iX + currentSnapshot.tDx; newSnapshot.iY = currentSnapshot.iY + currentSnapshot.tDy; newSnapshot.iPx = currentSnapshot.iX; newSnapshot.iPy = currentSnapshot.iY; newSnapshot.stage = 0; stack.Push(newSnapshot); continue; } retVal = null; break; case 3: currentSnapshot.jx = retVal; currentSnapshot.stage = 4; stack.Push(currentSnapshot); newSnapshot = new JumpSnapshot(); newSnapshot.iX = currentSnapshot.iX; newSnapshot.iY = currentSnapshot.iY + currentSnapshot.tDy; newSnapshot.iPx = currentSnapshot.iX; newSnapshot.iPy = currentSnapshot.iY; newSnapshot.stage = 0; stack.Push(newSnapshot); break; case 4: currentSnapshot.jy = retVal; if (currentSnapshot.jx != null || currentSnapshot.jy != null) { retVal = new GridPos(currentSnapshot.iX, currentSnapshot.iY); continue; } // moving diagonally, must make sure both of the vertical/horizontal // neighbors is open to allow the path if (iParam.SearchGrid.IsWalkableAt(currentSnapshot.iX + currentSnapshot.tDx, currentSnapshot.iY) && iParam.SearchGrid.IsWalkableAt(currentSnapshot.iX, currentSnapshot.iY + currentSnapshot.tDy)) { newSnapshot = new JumpSnapshot(); newSnapshot.iX = currentSnapshot.iX + currentSnapshot.tDx; newSnapshot.iY = currentSnapshot.iY + currentSnapshot.tDy; newSnapshot.iPx = currentSnapshot.iX; newSnapshot.iPy = currentSnapshot.iY; newSnapshot.stage = 0; stack.Push(newSnapshot); continue; } retVal = null; break; } } return retVal; }
private static GridPos? jump(JumpPointParam iParam, int iX, int iY, int iPx, int iPy) { if (!iParam.SearchGrid.IsWalkableAt(iX, iY)) { return null; } else if (iParam.SearchGrid.GetNodeAt(iX, iY).Equals(iParam.EndNode)) { return new GridPos(iX, iY); } int tDx = iX - iPx; int tDy = iY - iPy; GridPos? jx=null; GridPos? jy=null; if (iParam.CrossCorner) { // check for forced neighbors // along the diagonal if (tDx != 0 && tDy != 0) { if ((iParam.SearchGrid.IsWalkableAt(iX - tDx, iY + tDy) && !iParam.SearchGrid.IsWalkableAt(iX - tDx, iY)) || (iParam.SearchGrid.IsWalkableAt(iX + tDx, iY - tDy) && !iParam.SearchGrid.IsWalkableAt(iX, iY - tDy))) { return new GridPos(iX, iY); } } // horizontally/vertically else { if (tDx != 0) { // moving along x if ((iParam.SearchGrid.IsWalkableAt(iX + tDx, iY + 1) && !iParam.SearchGrid.IsWalkableAt(iX, iY + 1)) || (iParam.SearchGrid.IsWalkableAt(iX + tDx, iY - 1) && !iParam.SearchGrid.IsWalkableAt(iX, iY - 1))) { return new GridPos(iX, iY); } } else { if ((iParam.SearchGrid.IsWalkableAt(iX + 1, iY + tDy) && !iParam.SearchGrid.IsWalkableAt(iX + 1, iY)) || (iParam.SearchGrid.IsWalkableAt(iX - 1, iY + tDy) && !iParam.SearchGrid.IsWalkableAt(iX - 1, iY))) { return new GridPos(iX, iY); } } } // when moving diagonally, must check for vertical/horizontal jump points if (tDx != 0 && tDy != 0) { jx = jump(iParam, iX + tDx, iY, iX, iY); jy = jump(iParam, iX, iY + tDy, iX, iY); if (jx != null || jy != null) { return new GridPos(iX, iY); } } // moving diagonally, must make sure one of the vertical/horizontal // neighbors is open to allow the path if (iParam.SearchGrid.IsWalkableAt(iX + tDx, iY) || iParam.SearchGrid.IsWalkableAt(iX, iY + tDy)) { return jump(iParam, iX + tDx, iY + tDy, iX, iY); } else if (iParam.CrossAdjacentPoint) { return jump(iParam, iX + tDx, iY + tDy, iX, iY); } else { return null; } } else //if (!iParam.CrossCorner) { // check for forced neighbors // along the diagonal if (tDx != 0 && tDy != 0) { if ((iParam.SearchGrid.IsWalkableAt(iX + tDx, iY + tDy) && iParam.SearchGrid.IsWalkableAt(iX, iY + tDy) && !iParam.SearchGrid.IsWalkableAt(iX + tDx, iY)) || (iParam.SearchGrid.IsWalkableAt(iX + tDx, iY + tDy) && iParam.SearchGrid.IsWalkableAt(iX + tDx, iY) && !iParam.SearchGrid.IsWalkableAt(iX, iY + tDy))) { return new GridPos(iX, iY); } } // horizontally/vertically else { if (tDx != 0) { // moving along x if ((iParam.SearchGrid.IsWalkableAt(iX, iY + 1) && !iParam.SearchGrid.IsWalkableAt(iX - tDx, iY + 1)) || (iParam.SearchGrid.IsWalkableAt(iX, iY - 1) && !iParam.SearchGrid.IsWalkableAt(iX - tDx, iY - 1))) { return new GridPos(iX, iY); } } else { if ((iParam.SearchGrid.IsWalkableAt(iX + 1, iY) && !iParam.SearchGrid.IsWalkableAt(iX + 1, iY - tDy)) || (iParam.SearchGrid.IsWalkableAt(iX - 1, iY) && !iParam.SearchGrid.IsWalkableAt(iX - 1, iY - tDy))) { return new GridPos(iX, iY); } } } // when moving diagonally, must check for vertical/horizontal jump points if (tDx != 0 && tDy != 0) { jx = jump(iParam, iX + tDx, iY, iX, iY); jy = jump(iParam, iX, iY + tDy, iX, iY); if (jx != null || jy != null) { return new GridPos(iX, iY); } } // moving diagonally, must make sure both of the vertical/horizontal // neighbors is open to allow the path if (iParam.SearchGrid.IsWalkableAt(iX + tDx, iY) && iParam.SearchGrid.IsWalkableAt(iX, iY + tDy)) { return jump(iParam, iX + tDx, iY + tDy, iX, iY); } else { return null; } } }
private static void identifySuccessors(JumpPointParam iParam, Node iNode) { HeuristicDelegate tHeuristic = iParam.HeuristicFunc; List<Node> tOpenList = iParam.openList; int tEndX = iParam.EndNode.x; int tEndY = iParam.EndNode.y; GridPos tNeighbor; GridPos? tJumpPoint; Node tJumpNode; List<GridPos> tNeighbors = findNeighbors(iParam, iNode); for (int i = 0; i < tNeighbors.Count; i++) { tNeighbor = tNeighbors[i]; if(iParam.UseRecursive) tJumpPoint = jump(iParam, tNeighbor.x, tNeighbor.y, iNode.x, iNode.y); else tJumpPoint = jumpLoop(iParam, tNeighbor.x, tNeighbor.y, iNode.x, iNode.y); if (tJumpPoint != null) { tJumpNode = iParam.SearchGrid.GetNodeAt(tJumpPoint.Value.x, tJumpPoint.Value.y); if (tJumpNode == null) { if (iParam.EndNode.x == tJumpPoint.Value.x && iParam.EndNode.y == tJumpPoint.Value.y) tJumpNode = iParam.SearchGrid.GetNodeAt(tJumpPoint.Value); } if (tJumpNode.isClosed) { continue; } // include distance, as parent may not be immediately adjacent: float tCurNodeToJumpNodeLen = tHeuristic(Math.Abs(tJumpPoint.Value.x - iNode.x), Math.Abs(tJumpPoint.Value.y - iNode.y)); float tStartToJumpNodeLen = iNode.startToCurNodeLen + tCurNodeToJumpNodeLen; // next `startToCurNodeLen` value if (!tJumpNode.isOpened || tStartToJumpNodeLen < tJumpNode.startToCurNodeLen) { tJumpNode.startToCurNodeLen = tStartToJumpNodeLen; tJumpNode.heuristicCurNodeToEndLen = (tJumpNode.heuristicCurNodeToEndLen == null ? tHeuristic(Math.Abs(tJumpPoint.Value.x - tEndX), Math.Abs(tJumpPoint.Value.y - tEndY)) : tJumpNode.heuristicCurNodeToEndLen); tJumpNode.heuristicStartToEndLen = tJumpNode.startToCurNodeLen + tJumpNode.heuristicCurNodeToEndLen.Value; tJumpNode.parent = iNode; if (!tJumpNode.isOpened) { tOpenList.Add(tJumpNode); tJumpNode.isOpened = true; } } } } }
private static List<GridPos> findNeighbors(JumpPointParam iParam, Node iNode) { Node tParent = (Node)iNode.parent; int tX = iNode.x; int tY = iNode.y; int tPx, tPy, tDx, tDy; List<GridPos> tNeighbors = new List<GridPos>(); List<Node> tNeighborNodes; Node tNeighborNode; // directed pruning: can ignore most neighbors, unless forced. if (tParent != null) { tPx = tParent.x; tPy = tParent.y; // get the normalized direction of travel tDx = (tX - tPx) / Math.Max(Math.Abs(tX - tPx), 1); tDy = (tY - tPy) / Math.Max(Math.Abs(tY - tPy), 1); if (iParam.CrossCorner) { // search diagonally if (tDx != 0 && tDy != 0) { if (iParam.SearchGrid.IsWalkableAt(tX, tY + tDy)) { tNeighbors.Add(new GridPos(tX, tY + tDy)); } if (iParam.SearchGrid.IsWalkableAt(tX + tDx, tY)) { tNeighbors.Add(new GridPos(tX + tDx, tY)); } if (iParam.SearchGrid.IsWalkableAt(tX + tDx, tY + tDy)) { if (iParam.SearchGrid.IsWalkableAt(tX, tY + tDy) || iParam.SearchGrid.IsWalkableAt(tX + tDx, tY)) { tNeighbors.Add(new GridPos(tX + tDx, tY + tDy)); } else if (iParam.CrossAdjacentPoint) { tNeighbors.Add(new GridPos(tX + tDx, tY + tDy)); } } if (iParam.SearchGrid.IsWalkableAt(tX - tDx, tY + tDy)) { if (iParam.SearchGrid.IsWalkableAt(tX, tY + tDy) && !iParam.SearchGrid.IsWalkableAt(tX - tDx, tY)) { tNeighbors.Add(new GridPos(tX - tDx, tY + tDy)); } } if (iParam.SearchGrid.IsWalkableAt(tX + tDx, tY - tDy)) { if (iParam.SearchGrid.IsWalkableAt(tX + tDx, tY) && !iParam.SearchGrid.IsWalkableAt(tX, tY - tDy)) { tNeighbors.Add(new GridPos(tX + tDx, tY - tDy)); } } } // search horizontally/vertically else { if (tDx == 0) { if (iParam.SearchGrid.IsWalkableAt(tX, tY + tDy)) { tNeighbors.Add(new GridPos(tX, tY + tDy)); if (iParam.SearchGrid.IsWalkableAt(tX + 1, tY + tDy) && !iParam.SearchGrid.IsWalkableAt(tX + 1, tY)) { tNeighbors.Add(new GridPos(tX + 1, tY + tDy)); } if (iParam.SearchGrid.IsWalkableAt(tX - 1, tY + tDy) && !iParam.SearchGrid.IsWalkableAt(tX - 1, tY)) { tNeighbors.Add(new GridPos(tX - 1, tY + tDy)); } } else if (iParam.CrossAdjacentPoint) { if (iParam.SearchGrid.IsWalkableAt(tX + 1, tY + tDy) && !iParam.SearchGrid.IsWalkableAt(tX + 1, tY)) { tNeighbors.Add(new GridPos(tX + 1, tY + tDy)); } if (iParam.SearchGrid.IsWalkableAt(tX - 1, tY + tDy) && !iParam.SearchGrid.IsWalkableAt(tX - 1, tY)) { tNeighbors.Add(new GridPos(tX - 1, tY + tDy)); } } } else { if (iParam.SearchGrid.IsWalkableAt(tX + tDx, tY)) { tNeighbors.Add(new GridPos(tX + tDx, tY)); if (iParam.SearchGrid.IsWalkableAt(tX + tDx, tY + 1) && !iParam.SearchGrid.IsWalkableAt(tX, tY + 1)) { tNeighbors.Add(new GridPos(tX + tDx, tY + 1)); } if (iParam.SearchGrid.IsWalkableAt(tX + tDx, tY - 1) && !iParam.SearchGrid.IsWalkableAt(tX, tY - 1)) { tNeighbors.Add(new GridPos(tX + tDx, tY - 1)); } } else if (iParam.CrossAdjacentPoint) { if (iParam.SearchGrid.IsWalkableAt(tX + tDx, tY + 1) && !iParam.SearchGrid.IsWalkableAt(tX, tY + 1)) { tNeighbors.Add(new GridPos(tX + tDx, tY + 1)); } if (iParam.SearchGrid.IsWalkableAt(tX + tDx, tY - 1) && !iParam.SearchGrid.IsWalkableAt(tX, tY - 1)) { tNeighbors.Add(new GridPos(tX + tDx, tY - 1)); } } } } } else // if(!iParam.CrossCorner) { // search diagonally if (tDx != 0 && tDy != 0) { if (iParam.SearchGrid.IsWalkableAt(tX, tY + tDy)) { tNeighbors.Add(new GridPos(tX, tY + tDy)); } if (iParam.SearchGrid.IsWalkableAt(tX + tDx, tY)) { tNeighbors.Add(new GridPos(tX + tDx, tY)); } if (iParam.SearchGrid.IsWalkableAt(tX + tDx, tY + tDy)) { if (iParam.SearchGrid.IsWalkableAt(tX, tY + tDy) && iParam.SearchGrid.IsWalkableAt(tX + tDx, tY)) tNeighbors.Add(new GridPos(tX + tDx, tY + tDy)); } if (iParam.SearchGrid.IsWalkableAt(tX - tDx, tY + tDy)) { if (iParam.SearchGrid.IsWalkableAt(tX, tY + tDy) && iParam.SearchGrid.IsWalkableAt(tX - tDx, tY)) tNeighbors.Add(new GridPos(tX - tDx, tY + tDy)); } if (iParam.SearchGrid.IsWalkableAt(tX + tDx, tY - tDy)) { if (iParam.SearchGrid.IsWalkableAt(tX, tY - tDy) && iParam.SearchGrid.IsWalkableAt(tX + tDx, tY)) tNeighbors.Add(new GridPos(tX + tDx, tY - tDy)); } } // search horizontally/vertically else { if (tDx == 0) { if (iParam.SearchGrid.IsWalkableAt(tX, tY + tDy)) { tNeighbors.Add(new GridPos(tX, tY + tDy)); if (iParam.SearchGrid.IsWalkableAt(tX + 1, tY + tDy) && iParam.SearchGrid.IsWalkableAt(tX + 1, tY)) { tNeighbors.Add(new GridPos(tX + 1, tY + tDy)); } if (iParam.SearchGrid.IsWalkableAt(tX - 1, tY + tDy) && iParam.SearchGrid.IsWalkableAt(tX - 1, tY)) { tNeighbors.Add(new GridPos(tX - 1, tY + tDy)); } } if (iParam.SearchGrid.IsWalkableAt(tX + 1, tY)) tNeighbors.Add(new GridPos(tX + 1, tY)); if (iParam.SearchGrid.IsWalkableAt(tX - 1, tY)) tNeighbors.Add(new GridPos(tX - 1, tY)); } else { if (iParam.SearchGrid.IsWalkableAt(tX + tDx, tY)) { tNeighbors.Add(new GridPos(tX + tDx, tY)); if (iParam.SearchGrid.IsWalkableAt(tX + tDx, tY + 1) && iParam.SearchGrid.IsWalkableAt(tX, tY + 1)) { tNeighbors.Add(new GridPos(tX + tDx, tY + 1)); } if (iParam.SearchGrid.IsWalkableAt(tX + tDx, tY - 1) && iParam.SearchGrid.IsWalkableAt(tX, tY - 1)) { tNeighbors.Add(new GridPos(tX + tDx, tY - 1)); } } if (iParam.SearchGrid.IsWalkableAt(tX, tY + 1)) tNeighbors.Add(new GridPos(tX, tY + 1)); if (iParam.SearchGrid.IsWalkableAt(tX, tY - 1)) tNeighbors.Add(new GridPos(tX, tY - 1)); } } } } // return all neighbors else { tNeighborNodes = iParam.SearchGrid.GetNeighbors(iNode, iParam.CrossCorner, iParam.CrossAdjacentPoint); for (int i = 0; i < tNeighborNodes.Count; i++) { tNeighborNode = tNeighborNodes[i]; tNeighbors.Add(new GridPos(tNeighborNode.x, tNeighborNode.y)); } } return tNeighbors; }
public static List<GridPos> FindPath(JumpPointParam iParam) { List<Node> tOpenList = iParam.openList; Node tStartNode = iParam.StartNode; Node tEndNode = iParam.EndNode; Node tNode; bool revertEndNodeWalkable = false; // set the `g` and `f` value of the start node to be 0 tStartNode.startToCurNodeLen = 0; tStartNode.heuristicStartToEndLen = 0; // push the start node into the open list tOpenList.Add(tStartNode); tStartNode.isOpened = true; if (iParam.AllowEndNodeUnWalkable && !iParam.SearchGrid.IsWalkableAt(tEndNode.x, tEndNode.y)) { iParam.SearchGrid.SetWalkableAt(tEndNode.x, tEndNode.y, true); revertEndNodeWalkable = true; } // while the open list is not empty while (tOpenList.Count > 0) { // pop the position of node which has the minimum `f` value. tOpenList.Sort(); tNode = (Node)tOpenList[0]; tOpenList.RemoveAt(0); tNode.isClosed = true; if (tNode.Equals(tEndNode)) { if (revertEndNodeWalkable) { iParam.SearchGrid.SetWalkableAt(tEndNode.x, tEndNode.y, false); } return Node.Backtrace(tNode); // rebuilding path } identifySuccessors(iParam, tNode); } if (revertEndNodeWalkable) { iParam.SearchGrid.SetWalkableAt(tEndNode.x, tEndNode.y, false); } // fail to find the path return new List<GridPos>(); }
public static Path generatePath(Vector2 _StartPosition, Vector2 _EndPosition) { try { int var_SizeX = 20;//(int)Math.Abs(_StartPosition.X - _EndPosition.X)/16 + 2;//20; int var_SizeY = 20;//(int)Math.Abs(_StartPosition.Y - _EndPosition.Y)/16 + 2;//20; int var_StartX = (int)((_StartPosition.X % (Region.regionSizeX * Chunk.chunkSizeX * Block.BlockSize)) % (Chunk.chunkSizeX * Block.BlockSize) / Block.BlockSize); int var_StartY = (int)((_StartPosition.Y % (Region.regionSizeY * Chunk.chunkSizeY * Block.BlockSize)) % (Chunk.chunkSizeY * Block.BlockSize) / Block.BlockSize); int var_TargetX = (int)((_EndPosition.X % (Region.regionSizeX * Chunk.chunkSizeX * Block.BlockSize)) % (Chunk.chunkSizeX * Block.BlockSize) / Block.BlockSize); int var_TargetY = (int)((_EndPosition.Y % (Region.regionSizeY * Chunk.chunkSizeY * Block.BlockSize)) % (Chunk.chunkSizeY * Block.BlockSize) / Block.BlockSize); var_TargetX = var_TargetX - var_StartX + var_SizeX/2; var_TargetY = var_TargetY - var_StartY + var_SizeY/2; BaseGrid searchGrid = new DynamicGridWPool(SingletonHolder<NodePool>.Instance); JumpPointParam jumpParam = new JumpPointParam(searchGrid, true, true, false, HeuristicMode.EUCLIDEAN); GridPos startPos = new GridPos(var_SizeX / 2, var_SizeX / 2); GridPos endPos = new GridPos(var_TargetX, var_TargetY); for (int x = 0; x < var_SizeX; x++) { for (int y = 0; y < var_SizeY; y++) { int var_X = (int)_StartPosition.X + (-var_SizeX / 2 + x) * Block.BlockSize; int var_Y = (int)_StartPosition.Y + (-var_SizeX / 2 + y) * Block.BlockSize; Block var_Block = World.world.getBlockAtCoordinate(new Vector3(var_X, var_Y, 0)); bool var_IsWalkAble = false; if (var_Block != null) { if (var_Block.IsWalkAble) { var_IsWalkAble = true; } else if (var_Block.Objects.Count > 0) { if (x == var_SizeX / 2 && y == var_SizeY / 2) { var_IsWalkAble = true; } if (x == var_TargetX && y == var_TargetY) { var_IsWalkAble = true; } } } searchGrid.SetWalkableAt(new GridPos(x, y), var_IsWalkAble); } } jumpParam.CrossCorner = true; jumpParam.CrossAdjacentPoint = false; jumpParam.UseRecursive = false; // KP ;D jumpParam.Reset(startPos, endPos); List<GridPos> resultList = JumpPointFinder.FindPath(jumpParam); LinkedList<PathNode> var_PathNodes = new LinkedList<PathNode>(); foreach (GridPos var_GridPos in resultList) { PathNode var_PathNode = new PathNode(); var_PathNode.X = var_GridPos.x; var_PathNode.Y = var_GridPos.y; int var_X = (int)_StartPosition.X + (-var_SizeX / 2 + var_PathNode.X) * Block.BlockSize; int var_Y = (int)_StartPosition.Y + (-var_SizeX / 2 + var_PathNode.Y) * Block.BlockSize; Block var_Block = World.world.getBlockAtCoordinate(new Vector3(var_X, var_Y, 0)); var_PathNode.block = var_Block; var_PathNodes.AddLast(var_PathNode); } /*for (int y = 0; y < var_SizeY; y++) { for (int x = 0; x < var_SizeY; x++) { Console.ForegroundColor = ConsoleColor.White; if (x == 10 && y == 10) { Console.ForegroundColor = ConsoleColor.Green; } foreach (PathNode var_PathNode in var_PathNodes) { if (var_PathNode.X == x && var_PathNode.Y == y) { Console.ForegroundColor = ConsoleColor.Red; } } if (!searchGrid.IsWalkableAt(x,y)) { Console.Write("x"); } else { Console.Write("-"); } } Console.WriteLine(); }*/ Path var_Result = new Path(var_PathNodes); return var_Result; } catch (Exception ex) { Logger.Logger.LogErr(ex.ToString()); } return null; }