Checking for GPS waypoint being passed?

edit

I am working on an autonomous vehicle system that navigates and avoids obstacles.

Navigation alone, it goes from waypoint to waypoint by hitting the circular threshold points, basically saying "when within this distance, increment the target waypoint.

Now, when there is an obstacle and the threshold point is "inside" the obstacle, 99.9% of the times the threshold point won't be hit as the is threshold is set to very low due to the system using RTK. I am trying to use two intersecting lines (lat and long) to check whether or not the current waypoint has passed the next one, however that doesn't seem to work very well and I can't wrap my head around how to do it. Here's my code (in the 2D array, in the second dimension 0 is for lat and 1 is for long):

void trackWaypoint(double heading, double current_coords[NUMBER_OF_WAYPOINTS][2], double next_coords[NUMBER_OF_WAYPOINTS][2], uint32_t &wp) { // This is only for N and W hemispheres

if (wp != (NUMBER_OF_WAYPOINTS - 1)) {

    if ((heading >= 0) and (heading < 90)) {

        if (((next_coords[wp + 1][0] - current_coords[wp][0]) < 0) and ((next_coords[wp + 1][1] - current_coords[wp][1]) > 0)) {
            wp++;
        }
    }
    else if ((heading >= 90) and (heading < 180)) {

        if (((next_coords[wp + 1][0] - current_coords[wp][0]) > 0) and ((next_coords[wp + 1][1] - current_coords[wp][1]) > 0)) {
            wp++;
        }
    }
    else if ((heading >= 180) and (heading < 270)) {

        if (((next_coords[wp + 1][0] - current_coords[wp][0]) > 0) and ((next_coords[wp + 1][1] - current_coords[wp][1]) < 0)) {
            wp++;
        }
    }
    else { // [270, 360)

        if (((next_coords[wp + 1][0] - current_coords[wp][0]) < 0) and ((next_coords[wp + 1][1] - current_coords[wp][1]) < 0)) {
            wp++;
        }
    }
}

}

Here's two scenarios, A and B, assuming we are operating in Northern and Western hemispheres (sorry for lousy drawing):

In A, if the system decides to take the left path, it will not be able to to determine whether or not the next waypoint has been passed due to it thinking it hasn't intersected along the longitude (x) axis.

In A, if the system decides to take the upper path, it will not be able to to determine whether or not the next waypoint has been passed due to it thinking it hasn't intersected along the latitude (y) axis.

Is there a different way of doing it? If not, how could I account for both paths passing the next waypoint?