///<summary> ///Tests if the current node is the target node. ///Used for search termination. ///</summary> ///<param name="graph"></param> ///<param name="target"></param> ///<param name="currentNodeIdx"></param> ///<returns></returns> public static bool IsSatisfied( SparseGraph graph, int target, int currentNodeIdx) { return currentNodeIdx == target; }
///<summary> ///constructor ///</summary> ///<param name="graph"></param> ///<param name="source"></param> public GraphMinSpanningTree(SparseGraph graph, int source) { _graph = graph; _spanningTree = new List<NavGraphEdge>(Graph.NumNodes); _fringe = new List<NavGraphEdge>(Graph.NumNodes); _costToThisNode = new List<float>(Graph.NumNodes); for (int i = 0; i < CostToThisNode.Count; i++) { CostToThisNode[i] = -1; } if (source < 0) { for (int nd = 0; nd < Graph.NumNodes; ++nd) { if (SpanningTree[nd] == null) { Search(nd); } } } else { Search(source); } }
///<summary> ///calculate the straight line distance from node nd1 to node nd2 ///</summary> ///<param name="graph"></param> ///<param name="nd1"></param> ///<param name="nd2"></param> ///<returns></returns> public static float Calculate(SparseGraph graph, int nd1, int nd2) { float result = TorqueUtil.GetFastRandomFloat(0.9f, 1.1f)* (graph.GetNode(nd1).Position - graph.GetNode(nd2).Position).Length(); return result; }
///<summary> ///constructor ///</summary> ///<param name="graph"></param> ///<param name="source"></param> ///<param name="target"></param> ///<param name="bot">The bot requesting the search</param> public GraphSearchDijkstrasTimeSliced(SparseGraph graph, int source, int target, BotEntity bot) : base(SearchTypes.Dijkstra, bot) { _graph = graph; _shortestPathTree = new List<NavGraphEdge>(graph.NumNodes); _searchFrontier = new List<NavGraphEdge>(graph.NumNodes); _costToThisNode = new List<float>(graph.NumNodes); for (int i = 0; i < graph.NumNodes; i++) { ShortestPathTree.Add(null); SearchFrontier.Add(null); CostToThisNode.Add(0); } _source = source; Target = target; //create the PQ PQ = new IndexedPriorityQueueLow(CostToThisNode, Graph.NumNodes); //put the source node on the queue PQ.Insert(source); }
///<summary> ///constructor ///</summary> ///<param name="graph"></param> ///<param name="source"></param> ///<param name="target"></param> ///<param name="bot">Bot Requesting the path</param> public GraphSearchAStarTimeSliced( SparseGraph graph, int source, int target, BotEntity bot) : base(SearchTypes.AStar, bot) { _graph = graph; _shortestPathTree = new List<NavGraphEdge>(graph.NumNodes); _searchFrontier = new List<NavGraphEdge>(graph.NumNodes); _gCosts = new List<float>(graph.NumNodes); _fCosts = new List<float>(graph.NumNodes); for (int i = 0; i < graph.NumNodes; i++) { ShortestPathTree.Add(null); SearchFrontier.Add(null); GCosts.Add(0); FCosts.Add(0); } _source = source; _target = target; //create the priority queue _pq = new IndexedPriorityQueueLow(FCosts, Graph.NumNodes); //put the source node on the queue PQ.Insert(source); }
///<summary> ///constructor ///</summary> ///<param name="graph"></param> ///<param name="source"></param> ///<param name="target"></param> public GraphSearchAStar(SparseGraph graph, int source, int target) { _graph = graph; _source = source; _target = target; _shortestPathTree = new List<NavGraphEdge>(Graph.NumNodes); _searchFrontier = new List<NavGraphEdge>(Graph.NumNodes); _gCosts = new List<float>(Graph.NumNodes); _fCosts = new List<float>(Graph.NumNodes); Search(); }
///<summary> ///use to add the eight neighboring edges of a graph node that ///is positioned in a grid layout ///</summary> ///<param name="graph"></param> ///<param name="row"></param> ///<param name="col"></param> ///<param name="numCellsX"></param> ///<param name="numCellsY"></param> public static void AddAllNeighborsToGridNode( SparseGraph graph, int row, int col, int numCellsX, int numCellsY) { for (int i = -1; i < 2; ++i) { for (int j = -1; j < 2; ++j) { int nodeX = col + j; int nodeY = row + i; //skip if equal to this node if ((i == 0) && (j == 0)) continue; //check to see if this is a valid neighbor if (!ValidNeighbor(nodeX, nodeY, numCellsX, numCellsY)) continue; //calculate the distance to this node Vector2 posNode = graph.GetNode(row*numCellsX + col).Position; Vector2 posNeighbour = graph.GetNode(nodeY*numCellsX + nodeX).Position; float dist = (posNode - posNeighbour).Length(); //this neighbor is okay so it can be added NavGraphEdge newEdge = new NavGraphEdge( row*numCellsX + col, nodeY*numCellsX + nodeX, dist); graph.AddEdge(newEdge); //if graph is not a digraph, an edge needs to be added going //in the other direction if (graph.IsDigraph) continue; NavGraphEdge newReverseEdge = new NavGraphEdge( nodeY*numCellsX + nodeX, row*numCellsX + col, dist); graph.AddEdge(newReverseEdge); } } }
///<summary> ///constructor ///</summary> ///<param name="graph"></param> ///<param name="source"></param> ///<param name="target"></param> public GraphSearchBFS(SparseGraph graph, int source, int target) { _graph = graph; _source = source; _target = target; _isFound = false; _visited = new List<int>(Graph.NumNodes); for (int i = 0; i < Visited.Count; i++) { Visited[i] = (int) SearchStatus.Unvisited; } _route = new List<int>(Graph.NumNodes); for (int i = 0; i < Route.Count; i++) { Route[i] = (int) SearchStatus.NoParentAssigned; } _isFound = Search(); }
///<summary> ///constructor ///</summary> ///<param name="graph"></param> ///<param name="source"></param> ///<param name="target"></param> public GraphSearchDijkstra(SparseGraph graph, int source, int target) { _graph = graph; _source = source; _target = target; _shortestPathTree = new List<NavGraphEdge>(Graph.NumNodes); for (int i = 0; i < Graph.NumNodes; i++) ShortestPathTree.Add(null); _searchFrontier = new List<NavGraphEdge>(Graph.NumNodes); for (int i = 0; i < Graph.NumNodes; i++) SearchFrontier.Add(null); _costToThisNode = new List<float>(Graph.NumNodes); for (int i = 0; i < Graph.NumNodes; i++) CostToThisNode.Add(0); Search(); }
///<summary> ///Tests if the current node is linked to an active trigger of the ///desired type. Used for search termination. ///</summary> ///<param name="graph"></param> ///<param name="target"></param> ///<param name="currentNodeIdx"></param> ///<param name="bot">bot to verify against</param> ///<returns></returns> public static bool IsSatisfied( SparseGraph graph, //TODO: should not use target as node index and entity type int target, int currentNodeIdx, BotEntity bot) { bool bSatisfied = false; EntityTypes targetEntityType = Entity.Entity.ItemTypeToEntityType((ItemTypes) target); //get a reference to the node at the given node index NavGraphNode node = graph.GetNode(currentNodeIdx); //if the extrainfo field is pointing to a giver-trigger, test to //make sure it is active and that it is of the correct type. Trigger.Trigger t = node.ExtraInfo as Trigger.Trigger; if (t != null && t.IsActive && t.EntityType == targetEntityType && bot.FoundTriggers.List.Contains(t)) { bSatisfied = true; } return bSatisfied; }
///<summary> ///Get the cost of the costliest edge in the graph ///</summary> ///<param name="graph"></param> ///<returns>the cost of the costliest edge in the graph</returns> public static float GetCostliestGraphEdge(SparseGraph graph) { float greatest = Single.MinValue; foreach (NavGraphNode curNode in graph.Nodes) { if (GraphNode.IsInvalidIndex(curNode.Index)) continue; foreach (NavGraphEdge curEdge in graph.Edges[curNode.Index]) { if (curEdge.Cost > greatest) { greatest = curEdge.Cost; } } } return greatest; }
///<summary> ///constructor ///</summary> ///<param name="graph"></param> ///<param name="source"></param> public GraphSearchBFS(SparseGraph graph, int source) : this(graph, source, GraphNode.INVALID_NODE_INDEX) { }
//---------------------- CalculateAverageGraphEdgeLength ---------------------- // // //------------------------------------------------------------------------------ ///<summary> ///determines the average length of the edges in a navgraph (using the ///distance between the source and target node positions (not the cost ///of the edge as represented in the graph, which may account for all ///sorts of other factors such as terrain type, gradients etc) ///</summary> ///<param name="graph"></param> ///<returns></returns> public static float CalculateAverageGraphEdgeLength(SparseGraph graph) { float totalLength = 0; int numEdgesCounted = 0; foreach (NavGraphNode curNode in graph.Nodes) { if (GraphNode.IsInvalidIndex(curNode.Index)) continue; foreach (NavGraphEdge curEdge in graph.Edges[curNode.Index]) { //increment edge counter ++numEdgesCounted; //add length of edge to total length totalLength += (graph.GetNode(curEdge.From).Position - graph.GetNode(curEdge.To).Position).Length(); } } return totalLength/numEdgesCounted; }
///<summary> ///constructor ///</summary> ///<param name="owner"></param> public PathPlanner(BotEntity owner) { _owner = owner; _navGraph = GameManager.GameManager.Instance.Map.NavGraph; CurrentSearch = null; }
///<summary> ///constructor ///</summary> ///<param name="graph"></param> public GraphMinSpanningTree(SparseGraph graph) : this(graph, -1) { }
///<summary> ///sets up the game environment from map file ///</summary> ///<param name="filename"></param> ///<param name="mapData"></param> ///<returns></returns> public bool LoadMap(string filename, out MapData mapData) { mapData = null; try { mapData = MyGame.Instance.Content.Load<MapData>(filename); } catch (Exception e) { Assert.Fatal(false, "Map.LoadMap: Bad Map Filename -> " + e.Message); return false; } Clear(); //first of all read and create the navgraph. This must be done //before the entities are read from the map file because many of //the entities will be linked to a graph node (the graph node will //own a pointer to an instance of the entity) NavGraph = new SparseGraph(false); NavGraph.Load(mapData); LogUtil.WriteLineIfLogCreate("NavGraph for " + filename + " loaded okay"); LogUtil.WriteLineIfLogCreate(NavGraph.ToString()); //determine the average distance between graph nodes so that we can //partition them efficiently CellSpaceNeighborhoodRange = GraphUtil.CalculateAverageGraphEdgeLength(NavGraph) + 1; LogUtil.WriteLineIfLogCreate("Average edge length is " + GraphUtil.CalculateAverageGraphEdgeLength(NavGraph)); LogUtil.WriteLineIfLogCreate("Neighborhood range set to " + CellSpaceNeighborhoodRange); //load in the map size SizeX = mapData.SizeX; SizeY = mapData.SizeY; LogUtil.WriteLineIfLogCreate("Partitioning navgraph nodes..."); //partition the graph nodes PartitionNavGraph(); LogUtil.WriteLineIfLogCreate("Loading map..."); foreach (WallData wallData in mapData.WallList) { LogUtil.WriteLineIfLogCreate("Creating a wall <" + wallData.Name + "> between " + wallData.From + " and " + wallData.To); AddWall(wallData); } //note: add triggers before doors (or else!) foreach (DoorTriggerData doorTriggerData in mapData.DoorTriggerList) { LogUtil.WriteLineIfLogCreate("Creating a door trigger <" + doorTriggerData.Name + "> at " + doorTriggerData.Position); AddDoorTrigger(doorTriggerData); } foreach (DoorData doorData in mapData.DoorList) { LogUtil.WriteLineIfLogCreate("Creating a door <" + doorData.Name + "> between " + doorData.From + " and " + doorData.To); AddDoor(doorData); } foreach (SpawnPointData spawnPointData in mapData.SpawnPointList) { LogUtil.WriteLineIfLogCreate("Creating a spawn point <" + spawnPointData.Name + "> at " + spawnPointData.Position); AddSpawnPoint(spawnPointData); } foreach (HealthData healthData in mapData.HealthList) { LogUtil.WriteLineIfLogCreate( "Creating a health giver trigger <" + healthData.Name + "> at " + healthData.Position); AddHealth(healthData); } foreach (RailgunData railgunData in mapData.RailgunList) { LogUtil.WriteLineIfLogCreate( "Creating a rail gun weapon giver trigger <" + railgunData.Name + "> at " + railgunData.Position); AddRailgun(railgunData); } foreach (RocketLauncherData rocketLauncherData in mapData.RocketLauncherList) { LogUtil.WriteLineIfLogCreate( "Creating a rocket launcher weapon giver trigger <" + rocketLauncherData.Name + "> at " + rocketLauncherData.Position); AddRocketLauncher(rocketLauncherData); } foreach (ShotgunData shotgunData in mapData.ShotgunList) { LogUtil.WriteLineIfLogCreate( "Creating a shot gun weapon giver trigger <" + shotgunData.Name + "> at " + shotgunData.Position); AddShotgun(shotgunData); } LogUtil.WriteLineIfLogCreate(filename + " loaded okay"); //calculate the cost lookup table PathCosts = GraphUtil.CreateAllPairsCostsTable(NavGraph); return true; }
///<summary> ///creates a lookup table encoding the shortest path info between each ///node in a graph to every other ///</summary> ///<param name="graph"></param> ///<returns></returns> public static List<List<int>> CreateAllPairsTable(SparseGraph graph) { const int noPath = -1; List<int> row = new List<int>(graph.NumNodes); for (int i = 0; i < row.Count; i++) { row[i] = noPath; } List<List<int>> shortestPaths = new List<List<int>>(graph.NumNodes); for (int i = 0; i < shortestPaths.Count; i++) { shortestPaths[i] = new List<int>(row); } for (int source = 0; source < graph.NumNodes; ++source) { //calculate the SPT for this node GraphSearchDijkstra search = new GraphSearchDijkstra(graph, source); List<NavGraphEdge> spt = search.SpanningTree; //now we have the SPT it's easy to work backwards through it to //find the shortest paths from each node to this source node for (int target = 0; target < graph.NumNodes; ++target) { //if the source node is the same as the target just set to //target if (source == target) { shortestPaths[source][target] = target; } else { int nd = target; while ((nd != source) && (spt[nd] != null)) { shortestPaths[spt[nd].From][target] = nd; nd = spt[nd].From; } } } } return shortestPaths; }
///<summary> ///creates a lookup table of the cost associated from traveling from one ///node to every other ///</summary> ///<param name="graph"></param> ///<returns></returns> public static List<List<float>> CreateAllPairsCostsTable( SparseGraph graph) { //create a two dimensional vector List<List<float>> pathCosts = new List<List<float>>(graph.NumNodes); for (int i = 0; i < graph.NumNodes; i++) { pathCosts.Add(new List<float>(graph.NumNodes)); for (int j = 0; j < graph.NumNodes; j++) { pathCosts[i].Add(0); } } for (int source = 0; source < graph.NumNodes; ++source) { //do the search GraphSearchDijkstra search = new GraphSearchDijkstra(graph, source); //iterate through every node in the graph and grab the cost to //travel to that node for (int target = 0; target < graph.NumNodes; ++target) { if (source != target) { pathCosts[source][target] = search.GetCostToNode(target); } } } return pathCosts; }
///<summary> ///creates a graph based on a grid layout. This function requires the ///dimensions of the environment and the number of cells required ///horizontally and vertically ///</summary> ///<param name="graph"></param> ///<param name="cySize"></param> ///<param name="cxSize"></param> ///<param name="numCellsY"></param> ///<param name="numCellsX"></param> public static void CreateGrid( SparseGraph graph, int cySize, int cxSize, int numCellsY, int numCellsX) { //need some temporaries to help calculate each node center float cellWidth = (float) cySize/numCellsX; float cellHeight = (float) cxSize/numCellsY; float midX = cellWidth/2; float midY = cellHeight/2; //first create all the nodes for (int row = 0; row < numCellsY; ++row) { for (int col = 0; col < numCellsX; ++col) { graph.AddNode( new NavGraphNode( graph.NextFreeNodeIndex, new Vector2( midX + (col*cellWidth), midY + (row*cellHeight)))); } } //now to calculate the edges. (A position in a 2D array [x][y] is //the same as [y*NumCellsX + x] in a 1d array). Each cell has up to //eight neigbors. for (int row = 0; row < numCellsY; ++row) { for (int col = 0; col < numCellsX; ++col) { AddAllNeighborsToGridNode( graph, row, col, numCellsX, numCellsY); } } }
///<summary> ///Given a cost value and an index to a valid node this function ///examines all a node's edges, calculates their length, and ///multiplies the value with the weight. Useful for setting terrain ///costs. ///</summary> ///<param name="graph"></param> ///<param name="node"></param> ///<param name="weight"></param> public static void WeightNavGraphNodeEdges( SparseGraph graph, int node, float weight) { //make sure the node is present Assert.Fatal(node < graph.NumNodes, "GraphUtil.WeightNavGraphNodeEdges: node index out of range"); //set the cost for each edge foreach (NavGraphEdge curEdge in graph.Edges[node]) { //calculate the distance between nodes float dist = (graph.GetNode(curEdge.From).Position - graph.GetNode(curEdge.To).Position).Length(); //set the cost of this edge graph.SetEdgeCost(curEdge.From, curEdge.To, dist*weight); //if not a digraph, set the cost of the parallel edge to be //the same if (!graph.IsDigraph) { graph.SetEdgeCost(curEdge.To, curEdge.From, dist*weight); } } }
///<summary> ///constructor ///</summary> ///<param name="graph"></param> ///<param name="source"></param> public GraphSearchDijkstra(SparseGraph graph, int source) : this(graph, source, -1) { }
///<summary> ///draws the given graph ///</summary> ///<param name="graph"></param> ///<param name="color"></param> public static void Draw(SparseGraph graph, Color color) { Draw(graph, color, false); }
///<summary> ///draws the given graph ///</summary> ///<param name="graph"></param> ///<param name="color"></param> ///<param name="drawNodeIds"></param> public static void Draw( SparseGraph graph, Color color, bool drawNodeIds) { //just return if the graph has no nodes if (graph.NumNodes == 0) return; //draw the nodes foreach (NavGraphNode curNode in graph.Nodes) { if (GraphNode.IsInvalidIndex(curNode.Index)) continue; DrawUtil.Circle(curNode.Position, 2, color, 20); if (drawNodeIds) { TextUtil.DrawText( @"data\fonts\Arial6", //TODO: should be a parameter new Vector2( curNode.Position.X + 5, curNode.Position.Y - 5), new Color(200, 200, 200), curNode.Index.ToString()); } foreach (NavGraphEdge curEdge in graph.Edges[curNode.Index]) { DrawUtil.Line( curNode.Position, graph.GetNode(curEdge.To).Position, color); } } }