///<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);
}
}
}
