// Update is called once per frame
void Update()
{
if (destination != null)
{
Move();
if (Vector3.Distance(destination.transform.position, transform.position) < 3f)
{
location = destination;
destination = null;
}
}
}
public void FollowNavigationPath(NavigationPath path)
{
this.path = path;
waypoint = path.Waypoints[1];
waypoint.GetTargetPositionAndAlignment(out Vector3 targetPosition, out Vector3 targetForward);
flightPath = PathUtils.GetBezierCurve(
transform.position,
transform.position + transform.forward * 200,
targetPosition - targetForward * 200,
targetPosition,
10);
Debug.DrawRay(transform.position, Vector3.up * 5, Color.green, 5f);
Debug.DrawRay(transform.position + transform.forward * 50, Vector3.up * 5, Color.green, 5f);
Debug.DrawRay(targetPosition - targetForward * 50, Vector3.up * 5, Color.green, 5f);
Debug.DrawRay(targetPosition, Vector3.up * 5, Color.green, 5f);
for (int i = 0; i < flightPath.Count - 1; i++)
{
Debug.DrawLine(flightPath[i], flightPath[i + 1], Color.red, 5f);
}
transform.DOPath(flightPath.ToArray(), 15f, PathType.Linear, PathMode.Full3D);
}
public NavigationPath GetPathBetweenWaypoints(NavigationWaypoint start, NavigationWaypoint end)
{
HashSet <NavigationWaypoint> exploredWaypoints = new HashSet <NavigationWaypoint>();
Queue <NavigationWaypoint> waypointsQueue = new Queue <NavigationWaypoint>();
Dictionary <NavigationWaypoint, NavigationWaypoint> previousWaypoint = new Dictionary <NavigationWaypoint, NavigationWaypoint>();
// Add the start to the queue and mark it as explored
exploredWaypoints.Add(start);
waypointsQueue.Enqueue(start);
bool pathFound = false;
while (waypointsQueue.Count > 0 && pathFound == false)
{
// Remove current waypoint from queue
NavigationWaypoint waypoint = waypointsQueue.Dequeue();
// For each unexplored neighbour
foreach (NavigationWaypoint connectedWaypoint in waypoint.connectedWaypoints)
{
if (exploredWaypoints.Contains(connectedWaypoint))
{
continue;
}
// Set it to explored
exploredWaypoints.Add(connectedWaypoint);
// Check if its the goal
if (connectedWaypoint == end)
{
pathFound = true;
previousWaypoint.Add(end, waypoint);
break;
}
else // Else add it to the queue with it's origin waypoint
{
waypointsQueue.Enqueue(connectedWaypoint);
previousWaypoint.Add(connectedWaypoint, waypoint);
}
}
}
// Retrace our steps from the end waypoint to the start then reverse and return the path
if (pathFound)
{
NavigationPath path = new NavigationPath();
path.Waypoints.Add(end);
NavigationWaypoint waypoint = previousWaypoint[end];
while (waypoint != start)
{
path.Waypoints.Add(waypoint);
waypoint = previousWaypoint[waypoint];
}
path.Waypoints.Add(start);
path.Waypoints.Reverse();
return(path);
}
return(null);
}
