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