Strange behavior on elevations.

Hey,

I came across a strange navigation behavior. I'm not sure if it's a bug or something I missed.

The green line on the attached graphic is the expected path. Gray points are intermediate points that should be added to the path.

The red line is the current behavior. Yellow points are points that are in the path.

It seems that the height difference is ignored if the destination or the point where the change of direction is required are at the same height.