double x1 = position1_x;
double y1 = position1_y;
double x2 = position2_x;
double y2 = position2_y;
double dy = y2 - y1;
double dx = x2 - x1;
double length = sqrt(pow(dx , 2) + pow(dy , 2));
if(length > 2 * radius)
{
ERROR("Radius is too little : %f , l: %f", radius, length);
}
double alpha = angles::normalize_angle(2 * asin((length/2.0)/radius));
double theta1 = (M_PI - alpha) / 2.0;
double theta2 = atan2(dy, dx);
double theta3 ;
if(is_clock_wise == 1)
{
theta3 = theta2 - theta1;
}
else if(is_clock_wise == 2)
{
theta3 = theta2 + theta1;
}
else
{
ERROR("circle clock wise error!");
}
double x_c = x1 + radius * cos(theta3);
double y_c = y1 + radius * sin(theta3);
已知圆弧半径和方向(顺逆时针)