How can I create this navigation system?

Question

I try to make a navigation/waypoint system. Well I have a concept but no idea how I could create this system. Well here's an image and an explanation:

You can see 3 waypoints as well as an goal and a player. The function should take the player position (X,Y,Z), a waypoint position array as well as an goal position. If the player is now on the position like in the image then it should output the second waypoint position because it's the closest waypoint in the direction of the goal. If the player is on the second waypoint position then it needs to output the third waypoint and so on to the finish. Well that's my crude concept. I hope that somebody can help! Thanks in advance :D

---

NOTE: You can add any values you want to add the most important thing is that it helps me :)

Answer 1

The best way I can think of is having a list of waypoints in order from furthest to closest. Then given a waypoint you know you already passed you could just find the closest waypoint that you haven't passed yet. Something like.

public int nextWaypointIndex(Vector3 playerLocation, List<Vector3> orderedWaypoints,
                                int passedThisWaypoint) {

    int nextWaypoint = passedThisWaypoint + 1;
    float lowestDistance = Vector3.Distance(playerLocation, waypoints[nextWaypoint]);

    for (int i = passedThisWaypoint + 2; i < waypoints.Count; i++) {
        if (Vector3.Distance(playerLocation, waypoints[i]) <= lowestDistance) {
            nextWaypoint = i;
            lowestDistance = Vector3.Distance(playerLocation, waypoints[i]);
        }
    }

    return nextWaypoint;
}