Shortest path through ordered circular waypoints

Question

I am trying to implement an algorithm which computes the shortest path and and its associated distance from a current position to a goal through an ordered list of waypoints in a 2d plane. A waypoint is defined by its center coordinates (x, y) and its radius r. The shortest path have to intersect each waypoint circumference at least once. This is different from other path optimization problems because I already know the order in which the waypoints have to be crossed.

In the simple case, consecutive waypoints are distinct and not aligned and this can be solved using consecutive angle bisections. The tricky cases are :

• when three or more consecutive waypoints have the same center but different radii
• when consecutive waypoints are aligned such that a straight line passes through all of them

Here is a stripped down version of my Python implementation, which does not handle aligned waypoints, and handles badly concentric consecutive waypoints. I adapted it because it normally uses latitudes and longitudes, not points in the euclidean space.

def optimize(position, waypoints):
    # current position is on the shortest path, cumulative distance starts at zero
    shortest_path = [position.center]
    optimized_distance = 0

    # if only one waypoint left, go in a straight line
    if len(waypoints) == 1:
        shortest_path.append(waypoints[-1].center)
        optimized_distance += distance(position.center, waypoints[-1].center)

    else:
        # consider the last optimized point (one) and the next two waypoints (two, three)
        for two, three in zip(waypoints[:], waypoints[1:]):
            one = fast_waypoints[-1]

            in_heading = get_heading(two.center, one.center)
            in_distance = distance(one.center, two.center)
            out_distance = distance(two.center, three.center)

            # two next waypoints are concentric
            if out_distance == 0:
                next_target, nb_concentric = find_next_not_concentric(two, waypoints)
                out_heading = get_heading(two.center, next_target.center)
                angle = out_heading - in_heading
                leg_distance = two.radius
                leg_heading = in_heading + (0.5/nb_concentric) * angle
            else:
                out_heading = get_heading(two.center, three.center)
                angle = out_heading - in_heading
                leg_heading = in_heading + 0.5 * angle
                leg_distance = (2 * in_distance * out_distance * math.cos(math.radians(angle * 0.5))) / (in_distance + out_distance)


            best_leg_distance = min(leg_distance, two.radius)
            next_best = get_offset(two.center, leg_heading, min_leg_distance)
            shortest_path.append(next_best.center)
            optimized_distance += distance(one.center, next_best.center)

    return optimized_distance, shortest_path

I can see how to test for the different corner cases but I think this approach is bad, because there may be other corner cases I haven't thought of. Another approach would be to discretize the waypoints circumferences and apply a shortest path algorithm such as A*, but that would be highly inefficient.

So here is my question : Is there a more concise approach to this problem ?

Answer 1

For the record, I implemented a solution using Quasi-Newton methods, and described it in this short article. The main work is summarized below.

import numpy as np
from scipy.optimize import minimize

# objective function definition
def tasklen(θ, x, y, r):
    x_proj = x + r*np.sin(θ)
    y_proj = y + r*np.cos(θ)

    dists = np.sqrt(np.power(np.diff(x_proj), 2) + np.power(np.diff(y_proj), 2))

    return dists.sum()

# center coordinates and radii of turnpoints
X = np.array([0, 5, 0, 7, 12, 12]).astype(float)
Y = np.array([0, 0, 4, 7, 0, 5]).astype(float)
R = np.array([0, 2, 1, 2, 1, 0]).astype(float)

# first initialization vector is an array of zeros
init_vector = np.zeros(R.shape).astype(float)

# using scipy's solvers to minimize the objective function
result = minimize(tasklen, init_vector, args=(X, Y, R), tol=10e-5)

Answer 2

I would do it like this:

1. For each circle in order, pick any point on the circumference, and route the path through these points.
2. For each circle, move the point along the circumference in the direction that makes the total path length smaller.
3. Repeat 2. until no further improvement can be done.