/// <summary>
/// Resets the agent's current drive path
/// </summary>
private void ResetPath()
{
m_connectionPath = null;
m_currentTargetACOConn = null;
m_currentTargetACOConnIndex = 0;
m_currentACOConnRouteIndex = 0;
}
/// Generates ACOConnection list from the given goals and calculates an A* route between each
private List <ACOConnection> CalculateGoalsAndRoutes(List <GameObject> goals)
{
List <ACOConnection> goalACOConnections = new List <ACOConnection>();
foreach (GameObject goal in goals)
{
foreach (GameObject j in goals)
{
// If j isn't equal to current goal, add as a connection
if (goal != j)
{
/// Set ACO From and To
ACOConnection acoConnection = new ACOConnection();
acoConnection.SetConnection(goal, j, 1.0f);
/// Query A* navigate to see if route is possible
List <Connection> aStarRoute = this.NavigateAStar(acoConnection.FromNode, acoConnection.ToNode);
if (aStarRoute != null && aStarRoute.Count > 0)
{
/// Is a A* route, set and add
acoConnection.SetAStarRoute(aStarRoute);
goalACOConnections.Add(acoConnection);
}
else
{
Debug.LogError($"Unable to generate an A* path between '{goal.name}' and '{j.name}'");
}
}
}
}
return(goalACOConnections);
}
// Log Route
private void LogRoute(GameObject StartNode, int MaxPath, GameObject[] WaypointNodes, List <ACOConnection> Connections)
{
GameObject CurrentNode = null;
foreach (GameObject GameObjectNode in WaypointNodes)
{
if (GameObjectNode.Equals(StartNode))
{
CurrentNode = GameObjectNode;
}
}
ACOConnection HighestPheromoneConnection = null;
string Output = "Route (Q: " + Q + ", Alpha: " + Alpha + ", Beta: " + Beta + ", EvaporationFactor: " + EvaporationFactor + ", DefaultPheromone: " + DefaultPheromone + "):\n";
int PathCount = 1;
while (CurrentNode != null)
{
List <ACOConnection> AllFromConnections = AllConnectionsFromNode(CurrentNode, Connections);
if (AllFromConnections.Count > 0)
{
HighestPheromoneConnection = AllFromConnections[0];
foreach (ACOConnection aConnection in AllFromConnections)
{
if (aConnection.PheromoneLevel > HighestPheromoneConnection.PheromoneLevel)
{
HighestPheromoneConnection = aConnection;
}
}
CurrentNode = HighestPheromoneConnection.ToNode;
Output += "| FROM: " + HighestPheromoneConnection.FromNode.name + ", TO: " + HighestPheromoneConnection.ToNode.name + " (Pheromone Level: " + HighestPheromoneConnection.PheromoneLevel + ") | \n";
}
else
{
CurrentNode = null;
}
// If the current node is the start node at this point then we have looped
// through the path and should stop.
if (CurrentNode != null && CurrentNode.Equals(StartNode))
{
CurrentNode = null;
Output += "HOME (Total Nodes:" + WaypointNodes.Length + ", Nodes in Route: " + PathCount + ").\n";
}
// If the path count is greater than a max we should stop.
if (PathCount > MaxPath)
{
CurrentNode = null;
Output += "MAX PATH (Total Nodes:" + WaypointNodes.Length + ", Nodes in Route: " + PathCount + ").\n";
}
PathCount++;
}
Debug.Log(Output);
}
/// <summary>
/// Resets the agent's current drive path
/// </summary>
private void ResetPath()
{
m_acoConnectionPath = null;
m_currentTargetACOConn = null;
m_currentTargetACOConnIndex = 0;
m_currentACOConnRouteIndex = 0;
m_startToACOPath = null;
m_startToACONavInfo = null;
m_acoToStartPath = null;
m_acoToStartNavInfo = null;
m_totalDuration = 0f;
}
public List <ACOConnection> GenerateRoute(GameObject StartNode, int MaxPath, List <ACOConnection> Connections)
{
GameObject CurrentNode = StartNode;
List <ACOConnection> Route = new List <ACOConnection>();
ACOConnection HighestPheromoneConnection = null;
int PathCount = 1;
while (CurrentNode != null)
{
List <ACOConnection> AllFromConnections = AllConnectionsFromNode(CurrentNode, Connections);
if (AllFromConnections.Count > 0)
{
HighestPheromoneConnection = AllFromConnections[0];
foreach (ACOConnection aConnection in AllFromConnections)
{
if (aConnection.PheromoneLevel > HighestPheromoneConnection.PheromoneLevel)
{
HighestPheromoneConnection = aConnection;
}
}
Route.Add(HighestPheromoneConnection);
CurrentNode = HighestPheromoneConnection.ToNode;
}
else
{
CurrentNode = null;
}
// If the current node is the start node at this point then we have looped through the path and should stop.
if (CurrentNode != null && CurrentNode.Equals(StartNode))
{
CurrentNode = null;
}
// If the path count is greater than a max we should stop.
if (PathCount > MaxPath)
{
CurrentNode = null;
}
PathCount++;
}
return(Route);
}
/// <summary>
/// Drives the along a certain connection path
/// </summary>
/// <param name="startToACOAStarPath">A* path from the agent start to the ACO path</param>
/// <param name="acoConnections">The ACO connections path that goal around all goals</param>
/// <param name="acoToStartAStarPath">A* path from the final ACO node back to the agent's start path</param>
public void SetMovePath(List <Connection> startToACOAStarPath, List <ACOConnection> acoConnections, List <Connection> acoToStartAStarPath)
{
if (acoConnections == null || acoConnections != null && acoConnections.Count <= 0)
{
Debug.LogError("Unable to drive along connection path. acoConnections invalid");
return;
}
/// Set navigation paths for this agent. Contains three paths:
/// Agent start to the initial ACO start node
/// The Total ACO path
/// Final ACO node to the initial Agent start node
m_acoConnectionPath = acoConnections;
m_startToACOPath = startToACOAStarPath;
m_acoToStartPath = acoToStartAStarPath;
/// Set Move states to go from Start to the ACO start
m_currentDrivePathTarget = NavigationTarget.StartToACO;
/// Set movement vars to default values
m_currentTargetACOConnIndex = 0;
m_currentACOConnRouteIndex = 0;
m_currentTargetACOConn = m_acoConnectionPath[m_currentTargetACOConnIndex];
/// Set NavigateInfo for each A* path
m_startToACONavInfo = new NavigateToInfo
{
TargetNode = m_startToACOPath.FirstOrDefault().ToNode,
TargetIndex = 0
};
m_acoToStartNavInfo = new NavigateToInfo
{
TargetNode = m_acoToStartPath.FirstOrDefault().ToNode,
TargetIndex = 0,
};
/// Set start position for agent
this.transform.position = startToACOAStarPath.FirstOrDefault().FromNode.transform.position;
Debug.Log($"Agent '{this.name}' path set! Start to ACO '{m_startToACOPath.Count}' Waypoints, ACO Total '{m_acoConnectionPath.Count}' waypoints, ACO to Start '{m_acoToStartPath.Count}' waypoints");
m_ui.SetStatusText($"New Path: Moving to '{m_acoConnectionPath[m_acoConnectionPath.Count - 1].ToNode.name}'");
}
/// <summary>
/// Drives the along a certain connection path
/// </summary>
/// <param name="connectionPath">List of connections to drive along</param>
public void SetMovePath(GameObject start, List <ACOConnection> acoConnections)
{
if (acoConnections == null || acoConnections != null && acoConnections.Count <= 0)
{
Debug.LogError("Unable to drive along connection path. acoConnections invalid");
return;
}
m_connectionPath = acoConnections;
Debug.Log($"Squirrel '{this.name}' path set! '{m_connectionPath.Count}' connections");
/// Set movement vars to default values
m_currentTargetACOConnIndex = 0;
m_currentACOConnRouteIndex = 0;
m_currentTargetACOConn = m_connectionPath[m_currentTargetACOConnIndex];
/// Set start position for agent
this.transform.position = m_currentTargetACOConn.Route[m_currentACOConnRouteIndex].FromNode.transform.position;
m_ui.SetStatusText($"New Path: Moving to '{m_connectionPath[m_connectionPath.Count - 1].ToNode.name}'");
}
public void AddTravelledConnection(ACOConnection connection)
{
AntTravelledConnections.Add(connection);
}
private void NavigateACO()
{
if (m_currentTargetACOConn != null)
{
/// Look at next target node
GameObject targetNodeObj = m_currentTargetACOConn.Route[m_currentACOConnRouteIndex].ToNode;
PerformLookAt(targetNodeObj.transform);
if (!IsWaiting)
{
/// if not waiting, move toward next route node
PerformMovementTo(targetNodeObj.transform.position);
/// Check if agent reached next route node
float nextNodeDistance = Vector3.Distance(transform.position, targetNodeObj.transform.position);
if (nextNodeDistance < DESTINATION_TOLERANCE)
{
/// Reached next route node, increment to next route node or to new ACOConnection
m_currentACOConnRouteIndex++;
/// Check RouteIndex is within route bounds
if (m_currentACOConnRouteIndex >= m_currentTargetACOConn.Route.Count)
{
/// If index is more than route, we've reached end and can move to next ACOConnection
m_currentACOConnRouteIndex = 0;
m_currentTargetACOConnIndex++;
/// Check if reached end of ACOConnection path
if (m_currentTargetACOConnIndex >= m_acoConnectionPath.Count)
{
ACOConnection finalConnection = m_acoConnectionPath[m_acoConnectionPath.Count - 1];
OnReachedPathEnd?.Invoke(this, finalConnection.ToNode);
m_ui.SetStatusText($"Finished path to '{finalConnection.ToNode.name}'");
m_currentDrivePathTarget = NavigationTarget.ACOToStart;
Debug.Log($"Agent '{this.name}' finished ACO path. Navigating A* path to Start");
return;
}
else
{
/// Move to next node in Route
//Debug.Log($"Reached ACO goal {m_currentTargetACOConn.ToNode.name}");
OnReachedGoal?.Invoke(this, m_currentTargetACOConn.ToNode);
/// Continue moving through ACOConnection path if not at end
m_currentTargetACOConn = m_acoConnectionPath[m_currentTargetACOConnIndex];
OnTravelNewConnection?.Invoke(this, m_currentTargetACOConn);
return;
}
}
else
{
/// Still more Route nodes to travel to, invoke event to specify the connection
Connection nextRouteConnection = m_currentTargetACOConn.Route[m_currentACOConnRouteIndex];
OnTravelNewConnection?.Invoke(this, nextRouteConnection);
}
}
}
}
}
void Update()
{
if (m_currentTargetACOConn != null)
{
GameObject targetNodeObj = m_currentTargetACOConn.Route[m_currentACOConnRouteIndex].ToNode;
/// Look at next target node
PerformLookAt(targetNodeObj.transform);
if (!IsWaiting)
{
/// if not waiting, move toward next route node
PerformMovementTo(targetNodeObj.transform.position);
/// Check if agent reached next route node
float nextNodeDistance = Vector3.Distance(transform.position, targetNodeObj.transform.position);
if (nextNodeDistance < DESTINATION_TOLERANCE)
{
/// Reached next route node, increment to next route node or to new ACOConnection
m_currentACOConnRouteIndex++;
if (m_currentACOConnRouteIndex >= m_currentTargetACOConn.Route.Count)
{
m_currentACOConnRouteIndex = 0;
m_currentTargetACOConnIndex++;
/// Check if reached end of ACOConnection path
if (m_currentTargetACOConnIndex >= m_connectionPath.Count)
{
ACOConnection finalConnection = m_connectionPath[m_connectionPath.Count - 1];
OnReachedPathEnd?.Invoke(this, finalConnection.ToNode);
m_ui.SetStatusText($"Finished path to '{finalConnection.ToNode.name}'");
ResetPath();
return;
}
else
{
/// Continue moving through ACOConnection path if not at end
m_currentTargetACOConn = m_connectionPath[m_currentTargetACOConnIndex];
OnTravelNewConnection?.Invoke(this, m_currentTargetACOConn);
return;
}
}
}
// Check if agent reached final ACOConnection node
float finalNodeDistance = Vector3.Distance(transform.position, m_connectionPath[m_connectionPath.Count - 1].ToNode.transform.position);
if (finalNodeDistance < DESTINATION_TOLERANCE && m_currentTargetACOConnIndex >= m_connectionPath.Count)
{
// Invoke event for reached path end and remove target
ACOConnection finalConnection = m_connectionPath[m_connectionPath.Count - 1];
OnReachedPathEnd?.Invoke(this, finalConnection.ToNode);
m_ui.SetStatusText($"Finished path to '{finalConnection.ToNode.name}'");
ResetPath();
}
}
}
}
/*
* TotalNumAnts = Total number of ants in the simulation.
* Connections = Connections between nodes.
* WaypointNodes = All the waypoint nodes in the waypoint graph used by the ACO algorithm.
*/
public List <ACOConnection> ACO(int IterationThreshold, int TotalNumAnts, GameObject[] WaypointNodes, List <ACOConnection> Connections, GameObject StartNode, int MaxPathLength)
{
if (StartNode == null)
{
Debug.Log("No Start node.");
return(null);
}
// The node the ant is currently at.
GameObject currentNode;
// A list of all visited nodes.
List <GameObject> VisitedNodes = new List <GameObject>();
// Clear ants from previous runs.
Ants.Clear();
for (int i = 0; i < IterationThreshold; i++)
{
for (int i2 = 0; i2 < TotalNumAnts; i2++)
{
ACOAnt aAnt = new ACOAnt();
// Randomly choose start node.
currentNode = WaypointNodes[Random.Range(0, WaypointNodes.Length)];
aAnt.StartNode = currentNode;
VisitedNodes.Clear();
// Keep moving through the nodes until visited them all.
// Keep looping until the number of nodes visited equals the number of nodes.
while (VisitedNodes.Count < WaypointNodes.Length)
{
// Get all connections from node.
List <ACOConnection> ConnectionsFromNodeAndNotVisited = AllConnectionsFromNodeAndNotVisited(currentNode, Connections, VisitedNodes);
// Sum the product of the pheromone level and the visibility
// factor on all allowed paths.
float TotalPheromoneAndVisibility = CalculateTotalPheromoneAndVisibility(ConnectionsFromNodeAndNotVisited);
// Calculate the product of the pheromone level and the visibility
// factor of the proposed path.
// Loop through the paths and check if visited destination already.
foreach (ACOConnection aConnection in ConnectionsFromNodeAndNotVisited)
{
// Not visited the path before.
float PathProbability = (Mathf.Pow(aConnection.PheromoneLevel, Alpha) * Mathf.Pow((1 / aConnection.Distance), Beta));
PathProbability = PathProbability / TotalPheromoneAndVisibility;
// Set path probability. Path probability is reset
// to zero at the end of each run.
aConnection.PathProbability = PathProbability;
}
// Travel down the path with the largest probability - or have a random
// choice if there are paths with equal probabilities.
// Loop through the paths and check if visited destination already.
ACOConnection largestProbability = null;
if (ConnectionsFromNodeAndNotVisited.Count > 0)
{
largestProbability = ConnectionsFromNodeAndNotVisited[0];
for (int i3 = 1; i3 < ConnectionsFromNodeAndNotVisited.Count; i3++)
{
if (ConnectionsFromNodeAndNotVisited[i3].PathProbability > largestProbability.PathProbability)
{
largestProbability = ConnectionsFromNodeAndNotVisited[i3];
}
else if (ConnectionsFromNodeAndNotVisited[i3].PathProbability == largestProbability.PathProbability)
{
// Currently, 100% of the time chooses shortest connection if probabilities are the same.
if (ConnectionsFromNodeAndNotVisited[i3].Distance < largestProbability.Distance)
{
largestProbability = ConnectionsFromNodeAndNotVisited[i3];
}
}
}
}
// largestProbability contains the path to move down.
VisitedNodes.Add(currentNode);
if (largestProbability != null)
{
currentNode = largestProbability.ToNode;
aAnt.AddTravelledConnection(largestProbability);
aAnt.AddAntTourLength(largestProbability.Distance);
}
} //~END: While loop.
Ants.Add(aAnt);
}
// Update pheromone by formula Δτij.
// Loop through the paths and check if visited destination already.
foreach (ACOConnection aConnection in Connections)
{
float Sum = 0;
foreach (ACOAnt TmpAnt in Ants)
{
List <ACOConnection> TmpAntConnections = TmpAnt.AntTravelledConnections;
foreach (ACOConnection tmpConnection in TmpAntConnections)
{
if (aConnection.Equals(tmpConnection))
{
Sum += Q / TmpAnt.AntTourLength;
}
}
}
float NewPheromoneLevel = (1 - EvaporationFactor) * aConnection.PheromoneLevel + Sum;
aConnection.PheromoneLevel = NewPheromoneLevel;
// Reset path probability.
aConnection.PathProbability = 0;
}
}
MyRoute = GenerateRoute(StartNode, MaxPathLength, Connections);
// Output connections and Pheromone to the log.
//LogAnts();
LogMyRoute(MyRoute);
//LogRoute(StartNode, MaxPathLength, WaypointNodes, Connections);
//LogConnections(Connections);
return(MyRoute);
}
