I have a grid of nodes, which contain a list of neighbor nodes. I wanted to create a FindPath function, with a Start position and a Goal position (both 3d Vectors, using X and Z, UP is 0). I can easily get the nearest node to start, and the nearest node to goal, but I wanted to also make sure these nodes are in the direction the unit would be travelling.

For example, I don't want the unit to go AWAY from the goal, and then back track toward the goal, because the 'nearest' node to him was behind him. I'll supply a picture to make more sense.

So in this picture, you can see the 'nearest' nodes to start and goal are where the red X's are. Start is where the spider is located, and goal is where the black arrow is pointing. You can see, the nearest start node, is actually in the other direction of the goal, where the yellow circled one is in the direction of the goal, even though it is further than the 'nearest' one. The same goes for the nearest goal node.

I may be making this harder than it needs to be, but I can't seem to figure out the proper maths involved to get the nodes circled in yellow as 'start' and 'goal' nodes.

Here are the things I know:

startVector

goalVector

distanceFromStartToGoal

I need to use this information, while iterating throughout the list of nodes to determine:

startNode

goalNode

While iterating, I can figure out:

nodeDistanceToGoal

nodeDistanceToStart

So using all this information, I need to determine nearest node to startVector and the nearest node to goalVector, which are also in the direction from start to goal.

Any ideas?