三點求圓公式Matlab 和C 版程式

三點求圓公式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;
endtheta_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;
endfor 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];
endfunction 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;
endend

void plot_circle(vector3d & p1, vector3d & p2, vector3d & p3,matrixxd& xyz)

for (int j = 0; j < n; ++j) }
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)
}

三點定圓程式 matlab實現

function center,r solve circle pt1,pt2,pt3 effect solve the circle which across points pt1 pt2 and pt3 inputs pt1,pt2,pt3 x,y center the circle center...

halcon測試篇 三點擬合圓弧和圓

已知三個座標點，擬合對應的圓弧和圓。設定圓的寬度 dev set line width 3 dev set draw margin 確定三個點,在視窗上用滑鼠點三個點 draw point 200000 row1,column1 draw point 200000 row2,column2 draw...

matlab實現求平面或空間三點圍成三角形的面積

我們知道三角形的面積 1 2 a b sinc，其中c為ab兩邊的夾角。運用叉乘公式 matlab裡用cross 函式實現 我們可以得到absinc，但這時候結果還是乙個三維向量，用norm 函式取向量的模長即可。求三角形面積函式function s area a,b,c if length a 2...