public Path(string id, int Cost) { this.Cost = Cost; this.HeuristicCost = Cost; this.MarkerInstances = new List <MarkerInstance>(); MarkerInstance MI = new MarkerInstance(id); this.MarkerInstances.Add(MI); }
public Path RunHeuristicSearch(MarkerContainer markerContainer, string goalID) { bool searching = true; int totalExpansions = 0; Path goalPath = new Path("", -1); //Loop contiously until a goal state while (searching) { // Initialize a null index value int expandingPathIndex = -1; // Loop through all possible paths within this UniformCostSearch Object for (int i = 0; i < this.Paths.Count; i++) { // if expandingPathIndex is still null if (expandingPathIndex == -1) { // if the path has only unique marker instances within the path if (this.Paths[i].MarkerInstances.Count == this.Paths[i].MarkerInstances.Distinct().Count()) { // Set expandingPathIndex as the current iteration expandingPathIndex = i; } else { // Remove this path, there is a cycle this.Paths.RemoveAt(i); i--; } } else if (this.Paths[expandingPathIndex].HeuristicCost > this.Paths[i].HeuristicCost) { if (this.Paths[i].MarkerInstances.Count == this.Paths[i].MarkerInstances.Distinct().Count()) { expandingPathIndex = i; } else { // Remove this path, there is a cycle this.Paths.RemoveAt(i); i--; } } } if (expandingPathIndex == -1) { //no solution that doesn't have a cycle //return null goalpath searching = false; goalPath.TotalExpansions = totalExpansions; return(goalPath); } if (this.Paths[expandingPathIndex].HeuristicCost > goalPath.Cost && goalPath.Cost != -1) { searching = false; goalPath.TotalExpansions = totalExpansions; return(goalPath); } // Write down the last added node within the arraylist (this requires the arraylist keeps insertion order) string LastNodeID = this.Paths[expandingPathIndex].MarkerInstances.Last().MarkerID; // Create a new marker object Marker marker = new Marker(); for (int i = 0; i < markerContainer.Markers.Count; i++) { if (markerContainer.Markers[i].ID == LastNodeID) { marker = markerContainer.Markers[i]; } } // For every connectedto ID in the marker for (int i = 0; i < marker.ConnectedTo.Count; i++) { // Create Path Switch as true bool addPath = true; // for every marker instance in the current path for (int j = 0; j < this.Paths[expandingPathIndex].MarkerInstances.Count; j++) { // if there is a marker that already contains this ID (hence there would be a cycle) if (this.Paths[expandingPathIndex].MarkerInstances[j].MarkerID == marker.ConnectedTo[i]) { // Disable adding this path. addPath = false; } } // If addpath switch is still true if (addPath) { // Create new path with previous path values Path newPath = new Path(this.Paths[expandingPathIndex].MarkerInstances, this.Paths[expandingPathIndex].Cost); MarkerInstance markerInstance = new MarkerInstance(marker.ConnectedTo[i]); // Determine Distance between two points string ParentID = newPath.MarkerInstances.Last().MarkerID; string ChildID = markerInstance.MarkerID; int x1 = Convert.ToInt32(ParentID.Substring(0, 3)); int y1 = Convert.ToInt32(ParentID.Substring(3, 3)); int x2 = Convert.ToInt32(ChildID.Substring(0, 3)); int y2 = Convert.ToInt32(ChildID.Substring(3, 3)); double magnitude = Math.Sqrt(Math.Pow(x1 - x2, 2) + Math.Pow(y1 - y2, 2)); int x3 = Convert.ToInt32(goalID.Substring(0, 3)); int y3 = Convert.ToInt32(goalID.Substring(3, 3)); double goalStateMagnitude = Math.Sqrt(Math.Pow(x2 - x3, 2) + Math.Pow(y2 - y3, 2)); // Add distance between the two points to the cost newPath.Cost += Convert.ToInt32(magnitude); newPath.HeuristicCost = newPath.Cost + Convert.ToInt32(goalStateMagnitude); // Add instance to newPath totalExpansions++; newPath.MarkerInstances.Add(markerInstance); this.Paths.Add(newPath); // If the last added marker has the goal state ID if (marker.ConnectedTo[i] == goalID) { // If the cost of the current goal path is greater than the new path's cost that has a goal state // OR if goalpath cost is NULL (unassigned) if (goalPath.Cost > newPath.Cost || goalPath.Cost == -1) { // Set the new path as the goal path goalPath = newPath; // initialize lowest cost to a high value for comparisons // This should probably be changed to include a -1 value to represent NULL double lowestCost = 999999; // for every path in the current paths for (int j = 0; j < this.Paths.Count; j++) { // if the cost for the path is lower than the current lowestcost in the iteration // AND the path is not the path that we have just expanded if (this.Paths[j].HeuristicCost < lowestCost && this.Paths[expandingPathIndex] != this.Paths[j]) { // Assign the lowestcost to the path's cost lowestCost = this.Paths[j].HeuristicCost; } } // If the current goalpath cost is the lowest cost out of all of the paths if (goalPath.Cost <= lowestCost) { // Stop the loop, there is no better cost for the given paths. searching = false; // return the goal path goalPath.TotalExpansions = totalExpansions; return(goalPath); } } } } } // Remove this path as it is no longer required this.Paths.RemoveAt(expandingPathIndex); } // No more paths available, returning the currently assigned goalpath goalPath.TotalExpansions = totalExpansions; return(goalPath); }