%% 《理论力学(第3版)》（李俊峰，张雄编著）第54页：例3-1
%% 随工件旋转的参考系中刀尖轨迹绘制
clear;
close;

%% 输入参数
r = 10;  %工件半径
w = pi/5;
fun_r = @(t)r*sin(w*t);  % 刀尖运动方程（函数句柄）
fun_theta = @(t)w*t;  % 工件角度运动方程（函数句柄）
t = 10;  % 运动时间
frame_num = 100;  % 帧数
mode = 1;  % 是否生成视频文件，1，生成，0，不生成
name = 'Example_3_1';  % 视频文件名


%% 绘制

if mode  ==  1
    aviobj = VideoWriter(name);  % 新建视频文件
    open(aviobj);  % 打开文件
end

t = linspace(0,t,frame_num);

for ii=1:1:frame_num
    
    theta = t(ii);
    
    % 随工件旋转的参考系中刀尖轨迹绘制
    
    plot([-1.2*r,1.2*r],[0,0],'k','linewidth',1);  % 绘制x轴
    hold on
    title('旋转系中刀尖轨迹图');
    
    plot([0,0],[-1.2*r,1.2*r],'k','linewidth',1);  % 绘制y轴
    set(gca,'xtick',[],'ytick',[],'xcolor','w','ycolor','w');
    
    plot([-1.2*r*cos(-fun_theta(theta)),1.2*r*cos(-fun_theta(theta))],[-1.2*r*sin(-fun_theta(theta)),1.2*r*sin(-fun_theta(theta))],'k','linewidth',1);  % 绘制ξ轴
    plot([-1.2*r*cos(-fun_theta(theta)+0.5*pi),1.2*r*cos(-fun_theta(theta)+0.5*pi)],[-1.2*r*sin(-fun_theta(theta)+0.5*pi),1.2*r*sin(-fun_theta(theta)+0.5*pi)],'k','linewidth',1);  % 绘制η轴
    
    % 标注坐标轴 
    text(1.2*r,-0.075*r,'\itx','FontName','Times New Roman','FontSize',25);
    text(-0.175*r,1.2*r,'\ity','FontName','Times New Roman','FontSize',25);
    text(1.2*r*cos(-fun_theta(theta)),1.2*r*sin(-fun_theta(theta))-0.075*r,'\itξ','FontName','Times New Roman','FontSize',25);
    text(1.2*r*cos(-fun_theta(theta)+0.5*pi)-0.175*r,1.2*r*sin(-fun_theta(theta)+0.5*pi),'\itη','FontName','Times New Roman','FontSize',25);
    
    arrow([1.2*r,0],[0.25*r,0],0.2*1,2,0,'k');  % 绘制箭头
    arrow([0,1.2*r],[0,0.25*r],0.2*1,2,0,'k');
    
    cycle(r,[0,0],2);  % 绘制工件
    
    axis equal; 
    
    % 绘制刀尖轨迹
    t2=1:ii;
    x1=fun_r(t(t2)).*cos(fun_theta(t(t2)));
    y1=-fun_r(t(t2)).*sin(fun_theta(t(t2)));
    plot(x1,y1,'r','LineWidth',3);
    
    scatter(fun_r(theta)*cos(fun_theta(theta)),-fun_r(theta)*sin(fun_theta(theta)),25,'o','MarkerEdgeColor','k','MarkerFaceColor','k','LineWidth',1);  % 绘制刀尖
    axis([]);
    hold off

    xlim([-1.6*r 1.6*r]); 
    ylim([-1.6*r 1.6*r]);  % 限定显示范围 
    
    pause(0.01);  % 暂停0.01s，便于观察
    
    % 输出视频
    if mode == 1
        currFrame = getframe;
        writeVideo(aviobj,currFrame);
    end
    
end

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



%% 函数定义

% 功能：绘制箭头
% 输入参数：
%    xy: 箭头坐标，1行2列向量
%    fx: 箭头方向，表示箭头末端到箭头尖端的方向，1行2列向量
%    sz: 箭头大小，即箭头三角形底边长度
%    sc: 箭头长宽比，（箭头为等腰三角形）箭头三角形腰长与底边长度之比
%    mode: 模式， 模式为0表示箭头底边位于给定的箭头坐标处，模式为1表示箭头尖端位于给定的箭头坐标处
%    color: 箭头颜色， 与MATLAB绘图颜色代号相同，如'k'表示黑色
function arrow(xy,fx,sz,sc,mode,color)
    sc = sqrt(sc^2-1/4);
    theta = atan2(fx(2),fx(1));
    
    xy1 = [0,sz/2];
    xy2 = [0,-sz/2];
    xy3 = [sc*sz,0];
    
    [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];
    
    if mode==0   
        xy1=[xy1(1)+xy(1),xy1(2)+xy(2)];
        xy2=[xy2(1)+xy(1),xy2(2)+xy(2)];
        xy3=[xy3(1)+xy(1),xy3(2)+xy(2)];
    else
        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)];
        xy3=[xy(1),xy(2)];    
    end
    
    if nargin == 5
        fill([xy1(1),xy2(1),xy3(1)],[xy1(2),xy2(2),xy3(2)],'k');
    else
        fill([xy1(1),xy2(1),xy3(1)],[xy1(2),xy2(2),xy3(2)],color);
    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

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