三点求圆公式Matlab 和C++版程序
%% 给定三个点做圆弧轨迹规划
function P_P=plot_circle(p1,p2,p3)
%% 利用这三个点做一个平面方程
k_11=( p1(2)-p3(2) )*( p2(3)-p3(3) ) - ( p2(2)-p3(2) )*( p1(3)-p3(3) ) ;
k_12=( p2(1)-p3(1) )*( p1(3)-p3(3) ) - ( p1(1)-p3(1) )*( p2(3)-p3(3) ) ;
k_13=( p1(1)-p3(1) )*( p2(2)-p3(2) ) - ( p2(1)-p3(1) )*( p1(2)-p3(2) ) ;
k_14= -( k_11*p3(1) + k_12*p3(2) + k_13*p3(3) );
%% 过p1 p2的中点并且和 p1p2 垂直的平面方程
k_21=p2(1)-p1(1);
k_22=p2(2)-p1(2);
k_23=p2(3)-p1(3);
k_24= -( ( p2(1)^2-p1(1)^2 ) + ( p2(2)^2-p1(2)^2 ) + ( p2(3)^2-p1(3)^2 ) )/2;
%% 过p2 p3的中点并且和 p2p3 垂直的平面方程
k_31=p3(1)-p2(1);
k_32=p3(2)-p2(2);
k_33=p3(3)-p2(3);
k_34=-( ( p3(1)^2-p2(1)^2 ) + ( p3(2)^2-p2(2)^2 ) + ( p3(3)^2-p2(3)^2 ) )/2;
%% 圆心肯定在这三个平面上面。利用点法式方程求得圆心
K_123=[k_11 k_12 k_13;k_21 k_22 k_23;k_31 k_32 k_33];
k_4=[-k_14;-k_24;-k_34];
C=inv(K_123)*k_4;
R=sqrt( ( C(1)-p1(1) )^2 + ( C(2)-p1(2) )^2 + ( C(3)-p1(3) )^2 );
%% 以这个三个点重新确立一个空间直角坐标系
%% K_11 k_12 k_13 是和 p1p2p3 所在平面垂直的法向量， 故取该法向量的单位向量为 Z轴
W_x=k_11/sqrt(k_11^2+k_12^2+k_13^2);
W_y=k_12/sqrt(k_11^2+k_12^2+k_13^2);
W_z=k_13/sqrt(k_11^2+k_12^2+k_13^2);
%% 以 Op1 的单位向量 为 X 轴
U_x= ( p1(1)- C(1) )/R ;
U_y= ( p1(2)- C(2) )/R ;
U_z= ( p1(3)- C(3) )/R ;
%% 确定 Y 轴，为 Z 轴和 X轴的叉乘
V_x= W_y*U_z-W_z*U_y;
V_y= W_z*U_x-W_x*U_z;
V_z= W_x*U_y-W_y*U_x;
T=[U_x V_x W_x C(1);
U_y V_y W_y C(2);
U_z V_z W_z C(3);
0 0 0 1];
%% 对向量坐标进行相应的齐次化
p1=[p1;1];
p2=[p2;1];
p3=[p3;1];
n_p1=inv(T)*p1;
n_p2=inv(T)*p2;
n_p3=inv(T)*p3;
n_p1=n_p1(1:3);
n_p2=n_p2(1:3);
n_p3=n_p3(1:3);
%% 求圆心角
%% 为了方便求这三个点之间的顺序，还需要求得这三个点的圆心角，因为atan2 的角度范围是 -pi pi,故统一进行变换为正的角度
theta_p2_o_p1=atan2(n_p2(2),n_p2(1));
if theta_p2_o_p1 < 0
theta_p2_o_p1=theta_p2_o_p1+2*pi;
end
theta_p3_o_p1=atan2(n_p3(2),n_p3(1));
if theta_p3_o_p1 < 0
theta_p3_o_p1=theta_p3_o_p1+2*pi;
end
%% 经过上面的变换之后，如果 p1p2 的角度 小于 p1p3 的角度，那么就是逆时针转，反之就是顺时针转。
if theta_p2_o_p1 < theta_p3_o_p1
dir=1;
else
dir=-1;
end
%% 用五次多项式进行圆弧插补
delta_t=0.5;
t=0:delta_t:10;
nm=Five_ploynomial(0,10,0,1,0,0,0,0);
n=size(nm);
for i=1:1:n(2)
u(i)=R*cos(nm(i)*dir*theta_p3_o_p1);
v(i)=R*sin(nm(i)*dir*theta_p3_o_p1);
w(i)=0;
end
for j=1:1:i
p=T*[u(j);v(j);w(j);1];
x(j)=p(1);
y(j)=p(2);
z(j)=p(3);
end
P_P=[x;y;z];
end
function qt=Five_ploynomial(t0,tf,q0,qf,d_q0,d_qf,dd_q0,dd_qf)
A = ( 12*(qf - q0) - 6*(d_qf+d_q0)*tf - (dd_q0-dd_qf)*tf^2 ) /(2*tf^5);
B = ( -30*(qf - q0) + (16*d_q0 + 14*d_qf)*tf + (3*dd_q0-2*dd_qf)*tf^2 ) / (2*tf^4);
C = ( 20*(qf - q0) - (12*d_q0 + 8*d_qf)*tf - (3*dd_q0-dd_qf)*tf^2 ) / (2*tf^3);
D=dd_q0/2;
E = d_q0;
F = q0;
i=1;
for t=t0:0.5:tf
qt(i)=A*t^5+B*t^4+C*t^3+D*t^2+E*t+F;
i=i+1;
end
end
C++ 版程序
void plot_circle(Vector3d & p1, Vector3d & p2, Vector3d & p3,MatrixXd& XYZ)
{
//利用这三个点做一个平面方程
double k_11 = (p1(1) - p3(1))*(p2(2) - p3(2)) - (p2(1) - p3(1))*(p1(2) - p3(2));
double k_12 = (p2(0) - p3(0))*(p1(2) - p3(2)) - (p1(0) - p3(0))*(p2(2) - p3(2));
double k_13 = (p1(0) - p3(0))*(p2(1) - p3(1)) - (p2(0) - p3(0))*(p1(1) - p3(1));
double k_14 = -(k_11*p3(0) + k_12*p3(1) + k_13*p3(2));
// 过p1 p2的中点并且和 p1p2 垂直的平面方程
double k_21 = p2(0) - p1(0);
double k_22 = p2(1) - p1(1);
double k_23 = p2(2) - p1(2);
double k_24 = -( ( pow(p2(0) , 2) - pow(p1(0) , 2) ) + ( pow( p2(1) , 2) - pow( p1(1),2) ) + ( pow( p2(2) , 2) - pow( p1(2) , 2) )) / 2;
// 过p2 p3的中点并且和 p2p3 垂直的平面方程
double k_31 = p3(0) - p2(0);
double k_32 = p3(1) - p2(1);
double k_33 = p3(2) - p2(2);
double k_34 = -((pow(p3(0), 2) - pow(p2(0), 2)) + (pow(p3(1), 2) - pow(p2(1), 2)) + (pow(p3(2), 2) - pow(p2(2), 2))) / 2;
// 圆心肯定在这三个平面上面。利用点法式方程求得圆心
Matrix3d K_123=Matrix3d::Zero();
K_123 << k_11, k_12, k_13, k_21, k_22, k_23, k_31, k_32, k_33;
Vector3d k_4 = Vector3d::Zero();
k_4<< -k_14, -k_24, -k_34;
Vector3d C =( K_123.inverse()*k_4);
double R = sqrt( pow ( (C(0) - p1(0)),2) + pow ( (C(1) - p1(1)),2) + pow ( (C(2) - p1(2)),2));
// 以这个三个点重新确立一个空间直角坐标系
// K_11 k_12 k_13 是和 p1p2p3 所在平面垂直的法向量， 故取该法向量的单位向量为 Z轴
double W_x = k_11 / sqrt(pow( k_11,2) + pow( k_12,2) + pow( k_13,2));
double W_y = k_12 / sqrt(pow( k_11,2) + pow( k_12,2) + pow( k_13,2));
double W_z = k_13 / sqrt(pow( k_11,2) + pow( k_12,2) + pow( k_13,2));
// 以 Op1 的单位向量 为 X 轴
double U_x = (p1(0) - C(0)) / R;
double U_y = (p1(1) - C(1)) / R;
double U_z = (p1(2) - C(2)) / R;
// 确定 Y 轴，为 Z 轴和 X轴的叉乘
double V_x = W_y*U_z - W_z*U_y;
double V_y = W_z*U_x - W_x*U_z;
double V_z = W_x*U_y - W_y*U_x;
Matrix4d T = Matrix4d::Zero();
T<< U_x, V_x, W_x, C(0),
U_y, V_y, W_y, C(1),
U_z, V_z, W_z, C(2),
0, 0, 0, 1;
// 对向量坐标进行相应的齐次化
Vector4d p1_new;
Vector4d p2_new;
Vector4d p3_new;
p1_new << p1(0), p1(1), p1(2), 1;
p2_new << p2(0), p2(1), p2(2), 1;
p3_new << p3(0), p3(1), p3(2), 1;
Vector4d n_p1 = T.inverse()*p1_new;
Vector4d n_p2 = T.inverse()*p2_new;
Vector4d n_p3 = T.inverse()*p3_new;
//求圆心角
//为了方便求这三个点之间的顺序，还需要求得这三个点的圆心角，因为atan2 的角度范围是 - pi pi, 故统一进行变换为正的角度
double theta_p2_o_p1 = atan2(n_p2(1), n_p2(0));
if ( theta_p2_o_p1 < 0 )
theta_p2_o_p1 = theta_p2_o_p1 + 2 * pi;
double theta_p3_o_p1 = atan2(n_p3(1), n_p3(0));
if ( theta_p3_o_p1 < 0 )
theta_p3_o_p1 = theta_p3_o_p1 + 2 * pi;
//经过上面的变换之后，如果 p1p2 的角度 小于 p1p3 的角度，那么就是逆时针转，反之就是顺时针转。
int dir = 1;
if( theta_p2_o_p1 < theta_p3_o_p1 )
dir = 1;
else
dir = -1;
// 用五次多项式进行圆弧插补
double t0 = 0;
double tf = 10;
double delta_t = 0.5;
int n = (tf - t0) / delta_t + 1;
VectorXd qt(n);
Five_ploynomial(0, 10, 0, 1, 0, 0, 0, 0,qt);
VectorXd u(n);
VectorXd v(n);
VectorXd w(n);
VectorXd x(n);
VectorXd y(n);
VectorXd z(n);
for (int i = 0; i < n; ++i)
{
u(i) = R*cos(qt(i)*dir*theta_p3_o_p1);
v(i) = R*sin(qt(i)*dir*theta_p3_o_p1);
w(i) = 0;
}
for (int j = 0; j < n; ++j)
{
Vector4d temp;
temp << u(j), v(j), w(j), 1;
Vector4d p = T*temp;
XYZ(j,0) = p(0);
XYZ(j, 1) = p(1);
XYZ(j, 2) = p(2);
}
}
void Five_ploynomial(double t0, double tf, double q0, double qf, double d_q0, double d_qf, double dd_q0, double dd_qf, VectorXd & qt)
{
double A, B, C, D, E, F;
A = (double(12.00 * (qf - q0)) - 6 * (d_qf + d_q0)*tf - (dd_q0 - dd_qf)*pow(tf, 2)) / (2 * pow(tf, 5));
B = (double(-30 * (qf - q0) + (16 * d_q0 + 14 * d_qf)*tf + (3 * dd_q0 - 2 * dd_qf)*pow(tf, 2))) / (2 * pow(tf, 4));
C = (double(20 * (qf - q0) - (12 * d_q0 + 8 * d_qf)*tf - (3 * dd_q0 - dd_qf)*pow(tf, 2))) / (2 * pow(tf, 3));
D = dd_q0 / 2;
E = d_q0;
F = q0;
double i = 0;
for (double t = 0; t < tf + 0.5; t = t + 0.5)
{
qt(i) = A*pow(t, 5) + B*pow(t, 4) + C*pow(t, 3) + D*pow(t, 2) + E*t + F;
i = i + 1;
}
}
