public Edge(Node a, Node b, connectDir dir, float cost)
{
A = a;
B = b;
Direction = dir;
Cost = cost;
}
//---------------------------------------------------------------------------------------------------------------
// weights all graph nodes to given source node
public static bool Perform( Graph graph, Node source )
{
if( source == null || !graph.Nodes.Contains(source) )
return false;
if(_DEBUG)
Debug.Log("----->START DIJKSTRA " + source.ID);
MGraph = graph;
MGraph.Reset();
source.AggregatedCost = 0;
bool finished = false;
Queue = new List<Node>(graph.Nodes);
int count = 0;
while(!finished)
{
Node nextNode = Queue.OrderBy( c => c.AggregatedCost ).FirstOrDefault( c => !float.IsPositiveInfinity(c.AggregatedCost) );
if(nextNode != null)
{
ProcessNode(nextNode);
Queue.Remove(nextNode);
count++;
}
else
finished = true;
}
if(_DEBUG)
Debug.Log ("------>Finish Dijkstra " + count);
return true;
}
public static Node GetNextNodeRandom(Node n)
{
if(n != null && n.AdjacentEdges.Count > 0)
{
return n.AdjacentEdges[Random.Range(0, n.AdjacentEdges.Count)].GetOther(n);
}
return null;
}
public bool canTravelFromNode(Node n)
{
if(Contains(n))
{
switch(Direction)
{
case connectDir.AB: return n == A;
case connectDir.BA: return n == B;
default: return true;
}
}
return false;
}
public static Path GetPath(Vector3 start, Vector3 target)
{
if(graph == null)
{
throw new System.Exception("You must setup a graph first before attempting to calculate paths!");
return null;
}
Node srcNode = graph.GetNearestNode(start);
Node targetNode = graph.GetNearestNode(target);
if(srcNode != null && targetNode != null)
{
Dijkstra.Perform(graph, srcNode);
List<Node> nodes = Dijkstra.RetrieveShortestPath(srcNode, targetNode);
lastPath = nodes;
lastSrcNode = srcNode;
return new Path(nodes, start, target);
}
else
{
throw new System.Exception("could not find start & target node in pathfinder graph!");
return null;
}
}
static float GetEdgeCost(Node a, Node b)
{
return Vector3.Distance(a.pos, b.pos);
}
/// <summary>
/// fetches pathnode data from the scene and constructs the levelgraph
/// </summary>
public static void SetupGraph(GraphData data)
{
graph = new Graph();
if(data.hasContent())
{
// create nodes
var nodes = data.nodes.Select((graphNode n)=> {
Node node = new Node();
node.pos = n.position;
node.ID = n.ID;
return node;
})
.ToList();
// create edges
var edges = data.edges.Select((graphConnection c)=> {
Node a = nodes.Find(x=> x.ID==c.A);
Node b = nodes.Find(x=> x.ID==c.B);
if(a == null || b == null)
{
throw new System.Exception("could not load edge, wrong IDs stored: "
+ c.A + " / " + c.B + " nodecount: " + nodes.Count);
}
if(!c.supressed
&& (c.forced || GetEdgeCost(a, b) <= data.autoConnectionDist))
{
Edge e = new Edge(a, b, c.dir, GetEdgeCost(a, b));
a.AdjacentEdges.Add(e);
b.AdjacentEdges.Add(e);
return e;
}
else
return null;
})
.Where(z=> z != null)
.ToList();
// create graph
graph.Nodes = nodes;
graph.Edges = edges;
Debug.Log(graph.PrintEdges());
}
else
{
throw new System.Exception("could not load graph; Graphdata is empty.");
}
}
public Node GetOther(Node node)
{
if(node == A)
return B;
else if(node == B)
return A;
else return null;
}
public bool Contains(Node p)
{
return p == A || p == B;
}
public Edge(Node a, Node b, connectDir dir)
{
A = a;
B = b;
Cost = 0;
}
public Edge FindEdge(Node p1, Node p2)
{
if(p1 == null || p2 == null)
return null;
return Edges.Find( x => x.Contains(p1) && x.Contains(p2) );
}
//---------------------------------------------------------------------------------------------------------------
// traverses the graph from given target node to source
public static List<Node> RetrieveShortestPath( Node src, Node destination )
{
if( MGraph == null
|| !MGraph.Nodes.Contains(src)
|| !MGraph.Nodes.Contains(destination))
{
return null;
}
List<Node> path = new List<Node>();
Node current = destination;
path.Add(destination);
bool noPath = false;
if(!float.IsInfinity(current.AggregatedCost))
{
while( current.LowestCostEdge != null )
{
// Debug.Log("OTHER? " + (current.EdgeWithLowestCost.GetOther(current) != null) );
// Debug.Log(current.UniqueID + " to " + current.EdgeWithLowestCost.GetOther(current).UniqueID);
// Debug.Log((current as GridNode).TileX + " " + (current as GridNode).TileY + " | "
// + (current.EdgeWithLowestCost.GetOther(current) as GridNode).TileX
// + (current.EdgeWithLowestCost.GetOther(current) as GridNode).TileY);
//
path.Add(current);
if( current.LowestCostEdge == null
&& current.AggregatedCost != 0)
{
noPath = true;
}
current = current.LowestCostEdge.GetOther(current);
}
}
else
noPath = true;
if(noPath)
{
// if path could not be found, retrieve the last possible node and return a path to this
Node lastPossible = GetUnfinishedPath();
return RetrieveShortestPath( src, lastPossible );
}
else
{
path.Add(src);
path.Reverse();
return path;
}
}
//---------------------------------------------------------------------------------------------------------------
// calculates cost of the all edges to the given node
// if it provides a better path to source
static void ProcessNode(Node node)
{
foreach(Edge e in node.AdjacentEdges)
{
// Debug.Log("process edge of " + node.ID + ".... blocked? " + e.isBlocked + " canTravel? " + e.canTravelFromNode(node));
if( !e.isBlocked && e.canTravelFromNode(node) )
{
float cost = node.AggregatedCost + e.Cost;
if( cost < e.GetOther(node).AggregatedCost )
{
e.GetOther(node).AggregatedCost = cost;
e.GetOther(node).LowestCostEdge = e;
// Debug.Log(node.ID + " connected to " + e.GetOther(node).ID + "... -> " + cost);
}
}
}
}
