void findLastReachablePatrolWaypoint(Vector3 startPosition, RouteGameLogic patrolRoute, List <PathNode> pathNodes, PatrolRouteWaypoints waypointIterator, float destinationRadius, out Vector3 lastReachablePoint, out Vector3 firstUnreachableWayPoint, out int nextWaypointIndex, out bool completedRoute)
{
List <int> indicesConsideredThisTurn = new List <int>(); // to keep us from visiting the same node twice in a turn, either in a loop or in a ping-pong
// We can definitely reach the start point
lastReachablePoint = startPosition;
// The following is just to get past the compiler error of assigning an out param.
firstUnreachableWayPoint = startPosition;
PathNode tempPathNode = null;
PathNode realPathNode = null;
foreach (int waypointIndex in waypointIterator.GetWaypoints())
{
if (indicesConsideredThisTurn.IndexOf(waypointIndex) >= 0)
{
// we've already been here
nextWaypointIndex = waypointIndex;
completedRoute = false;
return;
}
Vector3 wayPoint = patrolRoute.routePointList[waypointIndex].Position;
firstUnreachableWayPoint = wayPoint;
Vector3 foundPoint;
// Try and find
if (findClosestWalkablePointToDestination(unit.Combat.HexGrid, wayPoint, startPosition, pathNodes, destinationRadius, out foundPoint, out tempPathNode))
{
// If a pathnode was found, update the realPathNode.
if (tempPathNode != null)
{
realPathNode = tempPathNode;
}
lastReachablePoint = foundPoint;
indicesConsideredThisTurn.Add(waypointIndex);
}
else
{
// At this point we have a start point we can reach and the next waypoint we can't reach. Try to find
// the closest point in the path node to the next waypoint. We're going to find the closest point to the
// firstUnreachableWayPoint in our pathNodes that pass through the lastPathNode that we can reach.
float closestDistance = AIUtil.Get2DSquaredDistanceBetweenVector3s(lastReachablePoint, firstUnreachableWayPoint);
PathNode closestPathNode = realPathNode;
// Loop through all our path nodes.
for (int i = 0; i < pathNodes.Count; ++i)
{
// Find the distance from this node to the first unreachable waypoint.
PathNode currentPathNode = pathNodes[i];
float currentDistance = AIUtil.Get2DSquaredDistanceBetweenVector3s(currentPathNode.Position, firstUnreachableWayPoint);
// If this node is closer to the destination than our current best node
if (currentDistance <= closestDistance)
{
// If we couldn't reach the first node, just move towards the first node as best we can.
if (realPathNode == null)
{
closestPathNode = pathNodes[i];
closestDistance = currentDistance;
continue;
}
// See if this path walks through the previous waypoint.
while (currentPathNode != null)
{
// If so, change our closest node.
if (currentPathNode == realPathNode)
{
closestPathNode = pathNodes[i];
closestDistance = currentDistance;
break;
}
currentPathNode = currentPathNode.Parent;
}
}
}
lastReachablePoint = closestPathNode.Position;
nextWaypointIndex = waypointIndex;
completedRoute = false;
return;
}
}
// used up all waypoints
nextWaypointIndex = -1;
completedRoute = true;
}