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.