# 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;

// 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