CPU Driving Part 1: Waypoints

How can you make a CPU car drive around a track in a video game?

In an old school, top down racing game, how do you make the CPU drivers? They need to follow the track, have a variety of racing styles, and employ all of the driving mechanics the game offers, such as drifting.

After exploring this topic for a while, I decided to go with a simple waypoint system. This blog post series is basically what I wish existed before I started. I'm not an expert on this subject, and I'm not saying what I did was ideal or even correct. But I've now had good success getting artificial drivers to drift around my courses, so I thought I'd document what I learned and built.

Let's start with the heart of the system, waypoints.

What are waypoints?

At their simplest, waypoints are just points dropped onto the track that a CPU driver should drive towards. Once it arrives at the first waypoint, it will then start driving towards the next one.

Here is the track this blog series will use,

and here it is with some basic waypoints plopped onto it

So if we just place a CPU car at the starting line, and have it drive towards 0, then 1, and so on, it will get around the track in a very primitive and unrealistic way. But hey, it's a start. Press play to see our amazing green driver in action.

frame: 0

The car keeps track of its target waypoint, and figures out the angle it needs to point straight at it

getAngleToWaypoint(): number {
    const waypoint = this.waypoints[this.targetWaypoint];
    const x = waypoint.x - this.x;
    const y = waypoint.y - this.y;

    const angle = Math.atan(Math.abs(y) / Math.abs(x));

    if (x >= 0 && y >= 0) {
        // upper right quadrant
        return angle;
    }

    if (x < 0 && y >= 0) {
        // upper left quadrant
        return Math.PI - angle;
    }

    if (x < 0 && y < 0) {
        // lower left quadrant
        return Math.PI + angle;
    }

    // lower right quadrant
    return 2 * Math.PI - angle;
}

It then uses this angle to move forward

handleAcceleration() {
    const airDrag = this.getAirDrag(this.speed);

    const acceleration = this.accelValue - FRICTION - airDrag;

    this.speed = Math.max(0, this.speed + acceleration);

    const cos = Math.cos(this.velocityAngle);
    const sin = Math.sin(this.velocityAngle);

    this.velocity.x = this.speed * cos;
    this.velocity.y = this.speed * sin;

    this.x += this.velocity.x;
    this.y += this.velocity.y;
}

It also checks to see if it has arrived at the waypoint, and if so, sets up the next waypoint as its target

updateCurrentWaypoint() {
    const currentWaypoint = this.waypoints[this.targetWaypoint];

    const distance = getDistance(
        this.x,
        this.y,
        currentWaypoint.x,
        currentWaypoint.y
    );

    // are we going to arrive within one frame?
    // then consider us there
    if (distance <= this.speed) {
        this.targetWaypoint += 1;

        if (this.targetWaypoint >= this.waypoints.length) {
            this.targetWaypoint = 0;
        }
    }
}

Here is the full code for this first basic driving car.

Turning towards the next waypoint

Let's improve our driver so it smoothly turns towards its target waypoint. This will be easier if waypoints have a radius added to them, so we can say we've arrived at a waypoint as long as we end up somewhere inside its circle.

As soon as we arrive within the radius, the driver will target the next waypoint, and start to gradually turn towards it. Here is what we'll have

frame: 0

Well... it's kind of working (watch it go past waypoint 3 to see what I mean). This will get better, but for now let's look at how the smooth turning works.

In the first version, it was easy to point the car directly towards the next waypoint. Just figure out what that angle is and set the car to it. But when gradually turning instead, the car can either turn left or right. How do we know which way is correct?

Turning right here is better than turning left, car art from Vecteezy

In this simple example the car could turn right 20 degrees to point at the waypoint, or left 340 degrees. Both will ultimately get there, but obviously turning right is better here.

To figure out which turn is the better one, let's simulate turning a little bit in both directions and pick the one that pointed the car more closely at the waypoint.

Ok, but how to know which turn was better? Thankfully the dot product can tell us, which you may have learned about in a math class. The dot product is obtained by multiplying the x and y values of each vector and adding them together. The larger the dot product of two vectors, the more those two vectors are pointing in the same direction.

dot product: (a.x*b.x) + (a.y*b.y)

(80.0*8.0) + (0.0*79.6) = 638.9

A simple little interactive dot product thing. Notice when you bring the red vector closer to the blue one by using the arrows, the dot product value gets larger and larger?

So simulate turning left a little, grab a dot product. Simulate turning right a little, and grab another dot product. Whichever dot product is larger, that's the way to turn.

It's also possible that just going straight is the best answer. So we'll actually compare three dot products.

Calculating dot products to figure out which way to turn, car art from Vecteezy

How large should the simulated turns be? I tend to do a few degrees. Enough to make sure the dot results are different enough, but not too much such that the results become meaningless. What about the length of the turning vectors? I think it's best to avoid really long vectors. I tend to take the remaining distance to the waypoint and divide that by two.

And here is the code for doing this

type TurnDecision = -1 | 0 | 1;

function dot(a: Point, b: Point): number {
    return a.x * b.x + a.y * b.y;
}

calcTurnDecision(waypoint: Point): TurnDecision {
    if (this.speed === 0) {
        // not moving? no need to turn
        return 0;
    }

    const distanceToWp = getDistance(
        this.x,
        this.y,
        waypoint.x,
        waypoint.y
    );
    const vectorMagnitude = distanceToWp / 2;

    // translate the waypoint's center vector to be based
    // off the vehicle's location, so that the vectors
    // can be compared
    const translatedWaypoint: Point = {
        x: waypoint.x - this.x,
        y: waypoint.y - this.y,
    };

    let bestD = Number.MIN_SAFE_INTEGER;
    let bestTurnResult = 0;

    for (let t = 1; t >= -1; t -= 1) {
        const turnedAngle = this.velocityAngle
            + degreesToRadians(t * 3);

        const turnedCos = Math.cos(turnedAngle);
        const turnedSin = Math.sin(turnedAngle);

        // form a new hypothetical velocity vector
        // based on the hypothetical turn
        const turnedVelocity: Point = {
            x: vectorMagnitude * turnedCos,
            y: vectorMagnitude * turnedSin,
        };

        // and calculate a new dot
        const turnedD = dot(turnedVelocity, translatedWaypoint);

        if (turnedD > bestD) {
            bestD = turnedD;
            bestTurnResult = t;
        }
    }

    return bestTurnResult as TurnDecision;
}

This code is just seeing if left, straight or right will get us closer to the waypoint, and returning the best result.

Here is that first smooth turning simulation again. The turns are still just as wild, but this time it also shows the turn decisions the car is making. The red line is the vector to the waypoint, the other lines are the simulated turns, with the thick green one being the one that was chosen.

frame: 0

Now that we know the direction of the turn, we need to know how much to turn in that direction.

What is the angle needed to point the car at the waypoint?, Car art from Vecteezy

We can see how much the car needs to turn by determining the angle between the car's direction vector and the vector from the car to the waypoint. This can again be accomplished by using the dot product. That is because the dot product of two vectors and the angle between those two vectors are related

The geometric dot product formula. This is saying the the dot product of vectors a and b divided by their lengths, is equal to the cosine of the angle between them. We can use this formula to get the angle in our code.

Now we know which way to turn and by how much, we're getting there.

By noting how far the car is away from the waypoint, and how fast it is going, we can roughly determine how many frames it takes to arrive. By dividing the needed turn angle by that number of frames, we know how much to turn the car each frame. Turning this small fraction of the angle each frame is ultimately what accomplishes the smooth turning.

function normalizedDot(
    a: Point,
    aLength: number,
    b: Point,
    bLength: number
): number {
    const d = dot(a, b);
    const lens = aLength * bLength;

    return d / lens;
}

calcVelocityAngleChangeRate(
    waypoint: Waypoint,
    turnDecision: -1 | 0 | 1
): number {
    const distanceToWp = getDistance(
        this.x,
        this.y,
        waypoint.x,
        waypoint.y
    );

    const framesTillWp =
        (distanceToWp - waypoint.radius) / this.speed;

    const translatedWaypoint = {
        x: waypoint.x - this.x,
        y: waypoint.y - this.y,
    };

    const normD = normalizedDot(
        this.velocity,
        this.speed,
        translatedWaypoint,
        distanceToWp
    );

    let velocityAngleSpan = Math.acos(normD) * 1.5;

    if (velocityAngleSpan > 2 * Math.PI) {
        velocityAngleSpan -= 2 * Math.PI;
        turnDecision = -turnDecision as TurnDecision;
    }

    return (velocityAngleSpan / framesTillWp) * turnDecision;
}

The previous function used dot(), and this code is using normalizedDot(). They are almost the same thing. A normalized dot product takes the dot product and divides it by the lengths of the vectors to get an answer that is between -1 and 1. Earlier when we were calculating dot products, we were directly comparing the result so there was no need to normalize them. But here normalizing the dot product is necessary when the ultimate goal is to get the angle between the vectors. As we saw in the dot product formula above, the normalized dot product is the cosine of that angle.

When calculating framesTillWp, we are using the distance to the next waypoint, minus its radius. That is because we don't care about arriving exactly at the center of the waypoint, just arriving within its circular boundary.

Now with the turn rate in hand, we can move the car

update(waypoints: Waypoint[]) {
    // move onto the next waypoint if we have
    // arrived at the current one
    this.updateCurrentWaypoint(waypoints);

    // grab the current waypoint
    const currentWaypoint = waypoints[this.targetWaypoint];

    // figure out how much to turn each frame to smoothly
    // arrive at that waypoint
    const angleChangeRate =
        this.determineAngleChangeRate(currentWaypoint);

    // update our turning accordingly
    this.velocityAngle += angleChangeRate;

    // and move the car
    this.handleAcceleration();
}

handleAcceleration() {
    const airDrag = this.getAirDrag(this.speed);

    const acceleration = this.accelValue - FRICTION - airDrag;

    this.speed = Math.max(0, this.speed + acceleration);

    const cos = Math.cos(this.velocityAngle);
    const sin = Math.sin(this.velocityAngle);

    this.velocity.x = this.speed * cos;
    this.velocity.y = this.speed * sin;

    this.x += this.velocity.x;
    this.y += this.velocity.y;
}

Here is the full code for the smooth turning car.

Fixing the wild turning

Ok that is all great, but if you play with the simulator above you can see the car drives like it's drunk. What's the deal?

In short, we don't have enough waypoints.

Adding more waypoints is only one way to solve this problem. But it really is the simplest, so that's what I went with. Alternatively, you could make the CPU smarter. But that gets really complicated, and why bother when just throwing in a few more waypoints works so well?

Let's look at going from waypoint 3 to 4 as an example

The car's exaggerated arc from three to four

When it first arrives at 3, it's roughly going up towards the top of the screen, and 4 is roughly to its direct right. So to get to 4, the car needs to turn about 80 degrees or so. But when calculating how much to turn per frame, we take that 80 degrees and divide it by how far the car needs to travel. Since 3 and 4 are so far apart, it has all the time in the world to turn towards 4. This gradual turning combined with the car's travel results in the arcing motion that we're not fans of. What we really want is for the car to smoothly point at 4 pretty quickly, and then head straight at it. Adding more waypoints ends up being kind of a middle ground between our first naive implementation and what we currently have.

The easiest way to do that is just to add more waypoints, adjust their radiuses to make the car start turning earlier or later, and move the waypoints around. There's a lot of art behind waypoint placement.

frame: 0

With the new waypoints in place, it's not driving bad at all!

If you scroll back up to the calcVelocityAngleChangeRate code, we actually take the angle between the car and waypoint and bump it up a bit by multiplying it by 1.5. This goes back to waypoints being a bit of an art form. Sometimes you just gotta play around with things until they feel ok. By bumping up the angle a bit, we get the car to turn a bit sharper, and so its traversal around the track is more realistic as this helps increase the long spans of driving straight between the curves.

Conclusion

And with that we have a basic CPU car driving around our track. Not a bad start. In the next parts of this blog series, we'll look into adding drifting, more cars and avoidance, as well as slowing down to take curves more realistically.