public Node SetNode(GridPos iPos, bool? iWalkable = null) { if (iWalkable.HasValue) { if (iWalkable.Value == true) { if (m_nodes.ContainsKey(iPos)) return m_nodes[iPos]; Node newNode = new Node(iPos.x, iPos.y, iWalkable); m_nodes.Add(iPos, newNode); return newNode; } else { removeNode(iPos); } } else { Node newNode = new Node(iPos.x, iPos.y, true); m_nodes.Add(iPos, newNode); return newNode; } return null; }
public override BaseGrid Clone() { int tWidth = width; int tHeight = height; Node[][] tNodes = this.m_nodes; StaticGrid tNewGrid = new StaticGrid(tWidth, tHeight, null); Node[][] tNewNodes = new Node[tWidth][]; for (int widthTrav = 0; widthTrav < tWidth; widthTrav++) { tNewNodes[widthTrav] = new Node[tHeight]; for (int heightTrav = 0; heightTrav < tHeight; heightTrav++) { tNewNodes[widthTrav][heightTrav] = new Node(widthTrav, heightTrav, tNodes[widthTrav][heightTrav].walkable); } } tNewGrid.m_nodes = tNewNodes; return tNewGrid; }
private Node[][] buildNodes(int iWidth, int iHeight, bool[][] iMatrix) { Node[][] tNodes = new Node[iWidth][]; for (int widthTrav = 0; widthTrav < iWidth; widthTrav++) { tNodes[widthTrav] = new Node[iHeight]; for (int heightTrav = 0; heightTrav < iHeight; heightTrav++) { tNodes[widthTrav][heightTrav] = new Node(widthTrav, heightTrav, null); } } if (iMatrix == null) { return tNodes; } if (iMatrix.Length != iWidth || iMatrix[0].Length != iHeight) { throw new System.ApplicationException("Matrix size does not fit"); } for (int widthTrav = 0; widthTrav < iWidth; widthTrav++) { for (int heightTrav = 0; heightTrav < iHeight; heightTrav++) { if (iMatrix[widthTrav][heightTrav]) { tNodes[widthTrav][heightTrav].walkable = true; } else { tNodes[widthTrav][heightTrav].walkable = false; } } } return tNodes; }
public JumpPointParam(BaseGrid iGrid, GridPos iStartPos, GridPos iEndPos, bool iAllowEndNodeUnWalkable = true, bool iCrossCorner = true, bool iCrossAdjacentPoint = true, HeuristicMode iMode = HeuristicMode.EUCLIDEAN) { switch (iMode) { case HeuristicMode.MANHATTAN: m_heuristic = new HeuristicDelegate(Heuristic.Manhattan); break; case HeuristicMode.EUCLIDEAN: m_heuristic = new HeuristicDelegate(Heuristic.Euclidean); break; case HeuristicMode.CHEBYSHEV: m_heuristic = new HeuristicDelegate(Heuristic.Chebyshev); break; default: m_heuristic = new HeuristicDelegate(Heuristic.Euclidean); break; } m_allowEndNodeUnWalkable = iAllowEndNodeUnWalkable; m_crossAdjacentPoint = iCrossAdjacentPoint; m_crossCorner = iCrossCorner; openList = new List<Node>(); m_searchGrid = iGrid; m_startNode = m_searchGrid.GetNodeAt(iStartPos.x, iStartPos.y); m_endNode = m_searchGrid.GetNodeAt(iEndPos.x, iEndPos.y); if (m_startNode == null) m_startNode = new Node(iStartPos.x, iStartPos.y, true); if (m_endNode == null) m_endNode = new Node(iEndPos.x, iEndPos.y, true); m_useRecursive = false; }
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 void Reset(GridPos iStartPos, GridPos iEndPos, BaseGrid iSearchGrid = null) { openList.Clear(); m_startNode = null; m_endNode = null; if (iSearchGrid != null) m_searchGrid = iSearchGrid; m_searchGrid.Reset(); m_startNode = m_searchGrid.GetNodeAt(iStartPos.x, iStartPos.y); m_endNode = m_searchGrid.GetNodeAt(iEndPos.x, iEndPos.y); if (m_startNode == null) m_startNode = new Node(iStartPos.x, iStartPos.y, true); if (m_endNode == null) m_endNode = new Node(iEndPos.x, iEndPos.y, true); }
public List<Node> GetNeighbors(Node iNode, bool iCrossCorners, bool iCrossAdjacentPoint) { int tX = iNode.x; int tY = iNode.y; List<Node> neighbors = new List<Node>(); bool tS0 = false, tD0 = false, tS1 = false, tD1 = false, tS2 = false, tD2 = false, tS3 = false, tD3 = false; GridPos pos = new GridPos(); if (this.IsWalkableAt(pos.Set(tX, tY - 1))) { neighbors.Add(GetNodeAt(pos)); tS0 = true; } if (this.IsWalkableAt(pos.Set(tX + 1, tY))) { neighbors.Add(GetNodeAt(pos)); tS1 = true; } if (this.IsWalkableAt(pos.Set(tX, tY + 1))) { neighbors.Add(GetNodeAt(pos)); tS2 = true; } if (this.IsWalkableAt(pos.Set(tX - 1, tY))) { neighbors.Add(GetNodeAt(pos)); tS3 = true; } if (iCrossCorners && iCrossAdjacentPoint) { tD0 = true; tD1 = true; tD2 = true; tD3 = true; } else if (iCrossCorners) { tD0 = tS3 || tS0; tD1 = tS0 || tS1; tD2 = tS1 || tS2; tD3 = tS2 || tS3; } else { tD0 = tS3 && tS0; tD1 = tS0 && tS1; tD2 = tS1 && tS2; tD3 = tS2 && tS3; } if (tD0 && this.IsWalkableAt(pos.Set(tX - 1, tY - 1))) { neighbors.Add(GetNodeAt(pos)); } if (tD1 && this.IsWalkableAt(pos.Set(tX + 1, tY - 1))) { neighbors.Add(GetNodeAt(pos)); } if (tD2 && this.IsWalkableAt(pos.Set(tX + 1, tY + 1))) { neighbors.Add(GetNodeAt(pos)); } if (tD3 && this.IsWalkableAt(pos.Set(tX - 1, tY + 1))) { neighbors.Add(GetNodeAt(pos)); } return neighbors; }
public bool Equals(Node p) { // If parameter is null return false: if ((object)p == null) { return false; } // Return true if the fields match: return (x == p.x) && (y == p.y); }
public static List<GridPos> Backtrace(Node iNode) { List<GridPos> path = new List<GridPos>(); path.Add(new GridPos(iNode.x, iNode.y)); while (iNode.parent != null) { iNode = (Node)iNode.parent; path.Add(new GridPos(iNode.x, iNode.y)); } path.Reverse(); return path; }