static public Queue <GridNode> GetPathToDestination(GridNode pDestination, GridNode pFromNode) { int iMinDistance = GetDistanceBetweenGridNodes(pFromNode, pDestination); List <PathfinderNode> pOpenList = new List <PathfinderNode>(iMinDistance); List <PathfinderNode> pClosedList = new List <PathfinderNode>(iMinDistance); pOpenList.Add(new PathfinderNode(pFromNode)); bool bFoundPath = false; do { int iCheapestOpenNodeIndex; PathfinderNode pCurrentNode = GetCheapestNodeFromList(pOpenList, out iCheapestOpenNodeIndex); pOpenList.RemoveAt(iCheapestOpenNodeIndex); pClosedList.Add(pCurrentNode); if (pCurrentNode.m_pCorrespondingGridNode == pDestination) { bFoundPath = true; break; } PopulateOpenListWithNeighbours(pCurrentNode, pDestination, ref pOpenList, pClosedList); } while (pOpenList.Count > 0); if (!bFoundPath) { Debug.LogError("Couldn't find a path between " + pFromNode + " to " + pDestination); return(null); } return(RetracePathFromEnd(pClosedList, pFromNode)); }
private static Vector3[] RetracePath(PathfinderNode startNode, PathfinderNode endNode) { List <PathfinderNode> path = endNode.TraceParents(startNode); Vector3[] waypoints = SimplifyPath(path); Array.Reverse(waypoints); return(waypoints); }
public void getMovementPaths(Vector2 startPosition, int moveDistance, bool highlight) { //Get no go areas from player controllers var unitPositions = Controller.getUnitPositions(); // Pathfinding stuff IntervalHeap <PathfinderNode> frontier = new IntervalHeap <PathfinderNode>(new PathfinderNode(new Vector2(), 0)); frontier.Add(new PathfinderNode(startPosition, 0)); Dictionary <Vector2, Vector2> cameFrom = new Dictionary <Vector2, Vector2>(); Dictionary <Vector2, int> costSoFar = new Dictionary <Vector2, int>(); cameFrom.Add(startPosition, new Vector2(-1, -1)); costSoFar.Add(startPosition, 0); while (frontier.Count > 0) { //Get current PathfinderNode current = frontier.FindMin(); frontier.DeleteMin(); // iterate through neighbours foreach (Vector2 next in map.getNeibour(current.position)) { if (unitPositions.Contains(next)) { continue; } int newCost = map.tileTypes[map.tiles[(int)next.x, (int)next.y]].cost + costSoFar[current.position]; if (newCost <= moveDistance && (!costSoFar.ContainsKey(next) || newCost < costSoFar[next])) { int priority = newCost; if (costSoFar.ContainsKey(next)) { costSoFar[next] = newCost; PathfinderNode newNode = new PathfinderNode(next, priority); frontier.Add(newNode); cameFrom[next] = current.position; } else { costSoFar.Add(next, newCost); PathfinderNode newNode = new PathfinderNode(next, priority); frontier.Add(newNode); cameFrom.Add(next, current.position); } } } } if (highlight) { map.highlightArea(new List <Vector2>(costSoFar.Keys), new Color(0, 1, 1)); } pathingData = cameFrom; }
/// <summary> /// Main A* pathfinding algorithm, expects a Start and End nodes located in the List Grid. /// The useH param toggle between using an heuristic or breadth-first search, using an heuristic requires nodes with X and Y params. /// </summary> public List <PathfinderNode> FindPath(PathfinderNode Start, PathfinderNode End, bool useH) { List <PathfinderNode> Open = new List <PathfinderNode>(); List <PathfinderNode> Closed = new List <PathfinderNode>(); Open.Add(Start); while (Open.Count > 0) { Open.Sort(Sort); PathfinderNode A = Open[0]; if (A == End) { break; } Open.RemoveAt(0); byte i = (byte)A.Children.Count; // Not expecting the amount of children to be bigger than one byte. while (i-- > 0) { PathfinderNode C = A.Children[i]; // A bit more memory to store the child by at least we're not gonna keep accessing the Children List. // If we are not using an heuristic (AKA have nodes with X, Y coordinates), use only G (Basically Breadth-First). int newF = (useH) ? C.G + GetH(C, End) : C.G; // Skip node if it's in the closed list. if (Closed.Contains(C)) { continue; } // If true, this node has already been processed and this new path isn't shorter, abort this check. if (Open.Contains(C) && C.F <= newF) { continue; } // Otherwise, update the values and add it to the open list. C.G = A.G + C.W; C.F = newF; C.P = A; Open.Add(C); } Closed.Add(A); } // Pathfinding is done, return the path List <PathfinderNode> Path = new List <PathfinderNode>(); PathfinderNode N = Open[0]; while (N != null) { Path.Add(N); N = N.P; } Path.Reverse(); return(Path); }
public abstract void ProcessNode( PathfinderNode currentNode, PathfinderNode startNode, PathfinderNode targetNode, PathfindingHeap <PathfinderNode> openSet, HashSet <PathfinderNode> closedSet, Dictionary <Point, PathfinderNode> activeNodes, Grid grid, int maxPathLength );
public override double CalculateHeuristic(PathfinderNode end) { double heuristic = base.CalculateHeuristic(end); if (Creature != null) { heuristic += 10.0; } return(heuristic); }
private float HeuristicCostEstimate(PathfinderNode <Cell> a, PathfinderNode <Cell> b) { if (a == null || b == null) { return(0f); } float dX = a.data.X - b.data.X; float dY = a.data.Y - b.data.Y; return((float)Math.Sqrt(Math.Pow(dX, 2) + Math.Pow(dY, 2))); }
public static int GetDistance(PathfinderNode nodeA, PathfinderNode nodeB) { Point a = nodeA.GetGridCoord(); Point b = nodeB.GetGridCoord(); int dstX = Mathf.Abs(a.x - b.x); int dstY = Mathf.Abs(a.y - b.y); if (dstX > dstY) { return((int)(14 * dstY + 10 * (dstX - dstY))); } return((int)(14 * dstX + 10 * (dstY - dstX))); }
static private void PopulateOpenListWithNeighbours(PathfinderNode pCurrentNode, GridNode pDestination, ref List <PathfinderNode> pOpenList, List <PathfinderNode> pClosedList) { List <GridNode> pCurrentNodeNeighbours = pCurrentNode.m_pCorrespondingGridNode.GetNeighboursAsList(); GridNode pNorthWestNeighbour = pCurrentNode.m_pCorrespondingGridNode.m_pNorthWestNode; GridNode pSouthEastNeighbour = pCurrentNode.m_pCorrespondingGridNode.m_pSouthEastNode; for (int i = 0; i < pCurrentNodeNeighbours.Count; ++i) { GridNode pCurrentNeighbour = pCurrentNodeNeighbours[i]; if (!pCurrentNeighbour.IsFree() || FindNodeInList(pCurrentNeighbour, pClosedList) != -1) { continue; } PathfinderNode pNode = null; float fDistanceFromStartForHere = pCurrentNode.m_fDistanceFromStart + 1; if (pCurrentNeighbour == pNorthWestNeighbour || pCurrentNeighbour == pSouthEastNeighbour) { fDistanceFromStartForHere += 0.5f; // Thecnically it's the same distance but going diagonally "horizontal" adds a risk of not being able to go diagonally "vertical" later } int iEstimatedDistanceFromDestinationForHere = GetDistanceBetweenGridNodes(pCurrentNeighbour, pDestination); bool bShouldUpdateAttributes = false; int iIndexInOpenList = FindNodeInList(pCurrentNeighbour, pOpenList); if (iIndexInOpenList == -1) { pNode = new PathfinderNode(pCurrentNeighbour); pOpenList.Add(pNode); bShouldUpdateAttributes = true; } else { pNode = pOpenList[iIndexInOpenList]; bShouldUpdateAttributes = fDistanceFromStartForHere + iEstimatedDistanceFromDestinationForHere < pNode.GetScore(); } if (bShouldUpdateAttributes) { pNode.SetValues(pCurrentNode, fDistanceFromStartForHere, iEstimatedDistanceFromDestinationForHere); } } }
/// <summary> /// Method to generate a List of nodes arranged in a square grid way. /// </summary> public static List <PathfinderNode> GenerateGrid(ushort sizeX, ushort sizeY) { List <PathfinderNode> grid = new List <PathfinderNode>(); int i = sizeY; while (i-- > 0) { int j = sizeX; while (j-- > 0) { PathfinderNode n = new PathfinderNode(); n.X = j; n.Y = i; n.W = 1; n.Children = new List <PathfinderNode>(); grid.Add(n); } } i = sizeY; while (i-- > 0) { int j = sizeX; while (j-- > 0) { PathfinderNode n = grid[i * sizeX + j]; int np = i * sizeX + j; if (np - sizeX >= 0 && grid[np - sizeX] != null) { n.Children.Add(grid[np - sizeX]); // Add top child if it exists. } if (j != 0 && grid[np - 1] != null) { n.Children.Add(grid[np - 1]); // Add left child if it exists. } if (np + sizeX < sizeY * sizeX && grid[np + sizeX] != null) { n.Children.Add(grid[np + sizeX]); // Add bottom child if it exists. } if (j != sizeX - 1 && grid[np + 1] != null) { n.Children.Add(grid[np + 1]); // Add right child if it exists. } } } return(grid); }
/// <summary> /// Temporary method to generate a List of nodes arranged in a square grid way. /// Mostly for testing purposes though it can still be used as an example. /// Note that there's most definitely a cleaner way to do this. /// </summary> public static List <PathfinderNode> GenerateTestList(int size) { List <PathfinderNode> grid = new List <PathfinderNode>(); int i = size; while (i-- > 0) { int j = size; while (j-- > 0) { PathfinderNode n = new PathfinderNode(); n.X = j; n.Y = i; n.W = 1; n.Children = new List <PathfinderNode>(); grid.Add(n); } } i = size; while (i-- > 0) { int j = size; while (j-- > 0) { PathfinderNode n = grid[i * size + j]; if (i * size + j - size >= 0 && grid[i * size + j - size] != null) { n.Children.Add(grid[i * size + j - size]); // Add top child if it exists. } if (j != 0 && grid[i * size + j - 1] != null) { n.Children.Add(grid[i * size + j - 1]); // Add left child if it exists. } if (i * size + j + size < size * size && grid[i * size + j + size] != null) { n.Children.Add(grid[i * size + j + size]); // Add bottom child if it exists. } if (j != size - 1 && grid[i * size + j + 1] != null) { n.Children.Add(grid[i * size + j + 1]); // Add right child if it exists. } } } return(grid); }
public static List <PathfinderNode> GetNeighbors( PathfinderNode node, Dictionary <Point, PathfinderNode> activeNodes, PathfinderNodeCreator nodeCreator ) { List <PathfinderNode> neighbors = new List <PathfinderNode>(); List <Point> neighborPoints = instance.grid.GetNeighbors(node.GetGridCoord()); List <Point> unActiveNeighbors = Pools.ListPoints; for (int i = 0; i < neighborPoints.Count; i++) { Point currentPoint = neighborPoints[i]; PathfinderNode currentNode = null; activeNodes.TryGetValue(currentPoint, out currentNode); // current node is already active if (currentNode != null) { neighbors.Add(currentNode); Pools.Point = currentPoint; // current node is not active } else { unActiveNeighbors.Add(currentPoint); currentNode = nodeCreator.CreateNode( currentPoint, instance.grid.GetMapNodeAt(currentPoint) ); activeNodes.Add(currentPoint, currentNode); neighbors.Add(currentNode); } } Pools.ListPoints = neighborPoints; Pools.ListPoints = unActiveNeighbors; return(neighbors); }
public override double CalculateHeuristic(PathfinderNode end) { Tile other = (Tile)end; /* * int d, dx = Math.Abs(other.X - X), dy = Math.Abs(other.Y - Y); * if (dx > dy) * { * d = dx - dy; * return (dx - d) * 1500 + d * 1000; * } * else * { * d = dy - dx; * return (dy - d) * 1500 + d * 1000; * } */ return(1200 * Vector2.Distance(VectorPosition, ((Tile)end).VectorPosition)); //return (int)(1200 * Vector2.DistanceSquared(VectorPosition, ((Tile)end).VectorPosition)); //return 1000 * (Math.Abs(other.X - X) + Math.Abs(other.Y - Y)); }
public void SetGrid(int sizeX, int sizeZ) { PathfinderGrid = new PathfinderNode[sizeX, sizeZ]; for (int x = 0; x < sizeX; x++) { for (int z = 0; z < sizeZ; z++) { PathfinderGrid[x, z] = new PathfinderNode(new Vector3(x, 0, z)); } } const float rayDistance = 0.5f; Ray ray; for (int x = 0; x < sizeX; x++) { for (int z = 0; z < sizeZ; z++) { ray = new Ray(new Vector3(x, 0, z), Vector3.forward); if (!Physics.Raycast(ray, rayDistance)) { PathfinderGrid[x, z].AddNeighbor(PathfinderGrid[x, z + 1]); } ray.direction = Vector3.back; if (!Physics.Raycast(ray, rayDistance)) { PathfinderGrid[x, z].AddNeighbor(PathfinderGrid[x, z - 1]); } ray.direction = Vector3.right; if (!Physics.Raycast(ray, rayDistance)) { PathfinderGrid[x, z].AddNeighbor(PathfinderGrid[x + 1, z]); } ray.direction = Vector3.left; if (!Physics.Raycast(ray, rayDistance)) { PathfinderGrid[x, z].AddNeighbor(PathfinderGrid[x - 1, z]); } } } }
private float DistanceBetween(PathfinderNode <Cell> a, PathfinderNode <Cell> b) { if (a == null || b == null) { return(0f); } float dX = a.data.X - b.data.X; float dY = a.data.Y - b.data.Y; if (Math.Abs(dX) + Math.Abs(dY) == 1) { return(1f); } if (Math.Abs(dX) == 1 && Math.Abs(dY) == 1) { return((float)Math.Sqrt(2)); } return((float)Math.Sqrt(Math.Pow(dX, 2) + Math.Pow(dY, 2))); }
static private PathfinderNode GetCheapestNodeFromList(List <PathfinderNode> pList, out int iIndex) { iIndex = 0; PathfinderNode pCheapestNode = pList[0]; float fCheapestCost = pCheapestNode.GetScore(); for (int i = 1; i < pList.Count; ++i) { PathfinderNode pCurrentNode = pList[i]; float fCurrentNodeScore = pCurrentNode.GetScore(); if (fCurrentNodeScore < fCheapestCost || (fCurrentNodeScore == fCheapestCost && pCurrentNode.m_iEstimatedDistanceFromDestination < pCheapestNode.m_iEstimatedDistanceFromDestination)) { pCheapestNode = pCurrentNode; fCheapestCost = fCurrentNodeScore; iIndex = i; } } return(pCheapestNode); }
static private Queue <GridNode> RetracePathFromEnd(List <PathfinderNode> pClosedList, GridNode pStart) { List <GridNode> pReversePath = new List <GridNode>(pClosedList.Count); PathfinderNode pCurrentNode = pClosedList[pClosedList.Count - 1]; pReversePath.Add(pCurrentNode.m_pCorrespondingGridNode); while (pCurrentNode.m_pCorrespondingGridNode != pStart) { pCurrentNode = pCurrentNode.m_pParentNode; pReversePath.Add(pCurrentNode.m_pCorrespondingGridNode); } Queue <GridNode> pPath = new Queue <GridNode>(pReversePath.Count); for (int i = pReversePath.Count - 1; i >= 0; --i) { pPath.Enqueue(pReversePath[i]); } return(pPath); }
private void DrawTargetedHeuristicLines(LinkedList <LineViewModel> lineList) { PathfinderNode current = _pathfinder.Solution[_target.X, _target.Y]; if (!current.Explored) { return; } while (current.X != _source.X || current.Y != _source.Y) { // Add Line var fromX = current.X; var fromY = current.Y; var toX = current.ParentX; var toY = current.ParentY; lineList.AddLast(new LineViewModel(fromX, fromY, toX, toY)); current = _pathfinder.Solution[toX, toY]; } }
public clsResult GenerateOil() { var ReturnResult = new clsResult("Oil", false); logger.Info("Generating Oil"); var A = 0; var B = 0; var C = 0; var D = 0; for ( A = 0; A <= PassageNodeCount - 1; A++ ) { for ( B = 0; B <= SymmetryBlockCount - 1; B++ ) { PassageNodes[B, A].OilCount = 0; } } //store passage node route distances var PassageNodePathMap = MakePassageNodeNetwork(); var GetPathStartNodes = new PathfinderNode[1]; PathfinderNetwork.PathList[] ResultPaths = null; PassageNodeDists = new float[SymmetryBlockCount, PassageNodeCount, SymmetryBlockCount, PassageNodeCount]; for ( A = 0; A <= PassageNodeCount - 1; A++ ) { for ( D = 0; D <= SymmetryBlockCount - 1; D++ ) { PassageNodeDists[D, A, D, A] = 0.0F; for ( B = 0; B <= PassageNodeCount - 1; B++ ) { for ( C = 0; C <= SymmetryBlockCount - 1; C++ ) { if ( PassageNodes[0, A].IsWater || PassageNodes[C, B].IsWater || (C != 0 & D != 0) ) { PassageNodeDists[D, A, C, B] = float.MaxValue; PassageNodeDists[C, B, D, A] = float.MaxValue; } else { GetPathStartNodes[0] = PassageNodePathMap.PassageNodePathNodes[D, A]; ResultPaths = PassageNodePathMap.Network.GetPath(GetPathStartNodes, PassageNodePathMap.PassageNodePathNodes[C, B], -1, 0); if ( ResultPaths == null ) { ReturnResult.ProblemAdd("Map is not all connected."); PassageNodePathMap.Network.Deallocate(); return ReturnResult; } if ( ResultPaths[0].PathCount != 1 ) { Debugger.Break(); } PassageNodeDists[D, A, C, B] = ResultPaths[0].Paths[0].Value; PassageNodeDists[C, B, D, A] = ResultPaths[0].Paths[0].Value; } } } } } PassageNodePathMap.Network.Deallocate(); //place oil var PlacedExtraOilCount = 0; var MaxBestNodeCount = 0; MaxBestNodeCount = 1; for ( A = 0; A <= OilAtATime - 1; A++ ) { MaxBestNodeCount *= PassageNodeCount; } var oilArgs = new clsOilBalanceLoopArgs { OilClusterSizes = new int[OilAtATime], PlayerOilScore = new double[TopLeftPlayerCount], OilNodes = new clsPassageNode[OilAtATime] }; //balanced oil while ( PlacedExtraOilCount < ExtraOilCount ) { //place oil farthest away from other oil and where it best balances the player oil score for ( A = 0; A <= OilAtATime - 1; A++ ) { oilArgs.OilClusterSizes[A] = Math.Min(ExtraOilClusterSizeMin + App.Random.Next() * (ExtraOilClusterSizeMax - ExtraOilClusterSizeMin + 1), Math.Max((int)(Math.Ceiling(Convert.ToDecimal((ExtraOilCount - PlacedExtraOilCount) / SymmetryBlockCount))), 1)); } oilArgs.OilPossibilities = new clsOilPossibilities(); OilBalanceLoop(oilArgs, 0); var bestPossibility = oilArgs.OilPossibilities.BestPossibility; if ( bestPossibility != null ) { for ( B = 0; B <= OilAtATime - 1; B++ ) { for ( A = 0; A <= SymmetryBlockCount - 1; A++ ) { PassageNodes[A, bestPossibility.Nodes[B].Num].OilCount += oilArgs.OilClusterSizes[B]; } PlacedExtraOilCount += oilArgs.OilClusterSizes[B] * SymmetryBlockCount; } for ( A = 0; A <= TopLeftPlayerCount - 1; A++ ) { oilArgs.PlayerOilScore[A] += bestPossibility.PlayerOilScoreAddition[A]; } } else { ReturnResult.WarningAdd("Could not place all of the oil. " + Convert.ToString(PlacedExtraOilCount) + " oil was placed."); break; } } //base oil for ( A = 0; A <= TopLeftPlayerCount - 1; A++ ) { for ( B = 0; B <= SymmetryBlockCount - 1; B++ ) { PassageNodes[B, PlayerBases[A].Nodes[0].Num].OilCount += BaseOilCount; } } return ReturnResult; }
/// <summary> /// The heuristic method used if the useH param of FindPath is set to true. /// </summary> private int GetH(PathfinderNode Node, PathfinderNode TargetNode) { return((int)Math.Sqrt((TargetNode.X - Node.X) * (TargetNode.X - Node.X) + (TargetNode.Y - Node.Y) * (TargetNode.Y - Node.Y))); }
public clsResult GenerateRamps() { var ReturnResult = new clsResult("Ramps", false); logger.Info("Generating Ramps"); var A = 0; var B = 0; var C = 0; var E = 0; double BestDist = 0; var BestNum = 0; var XY_int = new XYInt(); double Dist = 0; //make ramps for ( A = 0; A <= ConnectionCount - 1; A++ ) { Connections[A].IsRamp = false; } PathfinderNode[,] PassageNodePathNodes = null; var PassageNodeNetwork = MakePassageNodeNetwork(); PassageNodePathNodes = PassageNodeNetwork.PassageNodePathNodes; var PossibleRamps = new clsConnection[ConnectionCount]; var PossibleRampCount = 0; var GetPathStartNodes = new PathfinderNode[1]; PathfinderNetwork.PathList[] ResultPaths = null; //ramp connections whose points are too far apart var ConnectionsCanRamp = new bool[ConnectionCount]; for ( B = 0; B <= ConnectionCount - 1; B++ ) { C = Math.Abs(Connections[B].PassageNodeA.Level - Connections[B].PassageNodeB.Level); if ( C == 1 ) { if ( !(Connections[B].PassageNodeA.IsOnBorder || Connections[B].PassageNodeB.IsOnBorder) && Connections[B].PassageNodeA.MirrorNum == 0 && Connections[B].PassageNodeA.Num != Connections[B].PassageNodeB.Num ) { ConnectionsCanRamp[B] = true; } else { ConnectionsCanRamp[B] = false; } } else { ConnectionsCanRamp[B] = false; } } var Connectedness = new clsNodeConnectedness(); Connectedness.NodeConnectedness = new float[PassageNodeCount]; Connectedness.PassageNodeVisited = new bool[SymmetryBlockCount, PassageNodeCount]; Connectedness.PassageNodePathNodes = PassageNodePathNodes; Connectedness.PassageNodePathMap = PassageNodeNetwork.Network; double Value = 0; double BestDistB = 0; double BaseDist = 0; double RampDist = 0; var UpdateNodeConnectednessArgs = new clsUpdateNodeConnectednessArgs(); var UpdateNetworkConnectednessArgs = new clsUpdateNetworkConnectednessArgs(); UpdateNodeConnectednessArgs.Args = Connectedness; UpdateNetworkConnectednessArgs.Args = Connectedness; UpdateNetworkConnectednessArgs.PassageNodeUpdated = new bool[PassageNodeCount]; UpdateNetworkConnectednessArgs.SymmetryBlockCount = SymmetryBlockCount; for ( A = 0; A <= PassageNodeCount - 1; A++ ) { Connectedness.NodeConnectedness[A] = 0.0F; for ( B = 0; B <= PassageNodeCount - 1; B++ ) { for ( C = 0; C <= SymmetryBlockCount - 1; C++ ) { Connectedness.PassageNodeVisited[C, B] = false; } } UpdateNodeConnectednessArgs.OriginalNode = PassageNodes[0, A]; UpdateNodeConnectedness(UpdateNodeConnectednessArgs, PassageNodes[0, A]); } do { BestNum = -1; BestDist = 1.0F; //for connections that can already reach the other side BestDistB = 0.0F; //for connections that cant PossibleRampCount = 0; for ( B = 0; B <= ConnectionCount - 1; B++ ) { if ( ConnectionsCanRamp[B] && !Connections[B].IsRamp ) { if ( CheckRampAngles(Connections[B], Convert.ToDouble(80.0D * MathUtil.RadOf1Deg), Convert.ToDouble(120.0D * MathUtil.RadOf1Deg), 0.0D * MathUtil.RadOf1Deg) ) { GetPathStartNodes[0] = PassageNodePathNodes[Connections[B].PassageNodeA.MirrorNum, Connections[B].PassageNodeA.Num]; ResultPaths = PassageNodeNetwork.Network.GetPath(GetPathStartNodes, PassageNodePathNodes[Connections[B].PassageNodeB.MirrorNum, Connections[B].PassageNodeB.Num], -1, 0); BaseDist = double.MaxValue; XY_int.X = (int)((Connections[B].PassageNodeA.Pos.X + Connections[B].PassageNodeB.Pos.X) / 2.0D); XY_int.Y = (int)((Connections[B].PassageNodeA.Pos.Y + Connections[B].PassageNodeB.Pos.Y) / 2.0D); for ( E = 0; E <= TotalPlayerCount - 1; E++ ) { Dist = Convert.ToDouble((PlayerBases[E].Pos - XY_int).ToDoubles().GetMagnitude()); if ( Dist < BaseDist ) { BaseDist = Dist; } } RampDist = Math.Max(MaxDisconnectionDist * Math.Pow(RampBase, (BaseDist / 1024.0D)), 1.0F); if ( ResultPaths == null ) { Value = Connectedness.NodeConnectedness[Connections[B].PassageNodeA.Num] + Connectedness.NodeConnectedness[Connections[B].PassageNodeB.Num]; if ( double.MaxValue > BestDist ) { BestDist = double.MaxValue; BestDistB = Value; PossibleRamps[0] = Connections[B]; PossibleRampCount = 1; } else { if ( Value < BestDistB ) { BestDistB = Value; PossibleRamps[0] = Connections[B]; PossibleRampCount = 1; } else if ( Value == BestDistB ) { PossibleRamps[PossibleRampCount] = Connections[B]; PossibleRampCount++; } } } else if ( ResultPaths[0].PathCount != 1 ) { ReturnResult.ProblemAdd("Error: Invalid number of routes returned."); goto Finish; } else if ( ResultPaths[0].Paths[0].Value / RampDist > BestDist ) { BestDist = ResultPaths[0].Paths[0].Value / RampDist; PossibleRamps[0] = Connections[B]; PossibleRampCount = 1; } else if ( ResultPaths[0].Paths[0].Value / RampDist == BestDist ) { PossibleRamps[PossibleRampCount] = Connections[B]; PossibleRampCount++; } else if ( ResultPaths[0].Paths[0].Value <= RampDist ) { ConnectionsCanRamp[B] = false; } } else { ConnectionsCanRamp[B] = false; } } else { ConnectionsCanRamp[B] = false; } } if ( PossibleRampCount > 0 ) { BestNum = App.Random.Next() * PossibleRampCount; PossibleRamps[BestNum].IsRamp = true; for ( C = 0; C <= PossibleRamps[BestNum].ReflectionCount - 1; C++ ) { PossibleRamps[BestNum].Reflections[C].IsRamp = true; } PassageNodeNetwork.Network.FindCalc(); for ( E = 0; E <= PassageNodeCount - 1; E++ ) { UpdateNetworkConnectednessArgs.PassageNodeUpdated[E] = false; } if ( PossibleRamps[BestNum].PassageNodeA.MirrorNum == 0 ) { UpdateNetworkConnectedness(UpdateNetworkConnectednessArgs, PossibleRamps[BestNum].PassageNodeA); } else if ( PossibleRamps[BestNum].PassageNodeB.MirrorNum == 0 ) { UpdateNetworkConnectedness(UpdateNetworkConnectednessArgs, PossibleRamps[BestNum].PassageNodeB); } else { ReturnResult.ProblemAdd("Error: Initial ramp not in area 0."); goto Finish; } } else { break; } } while ( true ); var FloodArgs = new PathfinderNetwork.sFloodProximityArgs(); FloodArgs.StartNode = PassageNodeNetwork.PassageNodePathNodes[0, 0]; FloodArgs.NodeValues = PassageNodeNetwork.Network.NetworkLargeArrays.Nodes_ValuesA; for ( A = 0; A <= PassageNodeCount - 1; A++ ) { for ( B = 0; B <= SymmetryBlockCount - 1; B++ ) { FloodArgs.NodeValues[PassageNodeNetwork.PassageNodePathNodes[B, A].Layer_NodeNum] = float.MaxValue; } } PassageNodeNetwork.Network.FloodProximity(ref FloodArgs); for ( A = 0; A <= PassageNodeCount - 1; A++ ) { for ( B = 0; B <= SymmetryBlockCount - 1; B++ ) { if ( !PassageNodes[B, A].IsWater ) { if ( FloodArgs.NodeValues[PassageNodeNetwork.PassageNodePathNodes[B, A].Layer_NodeNum] == float.MaxValue ) { ReturnResult.ProblemAdd("Land is unreachable. Reduce variation or retry."); goto Finish; } } } } Finish: PassageNodeNetwork.Network.Deallocate(); return ReturnResult; }
public float GetNodePosDist(PathfinderNode NodeA, PathfinderNode NodeB) { var TagA = (clsNodeTag)NodeA.Tag; var TagB = (clsNodeTag)NodeB.Tag; return Convert.ToSingle((TagA.Pos - TagB.Pos).ToDoubles().GetMagnitude()); }
public PathfinderNode GetRandomChildNode(PathfinderNode InputNode, int MinClearance) { if ( InputNode.GetClearance < MinClearance ) { return null; } if ( InputNode.GetChildNodeCount == 0 ) { return InputNode; } var A = 0; do { A = Convert.ToInt32((App.Random.Next() * InputNode.GetChildNodeCount)); } while ( InputNode.get_GetChildNode(A).GetClearance < MinClearance ); var ReturnResult = GetRandomChildNode(InputNode.get_GetChildNode(A), MinClearance); return ReturnResult; }
public static PathResult FindPath( PathRequest request ) { instance.openSet.Clear(); instance.activeNodes.Clear(); instance.closedSet.Clear(); Vector3[] waypoints = new Vector3[0]; bool pathSuccess = false; float maxLength = request.maxLength; PathfinderImplementationStrategy implementationStrategy = request.aStarImpl; int maxPathLength = instance.grid.DistanceToNodeDistance(maxLength); Point startPoint = instance.grid.WorldCoordToNode(request.pathStart); Point endPoint = instance.grid.WorldCoordToNode(request.pathEnd); PathfinderNode startNode = implementationStrategy.CreateStarterNodes( startPoint, instance.grid.GetMapNodeAt(startPoint) ); PathfinderNode targetNode = implementationStrategy.CreateStarterNodes( endPoint, instance.grid.GetMapNodeAt(endPoint) ); /* * If startnode and targetnode are the same * it causes a heap error */ if (startNode.IsWalkable() && targetNode.IsWalkable()) { instance.openSet.Add(startNode); instance.activeNodes.Add(startNode.GetGridCoord(), startNode); instance.activeNodes.Add(targetNode.GetGridCoord(), targetNode); while (instance.openSet.Count > 0) { Profiler.BeginSample("Remove first from open set"); PathfinderNode currentNode = instance.openSet.RemoveFirst(); Profiler.EndSample(); Profiler.BeginSample("Add current to closed set"); instance.closedSet.Add(currentNode); Profiler.EndSample(); if (currentNode == targetNode) { pathSuccess = true; break; } if (instance.openSet.Contains(targetNode)) { pathSuccess = true; break; } Profiler.BeginSample("Process current node"); implementationStrategy.ProcessNode( currentNode, startNode, targetNode, instance.openSet, instance.closedSet, instance.activeNodes, instance.grid, maxPathLength ); Profiler.EndSample(); } } if (pathSuccess) { waypoints = RetracePath(startNode, targetNode); pathSuccess = waypoints.Length > 0; } List <Point> recycledPoints = Pools.ListPoints; //recycledPoints.Add(startPoint); //recycledPoints.Add(endPoint); Dictionary <Point, PathfinderNode> .Enumerator enumerator = instance.activeNodes.GetEnumerator(); while (enumerator.MoveNext()) { var current = enumerator.Current; Point p = current.Key; recycledPoints.Add(p); } for (int i = 0; i < recycledPoints.Count; i++) { Point current = recycledPoints[i]; Pools.Point = current; } Pools.ListPoints = recycledPoints; PathfinderVisualizer.Visualize(); //Debug.Log("path end"); return(new PathResult( waypoints, pathSuccess )); }
public static void Visit(PathfinderNode node) { instance.visitedNodes.Add(node); }
void ReconstructPath(Dictionary <PathfinderNode <Cell>, PathfinderNode <Cell> > from, PathfinderNode <Cell> current) { Queue <Cell> totalPath = new Queue <Cell>(); totalPath.Enqueue(current.data); while (from.ContainsKey(current)) { current = from[current]; totalPath.Enqueue(current.data); } path = new Queue <Cell>(totalPath.Reverse()); }
public PathfinderAStar(World world, Cell startCell, Cell endCell, Predicate <Cell> endCellPredicate = null) { if (startCell == null) { Console.Error.WriteLine("[Pathfinder Error] Start cell is null"); return; } if (endCell == null && endCellPredicate == null) { Console.Error.WriteLine("[Pathfinder Error] There is no end predicate or goal cell to reach"); return; } if (world.cellGraph == null) { world.cellGraph = new PathfinderCellGraph(world); } Dictionary <Cell, PathfinderNode <Cell> > nodes = world.cellGraph.nodes; if (!nodes.ContainsKey(startCell)) { Console.Error.WriteLine("[Pathfinder Error] Start cell not in list of nodes"); return; } if (endCell != null) { if (!nodes.ContainsKey(endCell)) { Console.Error.WriteLine("[Pathfinder Error] End cell not in list of nodes"); return; } } PathfinderNode <Cell> startNode = nodes[startCell]; PathfinderNode <Cell> goalNode = endCell == null ? null : nodes[endCell]; if (goalNode != null && (goalNode.edges.Length == 0 || goalNode.data.MovementCost == 0)) { Console.Error.WriteLine("[Pathfinder Error] Goal node is unreachable: no edges or movement cost is 0"); return; } List <PathfinderNode <Cell> > ClosedSet = new List <PathfinderNode <Cell> >(); SimplePriorityQueue <PathfinderNode <Cell> > OpenSet = new SimplePriorityQueue <PathfinderNode <Cell> >(); OpenSet.Enqueue(startNode, 0); Dictionary <PathfinderNode <Cell>, PathfinderNode <Cell> > from = new Dictionary <PathfinderNode <Cell>, PathfinderNode <Cell> >(); // Create gScore Dictionary <PathfinderNode <Cell>, float> gScore = new Dictionary <PathfinderNode <Cell>, float>(); foreach (PathfinderNode <Cell> node in nodes.Values) { gScore[node] = float.PositiveInfinity; } gScore[startNode] = 0; // Create fScore Dictionary <PathfinderNode <Cell>, float> fScore = new Dictionary <PathfinderNode <Cell>, float>(); foreach (PathfinderNode <Cell> node in nodes.Values) { fScore[node] = float.PositiveInfinity; } fScore[startNode] = HeuristicCostEstimate(startNode, goalNode); while (OpenSet.Count > 0) { PathfinderNode <Cell> current = OpenSet.Dequeue(); if (goalNode == null) { if (endCellPredicate != null && endCellPredicate(current.data)) { ReconstructPath(from, current); return; } } else if (current == goalNode) { ReconstructPath(from, current); return; } ClosedSet.Add(current); foreach (PathfinderEdge <Cell> edgeNeighbor in current.edges) { PathfinderNode <Cell> neighbor = edgeNeighbor.node; if (ClosedSet.Contains(neighbor)) { continue; // Already completed } float movementCostToNeighbor = neighbor.data.MovementCost * DistanceBetween(current, neighbor); float tentativeGScore = gScore[current] + movementCostToNeighbor; if (OpenSet.Contains(neighbor) && tentativeGScore >= gScore[neighbor]) { continue; } from[neighbor] = current; gScore[neighbor] = tentativeGScore; fScore[neighbor] = gScore[neighbor] + HeuristicCostEstimate(neighbor, goalNode); if (OpenSet.Contains(neighbor)) { OpenSet.UpdatePriority(neighbor, fScore[neighbor]); } else { OpenSet.Enqueue(neighbor, fScore[neighbor]); } } } }
private void SetBaseLevel(PathfinderNode Node, int NewLevel, clsBaseNodeLevels BaseLevel) { if ( Node.GetChildNodeCount == 0 ) { var A = 0; float Height = 0; float Lowest = NewLevel; for ( A = 0; A <= Node.GetConnectionCount - 1; A++ ) { Height = BaseLevel.NodeLevels[Node.get_GetConnection(A).GetOtherNode(Node).GetLayer_NodeNum]; if ( Height < Lowest ) { Lowest = Height; } } if ( NewLevel - Lowest > 1.0F ) { BaseLevel.NodeLevels[Node.GetLayer_NodeNum] = Lowest + 1.0F; } else { BaseLevel.NodeLevels[Node.GetLayer_NodeNum] = NewLevel; } } else { var A = 0; for ( A = 0; A <= Node.GetChildNodeCount - 1; A++ ) { SetBaseLevel(Node.get_GetChildNode(A), NewLevel, BaseLevel); } } }
private void SetBaseLevelRamp(clsSetBaseLevelRampArgs Args, PathfinderNode Node) { if ( Node.GetChildNodeCount == 0 ) { var NodeTag = (clsNodeTag)Node.Tag; var XY_int = MathUtil.PointGetClosestPosOnLine(Args.Connection.PassageNodeA.Pos, Args.Connection.PassageNodeB.Pos, NodeTag.Pos); var ConnectionLength = Convert.ToSingle((Args.Connection.PassageNodeA.Pos - Args.Connection.PassageNodeB.Pos).ToDoubles().GetMagnitude()); var Extra = ConnectionLength - Args.RampLength; var ConnectionPos = Convert.ToSingle((XY_int - Args.Connection.PassageNodeA.Pos).ToDoubles().GetMagnitude()); var RampPos = MathUtil.Clamp_sng((ConnectionPos - Extra / 2.0F) / Args.RampLength, 0.0F, 1.0F); var Layer_NodeNum = Node.GetLayer_NodeNum; RampPos = (float)(1.0D - (Math.Cos(RampPos * Math.PI) + 1.0D) / 2.0D); if ( RampPos > 0.0F & RampPos < 1.0F ) { var Dist2 = Convert.ToSingle((NodeTag.Pos - XY_int).ToDoubles().GetMagnitude()); if ( Dist2 < Args.RampRadius ) { var Dist2Factor = 1.0F; //Math.Min(3.0F - 3.0F * Dist2 / 384.0F, 1.0F) 'distance fading if ( Args.BaseLevel.NodeLevels[Layer_NodeNum] == (Args.BaseLevel.NodeLevels[Layer_NodeNum]) ) { Args.BaseLevel.NodeLevels[Layer_NodeNum] = Args.BaseLevel.NodeLevels[Layer_NodeNum] * (1.0F - Dist2Factor) + (Args.Connection.PassageNodeA.Level * (1.0F - RampPos) + Args.Connection.PassageNodeB.Level * RampPos) * Dist2Factor; } else { Args.BaseLevel.NodeLevels[Layer_NodeNum] = (Args.BaseLevel.NodeLevels[Layer_NodeNum] * (2.0F - Dist2Factor) + (Args.Connection.PassageNodeA.Level * (1.0F - RampPos) + Args.Connection.PassageNodeB.Level * RampPos) * Dist2Factor) / 2.0F; } } } } else { var A = 0; for ( A = 0; A <= Node.GetChildNodeCount - 1; A++ ) { SetBaseLevelRamp(Args, Node.get_GetChildNode(A)); } } }
private void UpdateNodeConnectedness(clsUpdateNodeConnectednessArgs Args, clsPassageNode PassageNode) { var A = 0; var tmpConnection = default(clsConnection); var tmpOtherNode = default(clsPassageNode); var PassableCount = 0; Args.Args.PassageNodeVisited[PassageNode.MirrorNum, PassageNode.Num] = true; for ( A = 0; A <= PassageNode.ConnectionCount - 1; A++ ) { tmpConnection = PassageNode.Connections[A].Connection; if ( !(tmpConnection.PassageNodeA.IsOnBorder || tmpConnection.PassageNodeB.IsOnBorder || tmpConnection.PassageNodeA.IsWater || tmpConnection.PassageNodeB.IsWater) && (tmpConnection.IsRamp || tmpConnection.PassageNodeA.Level == tmpConnection.PassageNodeB.Level) ) { tmpOtherNode = PassageNode.Connections[A].GetOther(); if ( !Args.Args.PassageNodeVisited[tmpOtherNode.MirrorNum, tmpOtherNode.Num] ) { UpdateNodeConnectedness(Args, tmpOtherNode); } PassableCount++; } } PathfinderNetwork.PathList[] Paths = null; var StartNodes = new PathfinderNode[1]; StartNodes[0] = Args.Args.PassageNodePathNodes[0, Args.OriginalNode.Num]; Paths = Args.Args.PassageNodePathMap.GetPath(StartNodes, Args.Args.PassageNodePathNodes[PassageNode.MirrorNum, PassageNode.Num], -1, 0); Args.Args.NodeConnectedness[Args.OriginalNode.Num] += (float)(PassableCount * Math.Pow(0.999D, Paths[0].Paths[0].Value)); }
public SimpleLink(PathfinderNode target, double cost) : base(target) { Cost = cost; }
public PathfinderLink(PathfinderNode target) { Target = target; }
public bool WithinRangeOfStart(PathfinderNode node, int nodeDistance) { return(Vector2.Distance(node.GetLocation().To2D(), origin) < nodeDistance); }
public void CalcNodePos(PathfinderNode Node, ref XYDouble Pos, ref int SampleCount) { if ( Node.GetLayer.GetNetwork_LayerNum == 0 ) { var NodeTag = default(clsNodeTag); NodeTag = (clsNodeTag)Node.Tag; Pos.X += NodeTag.Pos.X; Pos.Y += NodeTag.Pos.Y; } else { var A = 0; for ( A = 0; A <= Node.GetChildNodeCount - 1; A++ ) { CalcNodePos(Node.get_GetChildNode(A), ref Pos, ref SampleCount); } SampleCount += Node.GetChildNodeCount; } }
public override double CalculateHeuristic(PathfinderNode end) { return(1200 * Vector2.Distance(PositionVector, ((Tile)end).PositionVector)); }
private PathfinderNode GetNearestNodeConnection(PathfinderNetwork Network, XYInt Pos, int MinClearance, float MaxDistance) { var A = 0; var TravelNodes = new PathfinderNode[Network.get_GetNodeLayer(0).GetNodeCount * 10]; var TravelNodeCount = 0; var NodeTravelDists = new float[Network.get_GetNodeLayer(0).GetNodeCount]; var TravelNodeNum = 0; var CurrentNode = default(PathfinderNode); var OtherNode = default(PathfinderNode); var tmpConnection = default(PathfinderConnection); PathfinderNode BestNode = null; float TravelDist = 0; var Flag = default(bool); for ( A = 0; A <= Network.get_GetNodeLayer(0).GetNodeCount - 1; A++ ) { NodeTravelDists[A] = float.MaxValue; } TravelNodes[0] = GetNearestNode(Network, Pos, 1); if ( TravelNodes[0] == null ) { return null; } TravelNodeCount = 1; NodeTravelDists[TravelNodes[0].Layer_NodeNum] = 0.0F; while ( TravelNodeNum < TravelNodeCount ) { CurrentNode = TravelNodes[TravelNodeNum]; if ( CurrentNode.Clearance >= MinClearance ) { if ( BestNode == null ) { BestNode = CurrentNode; } else if ( NodeTravelDists[CurrentNode.Layer_NodeNum] < NodeTravelDists[BestNode.Layer_NodeNum] ) { BestNode = CurrentNode; } } for ( A = 0; A <= CurrentNode.GetConnectionCount - 1; A++ ) { tmpConnection = CurrentNode.get_GetConnection(A); OtherNode = tmpConnection.GetOtherNode(CurrentNode); TravelDist = NodeTravelDists[CurrentNode.Layer_NodeNum] + tmpConnection.GetValue; if ( BestNode == null ) { Flag = true; } else if ( TravelDist < NodeTravelDists[BestNode.Layer_NodeNum] ) { Flag = true; } else { Flag = false; } if ( Flag && TravelDist < NodeTravelDists[OtherNode.Layer_NodeNum] ) { NodeTravelDists[OtherNode.Layer_NodeNum] = TravelDist; TravelNodes[TravelNodeCount] = OtherNode; TravelNodeCount++; } } TravelNodeNum++; } return BestNode; }
public bool WithinRangeOfStart(PathfinderNode node, int nodeDistance) { return(node.GetHCost() < nodeDistance * 10); }
public void SetValues(PathfinderNode pParent, float fDistanceFromstart, int iEstimatedDistanceFromDestination) { m_pParentNode = pParent; m_fDistanceFromStart = fDistanceFromstart; m_iEstimatedDistanceFromDestination = iEstimatedDistanceFromDestination; }
public override void ProcessNode( PathfinderNode currentNode, PathfinderNode startNode, PathfinderNode targetNode, PathfindingHeap <PathfinderNode> openSet, HashSet <PathfinderNode> closedSet, Dictionary <Point, PathfinderNode> activeNodes, Grid grid, int maxPathLength ) { Vector3 currentLocation = grid.NodeToWorldCoord(currentNode.GetGridCoord()); List <PathfinderNode> neighbors = PathfinderHelper.GetNeighbors( currentNode, activeNodes, currentNodeCreator ); foreach (PathfinderNode neighbour in neighbors) { Vector3 neighbourLocation = grid.NodeToWorldCoord(neighbour.GetGridCoord()); if (!neighbour.IsWalkable() || closedSet.Contains(neighbour)) { continue; } CostResult newStrategyCost = currentCostStrategy.GetAdditionalCostAt( currentLocation, neighbourLocation ); //neighbour.UpdateAccumulatedStrategyCost(newStrategyCost); int newPhysicalGCost = currentNode.GetPhysicalGCost() + PathfinderHelper.GetDistance(currentNode, neighbour); int newStrategyGCost = currentNode.GetStrategyGCost() + neighbour.GetExtractor().Extract(newStrategyCost); int newMovementCostToNeighbour = newPhysicalGCost + newStrategyGCost; // bool smaller = newStrategyGCost < neighbour.GetStrategyGCost(); //if (smaller) // { //DrawGizmo.AddGizmo(Color.green, newStrategyGCost + " " + neighbour.GetStrategyGCost(), neighbour.GetLocation()); //} //Debug.Log(neighbour.GetGCost()); if (newMovementCostToNeighbour < neighbour.GetGCost() || !openSet.Contains(neighbour)) { //Debug.Log(neighbour.GetGCost()); //DrawGizmo.AddGizmo(Color.green, "" + currentNode.GetExtractor().Extract(newStrategyCost), neighbour.GetLocation()); neighbour.SetStrategyCost( newStrategyCost ); neighbour.SetStrategyGCost( newStrategyGCost ); neighbour.SetPhysicalGCost(newPhysicalGCost); neighbour.SetHCost(GetDistance(neighbour, targetNode)); neighbour.SetParent(currentNode); if (!openSet.Contains(neighbour) && neighbour.WithInRangeOfStart(maxPathLength) ) { openSet.Add(neighbour); PathfinderVisualizer.Visit(neighbour); /*DrawGizmo.AddGizmo(Color.grey, "", grid.NodeToWorldCoord( * neighbour.GetGridCoord()) * );*/ } else { openSet.UpdateItem(neighbour); } } else { } } }