void Start()
{
rb = GetComponent <Rigidbody2D>();
initialPosition = new TimeWaypoint(transform.position, transform.rotation, startVelocity);
lastVelocity = startVelocity;
_player = GameObject.Find("Player").transform;
}
/// <summary>
/// Estimates the time it takes to travel from one waypoint to another.
/// </summary>
/// <param name="from">The start node.</param>
/// <param name="to">The destination node.</param>
/// <returns>The estimated time it takes to travel between the nodes.</returns>
private double EstimateTravelTime(TimeWaypoint from, Waypoint to)
{
// Init, if not done yet
if (_timeGraph == null)
{
_timeGraph = new TimeGraph(_instance);
}
// --> Calculate time needed for driving the given distance
double travelTime = Distances.CalculateEuclid(from.OriginalWaypoint, to, _instance.WrongTierPenaltyDistance) / _timeGraph.Speed;
// Check whether we reached the goal already, thus, eliminating the need for turning
if (from.OriginalWaypoint == to)
{
// No turning necessary - only consider time for driving
return(travelTime);
}
else
{
// In addition to the drive time also consider the effort for turning
return
// --> Calculate time needed for turning towards new orientation
(Math.Abs(Circle.GetOrientationDifference(from.Orientation,
Circle.GetOrientation(from.OriginalWaypoint.X, from.OriginalWaypoint.Y, to.X, to.Y))) / TimeGraph.PI2 * _timeGraph.TurnSpeed +
// --> Add time for driving
travelTime);
}
}
/// <summary>
/// Calculates the time to move from one time-waypoint to another.
/// </summary>
/// <param name="from">The start node.</param>
/// <param name="to">The destination node.</param>
/// <returns>The time for traveling from one node to the other.</returns>
public double GetTimeNeededToTravel(TimeWaypoint from, TimeWaypoint to)
{
return
// --> Calculate time needed for turning towards new orientation
(Math.Abs(Circle.GetOrientationDifference(from.Orientation, to.Orientation)) / PI2 * TurnSpeed +
// --> Calculate time needed for driving the given distance
Distances.CalculateEuclid(from.OriginalWaypoint, to.OriginalWaypoint, Instance.WrongTierPenaltyDistance) / Speed);
}
/// <summary>
/// Creates a new time-waypoint graph.
/// </summary>
/// <param name="instance">The instance that the graph shall reflect.</param>
public TimeGraph(Instance instance)
{
// Save instance
Instance = instance;
// Determine robot characteristics (act as if robots have infinite acceleration)
Speed = instance.Bots.Min(b => b.MaxVelocity);
TurnSpeed = instance.Bots.Min(b => b.TurnSpeed) * TURN_SPEED_SLOW_FACTOR;
// Create base graph for storing time information
foreach (var from in instance.Waypoints)
{
foreach (var to in from.Paths)
{
TimeWaypoint timeWP = new TimeWaypoint()
{
// Emulated waypoint is the "to-part" of the connection
OriginalWaypoint = to,
// Store from waypoint just for completeness
InboundWaypoint = from,
// The orientation the bot is in when approaching the "to-part" of the connection is derived from the "from-part" of it
Orientation = Circle.GetOrientation(from.X, from.Y, to.X, to.Y),
};
TimeWaypointsOfConnections[from, to] = timeWP;
if (!TimeWaypointsOfOriginalWaypoints.ContainsKey(to))
{
TimeWaypointsOfOriginalWaypoints[to] = new HashSet <TimeWaypoint>();
}
TimeWaypointsOfOriginalWaypoints[to].Add(timeWP);
TimeWaypoints.Add(timeWP);
}
}
// Add time information for all connections to the graph
foreach (var timeWPFrom in TimeWaypoints)
{
foreach (var connectedWP in timeWPFrom.OriginalWaypoint.Paths)
{
TimeWaypoint timeWPTo = TimeWaypointsOfConnections[timeWPFrom.OriginalWaypoint, connectedWP];
timeWPFrom.Edges.Add(new Tuple <TimeWaypoint, double>(
// --> Store node we approach when using the connection
timeWPTo,
// --> Calculate time needed to travel along the connection, i.e. the time for turning towards the connected waypoint + the time for driving to it
GetTimeNeededToTravel(timeWPFrom, timeWPTo)));
}
}
// Add turn on the spot information too
foreach (var timeWPFrom in TimeWaypoints)
{
foreach (var timeWPTo in TimeWaypointsOfOriginalWaypoints[timeWPFrom.OriginalWaypoint].Where(to => to != timeWPFrom))
{
timeWPFrom.Edges.Add(new Tuple <TimeWaypoint, double>(
// --> Store node of time graph
timeWPTo,
// --> Calculate time for turning on the spot
GetTimeNeededToTravel(timeWPFrom, timeWPTo)));
}
}
}
/// <summary>
/// Uses A* to find the distance from the startNode to the destinationNode.
/// </summary>
/// <param name="start">The starting waypoint.</param>
/// <param name="destination">The destination waypoint.</param>
/// <param name="wrongTierPenalty">The penalty for the waypoints not being on the same tier.</param>
/// <param name="emulatePodCarrying">Indicates whether to calculate a path that is ensured to be safe for carrying a pod on it.</param>
/// <returns>The distance.</returns>
private double CalculateShortestPath(Waypoint start, Waypoint destination, double wrongTierPenalty, bool emulatePodCarrying)
{
if (start == null || destination == null)
{
return(double.PositiveInfinity);
}
if (start == destination)
{
return(0);
}
// Determine shortest time
double shortestTime = double.PositiveInfinity;
foreach (var startNode in _timeGraph.TimeWaypointsOfOriginalWaypoints[start])
{
Dictionary <TimeWaypoint, TimeWaypointSearchDatapoint> openLocations = new Dictionary <TimeWaypoint, TimeWaypointSearchDatapoint>();
Dictionary <TimeWaypoint, TimeWaypointSearchDatapoint> closedLocations = new Dictionary <TimeWaypoint, TimeWaypointSearchDatapoint>();
openLocations[startNode] = new TimeWaypointSearchDatapoint(0.0, EstimateTravelTime(startNode, destination), startNode, null, 0);
// Loop until end is found
while (true)
{
// Find lowest cost waypoint in openLocations
KeyValuePair <TimeWaypoint, TimeWaypointSearchDatapoint> currentNodeKVP = openLocations.ArgMin(w => w.Value.TimeTraveled + w.Value.TimeToGoal);
// Something wrong happened -can't find the end
if (currentNodeKVP.Equals(default(KeyValuePair <TimeWaypoint, TimeWaypointSearchDatapoint>)))
{
break;
}
TimeWaypoint currentNode = currentNodeKVP.Key;
// Grab the details about the current waypoint
TimeWaypointSearchDatapoint currentNodeData = currentNodeKVP.Value;
// If the closest is also the destination or out of iterations
if (currentNode.OriginalWaypoint == destination)
{
// Update traveled time
shortestTime = Math.Min(shortestTime, currentNodeData.TimeTraveled);
break;
}
// Transfer closest from open to closed list
closedLocations[currentNode] = currentNodeKVP.Value;
openLocations.Remove(currentNode);
// Expand all the moves
foreach (var edge in currentNode.Edges)
{
// Check whether the node is already on closed
if (closedLocations.ContainsKey(edge.Item1))
{
// Don't deal with anything already on the closed list (don't want loops)
continue;
}
// Can't use a pod storage location for the path
if (emulatePodCarrying && edge.Item1.OriginalWaypoint.PodStorageLocation && edge.Item1.OriginalWaypoint != destination)
{
continue;
}
// Tag on more distance for a node on the wrong level
double additionalDistance = 0;
if (edge.Item1.OriginalWaypoint.Tier != destination.Tier)
{
additionalDistance += wrongTierPenalty;
}
// If it's not in the open list, add it
if (!openLocations.ContainsKey(edge.Item1))
{
openLocations[edge.Item1] =
new TimeWaypointSearchDatapoint(
currentNodeData.TimeTraveled + edge.Item2, // The distance already traveled
EstimateTravelTime(edge.Item1, destination) + additionalDistance, // The approximate distance to the goal
edge.Item1, // The node itself
currentNodeData, // Parent data
currentNodeData.Depth + 1); // The current depth
}
else
{
// It's already in the open list, but see if this new path is better
TimeWaypointSearchDatapoint oldPath = openLocations[edge.Item1];
// Replace it with the new one, if better
if (oldPath.TimeTraveled > currentNodeData.TimeTraveled + edge.Item2)
{
oldPath.TimeTraveled = currentNodeData.TimeTraveled + edge.Item2; // The distance already traveled
oldPath.TimeToGoal = EstimateTravelTime(edge.Item1, destination) + additionalDistance; // The approximate distance to the goal
oldPath.Waypoint = edge.Item1; // The node itself
oldPath.ParentMove = currentNodeData; // Parent data
oldPath.Depth = currentNodeData.Depth + 1; // The current depth
}
}
}
}
}
// Return time
return(shortestTime);
}
/// <summary>
/// Creates a new waypoint search data object.
/// </summary>
/// <param name="timeTraveled">The time traveled so far.</param>
/// <param name="timeToGoal">The estimated time towards the destination.</param>
/// <param name="waypoint">The waypoint this data object belongs to.</param>
/// <param name="parentMove">The data of the parent.</param>
/// <param name="depth">The steps we did to come here.</param>
public TimeWaypointSearchDatapoint(double timeTraveled, double timeToGoal, TimeWaypoint waypoint, TimeWaypointSearchDatapoint parentMove, int depth)
{
TimeTraveled = timeTraveled; TimeToGoal = timeToGoal; Waypoint = waypoint; ParentMove = parentMove; Depth = depth;
}
