/// <summary>
/// main Constructor for graph.
/// </summary>
public Graph()
{
// _listOfEdges = new List<Edge>();
_listOfNodes = new List<Node>();
nodeConnections = new Dictionary<string, List<string>>();
nodeRef = new Dictionary<string, Node>();
_sourceNode = null; //_targetNode = null;
//_totalCost = -1;
//_optimalTraversal = new List<Node>();
}
/// <summary>
/// Given a point, create a random point within a radius of maxDistance.
/// Generate points until one is found that is in the free space.
/// </summary>
/// <param name="centerPt"></param>
/// <returns></returns>
private Vector2 surveyAndAdd(Node centerPt)
{
Random random = new Random();
bool added = false;
double radius = maxDistance; //guarantee that the point will be generated to be an edge of centerPt
List<Node> checkNode = new List<Node>();
Vector2 testvec = new Vector2();
while (!added)
{
double theta = random.NextDouble() * 2 * Math.PI;
double rad = random.NextDouble() * radius / 2;
testvec = new Vector2((centerPt.coord.X + rad * Math.Cos(theta)), (centerPt.coord.Y + rad * Math.Sin(theta)));
added = addPoint(testvec.X, testvec.Y);
}
return testvec;
}
// To remove each point
//1) find all edges that contain that point and remove
// 2) remove from nodes.
/// <summary>
/// After the PRM is created, remove a point and its associated edges
/// Also used as a help method in removePoints()
/// </summary>
/// <param name="point"></param> point to be removed
/// <param name="obstacle"></param> if used in removePoints()
/// <param name="newOb"></param> new obstacle
private void removePoint(Node point, bool obstacle, List<Polygon> newOb)
{
List<Node> graphNodes = PRMGraph.AllNodes;
// List<Edge> graphEdges = PRMGraph.AllEdges;
//List<Edge> edgesToRemove = new List<Edge>();
string pID = point.VertexID;
//foreach (Edge e in PRMGraph.AllEdges )
//{
// if (e.PointA == point || e.PointB == point)
// edgesToRemove.Add(e);
// else if (obstacle&&!IsPathClear(newOb, e.PointA.coord, e.PointB.coord))
// edgesToRemove.Add(e);
//}
//this could take a while.
List<KeyValuePair<string, List<string>>> connectionDict = PRMGraph.nodeConnections.ToList();
foreach(KeyValuePair<string, List<string>> kp in connectionDict)
{
string origin = kp.Key;
Vector2 a = PRMGraph.nodeRef[kp.Key].coord;
List<string> connects = kp.Value.ToList();
foreach(string i in connects)
{
Vector2 b = PRMGraph.nodeRef[i].coord;
if(pID ==i)
PRMGraph.removeConnection(origin, i);
else if(obstacle && !IsPathClear(newOb, a, b))
PRMGraph.removeConnection(origin, i);
}
}
//foreach (Edge e in edgesToRemove)
// PRMGraph.removeEdge(e);
PRMGraph.removeVertex(point);
}
/// <summary>
/// Get the k nearest neighbors given a point
/// </summary>
/// <param name="origin"></param>Given point
/// <param name="k"></param>How many neighbors to return
/// <param name="kNN"></param> Index within PRMGraph of the neighboring points
/// <param name="kNNDist"></param> distance of the neighboring points to the origin points
private void getKNearestNeighbors(Node origin, int k, out List<int> kNN, out List<double> kNNDist)
{
kNN = new List<int>(k); //should all be zeros
kNNDist = new List<double>(k);
double originx = origin.coord.X;
double originy = origin.coord.Y;
List<Node> graphNodes = PRMGraph.AllNodes;
double[] distances = new double[graphNodes.Count];
bool fill = false;
for (int i = 0; i < graphNodes.Count; i++)
{
Node tempNode = graphNodes[i];
double distance = Math.Sqrt(Math.Pow((tempNode.coord.X - originx), 2) + Math.Pow((tempNode.coord.Y - originy), 2));
distances[i] = distance;
//automatically fill first k
bool clearPath = IsPathClear(obstacles,origin.coord, graphNodes[i].coord);
if (!fill && clearPath)
{
kNN.Add(i);
kNNDist.Add(distance);
}
// don't include nodes that are the same as the origin
// find farthest element and replace with new distance if closer
//hopefully this saves memory/time because avoiding sorting an array because we only want k elements of that array
else if (clearPath && distance < kNNDist.Max() && distance != 0)
{
int maxIndex = kNNDist.IndexOf(kNNDist.Max());
kNN[maxIndex] = i;
kNNDist[maxIndex] = distance;
}
if (kNN.Count == k)
fill = true;
}
}
//returns index of neighbors within maxDistance
/// <summary>
/// Get the neighbors of a given point within maxDistance
/// </summary>
/// <param name="origin"></param> Given point
/// <param name="kNN"></param> Index within PRMGraph of the neighboring points
/// <param name="kNNDist"></param> distance of the neighboring points to the origin points
public void getDistanceNeighbors(List<Node> allNodes, Node origin, double maxD, out List<int> kNN, out List<double> kNNDist)
{
kNN = new List<int>(); //should all be zeros
kNNDist = new List<double>();
double originx = origin.coord.X;
double originy = origin.coord.Y;
List<Node> graphNodes = allNodes;
for (int i = 0; i < graphNodes.Count; i++)
{
Node tempNode = graphNodes[i];
double distance = Math.Sqrt(Math.Pow((tempNode.coord.X - originx), 2) + Math.Pow((tempNode.coord.Y - originy), 2));
bool clearPath = IsPathClear(this.obstacles, origin.coord, graphNodes[i].coord);
// don't include nodes that are the same as the origin
//If distance is less than maxDistance, add to list
if (clearPath && (distance < maxD) && distance != 0)
{
kNN.Add(i);
kNNDist.Add(distance);
}
}
}
/// <summary>
/// After PRM is created, add an addition point to the PRM
/// </summary>
/// <param name="x"></param> x coordinate of new point
/// <param name="y"></param>y coordinate of new point
/// <returns></returns> If adding the point was successful
public bool addPoint(double x, double y)
{
bool added = false;
Node newNode = new Node(x, y, false, fieldName);
List<Node> tempCheck = new List<Node>();
tempCheck.Add(newNode);
List<Node> tempList = new List<Node>();
tempList = IsIntersecting(tempCheck, mapPoint);
if (tempList.Count == 0)
{
PRMGraph.AddVertex(newNode);
List<Node> graphNodes = PRMGraph.AllNodes;
List<int> kNNIndices;
List<double> kNNDists;
getDistanceNeighbors(graphNodes, newNode,maxDistance, out kNNIndices, out kNNDists);
for (int j = 0; j < kNNIndices.Count; j++)
{
//PRMGraph.AddEdge(new Edge(newNode, graphNodes[kNNIndices[j]], kNNDists[j]));
// PRMGraph.AddEdge(new Edge(graphNodes[kNNIndices[j]], newNode, kNNDists[j]));
PRMGraph.AddConnection(newNode.VertexID, graphNodes[kNNIndices[j]].VertexID);
PRMGraph.AddConnection(graphNodes[kNNIndices[j]].VertexID, newNode.VertexID);
}
//PRMGraph.AddVertex(newNode);
added = true;
}
return added;
}
public void Push(Node node)
{
if (q.Count == 0)
{
q.Add(node);
return;
}
// If the Queue doesnt contain the node
// The queue is made up of pointers so if the node gets changed (aka cost and visited) outside of the queue, it will
//also update inside the queue
if(!q.Contains(node))
{
for (int i = 0; i < q.Count; i++)
{
if (q[i].AggregateCost > node.AggregateCost)
{
q.Insert(i, node);
return;
}
}
q.Add(node);
return;
}
}
/// <summary>
/// Perform the Calculations for Dijkstras
/// </summary>
/// <param name="targetNode"></param>
/// <returns></returns> if the Path to the target node is possible
private bool PerformCalculationForAllNodes(Node targetNode)
{
Node currentNode = _sourceNode;
bool reachable = true;
// Start by marking the source node as visited
currentNode.Visited = true;
int count = 0;
bool finish = false;
Node previousBest = currentNode;
PriorityQueue queue = new PriorityQueue();
queue.Push(currentNode);
do
{
count++;
//Node nextBestNode = null;
//"Visit" the node with the lowest cost
Node visitedNode = queue.Pop();
nodeRef[visitedNode.VertexID].Visited = true;
// If the visited node is the target node, then we have found the shortest path
if (visitedNode.VertexID == targetNode.VertexID)
{
finish = true;
break;
}
// Console.WriteLine("{0}", visitedNode.VertexID);
// find all connected nodes that have not already been visited(popped from the queue)
List<string> connectedEdges = getConnections(visitedNode.VertexID);
if (connectedEdges.Count == 0)
{
visitedNode.Deadend = true;
}
else
{
//find the cost of each connected node and update its node that results in the lowest cost
for (int j = 0; j < connectedEdges.Count; j++)
{
string connectedEdgeID = connectedEdges[j];
Node n = nodeRef[connectedEdgeID];
double distance = Math.Sqrt(Math.Pow((nodeRef[connectedEdgeID].coord.X - visitedNode.coord.X), 2) + Math.Pow((nodeRef[connectedEdgeID].coord.Y - visitedNode.coord.Y), 2));
if (nodeRef[connectedEdgeID].AggregateCost == Node.INFINITY
|| (visitedNode.AggregateCost + distance) < nodeRef[connectedEdgeID].AggregateCost)
{
nodeRef[connectedEdgeID].AggregateCost = visitedNode.AggregateCost + distance;
// update the pointer to the edge with the lowest cost in the other node
nodeRef[connectedEdgeID].lowestNode = visitedNode.VertexID;
}
//put connected nodes on the queue
queue.Push((nodeRef[connectedEdgeID]));
}
}
// If the queue empties that means we've visited all possible nodes and the path is not possible
if (queue.q.Count == 0)
{
finish = true;
reachable = false;
}
} while (!finish); // Loop until either queue is empty or visits targetNode
return reachable;
}
/// <summary>
/// After Dijkstra calculations are complete: retrieve the shortest path ffrom sourceNOde to targetNode
/// </summary>
/// <param name="targetNode"></param> target
/// <returns></returns> shortestpath
public List<Node> RetrieveShortestPath(Node targetNode)
{
List<Node> shortestPath = new List<Node>();
if (targetNode == null)
{
throw new InvalidOperationException("Target node is null.");
}
else
{
Node currentNode = targetNode;
shortestPath.Add(currentNode);
while (currentNode.lowestNode != "")
{
currentNode = nodeRef[currentNode.lowestNode];
Console.WriteLine("L{0}", currentNode.VertexID);
shortestPath.Add(currentNode);
}
}
// reverse the order of the nodes, because we started from target node first
shortestPath.Reverse();
return shortestPath;
}
/// <summary>
/// Remove node from graph
/// </summary>
/// <param name="node"></param>
public void removeVertex(Node node)
{
_listOfNodes.Remove(node);
nodeConnections.Remove(node.VertexID);
nodeRef.Remove(node.VertexID);
this.Reset();
}
/// <summary>
/// Dijkstra Initialization
/// </summary>
/// <param name="targetNode"></param>
/// <returns></returns>
public bool CalculateShortestPath(Node targetNode)
{
bool reachable = true;
if (_sourceNode == null) // || _targetNode == null)
{
return false;
}
// Algorithm starts here
// Reset stats
this.Reset();
// Set the cost on the source node to 0 and flag it as visited
_sourceNode.AggregateCost = 0;
// if the targetnode is not the sourcenode
// if (_targetNode.AggregateCost == Node.INFINITY) {
// Start the traversal across the graph
reachable = PerformCalculationForAllNodes(targetNode);
//}
//_totalCost = _targetNode.AggregateCost;
return reachable;
}
/// <summary>
/// Adds a vertex to the graph.
/// </summary>
/// <param name="node"></param>
public void AddVertex(Node node)
{
_listOfNodes.Add(node);
nodeConnections.Add(node.VertexID, new List<string>());
nodeRef.Add(node.VertexID, node);
// Reset stats due to a change to the graph.
this.Reset();
}
