static bool CheckInvalid(GridNode gridNode) { return(gridNode.IsNull() || GridClosedSet.Contains(gridNode) || gridNode.Unpassable()); }
static bool CheckNeighborInvalid() { return(neighbor.IsNull() || GridClosedSet.Contains(neighbor) || neighbor.Unpassable()); }
/// <summary> /// Finds a path and outputs it to <c>outputPath</c>. Note: outputPath is unpredictably changed. /// </summary> /// <returns> /// Returns <c>true</c> if path was found and necessary, <c>false</c> if path to End is impossible or not found. /// </returns> /// <param name="startNode">Start node.</param> /// <param name="endNode">End node.</param> /// <param name="outputPath">Return path.</param> public static bool FindRawPath(GridNode _startNode, GridNode _endNode, FastList <GridNode> _outputPath, int _unitSize = 1) { startNode = _startNode; endNode = _endNode; outputPath = _outputPath; unitSize = _unitSize; #region Broadphase and Preperation if (endNode.Unwalkable) { return(false); } if (startNode.Unwalkable) { return(false); } outputPath.FastClear(); if (System.Object.ReferenceEquals(startNode, endNode)) { outputPath.Add(endNode); return(true); } GridHeap.FastClear(); GridClosedSet.FastClear(); #endregion #region AStar Algorithm GridHeap.Add(startNode); GridNode.HeuristicTargetX = endNode.gridX; GridNode.HeuristicTargetY = endNode.gridY; GridNode.PrepareUnpassableCheck(unitSize); //Prepare Unpassable check optimizations if (_endNode.Unwalkable) { return(false); } while (GridHeap.Count > 0) { currentNode = GridHeap.RemoveFirst(); #if false Gizmos.DrawCube(currentNode.WorldPos.ToVector3(), Vector3.one); #endif if (currentNode.gridIndex == endNode.gridIndex) { //Retraces the path then outputs it into outputPath //Also Simplifies the path DestinationReached(); return(true); } /* * for (i = 0; i < 8; i++) { * neighbor = currentNode.NeighborNodes [i]; * if (CheckNeighborInvalid ()) { * //continue; * //microoptimization... continue is more expensive than letting the loop pass at the end * } else { * //0-3 = sides, 4-7 = diagonals * if (i < 4) { * newMovementCostToNeighbor = currentNode.gCost + 100; * } else { * if (i == 4) { * if (!GridManager.UseDiagonalConnections) * break; * } * newMovementCostToNeighbor = currentNode.gCost + 141; * } * * AnalyzeNode(); * } * } */ hasInvalidEdge = false; for (int i = 0; i < 4; i++) { neighbor = currentNode.NeighborNodes[i]; if (CheckNeighborInvalid()) { hasInvalidEdge = true; } else { newMovementCostToNeighbor = currentNode.gCost + 100; AnalyzeNode(); } } if (hasInvalidEdge) { const int maxCornerObstructions = 2; #region inlining diagonals neighbor = currentNode.NeighborNodes[4]; if (!CheckNeighborInvalid()) { if (GetObstructionCount(0, 1) <= maxCornerObstructions) { newMovementCostToNeighbor = currentNode.gCost + 141; AnalyzeNode(); } } neighbor = currentNode.NeighborNodes[5]; if (!CheckNeighborInvalid()) { if (GetObstructionCount(0, 2) <= maxCornerObstructions) { newMovementCostToNeighbor = currentNode.gCost + 141; AnalyzeNode(); } } neighbor = currentNode.NeighborNodes[6]; if (!CheckNeighborInvalid()) { if (GetObstructionCount(3, 1) <= maxCornerObstructions) { newMovementCostToNeighbor = currentNode.gCost + 141; AnalyzeNode(); } } neighbor = currentNode.NeighborNodes[7]; if (!CheckNeighborInvalid()) { if (GetObstructionCount(3, 2) <= maxCornerObstructions) { newMovementCostToNeighbor = currentNode.gCost + 141; AnalyzeNode(); } } #endregion } else { //no need for specific stuff when edges are all valid for (int i = 4; i < 8; i++) { neighbor = currentNode.NeighborNodes[i]; if (CheckNeighborInvalid()) { } else { newMovementCostToNeighbor = currentNode.gCost + 141; AnalyzeNode(); } } } GridClosedSet.Add(currentNode); } #endregion return(false); }