%%%%%%%Particle swarm optimization algorithm to determine the curvature of the flexible needle three-dimensional path planning, in the case of obstacles, for other algorithms to determine the impact of parameters on the PSO results, the call isPSO_flexible_needle_3D_ob_1.m%%%%%% 
function [dist,length,time,g]=pos_parameter_effect_itera(cob,xt,yt,zt,r,rob,NP,w,c1,c2)
t1=clock;
xo=r;
yo=0; %%%xo,yo indicates the center of the first arc
zo=0;

n=ceil(sqrt((xt-xo)^2+(yt-yo)^2+(zt-zo)^2)/r);
n=3;

D=3*n;%%%%ceil function is used to round up. This formula determines the minimum number of turning points by the distance between the target point and the needle point, and then uses (l,theta,phi) to represent arc length, arc center Angle and rotation motion Angle respectively. The value of phi on the plane is pi
% T=1000;%%%%Number of iterations
T=400;


lmin=0;
lmax=0.5*pi*r;%%%%%Let this arc can be completed, but in fact it may be at most pi/2 or pi, and this function is implemented first
thetamin=0;
thetamax=0.5*pi;
phimin=0;
phimax=2*pi;

% phi=pi;%%%phi is pi on a plane, but not in three dimensions
%%%Note that l and theta have a relationship of l=theta*r

vmax=40;
vmin=-vmax;


%%%%%Initializing population individuals (limiting location and speed)%%%%%%
x=zeros(NP,D); 
x(:,1:n)=rand(NP,n)*(lmax-lmin)+lmin;
x(:,n+1:2*n)=x(:,1:n)/r;  %%%This is in terms of constant curvature
x(:,2*n+1:3*n)=rand(NP,n)*(phimax-phimin)+phimin; %%%2D is pi every time you rotate it

v=rand(NP,D)*(vmax-vmin)+vmin;


%%%%Initialize the optimal position and value of the individual

p=x;
pbest=ones(NP,1);
for i=1:NP
    pbest(i)=fitnessfunc_3D_yuanxin_m(x(i,:),n,r,xt,yt,zt,cob,rob); %%%%%fitnessfunc indicates the objective function, and the parameters are tentatively x(i,:) and n, which will be adjusted later
end
%%%%%Initializes the global optimal location and optimal value
g=ones(1,D);
gbest=inf;

for i=1:NP
    if(pbest(i)<gbest)
        g=p(i,:);
        gbest=pbest(i);
    end
end
gb=ones(1,T);


%%%%%Iterate according to the formula until the accuracy or number of iterations is satisfied%%%%%
for i=1:T
    for j=1:NP
        %%%%%Update individual optimal position and optimal value
        pd=fitnessfunc_3D_yuanxin_m(x(j,:),n,r,xt,yt,zt,cob,rob);
        if (pd<pbest(j))
            p(j,:)=x(j,:);
            pbest(j)=pd;
        end
        %%%%%%Update the global optimal location and optimal value
        if(pbest(j)<gbest)
            g=p(j,:);
            gbest=pbest(j);
        end
        %%%%%Update location and speed
        v(j,:)=w*v(j,:)+c1*rand()*(p(j,:)-x(j,:))+c2*rand()*(g-x(j,:));
        x(j,:)=x(j,:)+v(j,:);
        
        %%%%%%%Boundary condition processing
        for ii=1:n
            if(v(j,ii)>vmax || v(j,ii)<vmin)  
                v(j,ii)=rand()*(vmax-vmin)+vmin;
            end
            if(x(j,ii)>lmax || x(j,ii)<lmin)
                x(j,ii)=rand()*(lmax-lmin)+lmin;
            end
            if(x(j,ii+2*n)>phimax || x(j,ii+2*n)< phimax)
               x(j,ii+2*n)=x(j,ii+2*n)-floor(x(j,ii+2*n)/(2*pi))*2*pi;
            end
        end
        x(j,n+1:2*n)=x(j,1:n)/r;
    end
    %%%%%Records the global historical optimal value
    gb(i)=gbest;
   
end

%%%%To store the end coordinates of the arc

P=[0 0 0]; %%P is used to store the arc endpoint coordinates
length=sum(g(1:n));


Tt=eye(4,4);
for i=1:n
    alpha=g(:,2*n+i);
    L=g(:,i);
    theta=g(:,n+i);
    
    %%%Find rotation matrix
    Rotza=[cos(alpha) -sin(alpha) 0 0
    sin(alpha) cos(alpha) 0 0
    0 0 1 0
    0 0 0 1];
    r=L/theta;
    h=r*(1-cos(theta));
    d=r*sin(theta);
    Transh0d=[1 0 0 h
            0 1 0 0
            0 0 1 d
            0 0 0 1];
    Rotyt=[cos(theta) 0 sin(theta) 0
            0         1     0        0
            -sin(theta) 0 cos(theta)  0
             0 0 0 1];
    T=Rotza*Transh0d*Rotyt;
    %%%%Find rotation matrix
    
    Tt=Tt*T;
    P=[P;Tt(1:3,4)'];
end

dist=sqrt((xt-P(n+1,1))^2+(yt-P(n+1,2))^2+(zt-P(n+1,3))^2);
t2=clock;
time=etime(t2,t1);

%%%%Used to verify that endpoint calculations are correct