%% 《理论力学(第3版)》（李俊峰，张雄编著）第71页：例3-11
%% 曲柄齿轮传动机构
clear;
close;

%% 输入参数

R0 = 3;  % 定轮0半径
R1 = 2;  % 动轮I半径
R2 = 3;  % 动轮II半径
omega3 = 1;  % 曲柄3的角速度
lw = 1;  % 线宽
color = 'k';  % 颜色
n = 1;  % 标志点个数
sz = 10;  % 标志点大小
num = 1000;  % 输出帧数
t = 1;  % 运动时间
frame_num = 200; % 帧数
mode = 1;  % 是否生成视频文件，1，生成，0，不生成
name = 'Example_3_11_2';  % 视频文件名

%% 绘制

if mode ==  1
    aviobj1 = VideoWriter(name);  % 新建视频文件，存储动画
    open(aviobj1);  % 打开文件
end

t = linspace(0,t,frame_num);

theta3 = t*omega3;  % 曲柄3角度

omega1 = (R0+R1)/R1*omega3;
theta1 = omega1*t+3/2*pi;  % 动轮1角度

omega2 = (R2-R0)/R2*omega3;  %动轮2角度
theta2 = omega2*t+3/2*pi;

l = R0+R1*2+R2;  % 曲柄3长度

for ii = 1:1:frame_num
    
    cyclefix(R0,[0,0],lw,color,3/2*pi,0,sz,7/6*pi,3/2*pi,10,7/6*pi); % 定轮0
    text(0,-R0/3,'定轮0')
    axis equal;
    hold on
    set(gca,'xtick',[],'ytick',[],'ztick',[],'xcolor','w','ycolor','w','zcolor','w');
    
    xy1 = (R0+R1)*[cos(theta3(ii)), sin(theta3(ii))];
    cycle(R1,xy1,lw,color,theta1(ii),n,sz);  % 绘制动轮1
    text(xy1(1),xy1(2)-R1/2,'动轮1')
    
    xy2 = l*[cos(theta3(ii)),sin(theta3(ii))];
    cycle(R2,xy2,lw,color,theta2(ii),n,sz);  % 绘制动轮2
    text(xy2(1),xy2(2)-R2/3,'动轮2')
    
    plot([0,xy2(1)],[0,xy2(2)],color,'linewidth',2)  % 绘制曲柄3
    text(xy1(1)/2-R1,xy1(2)/2+R1/2,'曲柄3')
    
    axis equal;
    axis([-l*1.5 l*1.5 -l*1.5 l*1.5]);  % 限定显示范围
    
    %  标注角速度
    str={strcat('\omega_1',' = ',num2str(omega1),' rad/s'),strcat('\omega_2',' = ',num2str(omega2),' rad/s'),strcat('\omega_3',' = ',num2str(omega3),' rad/s')};
    text(l*0.7,-l*0.8,str)
    hold off
    pause(0.01); 
    
    % 输出视频
    if mode ==  1
        currFrame  =  getframe(gcf);
        writeVideo(aviobj1,currFrame);
    end
    
end   

if mode == 1
    close(aviobj1);   % 关闭视频文件
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),sz,'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

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

    for i = 0:1:fixn
        dfix = (fixe-fix0)/(fixn-1);
        alpha = fix0+dfix*i;
        p1_x = r*cos(alpha);
        p1_y = r*sin(alpha);
        posx = (r+r/10)*cos(alpha)-p1_x;
        posy = (r+r/10)*sin(alpha)-p1_y;
        fixa_temp = fixa-alpha;
        [posx,posy] = rotation(posx,posy,fixa_temp);
        p2_x = p1_x+posx;
        p2_y = p1_y+posy;
        plot([p1_x,p2_x],[p1_y,p2_y],color,'linewidth',lw)
        hold on     
    end
    
    scatter(xy(1),xy(2),sz,'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