How can i find optimal path using particle swarm optimization which is interpolated through the given control(handle) points?

if true
  % code
end 

   %%%%%%%%%%Create model%%%%%%%%%%%
    function model=CreateModel()

        % Source
      xs=1;
      ys=1;

      % Target (Destination)
      xt=10;
      yt=5;

      xobs=[1 3 6 9 11 1 3 6 9 11 2 4.5 7.5 10 4.5 7.5];
      yobs=[9 9 9 9 9 6.5 6.5 6.5 6.5 6.5 2.5 4 4 2.5 1 1];
      robs=[0.5 0.5 0.5 0.5 0.5 0.5 0.5 0.5 0.5 0.5 0.5 0.5 0.5 0.5 0.5 0.5];

      n=16;

      xmin=-10;
      xmax= 10;

      ymin=-10;
      ymax= 10;

      model.xs=xs;
      model.ys=ys;
      model.xt=xt;
      model.yt=yt;
      model.xobs=xobs;
      model.yobs=yobs;
      model.robs=robs;
      model.n=n;
      model.xmin=xmin;
      model.xmax=xmax;
      model.ymin=ymin;
      model.ymax=ymax;

end

%%%%%%%%%%%CreateRandomSolution%%%%%%%%%%%%%%%
function sol1=CreateRandomSolution(model)

      n=model.n;

      xmin=model.xmin;
      xmax=model.xmax;

      ymin=model.ymin;
      ymax=model.ymax;

      sol1.x=unifrnd(xmin,xmax,1,n);
      sol1.y=unifrnd(ymin,ymax,1,n);

end

%%%%%%%%%%%%%MyCost%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [z, sol]=MyCost(sol1,model)

      sol=ParseSolution(sol1,model);

      beta=100;
      z=sol.L*(1+beta*sol.Violation);

end

%%%%%%%%%%obstaclefunction%%%%%%%%%%%%%%%%%%%%%%%%%
function J=obstaclefunction(x,w1)
% An example function to represent sensed obstacles:
J=...
w1*max([exp(?0.8*((x(1,1)?20)^2+(x(2,1)?15)^2)),...
exp(?0.8*((x(1,1)?8)^2+(x(2,1)?10)^2)),...
exp(?0.8*((x(1,1)?10)^2+(x(2,1)?10)^2)),...
exp(?0.8*((x(1,1)?12)^2+(x(2,1)?10)^2)),...
exp(?0.8*((x(1,1)?24)^2+(x(2,1)?20)^2)),...
exp(?0.8*((x(1,1)?18)^2+(x(2,1)?20)^2))]);

%%%%%%%%%%%%%%%%%%%ParseSolution%%%%%%%%%%%%%%%%%%%%%%
function sol2=ParseSolution(sol1,model)

      x=sol1.x;
      y=sol1.y;

      xs=model.xs;
      ys=model.ys;
      xt=model.xt;
      yt=model.yt;
      xobs=model.xobs;
      yobs=model.yobs;
      robs=model.robs;

      XS=[xs x xt];
      YS=[ys y yt];
      k=numel(XS);
      TS=linspace(0,1,k);

      tt=linspace(0,1,100);
      xx=spline(TS,XS,tt);
      yy=spline(TS,YS,tt);

      dx=diff(xx);
      dy=diff(yy);

      L=sum(sqrt(dx.^2+dy.^2));

      nobs = numel(xobs); % Number of Obstacles
      Violation = 0;
      for k=1:nobs
          d=sqrt((xx-xobs(k)).^2+(yy-yobs(k)).^2);
          v=max(1-d/robs(k),0);
          Violation=Violation+mean(v);
      end

      sol2.TS=TS;
      sol2.XS=XS;
      sol2.YS=YS;
      sol2.tt=tt;
      sol2.xx=xx;
      sol2.yy=yy;
      sol2.dx=dx;
      sol2.dy=dy;
      sol2.L=L;
      sol2.Violation=Violation;
      sol2.IsFeasible=(Violation==0);

%     figure;
%     plot(xx,yy);
%     hold on;
%     plot(XS,YS,'ro');
%     xlabel('x');
%     ylabel('y');
%     
%     figure;
%     plot(tt,xx);
%     hold on;
%     plot(TS,XS,'ro');
%     xlabel('t');
%     ylabel('x');
%     
%     figure;
%     plot(tt,yy);
%     hold on;
%     plot(TS,YS,'ro');
%     xlabel('t');
%     ylabel('y');

end

%%%%%%%%%%%%%%PlotSolution%%%%%%%%%%%%%%%%%%%%%%%%
function PlotSolution(sol,model)

      xs=model.xs;
      ys=model.ys;
      xt=model.xt;
      yt=model.yt;
      xobs=model.xobs;
      yobs=model.yobs;
      robs=model.robs;

      XS=sol.XS;
      YS=sol.YS;
      xx=sol.xx;
      yy=sol.yy;

      theta=linspace(0,2*pi,100);
      for k=1:numel(xobs)
          fill(xobs(k)+robs(k)*cos(theta),yobs(k)+robs(k)*sin(theta),[0.5 0.7 0.8]);
          hold on;
      end
      plot(xx,yy,'k','LineWidth',2);
      plot(XS,YS,'ro');
      plot(xs,ys,'bs','MarkerSize',12,'MarkerFaceColor','y');
      plot(xt,yt,'kp','MarkerSize',16,'MarkerFaceColor','g');
      hold off;
      grid on;
      axis equal;

end

%%%%%%%%%%%%%%PSO%%%%%%%%%%%%%%%%%%%%%%%%%%%
clc;
clear;
close all;

%%Problem Definition

model=CreateModel();

model.n=4;  % number of Handle Points

CostFunction=@(x) MyCost(x,model);    % Cost Function

nVar=model.n;       % Number of Decision Variables

VarSize=[1 nVar];   % Size of Decision Variables Matrix

VarMin.x=model.xmin;           % Lower Bound of Variables
VarMax.x=model.xmax;           % Upper Bound of Variables
VarMin.y=model.ymin;           % Lower Bound of Variables
VarMax.y=model.ymax;           % Upper Bound of Variables

%%PSO Parameters

MaxIt=10;          % Maximum Number of Iterations

nPop=150;           % Population Size (Swarm Size)

w=1;                % Inertia Weight
wdamp=0.98;         % Inertia Weight Damping Ratio
c1=1.5;             % Personal Learning Coefficient
c2=1.5;             % Global Learning Coefficient

% % Constriction Coefficient
% phi1=2.05;
% phi2=2.05;
% phi=phi1+phi2;
% chi=2/(phi-2+sqrt(phi^2-4*phi));
% w=chi;               % Inertia Weight
% wdamp=1;             % Inertia Weight Damping Ratio
% c1=chi*phi1;         % Personal Learning Coefficient
% c2=chi*phi2;         % Global Learning Coefficient

alpha=0.1;
VelMax.x=alpha*(VarMax.x-VarMin.x);    % Maximum Velocity
VelMin.x=-VelMax.x;                    % Minimum Velocity
VelMax.y=alpha*(VarMax.y-VarMin.y);    % Maximum Velocity
VelMin.y=-VelMax.y;                    % Minimum Velocity

%%Initialization

% Create Empty Particle Structure
empty_particle.Position=[];
empty_particle.Velocity=[];
empty_particle.Cost=[];
empty_particle.Sol=[];
empty_particle.Best.Position=[];
empty_particle.Best.Cost=[];
empty_particle.Best.Sol=[];

% Initialize Global Best
GlobalBest.Cost=inf;

% Create Particles Matrix
particle=repmat(empty_particle,nPop,1);

% Initialization Loop
for i=1:nPop

      % Initialize Position
      if i > 1
          particle(i).Position=CreateRandomSolution(model);
      else
          % Straight line from source to destination
          xx = linspace(model.xs, model.xt, model.n+2);
          yy = linspace(model.ys, model.yt, model.n+2);
          particle(i).Position.x = xx(2:end-1);
          particle(i).Position.y = yy(2:end-1);
      end

      % Initialize Velocity
      particle(i).Velocity.x=zeros(VarSize);
      particle(i).Velocity.y=zeros(VarSize);

      % Evaluation
      [particle(i).Cost, particle(i).Sol]=CostFunction(particle(i).Position);

      % Update Personal Best
      particle(i).Best.Position=particle(i).Position;
      particle(i).Best.Cost=particle(i).Cost;
      particle(i).Best.Sol=particle(i).Sol;

      % Update Global Best
      if particle(i).Best.Cost<GlobalBest.Cost

          GlobalBest=particle(i).Best;

      end

end

% Array to Hold Best Cost Values at Each Iteration
BestCost=zeros(MaxIt,1);

%%PSO Main Loop

for it=1:MaxIt

      for i=1:nPop

          % x Part

          % Update Velocity
          particle(i).Velocity.x = w*particle(i).Velocity.x ...
              + c1*rand(VarSize).*(particle(i).Best.Position.x-particle(i).Position.x) ...
              + c2*rand(VarSize).*(GlobalBest.Position.x-particle(i).Position.x);

          % Update Velocity Bounds
          particle(i).Velocity.x = max(particle(i).Velocity.x,VelMin.x);
          particle(i).Velocity.x = min(particle(i).Velocity.x,VelMax.x);

          % Update Position
          particle(i).Position.x = particle(i).Position.x + particle(i).Velocity.x;

          % Velocity Mirroring
          OutOfTheRange=(particle(i).Position.x<VarMin.x | particle(i).Position.x>VarMax.x);
          particle(i).Velocity.x(OutOfTheRange)=-particle(i).Velocity.x(OutOfTheRange);

          % Update Position Bounds
          particle(i).Position.x = max(particle(i).Position.x,VarMin.x);
          particle(i).Position.x = min(particle(i).Position.x,VarMax.x);

          % y Part

          % Update Velocity
          particle(i).Velocity.y = w*particle(i).Velocity.y ...
              + c1*rand(VarSize).*(particle(i).Best.Position.y-particle(i).Position.y) ...
              + c2*rand(VarSize).*(GlobalBest.Position.y-particle(i).Position.y);

          % Update Velocity Bounds
          particle(i).Velocity.y = max(particle(i).Velocity.y,VelMin.y);
          particle(i).Velocity.y = min(particle(i).Velocity.y,VelMax.y);

          % Update Position
          particle(i).Position.y = particle(i).Position.y + particle(i).Velocity.y;

          % Velocity Mirroring
          OutOfTheRange=(particle(i).Position.y<VarMin.y | particle(i).Position.y>VarMax.y);
          particle(i).Velocity.y(OutOfTheRange)=-particle(i).Velocity.y(OutOfTheRange);

          % Update Position Bounds
          particle(i).Position.y = max(particle(i).Position.y,VarMin.y);
          particle(i).Position.y = min(particle(i).Position.y,VarMax.y);

          % Evaluation
          [particle(i).Cost, particle(i).Sol]=CostFunction(particle(i).Position);

          % Update Personal Best
          if particle(i).Cost<particle(i).Best.Cost

              particle(i).Best.Position=particle(i).Position;
              particle(i).Best.Cost=particle(i).Cost;
              particle(i).Best.Sol=particle(i).Sol;

              % Update Global Best
              if particle(  *if true
    % code
  end* i).Best.Cost<GlobalBest.Cost
                  GlobalBest=particle(i).Best;
              end

          end

      end

      % Update Best Cost Ever Found
      BestCost(it)=GlobalBest.Cost;

      % Inertia Weight Damping
      w=w*wdamp;

      % Show Iteration Information
      if GlobalBest.Sol.IsFeasible
          Flag=' *';
      else
          Flag=[', Violation = ' num2str(GlobalBest.Sol.Violation)];
      end
      disp(['Iteration ' num2str(it) ': Best Cost = ' num2str(BestCost(it)) Flag]);

      % Plot Solution
      figure(1);
      PlotSolution(GlobalBest.Sol,model);
      pause(0.01);

end

%%Results

figure;
plot(BestCost,'LineWidth',2);
xlabel('Iteration');
ylabel('Best Cost');
grid on;
%%%%%%%%%%%%%%END PROGRAM%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%I am working on particle swarm optimization(PSO) for to find optimal path for mobile robot. Here, i have attached the code and output images of PSO. Initially, i will give start and end points as input and it will give optimal path which is interpolated through four handle (control) points. In this case, handle(control) points are generated randomly by code.
But in my problem, I will also give number of handle(control) points as a input to the code and the path follow through the given handle points only. I will provide a output image of PSO. In this, how can i give my handle(control) points such as (1,4), (3,5),(6,5), (4,2), (6,2), (8,3), (6,4) as a input for PSO? And how can i find optimal path between start and end points which is interpolated through the given handle(control) points?