%% 《理论力学(第3版)》（李俊峰，张雄编著）第62页：例3-3
%% 显示运动动画及A点速度、加速度
clear;
close;

%% 输入参数
l = 3;  % 杆长
t = 5;  % 运动时间
w = pi/2;  % 角速度
frame_num = 100;  % 帧数
mode = 1;   % 是否生成视频文件，1，生成，0，不生成
name = 'Example_3_3';  % 视频文件名


%% 计算
%计算速度和加速度
t = linspace(0,t,frame_num);
t0 = t(end);
phi = w.*t;  % 定义全部的转角,以铅垂为0度,逆时针为正
v = w*l.*ones([1,frame_num]);  % 滑块的速度大小
vx = v.*cos(phi);  %  水平速度大小,以向右为正
vy = v.*sin(phi);  %  竖直速度大小,以向上为正
a = (w^2)*l.*ones([1,frame_num]);  % 滑块的加速度大小
ax = -a.*sin(phi);  % 滑块水平加速度大小
ay = a.*cos(phi);  % 滑块竖直加速度大小

rod_xy = [l*sin(phi(1))*ones(1,frame_num);(-1)*l*cos(phi)];  % T形杆上接触点的绝对坐标

crank_xy = [l*sin(phi);(-1)*l*cos(phi)];  % 曲柄上参考点的绝对坐标

r_xy = (rod_xy - crank_xy);  % 曲柄对杆的相对坐标(与曲柄上的点固连的平动坐标系）
for ii = 1:1:frame_num
    [r_xy(1,ii),r_xy(2,ii)] = rotation(r_xy(1,ii),r_xy(2,ii),(-phi(ii)));  % 该时刻相对运动轨迹的相对坐标（固连坐标系）
end
%% 绘制

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

% 预分配一个数组 M 来存储影片帧
loops = length(phi);
M1(loops) = struct('cdata',[],'colormap',[]);
M2(loops) = struct('cdata',[],'colormap',[]);
M3(loops) = struct('cdata',[],'colormap',[]);

for ii = 1:1:frame_num
    
    figure(1);%绘制运动动画
    set (gcf,'Position',[0,210,500,400], 'color','w');
    % 限定显示范围 
    clf;
    xlim([-1.3*l 0.13*l]);
    ylim([-3*l 2*l]);
    axis equal;
    set(gca,'xtick',[],'ytick',[],'xcolor','w','ycolor','w');
    hold on
   
   % 画出固定销
   plot([0,0.067*l],[0,0.133*l],'k','linewidth',1.5);
   plot([0,-0.067*l],[0,0.133*l],'k','linewidth',1.5);
   plot([0.067*l,-0.067*l],[0.133*l,0.133*l],'k','linewidth',1.5);
   
   % 画出固定装置
   plot([-0.3*l,-0.167*l],[-1.67*l,-1.67*l],'k','linewidth',1.5);
   plot([-0.3*l,-0.167*l],[-2*l,-2*l],'k','linewidth',1.5);
   plot([-0.3*l,-0.3*l],[-1.67*l,-2*l],'k','linewidth',1.5);
   plot([-0.167*l,-0.167*l],[-1.67*l,-2*l],'k','linewidth',1.5);
   
   plot([0.3*l,0.167*l],[-1.67*l,-1.67*l],'k','linewidth',1.5);
   plot([0.3*l,0.167*l],[-2*l,-2*l],'k','linewidth',1.5);
   plot([0.3*l,0.3*l],[-1.67*l,-2*l],'k','linewidth',1.5);
   plot([0.167*l,0.167*l],[-1.67*l,-2*l],'k','linewidth',1.5);
   
   % 确定角度,以铅直为0度,逆时针为正
   theta = phi(ii); 
   
   % 以O为原点,确定A点坐标
   scatter(l*sin(theta),(-1)*l*cos(theta),25,'o','MarkerEdgeColor','k','MarkerFaceColor','k','LineWidth',0.6); 
   scatter(0,0,25,'o','MarkerEdgeColor','k','MarkerFaceColor','k','LineWidth',0.6); 
   
   % 画出OA杆
   plot([0,l*sin(theta)],[0,(-1)*l*cos(theta)],'k','linewidth',l); 
   
   % 画出滑块
   plot([l*sin(theta)-0.13*l,l*sin(theta)-0.13*l],[-l*cos(theta)-0.065*l,-l*cos(theta)+0.065*l],'k','linewidth',1.5);
   plot([l*sin(theta)+0.13*l,l*sin(theta)+0.13*l],[-l*cos(theta)-0.065*l,-l*cos(theta)+0.065*l],'k','linewidth',1.5);
   plot([l*sin(theta)-0.13*l,l*sin(theta)+0.13*l],[-l*cos(theta)-0.065*l,-l*cos(theta)-0.065*l],'k','linewidth',1.5);
   plot([l*sin(theta)-0.13*l,l*sin(theta)+0.13*l],[-l*cos(theta)+0.065*l,-l*cos(theta)+0.065*l],'k','linewidth',1.5);
   
   % 画出连杆BCD
   plot([-1.2*l,1.2*l],[-l*cos(theta)+0.08*l,-l*cos(theta)+0.08*l],'k','linewidth',1.5);
   plot([-1.2*l,1.2*l],[-l*cos(theta)-0.08*l,-l*cos(theta)-0.08*l],'k','linewidth',1.5);
   plot([-1.2*l,-1.2*l],[-l*cos(theta)-0.08*l,-l*cos(theta)+0.08*l],'k','linewidth',1.5);
   plot([1.2*l,1.2*l],[-l*cos(theta)-0.08*l,-l*cos(theta)+0.08*l],'k','linewidth',1.5);
   plot([-0.13*l,0.13*l],[-l*cos(theta)-1.63*l,-l*cos(theta)-1.63*l],'k','linewidth',1.5);
   plot([-0.13*l,-0.13*l],[-l*cos(theta)-1.63*l,-l*cos(theta)-0.1*l],'k','linewidth',1.5);
   plot([0.13*l,0.13*l],[-l*cos(theta)-1.63*l,-l*cos(theta)-0.1*l],'k','linewidth',1.5);
   
   % 加上横竖轴线
   plot([0,0],[-2.33*l,0.33*l],'-.k','linewidth',0.7);
   plot([-1.2*l,0.33*l],[-l*cos(theta),-l*cos(theta)],'-.k','linewidth',0.7);
   
   rr_xy = r_xy;
   for jj = 1:1:frame_num
       [rr_xy(1,jj),rr_xy(2,jj)] = rotation(r_xy(1,jj),r_xy(2,jj),phi(ii));  % 该时刻相对运动轨迹的绝对坐标
   end
   rr_xy = rr_xy + repmat(crank_xy(:,ii),1,frame_num);
   plot(rr_xy(1,1:ii),rr_xy(2,1:ii),'k','linewidth',1);  % 绘制相对运动轨迹
   
   scatter(rod_xy(1,ii),rod_xy(2,ii),25,'o','MarkerEdgeColor','r','MarkerFaceColor','r','LineWidth',1);
 % scatter(crank_xy(1,ii),crank_xy(2,ii),25,'o','MarkerEdgeColor','r','MarkerFaceColor','r','LineWidth',1);  % 绘制点
   
   hold off;
   drawnow;
   
    % 输出视频
    if mode == 1
        currFrame = getframe;
        M1(ii) = currFrame;
        writeVideo(aviobj1,currFrame);
    end
    
    
    % 绘制速度随时间变化动画
    figure(2);
    set (gcf,'Position',[525,210,500,400], 'color','w');
    
    % 限定显示范围
    clf;
    axis([0 t0*1.1 min([vx,vy,v]) max([vx,vy,v])]);
    axis equal;
    
    % 绘制
    plot(t,vx,'r','linewidth',2);
    hold on;
    plot(t,vy,'g','linewidth',2);
    plot(t,v,'c','linewidth',2);
    scatter(t(ii),vx(ii),25,'o','MarkerEdgeColor','r','MarkerFaceColor','r','LineWidth',1);
    scatter(t(ii),vy(ii),25,'o','MarkerEdgeColor','g','MarkerFaceColor','g','LineWidth',1);
    scatter(t(ii),v(ii),25,'o','MarkerEdgeColor','c','MarkerFaceColor','c','LineWidth',1);
    
    % 标注
    title('The velocity','Fontname','Times New Roman','FontSize',15);  % 标题
    xlabel('{\itt}/s','FontSize',20,'Fontname', 'Times New Roman');  % x轴
    ylabel('{\itv} m/s','FontSize',20,'Fontname', 'Times New Roman');  % y轴
    legend('\itv_x','\itv_y','\itv','Location','southeastoutside','Fontname','Times New Roman','FontSize',20);  % 图例
    set(gca,'FontSize',20,'Fontname', 'Times New Roman');
    
    hold off;
    
    % 输出视频
    if mode == 1
        currFrame = getframe(gcf);
        M2(ii) = currFrame;
        writeVideo(aviobj2,currFrame);
    end
    
    
    % 绘制加速度随时间变化动画
    figure(3);
    set (gcf,'Position',[1050,210,500,400], 'color','w');
    % 限定显示范围
    clf;
    axis([0 t0*1.1 min([ax,ay,a]) max([ax,ay,a])]);
    axis equal;
    % 绘制
    plot(t,ax,'r','linewidth',2);
    hold on;
    plot(t,ay,'g','linewidth',2);
    plot(t,a,'c','linewidth',2);
    scatter(t(ii),ax(ii),25,'o','MarkerEdgeColor','r','MarkerFaceColor','r','LineWidth',1);
    scatter(t(ii),ay(ii),25,'o','MarkerEdgeColor','g','MarkerFaceColor','g','LineWidth',1);
    scatter(t(ii),a(ii),25,'o','MarkerEdgeColor','c','MarkerFaceColor','c','LineWidth',1);
    % 标注
    title('The acceleration','Fontname','Times New Roman','FontSize',15);  % 标题
    xlabel('{\itt}/s','FontSize',20,'Fontname', 'Times New Roman');  % x轴
    ylabel('{\ita} m/s^2','FontSize',20,'Fontname', 'Times New Roman');  % y轴
    legend('\ita_x','\ita_y','\ita','Location','southeastoutside','Fontname','Times New Roman','FontSize',20);  % 图例
    set(gca,'FontSize',20,'Fontname', 'Times New Roman');
    
    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);

    % 绘制运动动画
    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)

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