How do I calculate paths for objects with limited acceleration?

Question

For example, say I have a car and a car has a specific minimum turning radius and I want to drive that car from point a to point b, but the car isn't facing point b. How do I compute a path to point b? Being able to specify the orientation at the point b would also be good (say you want to drive to your driveway and then pull in to your garage -- it doesn't do much good if you got to your driveway by driving over your lawn and are facing sideways :)

A pointer to documentation (or even just a name) would be perfectly fine -- I'm having trouble finding anything at all.

In my attempts, they work in simple cases, but fail miserably in situations such as when point b is closer to a than the minimum turning radius.

For example, how would you determine a path similar to this (the bold path):

edit: In my actual problem, there are some simple pathing constraints, but I already have an A* algorithm in place that works, but it allows things to make instantaneous heading changes, so it looks silly seeing a car suddenly make a 90˚ turn on a dime when they get to a turn point.

Answer 1

I haven't worked through the full equations for this yet, but here's some visuals to help wrap our heads around the problem. It boils down to some geometry:

(Car icons via Kenney)

From any given starting point and orientation, we can draw two circles with our minimum turning radius - one on the left, one on the right. These describe the points on the tightest possible start to our path.

We can do the same for any desired end position and orientation. These circles describe the tightest possible end to our path.

Now the problem reduces to finding a path that joins one of the start circles to one of the end circles, kissing each one along its tangent.

(This is assuming we don't need to pathfind around obstacles in-between, which wasn't mentioned in the question. Stormwind's answer gets into how we can use navigation graph information for these types of problems. Once we have the sequence of nodes to pass through, we can apply the method below to each segment of the plan.)

If, for simplicity, we use straight lines, we get something like this:

This gives us the limiting case. Once you've found a path by this method, you can artificially inflate one or both start & end circles to get a less direct but smoother path, up until the point where the two circles kiss.

Computing these paths

Let's work out the cases for one turning direction - say we begin our path by turning right.

The center of our right turning circle is:

startRightCenter = carStart.position + carStart.right * minRadius

Let's call the angle of the straight section of our path (measured from the positive x-axis) pathAngle

If we draw a vector from rightCenter to the point where we leave the turning circle (at which point we must be facing pathAngle), then that vector is...

startOffset = minRadius * (-cos(pathAngle), sin(pathAngle))

That means the point where we leave the circle must be...

departure = startRightCenter + startOffset

The point where we re-enter a turning circle depends on whether we're aiming to end with a left or a right turn:

// To end with a right turn:
reentry = endRightCenter + startOffset

// To end with a left turn: (crossover)
reentry = endLeftCenter - startOffset

Now, if we've done our job right, the line joining departure to reentry ought to be perpendicular to startOffset:

dot(reentry - departure,  startOffset) = 0

And solving this equation will give us the angle(s) at which this is true. (I use a plural here because technically there are two such angles, but one of them involves driving in reverse which is usually not what we want)

Let's substitute the right turn to right turn case as an example:

dot(endRightCenter + startOffset - startRightCenter - startOffset, startOffset) = 0
dot(endRightCenter - startRightCenter, startOffset) = 0
pathAngle = atan2(endRightCenter - startRightCenter)

The crossover case is more complicated - it's the one I haven't worked out all the math for yet. I'll post the answer without for now, in case it's useful to you while I work out the remaining details.

Edit: Destination inside minimum turning radius

It turns out, this method often works out-of-the box even when the destination is closer than our minimum turning distance. At least some part of one of the re-entry circles ends up outside the turn radius, letting us find a viable path as long as we don't mind it getting a bit pretzel-like...

If we don't like the path we get that way (or if one isn't feasible - I haven't checked every case exhaustively - maybe there are impossible ones), we can always drive straight forward or back until we get a suitable kissing contact between a start & end circle, as diagrammed above.

Answer 2

This very much depends on the rest of your data model for the navigation. Ie. what data you have handy, what you can easily add data and how you consume it.

Taking a similar scenario from a traffic system at water, and with the assumtion that

• you are in a game loop
• you have a node path system
• your cars behave like an autonomous objects that control themselves "from inside", using own force and steering
• your cars do not move like on rails

you could have something like below (forgive me for the childish appearance of the pictures)

(The red squares are the nodes, red lines are node interconnections. Assume you used a pathfinding solver that gave nodes 1-9 to drive through; nodes 4-9 seen on the picture and you want to go through nodes indicated by the green line, to the garage at node #9; however you do not want to go precisely at the green line, instead stay naturally at the right side lane and do smooth manouvers).

Each node would have metadata which holds for example a radius, or multiple ones, for various purposes. One of them is the blue circle, which provides aiming guidance for the cars.

At any occasion, the vehicle needs to be aware of the next two node points P(next) and P(next+1), and their positions. Naturally, the car has a position as well. A car aims at the right side tangent of the blue metadata-circle of P(next). So do cars going in opposite direction, hence they won't collide. Aiming at the tangent means the car can approach the circle from any direction, and always keep right. This is a rough basic principle, that can be improved in many ways.

P(next+1) is needed to determine a distance - as the car reaches P(next), or gets inside some radius of it's metadata, it can adjust it's steering angle depending on how far away P(next+1) is. Ie. if it's close, turn much, if it's far away, turn little. Apparently there needs to be other rules & edge conditions as well, for example calculation of a distance between the car and a help line based on the right side tangents of P(next) and P(next+1), and a correction by that - a will to stay on the dashed (above pic) and dotted (below pic) line.

In any case, as the car passes one node, it forgets it and begins to look at the next two ones.

To your question. Apparently, when reaching node 7 (in the pic above, seen as node 2 in the picture below), it cannot turn enough.

One possible solution it to construct some help lines and maintain a goal all time, and then have the car move by it's own physics settings (accelerate at a specified rate, reverse slower, take node metadata speedlimits into account, brakes at a given or calculated G, etc.). As said, the car is an autonomous, self-describing, self-carrying object in this scenario.

Have the green help lines 1,2,3. As the car reaches the magenta circle, it begins it's turn to right. At this point, you can already calculate that it won't succeed (you know the max turn rate and can calculate the curve, and can see that it will cross both helplines 2 and 3). Turn steering full right and let it drive ahead (by physics increments) and slow it down as it reaches help line 3 (gets close to - use thresholds, f(dist to helpline) etc). When it is at help line 3, go into reverse mode, turn the steering to full opposite. Let it reverse until the it reaches help line 4 (the connection line between node 1 and 2 - google for "point at side of line algorithm"). Slow down, as it reaches it, go into drive ahead mode again, turn the wheel. Repeat until the road is clear - apparently it was sufficient with 1 extra manouver this time.

This is the general idea: During the game loop, or when checking the games task que system:

• Check car position, speed, angle etc. against the current edge limits & goal,
• If not reached yet, continue with what you were doing (let physics move it; the car has an rpm and a gear). Insert a new check in your que system, to happen in for example 0.1 s.
• If reached, calqculate new conditions, set the data and begin. Insert a new check to happen in the que system in for example 0.1 s.
• Complete the loop cycle - continue, repeat.

By giving the nodes and the cars suffucient data, there will be movement and continuation.

Edit: And adding: This naturally needs fine tuning. Your simulation behavious may require different help lines, metadata, circles, anything. This would give an idea of one possible solution though.

Answer 3

I ended up doing what DMGregory suggested and it works well. Here's some relevant code (though not standalone) that can be used for computing the two styles of tangents. I'm sure this code isn't efficient, and it's probably not even correct in all situations, but it's working for me so far:

bool Circle::outer_tangent_to(const Circle & c2, LineSegment & shared_tangent) const {
    if (this->direction != c2.direction) {
        return false;
    }
    if (this->radius != c2.radius) {
        // how to add it: http://mathworld.wolfram.com/Circle-CircleTangents.html
        // just subtract smaller circle radius from larger circle radius and find path to center
        //  then add back in the rest of the larger circle radius
        throw ApbException("circles with different length radius not supported");
    }

    auto vector_to_c2 = c2.center - this->center;
    glm::vec2 normal_to_c2;
    if (this->direction == Circle::CW) {
        normal_to_c2 = glm::normalize(glm::vec2(-vector_to_c2.y, vector_to_c2.x));
    } else {
        normal_to_c2 = glm::normalize(glm::vec2(vector_to_c2.y, -vector_to_c2.x));
    }

    shared_tangent = LineSegment(this->center + (normal_to_c2 * this->radius),
                                 c2.center + (normal_to_c2 * this->radius));
    return true;
}


bool Circle::inner_tangent_to(const Circle & c2, LineSegment & tangent) const {

    if (this->radius != c2.radius) {
        // http://mathworld.wolfram.com/Circle-CircleTangents.html
        // adding this is non-trivial
        throw ApbException("inner_tangents doesn't support circles with different radiuses");
    }

    if (this->direction == c2.direction) {
        // inner tangents require opposing direction circles
        return false;
    }

    auto vector_to_c2 = c2.center - this->center;
    auto distance_between_circles = glm::length(vector_to_c2);

    if ( distance_between_circles < 2 * this->radius) {
//      throw ApbException("Circles are too close and don't have inner tangents");
        return false;
    } else {
        auto normalized_to_c2 = glm::normalize(vector_to_c2);
        auto distance_to_midpoint = glm::length(vector_to_c2) / 2;
        auto midpoint = this->center + (vector_to_c2 / 2.0f);

        // if hypotenuse is oo then cos_angle = 0 and angle = 90˚
        // if hypotenuse is radius then cos_angle = r/r = 1 and angle = 0
        auto cos_angle = radius / distance_to_midpoint;
        auto angle = acosf(cos_angle);

        // now find the angle between the circles
        auto midpoint_angle = glm::orientedAngle(glm::vec2(1, 0), normalized_to_c2);

        glm::vec2 p1;
        if (this->direction == Circle::CW) {
            p1 = this->center + (glm::vec2{cos(midpoint_angle + angle), sin(midpoint_angle + angle)} * this->radius);
        } else {
            p1 = this->center + (glm::vec2{cos(midpoint_angle - angle), sin(midpoint_angle - angle)} * this->radius);
        }

        auto tangent_to_midpoint = midpoint - p1;
        auto p2 = p1 + (2.0f * tangent_to_midpoint);
        tangent = {p1, p2};

        return true;
    }
};

Here are two movies of the code above in action:

Here's an "outer" path: http://youtube.com/watch?v=99e5Wm8OKb0 and here's a "crossing" path: http://youtube.com/watch?v=iEMt8mBheZU

If this code helps but you have questions about some of the parts that aren't shown here, just post a comment and I should see it in a day or two.