public NavArea2D GetClosestArea(Vector3 point)
{
float closestPointDistance = float.PositiveInfinity;
NavArea2D closestNavArea = null;
foreach (NavArea2D area in navAreas)
{
if (area.bounds.Contains(point))
{
return(area);
}
else
{
Vector3 nearest = area.bounds.ClosestPoint(point);
float distance = Vector3.Distance(point, nearest);
if (distance < closestPointDistance)
{
closestNavArea = area;
closestPointDistance = distance;
}
}
}
return(closestNavArea);
}
public NavArea2DNode(NavArea2D area)
{
this.area = area;
distanceToTarget = -1f;
cost = 1f;
weight = 1f;
}
public void SetDestination(Vector3 point)
{
NavArea2D area = navManager.GetClosestArea(point);
if (!area)
{
Debug.LogWarning($"Failed to find target area of point {point.ToString()} for agent", this);
return;
}
if (!currentArea)
{
currentArea = navManager.GetClosestArea(transform.position);
}
path = navManager.GetPath(currentArea, area);
if (path == null)
{
Debug.LogWarning($"Failed to find path to point {point.ToString()} for agent", this);
return;
}
bool hasLink = currentArea.GetLinkToArea(path.Peek(), out lastLink);
if (path.Count > 1 && !hasLink)
{
path.Clear();
Debug.LogWarning($"Failed initialize first link to point {point.ToString()} for agent", this);
hasDestination = false;
return;
}
targetArea = area;
targetPosition = point;
hasDestination = true;
}
public Stack <NavArea2D> GetPath(NavArea2D start, NavArea2D goal)
{
int maxIterations = 2000;
List <NavArea2DNode> openNodes = new List <NavArea2DNode>();
List <NavArea2DNode> closedNodes = new List <NavArea2DNode>();
NavArea2DNode startNode = new NavArea2DNode(start);
NavArea2DNode goalNode = new NavArea2DNode(goal);
NavArea2DNode current = startNode;
openNodes.Add(current);
while (maxIterations > 0 && openNodes.Count > 0 && (closedNodes.Contains(goalNode) == false))
{
maxIterations--;
current = openNodes[0];
openNodes.Remove(current);
closedNodes.Add(current);
List <NavArea2DNode> neighbors = current.GetNeighbors();
foreach (NavArea2DNode neighbor in neighbors)
{
if (closedNodes.Contains(neighbor) == false)
{
if (openNodes.Contains(neighbor) == false)
{
neighbor.parent = current;
// TODO: Improve this Distance function to consider more relevant values
neighbor.distanceToTarget = neighbor.DistanceTo(goalNode);
neighbor.cost = neighbor.weight + neighbor.parent.cost;
openNodes.Add(neighbor);
openNodes = openNodes.OrderBy(node => node.F).ToList();
}
}
}
}
if (closedNodes.Contains(goalNode) == false)
{
return(null);
}
NavArea2DNode temp = current;
if (closedNodes.Contains(current) == false)
{
return(null);
}
Stack <NavArea2D> path = new Stack <NavArea2D>();
do
{
path.Push(temp.area);
temp = temp.parent;
} while (temp != startNode && temp != null);
return(path);
}
public float DistanceTo(NavArea2D other)
{
if (other.bounds.Intersects(_bounds))
{
// touching
return(0);
}
Vector3 closestPointToOther = _bounds.ClosestPoint(other.center);
Vector3 otherClosestPointToThis = other.bounds.ClosestPoint(center);
return(Vector3.Distance(closestPointToOther, otherClosestPointToThis));
}
public bool GetLinkToArea(NavArea2D otherArea, out NavArea2DLink outLink)
{
foreach (NavArea2DLink link in links)
{
if (link.linkedArea == otherArea)
{
outLink = link;
return(true);
}
}
outLink = new NavArea2DLink();
return(false);
}
private void NavigateAreas()
{
currentArea = navManager.GetContainingArea(transform.position);
if (!movementController.isOnStairs && currentArea == targetArea)
{
// arrived in target area, basic nav takes over from here
path.Pop();
}
else if (movementController.isOnStairs)
{
// move along stairs
Vector3 moveTarget;
if (lastLink.linkStairsPoint != null)
{
moveTarget = lastLink.linkStairs.GetOppositePoint(lastLink.linkStairsPoint).position;
}
else
{
moveTarget = movementController.GetNearestStairsMountPoint();
}
// move along stairs
float xDiff = (moveTarget - transform.position).x;
movementController.Move(Vector2.right * Mathf.Sign(xDiff));
}
else
{
NavArea2D nextArea = path.Peek();
if (currentArea == nextArea)
{
// already in next area
path.Pop();
}
else
{
NavArea2DLink link;
bool hasLink = currentArea.GetLinkToArea(path.Peek(), out link);
if (hasLink)
{
lastLink = link;
if (link.linkType == NavLinkType.NORMAL)
{
// move towards next area
float xDiff = (link.linkedArea.center - transform.position).x;
movementController.Move(Vector2.right * Mathf.Sign(xDiff));
}
else if (link.linkType == NavLinkType.STAIRS)
{
if (currentArea == nextArea)
{
// already in next area
path.Pop();
}
// move to stairs to get to next area
float xDiff = (link.linkStairsPoint.position - transform.position).x;
float xDist = Mathf.Abs(xDiff);
movementController.Move(Vector2.right * Mathf.Sign(xDiff));
if (xDist < closeOnTargetDistance)
{
movementController.MountStairs(link.linkStairs);
}
}
}
else
{
path.Clear();
Debug.LogWarning("Path no longer valid, aborting");
return;
}
}
}
}
