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