空间圆弧插补算法，matlab实现封装函数

%圆弧插补函数，输入三个坐标，以及插补点个数，得到一组坐标
function [trajx,trajy,trajz]=Cir_interpol(N,pos1,pos2,pos3)
x1=pos1(1);y1=pos1(2);z1=pos1(3);
x2=pos2(1);y2=pos2(2);z2=pos2(3);
x3=pos3(1);y3=pos3(2);z3=pos3(3);
A1 = y1z2 - y1z3 -z1y2 + z1y3 + y2z3 - y3z2;
B1 = -x1z2 + x1z3 + z1x2 - z1x3 - x2z3 + x3z2;
C1 = x1y2 - x1y3 - y1x2 + y1x3 + x2y3 - x3y2;
D1 = -x1y2z3 + x1y3z2 + x2y1z3 - x3y1z2 -x2y3z1 + x3y2z1;
% fplane = @(x,y,z) A1x + B1y +C1z +D1; %三点确定的面的方程
A2 = 2(x2-x1);
B2 = 2*(y2-y1);
C2 = 2*(z2-z1);
D2 = x1^2+y12+z1^2 - x2^2-y22-z2^2;
A3 = 2*(x3-x1);
B3 = 2*(y3-y1);
C3 = 2*(z3-z1);
D3 = x1^2+y12+z1^2 - x3^2-y32-z3^2;
T = [A1 B1 C1;A2 B2 C2;A3 B3 C3];
D = [D1 D2 D3]’;
r0 = -inv(T)D ;%圆心坐标向量
R = sqrt((pos1(1)-r0(1))^{2+(pos1(2)-r0(2))}2+(pos1(3)-r0(3))^2);%半径
%step2:得到13两点间的间距d
d13=sqrt((pos1(1)-pos3(1))^{2+(pos1(2)-pos3(2))}2+(pos1(3)-pos3(3))^2);
o13=(pos1+pos3)/2;%得到13两点的中点，当弧角180度时，圆心在该点，以该点为衡量，判定优劣弧
%step3:得到13中点与2点间距do2
do2=sqrt((o13(1)-pos2(1))^{2+(o13(2)-pos2(2))}2+(o13(3)-pos2(3))^2);
%ste4:判断优弧还是劣弧
if(do2<d13/2)
theta=2asin(d13/2/R);
else
theta=2pi-2asin(d13/2/R);
end
%step5:寻找该平面的法向量n，并将其单位化，将n1向量绕其旋转得到ni向量，加上圆心坐标即为圆弧插补点坐标
n1=pos3-r0’;
n2=pos1-r0’;
n=cross(n1,n2);
len=sqrt(n(1)²⁺ⁿ⁽²⁾2+n(3)^2);
n=n/len;%得到单位法向量
u=n(1);
v=n(2);
w=n(3);
%插值个数为30个
xx(N)=0;
yy(N)=0;
zz(N)=0;
%计算每一个插值点坐标,向量绕旋转轴n的旋转矩阵T
for c=1:N
tmp=transT((c-1)/(N-1)*theta,u,v,w)*n2’;
xx©=tmp(1);
yy©=tmp(2);
zz©=tmp(3);
end
trajx(N)=0;
trajy(N)=0;
trajz(N)=0;
for t=1:1:N;
trajx(t)=xx(t)+r0(1);
trajy(t)=yy(t)+r0(2);
trajz(t)=zz(t)+r0(3);
end
end

空间圆弧插补算法，matlab实现封装函数相关推荐

基于带约束S型加减速曲线的空间直线插补与空间圆弧插补算法（Matlab）
写在前面学习代码都记录在个人github上,欢迎关注~ Matlab机器人工具箱版本9.10 在前面的博文中: 基于抛物线过渡(梯形加减速)的空间直线插补算法与空间圆弧插补算法(Matlab) 基于 ...
matlab设置ga算法,matlab遗传算法ga函数
该楼层疑似违规已被系统折叠隐藏此楼查看此楼 function optimization4() A=[];b=[]; Aeq=[];beq=[];LB=[0.1;0.03;0.03;0.1;0.03; ...
matlab csp共空间代码,运动想象中共空间模式算法（CSP）的实现
最近在研究运动想象算法,其中CSP来提取特征用的比较多,尤为是在二分类的问题中,以前写过一篇如何在MNE库中实现CSP算法的博客,用的是MNE库中已经写好的算法,如今想本身实现该算法,研究了几天发现坑 ...
wnoise matlab,MATLAB中用wnoise函数测试去噪算法
MATLAB中用wnoise函数测试去噪算法 sqrt_snr=3; init=231434; [x,xn]=wnoise(3,11,sqrt_snr,init); % WNOISE generate ...
智能算法 | MATLAB实现SA模拟退火算法函数寻优
智能算法 | MATLAB实现SA模拟退火算法函数寻优目录智能算法 | MATLAB实现SA模拟退火算法函数寻优基本介绍算法描述程序设计参考资料基本介绍模拟退火算法(Simulated ...
如何利用MATLAB的surf函数，画出三元一次方程的空间平面？
目录 0 前言 1 空间曲面的MATLAB命令及其用法 1 surf函数语法及其说明 [^1] 2 mesh函数及其说明[^2] 2 surf函数的实现过程 3 图像的优化 4 结论 0 前言疫情期 ...
(转载)基于鱼群算法的函数寻优算法(matlab实现)
1 理论基础 1.1 人工鱼群算法概述人工鱼群算法是李晓磊等人于2002年提出的一类基于动物行为的群体智能优化算法.该算法是通过模拟鱼类的觅食.聚群.追尾.随机等行为在搜索域中进行寻 ...
matlab paticalcoff,关于DOA估计中加权前后向空间平滑算法的仿真问题
参考论文:<相干信源波达方向估计的加权空间平滑算法> 王布宏王永良陈辉武汉空军雷达学院重点实验室武汉430010 摘要提出了一种用于空间相干源 DOA 估计的加权空间平滑算法 ...
【MATLAB】黄金分割算法实现单变量函数最优值求解
用黄金分割算法求单变量函数f(x)在单峰区间[a,b]上的近似极小点函数实现: function [i,s,phis,ds,dphi,G] = golds(phi, a,b,epsilon, del ...

空间圆弧插补算法，matlab实现封装函数

空间圆弧插补算法，matlab实现封装函数相关推荐

最新文章

热门文章