%% 《理论力学(第3版)》（李俊峰，张雄编著）第63页：例3-4
%% 绘制杆的运动动画及杆的速度及加速度
clear;
close;

%% 输入参数
R = 1.5;  % 凸轮的半径
r = R*0.6;  % 固定铰支座到凸轮圆心的距离
xr = 0;  % 杆中心点横坐标（xr<=R-r）
wid = 2;  % 杆的宽度
len = 4;  % 杆的长度
w = pi;  % 角速度
t = 4;  % 运动时间
n = 1;  % 凸轮转动周期
frame_num = 100;  % 帧数
mode = 1;  % 是否生成视频文件，1，生成，0，不生成
name = 'cam';  % 视频文件名

%% 计算

t = linspace(0,t,frame_num);
theta = w*t+pi/2;

xy = [r*cos(theta);r*sin(theta)];  % 某时刻凸轮圆心的坐标

y = (R.^2+(-1).*(xr+(-1).*r.*cos(theta)).^2).^(1/2)+r.*sin(theta);  % 杆接触点径向位置

v = (r.*cos(theta)+(-1).*r.*(xr+(-1).*r.*cos(theta)).*(R.^2+(-1).*(xr+( ...
  -1).*r.*cos(theta)).^2).^(-1/2).*sin(theta))*w;  % 杆速度

a = ((-1).*r.*cos(theta).*(xr+(-1).*r.*cos(theta)).*(R.^2+(-1).*(xr+( ...
  -1).*r.*cos(theta)).^2).^(-1/2)+(-1).*r.*sin(theta)+(-1).*r.^2.*( ...
  xr+(-1).*r.*cos(theta)).^2.*(R.^2+(-1).*(xr+(-1).*r.*cos(theta)) ...
  .^2).^(-3/2).*sin(theta).^2+(-1).*r.^2.*(R.^2+(-1).*(xr+(-1).*r.* ...
  cos(theta)).^2).^(-1/2).*sin(theta).^2)*w^2;  % 杆加速度

ring_xy0 = [xr,y(1)];  % 初始时刻圆轮上接触点的绝对坐标
ring_xy = zeros(2,frame_num);
for ii = 1:1:frame_num
    [ring_xy(1,ii),ring_xy(2,ii)] = rotation(ring_xy0(1),ring_xy0(2),theta(ii)-pi/2);
end

rod_xy = [xr*ones(1,frame_num);y];  % 杆上参考点的绝对坐标

r_xy = ring_xy - rod_xy;  % 初始接触点对杆的相对坐标
%% 绘制
if mode == 1
    
    aviobj1 = VideoWriter(name);  % 新建视频文件，存储动画
    aviobj2 = VideoWriter(strcat(name,'_v'));  % 新建视频文件，存储速度 
    aviobj3 = VideoWriter(strcat(name,'_a'));  % 新建视频文件，存储加速度
    open(aviobj1);  % 打开文件
    open(aviobj2);
    open(aviobj3);
    
    % 预分配一个数组 M 来存储影片帧
    loops = frame_num;
    M1(loops) = struct('cdata',[],'colormap',[]);
    M2(loops) = struct('cdata',[],'colormap',[]);
    M3(loops) = struct('cdata',[],'colormap',[]);
    
end


lw = 1;  % 线宽

for ii = 1:1:frame_num
    
    % 绘制运动动画
    figure(1);
    set (gcf,'Position',[0,200,560,420], 'color','w')
    
    color = 'k';  % 绘图颜色
    
    xys = [0,0];
    fxs = [0,1];
    sz = R/10;
    support(xys,fxs,lw,sz,color);  % 固定铰支座（坐标原点）
    text(xys(1)+R/5,xys(2),'\itO','Fontname','Times New Roman','FontSize',14); % 标注坐标原点
    hold on;
    
    set(gca,'xtick',[],'ytick',[],'xcolor','w','ycolor','w');
    
    xyw1 = [xr-wid/20-wid/1000,4*R];
    wall(xyw1,[1,0],lw,1.5,5,color);  % 左侧固定墙
    xyw2 = [xr+wid/20+wid/1000,4*R];
    wall(xyw2,[-1,0],lw,1.5,5,color);  % 右侧固定墙
    axis equal;
    
    cycle(R,xy(:,ii),lw,color);  % 各个时刻凸轮
    
    posx = xr;   % 接触点横坐标
    posy = y(ii);  % 接触点纵坐标
    
    text(posx+R/5,posy+R/5,'\itA','Fontname','Times New Roman','FontSize',14); % 标注接触点
    
    plot([0,xy(1,ii)],[0,xy(2,ii)],'k--','linewidth',lw);  % 各个时刻铰支座到凸轮圆心连线
    plot([posx,posx],[posy,posy+len],'k','linewidth',wid);  % 各个时刻杆的位置    
    text(posx+R/5,posy+len+R/5,'\itB','Fontname','Times New Roman','FontSize',14); % 标注杆端点
    text(xy(1,ii)+R/5,xy(2,ii)+R/5,'\itC','Fontname','Times New Roman','FontSize',14); % 标注凸轮圆心
    
    rr_xy = r_xy + repmat(rod_xy(:,ii),1,frame_num);  % 该时刻相对运动轨迹的绝对坐标
    
    plot(rr_xy(1,1:ii),rr_xy(2,1:ii),'k','linewidth',1);  % 绘制相对运动轨迹
    
    scatter(ring_xy(1,ii),ring_xy(2,ii),25,'o','MarkerEdgeColor','r','MarkerFaceColor','r','LineWidth',1);
    scatter(rod_xy(1,ii),rod_xy(2,ii),25,'o','MarkerEdgeColor','r','MarkerFaceColor','r','LineWidth',1);  % 绘制点
    
    axis([-2*R 2*R -2*R 2*R+len]);
    
    hold off;
    
    % 输出视频
    if mode == 1
        currFrame  =  getframe(gcf);
        M1(ii) = currFrame;
        writeVideo(aviobj1,currFrame);
    end
    
    % 绘制杆速度
    figure(2);
    set (gcf,'Position',[600,200,560,420], 'color','w')
    plot(t,v(:),'r','linewidth',2);
    hold on;
    xlim([0 t(end)*1.1]);
    
    % 绘制散点
    scatter(t(ii),v(ii),25,'o','MarkerEdgeColor','r','MarkerFaceColor','r','LineWidth',1);
    
    title('Velocity','Fontname','Times New Roman','FontSize',15);  % 标题
    xlabel('{\itt}/s','FontSize',20,'Fontname', 'Times New Roman');  % x轴
    ylabel('{\itvelocity} m/s','FontSize',20,'Fontname', 'Times New Roman');  % y轴
    legend('\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',[1200,200,560,420], 'color','w')
    plot(t,a(:),'r','linewidth',2);
    hold on;
    xlim([0 t(end)*1.1]);
    
    % 绘制散点
    scatter(t(ii),a(ii),25,'o','MarkerEdgeColor','r','MarkerFaceColor','r','LineWidth',1);
    
    title('Acceleration','Fontname','Times New Roman','FontSize',15);  % 标题
    xlabel('{\itt}/s','FontSize',20,'Fontname', 'Times New Roman');  % x轴
    ylabel('{\itAcceleration} m/s^2','FontSize',20,'Fontname', 'Times New Roman');  % y轴
    legend('\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

%% 函数定义

%功能：绘制固定简支支撑
% 输入参数
%    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 = [0,-sz/n];
        hold on
        [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

% 功能：绘制圆
% 输入参数
%    r： 圆半径
%    xy： 圆心坐标，1行2列向量
%    lw： 线宽，表示绘制的圆的线宽
% 以下为非必要参数
%    color： 颜色，与MATl_AB绘图颜色代号相同，如'k'表示黑色
%    theta： 转动角度，表示圆滚动的角度
%    n： 标志点数量（为表示圆的滚动，需要添加一定数量的标志点）
%    sz： 标志点大小
function cycle(r,xy,lw,color,theta,n,sz)

    scatter(xy(1),xy(2),25,'o','MarkerEdgeColor',color,'MarkerFaceColor',color,'LineWidth',lw)
    hold on
    alpha = 0:2*pi/1000:2*pi;
    alpha(end) = 2*pi;
    x = r*sin(alpha);
    y = r*cos(alpha);
    if nargin  ==   7
        alpha = 0:2*pi/n:2*pi;
        alpha(end) = 2*pi;
        x_p = 1.0*r*sin(alpha);
        y_p = 1.0*r*cos(alpha);
        [x_p,y_p] = rotation(x_p,y_p,theta);
        plot(x+xy(1),y+xy(2),color,'linewidth',lw)
        hold on;
        scatter(x_p+xy(1),y_p+xy(2),sz,'o','MarkerEdgeColor',color,'MarkerFaceColor',color,'LineWidth',lw/2);
    else
        if nargin  ==   4
            plot(x+xy(1),y+xy(2),color,'linewidth',lw)
            hold on;
        else
            plot(x+xy(1),y+xy(2),'k','linewidth',lw)
            hold on;           
        end
    end
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

% 函数定义
%功能：绘制小圆环
% 输入参数
%    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