%% 《理论力学(第3版)》（李俊峰，张雄编著）第75页：例3-14
%% 计算车轮上B点的速度、角加速度
clear;
close;

%% 输入参数

R = 3;  % 轮心轨道半径
r = 1;  % 车轮半径
u = 3*pi/2;  % 轮心速度
t = 1;  % 运动时间
do = [1,1,1]; % 观察方向
frame_num = 200; % 帧数
mode = 1;   % 是否生成视频文件，1，生成，0，不生成
name = 'Example_3_14';  % 视频文件名

%% 计算

alpha=atan(r/R);
omega_e=-u/R;  % 牵连角速度大小
omega=omega_e/sin(alpha);  % 角速度大小
omega_r=omega*cos(alpha);  % 相对角速度大小

t=linspace(0,t,frame_num);
t0=t(end);

theta_e=omega_e*t+pi/2;
theta_r=omega_r*t;

omega_e_vector=repmat([0;0;omega_e],1,frame_num);  % 牵连角速度
omega_r_vector=[omega_r*cos(theta_e);omega_r*sin(theta_e);zeros(1,frame_num)];  % 相对角速度
omega_vector=omega_e_vector+omega_r_vector;  % 角速度

epsilon=cross(omega_e_vector,omega_vector);  % 角加速度

r_E=[R*cos(theta_e);R*sin(theta_e);zeros(1,frame_num)];
r_BE=[R*cos(theta_r).*cos(theta_e-pi/2);R*cos(theta_r).*sin(theta_e-pi/2);R*cos(theta_r)];
r_B=r_E+r_BE;

v_B = cross(omega_vector,r_B);  % vB
a_B = cross(epsilon,r_B)+cross(omega_vector,v_B);  % aB

abs_v_B = vecnorm(v_B);
abs_a_B = vecnorm(a_B);

rod_1_xyz_r = zeros(3,frame_num);  %  杆1端点坐标
rod_1_xyz_r(3,:) = rod_1_xyz_r(3,:)+r;
rod_1_xyz_l = [R*cos(theta_e);R*sin(theta_e);zeros(1,frame_num)+r];

theta_o=linspace(0,pi/2,frame_num);
%% 绘制

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

for ii = 1:1:frame_num
        
    % 绘制运动动画
    figure(1);
    set (gcf,'Position',[0,200,560,420], 'color','w')
    
    plot3([rod_1_xyz_r(1,ii),rod_1_xyz_l(1,ii)],...
        [rod_1_xyz_r(2,ii),rod_1_xyz_l(2,ii)],...
        [rod_1_xyz_r(3,ii),rod_1_xyz_l(3,ii)],'k','linewidth',2);  % 绘制杆
    hold on
    plot3([0,0],...
        [0,0],...
        [0,2*r],'k','linewidth',1);  % 绘制z轴
    plot3([0,R+r],...
        [0,0],...
        [0,0],'k','linewidth',1);  % 绘制x轴
    plot3([0,0],...
        [0,R+r],...
        [0,0],'k','linewidth',1);  % 绘制y轴
    set(gca,'xtick',[],'ytick',[],'ztick',[],'xcolor','w','ycolor','w','zcolor','w');
    axis('equal');
    axis([-1.2*R 1.2*R -1.2*R 1.2*R 0 3*r]);
    
    plot3([0,0],[0,0],[-r,2*r],'k','linewidth',1);  % 绘制旋转轴
    
    plot3(R*cos(theta_o),R*sin(theta_o),0*zeros(1,frame_num),'k','linewidth',1);  % 绘制自转轴
    
    eul = [theta_e(ii)+pi/2,pi/2,-theta_r(ii)];  % 欧拉角
    cylindrical([rod_1_xyz_l(1,ii),rod_1_xyz_l(2,ii),rod_1_xyz_l(3,ii)],r,r/10,eul,[0.5,0.5,0.5],1,20);  % 绘制圆盘
    
    view(do);  % 调整视角
    hold off

    % 输出视频
    if mode == 1
        currFrame  =  getframe(gcf);
        M1(ii) = currFrame;
        writeVideo(aviobj1,currFrame);
    end
    
     
    % 绘制B点速度曲线
    figure(2);
    
    set (gcf,'Position',[600,200,560,420], 'color','w')
    
    % 绘制B点速度
    plot(t,v_B(1,:),'r','linewidth',2);
    hold on;
    xlim([0 t0*1.1]);
    plot(t,v_B(2,:),'g','linewidth',2);
    plot(t,v_B(3,:),'c','linewidth',2);
    plot(t,abs_v_B,'k','linewidth',2);
    
    %绘制曲线上的散点
    scatter(t(ii),v_B(1,ii),25,'o','MarkerEdgeColor','r','MarkerFaceColor','r','LineWidth',1);
    scatter(t(ii),v_B(2,ii),25,'o','MarkerEdgeColor','g','MarkerFaceColor','g','LineWidth',1);
    scatter(t(ii),v_B(3,ii),25,'o','MarkerEdgeColor','c','MarkerFaceColor','c','LineWidth',1);
    scatter(t(ii),abs_v_B(ii),25,'o','MarkerEdgeColor','k','MarkerFaceColor','k','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_{Bx}','\itv_{By}','\itv_{Bz}','\itv_B','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
    
    % 绘制B点加速度曲线
    figure(3);
    
    set (gcf,'Position',[1200,200,560,420], 'color','w')
    
    % 绘制B点加速度
    plot(t,a_B(1,:),'r','linewidth',2);
    hold on;
    xlim([0 t0*1.1]);
    plot(t,a_B(2,:),'g','linewidth',2);
    plot(t,a_B(3,:),'c','linewidth',2);
    plot(t,abs_a_B,'k','linewidth',2);
    
    %绘制曲线上的散点
    scatter(t(ii),a_B(1,ii),25,'o','MarkerEdgeColor','r','MarkerFaceColor','r','LineWidth',1);
    scatter(t(ii),a_B(2,ii),25,'o','MarkerEdgeColor','g','MarkerFaceColor','g','LineWidth',1);
    scatter(t(ii),a_B(3,ii),25,'o','MarkerEdgeColor','c','MarkerFaceColor','c','LineWidth',1);
    scatter(t(ii),abs_a_B(ii),25,'o','MarkerEdgeColor','k','MarkerFaceColor','k','LineWidth',1);
    
    title('Acceleration','Fontname','Times New Roman','FontSize',15);  % 标题
    xlabel('{\itt}/s','FontSize',20,'Fontname', 'Times New Roman');  % x轴
    ylabel('{\itAcceration} m/s^2','FontSize',20,'Fontname', 'Times New Roman');  % y轴
    legend('\ita_{Bx}','\ita_{By}','\ita_{Bz}','\ita_B','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

%% 函数定义

% 功能 绘制圆柱
% xyz 基点位置（底面圆心位置）
% R 圆柱半径
% h 圆柱高度
% eul 圆柱313欧拉角,3行1列向量
% color 圆柱颜色
% 以下为非必要参数
% n 标志点数量（为表示圆柱的滚动，需要添加一定数量的标志点）
% sz 标志大小
function cylindrical(xyz,R,h,eul,color,n,sz)

    alpha = linspace(0,pi*2,200);
    ring_xyz_1 = [R*cos(alpha);R*sin(alpha);0*R*cos(alpha)];
    ring_xyz_2 = [R*cos(alpha);R*sin(alpha);0*R*cos(alpha)+h];
    
    z0 = (0:h/100:h)';
    x0 = R*(1-z0/h);
    
    n1 = length(z0);
    n2 = length(alpha);
    
    y1 = x0.*sin(alpha);
    x1 = x0.*cos(alpha);
    z1 = 0*repmat(z0,1,numel(alpha));  % 圆柱底面
    
    y2 = y1;
    x2 = x1;
    z2 = z1+h;  % 圆柱顶面
    
    n3 = 50;
    n4 = 100;
    
    y3 = linspace(-R,R,n4);
    z3 = linspace(0,h,n3)';
    x3 = sqrt(R^2-y3.^2);
    y3 = repmat(y3,n3,1);
    z3 = repmat(z3,1,n4);
    x3 = repmat(x3,n3,1);
    
    
    y4 = y3;
    z4 = z3;
    x4 = -x3;
    
  
    rotm = myeul2rotm(eul); % 旋转矩阵
    
    xyz_1 = [x1(:)';y1(:)';z1(:)'];  % 圆柱底面
    xyz_2 = [x2(:)';y2(:)';z2(:)'];  % 圆柱顶面
    xyz_3 = [x3(:)';y3(:)';z3(:)'];  % 侧面1
    xyz_4 = [x4(:)';y4(:)';z4(:)'];  % 侧面2
    xyz_1 =  rotm*xyz_1;
    xyz_2 =  rotm*xyz_2;
    xyz_3 =  rotm*xyz_3;
    xyz_4 =  rotm*xyz_4;
    ring_xyz_1 = rotm*ring_xyz_1;  % 底面圆
    ring_xyz_2 = rotm*ring_xyz_2;  % 顶面圆
    
    if nargin == 7
        sg_alpha = linspace(0,pi/2,n);
        sg_xyz = [R*cos(sg_alpha);R*sin(sg_alpha);0*R*cos(sg_alpha)+h];
        sg_xyz = rotm*sg_xyz;
    end
    
    x1 = reshape(xyz_1(1,:), n1, n2)+xyz(1);
    y1 = reshape(xyz_1(2,:), n1, n2)+xyz(2);
    z1 = reshape(xyz_1(3,:), n1, n2)+xyz(3);
   
    x2 = reshape(xyz_2(1,:), n1, n2)+xyz(1);
    y2 = reshape(xyz_2(2,:), n1, n2)+xyz(2);
    z2 = reshape(xyz_2(3,:), n1, n2)+xyz(3);
    
    x3 = reshape(xyz_3(1,:), n3, n4)+xyz(1);
    y3 = reshape(xyz_3(2,:), n3, n4)+xyz(2);
    z3 = reshape(xyz_3(3,:), n3, n4)+xyz(3);
    
    x4 = reshape(xyz_4(1,:), n3, n4)+xyz(1);
    y4 = reshape(xyz_4(2,:), n3, n4)+xyz(2);
    z4 = reshape(xyz_4(3,:), n3, n4)+xyz(3);   
    
    surf(x1,y1,z1,'FaceColor',color,'EdgeColor',color);
    hold on;
    surf(x2,y2,z2,'FaceColor',color,'EdgeColor',color);
    surf(x3,y3,z3,'FaceColor',color,'EdgeColor',color);
    surf(x4,y4,z4,'FaceColor',color,'EdgeColor',color);
    
    plot3(ring_xyz_1(1,:)+xyz(1),ring_xyz_1(2,:)+xyz(2),ring_xyz_1(3,:)+xyz(3),'k','linewidth',2);
    plot3(ring_xyz_2(1,:)+xyz(1),ring_xyz_2(2,:)+xyz(2),ring_xyz_2(3,:)+xyz(3),'k','linewidth',2);
    if nargin == 7
        sg_xyz = [sg_xyz(1,:)+xyz(1);sg_xyz(2,:)+xyz(2);sg_xyz(3,:)+xyz(3)];
        scatter3(sg_xyz(1,:),sg_xyz(2,:),sg_xyz(3,:),sz,'o','MarkerEdgeColor','r','MarkerFaceColor','r');
    end
    
end

% 功能 由欧拉角313转旋转矩阵
% eul 欧拉角
% rotm 旋转矩阵
function rotm = myeul2rotm(eul)
    psi = eul(1);
    theta = eul(2);
    phi = eul(3);
    rotm = [cos(phi)*cos(psi)-cos(theta)*sin(phi)*sin(psi),-cos(psi)*sin(phi)-cos(theta)*cos(phi)*sin(psi),sin(theta)*sin(psi);
        cos(theta)*cos(psi)*sin(phi)+cos(phi)*sin(psi),cos(theta)*cos(phi)*cos(psi)-sin(phi)*sin(psi),-cos(psi)*sin(theta);
        sin(theta)*sin(phi),cos(phi)*sin(theta),cos(theta)];
end