Moving a Car to a Point at a Certain Angle


My goal is to move a car from its current 2D position and angle to a point’s location and angle. I am experimenting with Bezier curves to find a smooth path between the two, but then I realized, those curves are still made up of points, so I have the same problem.

The points along the path have less distance and angle difference between them, but I still need to drive from one to the next. I have written some code to try and do this, but my efforts thus far have met with little success. I have tried tweaking the numbers and even changing the formulas a bit, but the best I can do is getting close. Even when I have reached the destination location or angle, the other one is still off, so the car continues and misses the mark. Here is the calculation-only portion of my code:

// Constants
const double max_angle_change = PI / 180;
const double max_speed = 3;
const double angle_close_enough = PI / 360;
const double distance_close_enough = 0.1;

// Variables
bool goal_reached = false;
PointXY car = {100, 100};
PointXY point = {120, 105};

while (!goal_reached)
  // The Y is reversed since I am going to output to the screen, where up is decreasing Y.
  double angle_difference = atan2(car.y - point.y, point.x - car.x);

  // Distance to the point. Y is reversed for the same reason as above.
  double distance = sqrt((point.x - car.x) * (point.x - car.x) + (car.y - point.y) * (car.y - point.y));

  // Make the maximum angle change unless the point is closer, and then use the angle difference.
  double angle_change = max_angle_change;
  if (angle_difference < max_angle_change)
    angle_change = angle_difference;

  // If the point is at a negative angle, then reverse the angle change.
  if (angle_difference < 0)
    angle_change *= -1;

  car_radians += angle_change;

  // Drive at the maximum speed unless the distance is less than it, then use the distance.
  double speed_change = max_speed_change;
  if (distance < max_speed_change)
    speed_change = distance;

  // Drive the car with the given angle and speed changes
  car.x += speed_change * cos(car_radians);
  car.y -= speed_change * sin(car_radians);

  if ((distance < distance_close_enough) && (angle_difference < angle_close_enough))
    std::cout << "Point reached!";
    goal_reached = true;

I’m hoping that I just have something slightly off, but maybe another approach is even better. I’m open to suggestions.