beziermotion.m

This is a program for trajectory generation using Bezier Curve.

clear;
clf;
clc;

P(1,:)=[100 100];
P(2,:)=[200 300]; 
P(3,:)=[400 300];
P(4,:)=[300 100];

N=length(P); % Number of control points

V=N-1; % Number of verteces

u_approx=0.256e-3; 

u=[0:u_approx:1]; % Parameter dissipation   

    for i=1:1:length(u) 
        for p=1:1:N
            basis_B(i,p)=basis(V,p-1,u(i));
        end
    end
    
    for i=1:1:length(u)
        for q=1:1:N-1
            basis_dB(i,q)=basis(V-1,q-1,u(i));
        end
    end
    
            
    
    for i=1:1:length(u)
        
         for j=1:1:2 % X and Y coordinates, 2D
             
             B(i,j)=0; % Initialization of Bezier Curve
             
             for p=1:1:N 
                                  
                 B(i,j)=B(i,j)+P(p,j)*basis_B(i,p); %Bezier Curve Points at every u_approx
                 
             end

         end
         
    end
    
    % Q(0)=V*(P(1)-P(0))...
    
    for m=2:1:length(P)
        
        for j=1:1:2 % x and y in 2D
            
            Q(m-1,j)=V*(P(m,j)-P(m-1,j));  
            
        end
        
    end
    
    
         
             
     for i=1:1:length(u)
         
        
        for j=1:1:2           
            
            dB(i,j)=0;  
            
            
            for q=1:1:N-1   
                
                
            dB(i,j)=dB(i,j)+Q(q,j)*basis_dB(i,q); % Derivative dB of bezier curve B.
           
                        
            end

        end

    end
    

    %Total Length Estimation using integration by trapezoid rule.
    
    h=u_approx;
  
    for db=1:1:length(dB)
        if(db==1)
            Length(db)=(h/2)*(abs(dB(db,1))+abs(dB(db,2)));
            
        elseif(db==length(dB))
            Length(db)=(h/2)*(abs(dB(db,1))+abs(dB(db,2)));
            
        else
            Length(db)=(h)*(abs(dB(db,1))+abs(dB(db,2)));
        end
        TotalLength=sum(Length);
    end
        
           
figure(1);

plot(P(:,1),P(:,2),'-go','LineWidth',2.0); hold on  % Plot of Verteces

plot(Q(:,1),Q(:,2),'-ro','LineWidth',2.0); hold on

plot(B(:,1),B(:,2),'k.','LineWidth',2.0); hold on  % Plot of Bezier Curve

plot(dB(:,1),dB(:,2),'b.','LineWidth',2.0); hold off  % Plot of Bezier Curve