%% 《理论力学(第3版)》（李俊峰，张雄编著）曲柄滑块机构
%% 绘制曲柄上M点的轨迹、速度及加速度曲线
%% 输入参数

clear all
clear

r=1;
l_OA=r;  % 曲柄OA长度
l_AB=sqrt(2)*r;  % 连杆AB长度
w=pi;  % 角速度
t0=2;  % 运动时间
signal='AB';  %  标志M位于OA杆或AB杆
xyM=0.9;  % 标志M点位置的参数
frame_num=100;  % 帧数
mode = 1;   % 是否生成视频文件，1，生成，0，不生成
name = 'slider_crank';  % 视频文件名

if mode == 1
    aviobj1 = VideoWriter(name);  % 新建视频文件，存储动画
    aviobj2 = VideoWriter(strcat(name,'_v'));  % 新建视频文件，存储速度 
    aviobj3 = VideoWriter(strcat(name,'_a'));  % 新建视频文件，存储加速度
    open(aviobj1);  % 打开文件
    open(aviobj2);
    open(aviobj3);
end

%% 绘制

t = 0:t0/frame_num:t0;
t(end) = t0;
fai = t*w;  % 以OA与水平面的夹角为参量计算运动

% O点坐标
xO = 0*zeros(1,length(fai));
yO = 0*zeros(1,length(fai));

% A点坐标及速度
xA = l_OA*cos(fai);
yA = l_OA*sin(fai);

vxA = -l_OA*sin(fai)*w;
vyA = l_OA*cos(fai)*w;

axA = -l_OA*cos(fai)*w^2;
ayA = -l_OA*sin(fai)*w^2;

% B点坐标及速度
xB = sqrt(l_AB^2-(l_OA^2*sin(fai).^2))+l_OA*cos(fai);
yB = 0*zeros(1,length(fai));

vxB = -l_OA^2*sin(fai).*cos(fai)./sqrt(l_AB^2-l_OA^2*sin(fai).^2)-l_OA*sin(fai)*w;
vyB = 0*zeros(1,length(fai));

axB = ((-1).*l_OA.*cos(fai)+(-1).*l_OA.^4.*cos(fai).^2.*sin(fai).^2.*( ...
  l_AB.^2+(-1).*l_OA.^2.*sin(fai).^2).^(-3/2)+(-1).*l_OA.^2.*cos(fai) ...
  .^2.*(l_AB.^2+(-1).*l_OA.^2.*sin(fai).^2).^(-1/2)+l_OA.^2.*sin(fai) ...
  .^2.*(l_AB.^2+(-1).*l_OA.^2.*sin(fai).^2).^(-1/2))*w^2;
ayB = 0*zeros(1,length(fai));

% 计算∠ABO大小、角速度、角加速度
fai2 = asin(l_OA/l_AB*sin(fai));

vfai2 = (l_AB.^(-1).*l_OA.*cos(fai).*(1+(-1).*l_AB.^(-2).*l_OA.^2.*sin(fai).^2) ...
  .^(-1/2))*w;

afai2 = (l_AB.^(-3).*l_OA.^3.*cos(fai).^2.*sin(fai).*(1+(-1).*l_AB.^(-2).* ...
  l_OA.^2.*sin(fai).^2).^(-3/2)+(-1).*l_AB.^(-1).*l_OA.*sin(fai).*(1+( ...
  -1).*l_AB.^(-2).*l_OA.^2.*sin(fai).^2).^(-1/2))*w^2;

%计算M点坐标、速度、加速度
if strcmp(signal,'OA')
    xM = xA*xyM;
    yM = yA*xyM;
    
    vyM = vyA*xyM;
    vxM = vxA*xyM;
    
    vM = sqrt(vxM.^2+vyM.^2);  % M点速度大小
    
    axM = axA*xyM;
    ayM = ayA*xyM;
    
    aM = w^2*l_OA*xyM*ones(1,length(ayM));  % M点加速度大小
else
    xM = xB-l_AB*cos(fai2)*xyM;
    yM = yB+l_AB*sin(fai2)*xyM;
    
    vxM = -vfai2*l_AB.*xyM.*sin(fai2)+vxB;
    vyM = -vfai2*l_AB.*xyM.*cos(fai2)+vyB;
    
    vM = sqrt(vxM.^2+vyM.^2);  % M点速度大小
    
    axM = -afai2*l_AB.*xyM.*sin(fai2)+vfai2.^2*l_AB.*xyM.*cos(fai2)+axB;
    ayM = -afai2*l_AB.*xyM.*cos(fai2)-vfai2.^2*l_AB.*xyM.*sin(fai2)+ayB;
    
    aM = sqrt(axM.^2+ayM.^2);  % M点加速度大小
end
    

% 预分配一个数组 M 来存储影片帧
loops = length(fai);
M1(loops) = struct('cdata',[],'colormap',[]);
M2(loops) = struct('cdata',[],'colormap',[]);
M3(loops) = struct('cdata',[],'colormap',[]);
for ii = 1:1:length(fai) 
    % 绘制运动动画
    figure(1);
    
    % 绘制连杆
    set (gcf,'Position',[0,200,560,420], 'color','w')
    plot([xO(ii),xA(ii),xB(ii)],[yO(ii),yA(ii),yB(ii)],'k','linewidth',2)
    axis equal;
    hold on;
    set(gca,'xtick',[],'ytick',[],'xcolor','w','ycolor','w');
    
    % 绘制连接圆环及支撑
    support([xO(ii),yO(ii)],[0,1],2,r/20,'k');
    ring([xA(ii),yA(ii)],2,r/20,'k');
    ring([xB(ii),yB(ii)],2,r/20,'k');
    
    % 绘制滑块及墙壁
    rectangele([xB(ii),yB(ii)],r/3,r/3*0.618,0,2,'k')
    wall([xB(1),yB(1)-r/7],[0,1],2,r/3,5,'k')
    wall([xB(1),yB(1)+r/7],[0,-1],2,r/3,5,'k')
    plot([0.3*r, 2.4*r],[-r/7,-r/7],'k','linewidth',1); 
    plot([0.3*r, 2.4*r],[r/7,r/7],'k','linewidth',1); 
    
    % 绘制M点      
    scatter(xM(ii),yM(ii),25,'o','MarkerEdgeColor','k','MarkerFaceColor','k','LineWidth',1); 
    plot(xM(1:1:ii),yM(1:1:ii),'k','linewidth',1);%绘制M点轨迹
    
    title('Animation','Fontname','Times New Roman','FontSize',15);
    
    axis([-1.2*r l_OA+l_AB+r/2*1.1 -1.2*r 1.2*r]); % 限定显示范围
    hold off;
    drawnow;
    % 输出视频
    if mode == 1
        currFrame  =  getframe;
        M1(ii) = currFrame;
        writeVideo(aviobj1,currFrame);
    end
    
    % 绘制速度随时间变化动画
    figure(2);
    
    set (gcf,'Position',[600,200,560,420], 'color','w')
    plot(t,vxM,'r','linewidth',2);
    hold on;
    xlim([0 t0*1.1]);
    plot(t,vyM,'g','linewidth',2);
    plot(t,vM,'c','linewidth',2);
    
    scatter(t(ii),vxM(ii),25,'o','MarkerEdgeColor','r','MarkerFaceColor','r','LineWidth',1);
    scatter(t(ii),vyM(ii),25,'o','MarkerEdgeColor','g','MarkerFaceColor','g','LineWidth',1);
    scatter(t(ii),vM(ii),25,'o','MarkerEdgeColor','c','MarkerFaceColor','c','LineWidth',1);
    
    title('The velocity of point \itM','Fontname','Times New Roman','FontSize',12);  % 标题
    xlabel('{\itt}/s','FontSize',12,'Fontname', 'Times New Roman');  % x轴
    ylabel('{velocity} m/s','FontSize',12,'Fontname', 'Times New Roman');  % y轴
    legend('\itv_x','\itv_y','\itv','Location','south','Fontname','Times New Roman','FontSize',12);  % 图例
    set(gca,'FontSize',12,'Fontname', 'Times New Roman');
    axis([0 t0*1.1 min([vxM,vyM,vM]) max([vxM,vyM,vM])]);
    
    hold off;
    
     % 输出视频
    if mode == 1
        currFrame  =  getframe(gcf);
        M2(ii) = currFrame;
        writeVideo(aviobj2,currFrame);
    end
    figure(3)
    
    set (gcf,'Position',[1200,200,560,420], 'color','w')
    plot(t,axM,'m','linewidth',2);
    hold on;
    xlim([0 t0*1.1]);
    plot(t,ayM,'b','linewidth',2);
    plot(t,aM,'k','linewidth',2);
    
    scatter(t(ii),axM(ii),25,'o','MarkerEdgeColor','m','MarkerFaceColor','m','LineWidth',1);
    scatter(t(ii),ayM(ii),25,'o','MarkerEdgeColor','b','MarkerFaceColor','b','LineWidth',1);
    scatter(t(ii),aM(ii),25,'o','MarkerEdgeColor','k','MarkerFaceColor','k','LineWidth',1);
    
    title('The acceleration of point \itM','Fontname','Times New Roman','FontSize',12);  % 标题
    xlabel('{\itt}/s','FontSize',12,'Fontname', 'Times New Roman');  % x轴
    ylabel('{acceleration} m/s^2','FontSize',12,'Fontname', 'Times New Roman');  % y轴
    legend('\ita_x','\ita_y','\ita','Location','south','Fontname','Times New Roman','FontSize',12);  % 图例
    set(gca,'FontSize',12,'Fontname', 'Times New Roman');
    axis([0 t0*1.1 min([axM,ayM,aM]) max([axM,ayM,aM])]);
    
    hold off;
    
    % 输出视频
    if mode == 1
        currFrame  =  getframe(gcf);
        M3(ii) = currFrame;
        writeVideo(aviobj3,currFrame);
    end
end

if mode == 1
    close(aviobj1);   % 关闭视频文件
    close(aviobj2);
    close(aviobj3);
end

% 绘制运动动画
figure(1);
clf;
set(gca,'xtick',[],'ytick',[],'xcolor','w','ycolor','w');
movie(gcf,M1) ;

% 绘制速度时间曲线
figure(2);
clf;
set(gca,'xtick',[],'ytick',[],'xcolor','w','ycolor','w');
movie(gcf,M2);

% 绘制加速度时间曲线
figure(3);
clf;
set(gca,'xtick',[],'ytick',[],'xcolor','w','ycolor','w');
movie(gcf,M3)
%% 函数定义
%功能：绘制小圆环
% 输入参数
%    xy： 坐标，1行2列向量
%    lw： 线宽，表示绘制支撑的线宽
%    sz： 大小，表示圆的半径
%    color: 颜色， 与MATl_AB绘图颜色代号相同，如'k'表示黑色
function ring(xy,lw,sz,color)
    alpha = 0:2*pi/1000:2*pi;
    alpha(end) = 2*pi;
    x = sz*sin(alpha)+xy(1);
    y = sz*cos(alpha)+xy(2);
    fill(x,y,'w');
    hold on;
    plot(x,y,color,'linewidth',lw);
end

%功能：绘制矩形
% 输入参数
%    xy： 中心点坐标，1行2列向量
%    len： 长度
%    wid： 宽度
%    theta： 旋转角度
%    lw： 线宽，表示绘制的线宽
%    color: 颜色， 与MATl_AB绘图颜色代号相同，如'k'表示黑色
function rectangele(xy,len,wid,theta,lw,color)
    xy1 = [len/2,wid/2];
    xy2 = [-len/2,wid/2];
    xy3 = [-len/2,-wid/2];
    xy4 = [len/2,-wid/2];
    [xy1(1),xy1(2)] = rotation(xy1(1),xy1(2),theta);
    [xy2(1),xy2(2)] = rotation(xy2(1),xy2(2),theta);
    [xy3(1),xy3(2)] = rotation(xy3(1),xy3(2),theta);
    [xy4(1),xy4(2)] = rotation(xy4(1),xy4(2),theta);
    xy1 = xy1 + xy;
    xy2 = xy2 + xy;
    xy3 = xy3 + xy;
    xy4 = xy4 + xy;
    plot([xy1(1),xy2(1),xy3(1),xy4(1),xy1(1)],[xy1(2),xy2(2),xy3(2),xy4(2),xy1(2)],color,'linewidth',lw);
end

%功能：绘制固定简支支撑
% 输入参数
%    xy： 坐标，1行2列向量
%    fx： 方向，对称轴所在方向
%    lw： 线宽，表示绘制支撑的线宽
%    sz： 大小，表示圆的半径
%    color: 颜色， 与MATl_AB绘图颜色代号相同，如'k'表示黑色
function support(xy,fx,lw,sz,color)
    theta = atan2(fx(2),fx(1));
    
    sz2=4*sz;
    xy1 = [0,sz2/2];
    xy2 = [0,-sz2/2];
    xy3 = [sqrt(3)*sz2/2,0];
    xy4 = [0,-sz2];
    xy5 = [0,sz2];

    [temp1,temp2] = rotation(xy1(1),xy1(2),theta);
    xy1 = [temp1,temp2];
    
    [temp1,temp2] = rotation(xy2(1),xy2(2),theta);
    xy2 = [temp1,temp2];
    
    [temp1,temp2] = rotation(xy3(1),xy3(2),theta);
    xy3 = [temp1,temp2];
    
    [temp1,temp2] = rotation(xy4(1),xy4(2),theta);
    xy4 = [temp1,temp2];
    
    [temp1,temp2] = rotation(xy5(1),xy5(2),theta);
    xy5 = [temp1,temp2];

    %绘制斜线
    for ii = 1:1:4
        x = [0,-sz2/4];
        y = [-sz2*0.9+(ii-1)*sz2/2,-sz2*3/4*0.9+(ii-1)*sz2/2];
        [x_new,y_new] = rotation(x,y,theta);
        x_new=x_new+xy(1)-xy3(1);
        y_new=y_new+xy(2)-xy3(2);
        plot(x_new,y_new,color,'linewidth',lw/2);   
        hold on;
    end    
    xy1=[xy1(1)+xy(1)-xy3(1),xy1(2)+xy(2)-xy3(2)];
    xy2=[xy2(1)+xy(1)-xy3(1),xy2(2)+xy(2)-xy3(2)];
    xy4=[xy4(1)+xy(1)-xy3(1),xy4(2)+xy(2)-xy3(2)];
    xy5=[xy5(1)+xy(1)-xy3(1),xy5(2)+xy(2)-xy3(2)];
    xy3=[xy(1),xy(2)];    
    
    if nargin == 5
        color='k';
    end
    plot([xy1(1),xy2(1),xy3(1),xy1(1)],[xy1(2),xy2(2),xy3(2),xy1(2)],color,'linewidth',lw);
    
    ring(xy,lw,sz,color);
    
    plot([xy4(1),xy5(1)],[xy4(2),xy5(2)],color,'linewidth',lw);    
    axis('equal');
end

%功能：绘制固定墙壁
% 输入参数
%    xy： 墙壁中点坐标，1行2列向量
%    fx： 方向，对称轴所在方向
%    lw： 线宽，表示绘制支撑的线宽
%    sz： 墙壁长度
%    n：斜线数量
%    color: 颜色， 与MATl_AB绘图颜色代号相同，如'k'表示黑色
function wall(xy,fx,lw,sz,n,color)
    theta = atan2(fx(2),fx(1))-pi/2;

    %绘制斜线
    for ii = 1:1:n
        x = [-(sz/2)+(ii-1)*sz/n,-(sz/2-sz/(n+1))+(ii-1)*sz/n];
        y = [-sz/n,0];
        [x_new,y_new] = rotation(x,y,theta);
        x_new = x_new+xy(1);
        y_new = y_new+xy(2);
        plot(x_new,y_new,color,'linewidth',lw/2);   
        hold on;
    end    
    
    x = [-sz/2,sz/2];
    y = [0,0];
    [x_new,y_new] = rotation(x,y,theta);
    x_new = x_new+xy(1);
    y_new = y_new+xy(2);
    plot(x_new,y_new,color,'linewidth',lw);   
end

% 功能：将矢量[x,y]逆时针旋转theta角
% 输入参数
%    x,y： 旋转前坐标，1行n列向量
%    theta： 旋转角度
% 输出参数
%    x_new, y_new： 旋转后坐标，1行n列向量
function [x_new,y_new] = rotation(x,y,theta)
    R = [cos(theta),-sin(theta)
         sin(theta), cos(theta)];
    temp = R*[x;y];
    x_new = temp(1,:);
    y_new = temp(2,:);
end