%% 《理论力学(第3版)》（李俊峰，张雄编著）第75页：例3-15
%% 计算转子的角速度ω和角加速度ε以及转子上C点的速度vC和加速度aC
clear;
close;

%% 输入参数
alpha = pi/3;  % 中心轴与水平线的夹角
a = 10;  % 杆1长度
h = 3;  % 杆2长度
l = 8;  % 杆3长度
R = 4;  % 圆盘半径
omega_1 = 2;  % 框架绕固定铅垂角速度，rad/s
omega_2 = 8;  % 转子绕其自身的中心轴角速度，rad/s
t = 1;  % 运动时间
do = [-1,-1,1]; % 观察方向
frame_num = 200; % 帧数
mode = 1;   % 是否生成视频文件，1，生成，0，不生成
name = 'Example_3_15';  % 视频文件名

%% 计算

t = linspace(0,t,frame_num);  % 运动时间离散
t0 = t(end);

theta_1 = t*omega_1;  %  杆1角度
theta_2 = t*omega_2;  %  自转角度

alpha_2 = atan2(l*cos(alpha),a);  % 参数
l_2 = sqrt((l*cos(alpha))^2+a^2);

rod_1_xyz_r = zeros(3,frame_num);  %  杆1端点坐标
rod_1_xyz_l = [a*cos(theta_1);a*sin(theta_1);zeros(1,frame_num)];

rod_2_xyz_r = rod_1_xyz_l;  %  杆2端点坐标
rod_2_xyz_l = rod_1_xyz_l;
rod_2_xyz_l(3,:) = rod_2_xyz_l(3,:)+h;

rod_3_xyz_r = rod_2_xyz_l;  %  杆3端点坐标
rod_3_xyz_l = rod_2_xyz_l;
rod_3_xyz_l(3,:) = rod_3_xyz_l(3,:)+l*sin(alpha);
rod_3_xyz_l = [l_2*cos(theta_1-alpha_2);l_2*sin(theta_1-alpha_2);rod_3_xyz_l(3,:)];

omega = [omega_2*cos(alpha)*cos(theta_1);omega_2*cos(alpha)*sin(theta_1);omega_2*sin(alpha)+omega_1*ones(1,frame_num)];  % 角速度
epsilon = [-omega_1*omega_2*cos(alpha)*sin(theta_1);omega_1*omega_2*cos(alpha)*cos(theta_1);zeros(1,frame_num)];  % 角加速度

H = R/20;  % 圆盘高度
C_xyz0 = [0;R;H];
C_xyz = zeros(3,frame_num);  %  C点位置向量

for ii = 1:1:frame_num
    eul = [theta_1(ii),pi/2-alpha,theta_2(ii)];
    rotm = myeul2rotm(eul); % 旋转矩阵
    C_xyz(:,ii) = rotm*C_xyz0+[rod_3_xyz_l(1,ii),rod_3_xyz_l(2,ii),rod_3_xyz_l(3,ii)]';
end

O1_xyz = rod_2_xyz_l;  % O'坐标

rO1C = C_xyz-O1_xyz;  % rO'C

vO1C = cross(omega,rO1C);  % vO'C
aO1C = cross(epsilon,rO1C)+cross(omega,vO1C);  % aO'C

vO1 = cross(repmat([0,0,omega_1]',1,frame_num),O1_xyz);  % vO'
aO1 = cross(repmat([0,0,omega_1]',1,frame_num),vO1);  % aO'

vC = vO1+vO1C;  % vC
aC = aO1+aO1C;  % aC

abs_vC = vecnorm(vC);
abs_aC = vecnorm(aC);

%% 绘制

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_2_xyz_l(1,ii),rod_3_xyz_l(1,ii)],...
        [rod_1_xyz_r(2,ii),rod_1_xyz_l(2,ii),rod_2_xyz_l(2,ii),rod_3_xyz_l(2,ii)],...
        [rod_1_xyz_r(3,ii),rod_1_xyz_l(3,ii),rod_2_xyz_l(3,ii),rod_3_xyz_l(3,ii)],'k','linewidth',2);  % 绘制杆
    hold on
    
    set(gca,'xtick',[],'ytick',[],'ztick',[],'xcolor','w','ycolor','w','zcolor','w');
    axis('equal');
    axis([-a-l-R a+l+R -a-l-R a+l+R -h h+R+l]);
    
    plot3([0,0],[0,0],[-h,h+R],'k','linewidth',1);  % 绘制旋转轴
    
    cylindrical([0,0,-h/2],h/4,h,[0,0,0],[0.5,0.5,0.5]);  % 绘制中心圆筒
    
    descartes([0,0,0]',a,a,a,[0,0,0],'\itX','\itY','\itZ','k',20);  % 绘制固定坐标系坐标轴
    
    eul = [theta_1(ii),pi/2-alpha,theta_2(ii)];  % 欧拉角
    cylindrical([rod_3_xyz_l(1,ii),rod_3_xyz_l(2,ii),rod_3_xyz_l(3,ii)],R,H,eul,[0.5,0.5,0.5],1,20);  % 绘制圆盘
    
    descartes([rod_3_xyz_l(1,ii),rod_3_xyz_l(2,ii),rod_3_xyz_l(3,ii)]'+(rod_3_xyz_l(:,ii)-rod_3_xyz_r(:,ii))/norm(rod_3_xyz_l(:,ii)-rod_3_xyz_r(:,ii))*H*1.5,...
        a,a,a,eul,'\itx','\ity','\itz','k',20);  % 绘制固连坐标系坐标轴
    
    plot3(C_xyz(1,1:ii),C_xyz(2,1:ii),C_xyz(3,1:ii),'k','linewidth',1);  % 绘制C点轨迹
    view(do);  % 调整视角
    hold off

    % 输出视频
    if mode == 1
        currFrame  =  getframe(gcf);
        M1(ii) = currFrame;
        writeVideo(aviobj1,currFrame);
    end
    
    % 绘制C点速度曲线
    figure(2);
    
    set (gcf,'Position',[600,200,560,420], 'color','w')
    
    % 绘制C点速度
    plot(t,vC(1,:),'r','linewidth',2);
    hold on;
    xlim([0 t0*1.1]);
    plot(t,vC(2,:),'g','linewidth',2);
    plot(t,vC(3,:),'c','linewidth',2);
    plot(t,abs_vC,'k','linewidth',2);
    
    %绘制曲线上的散点
    scatter(t(ii),vC(1,ii),25,'o','MarkerEdgeColor','r','MarkerFaceColor','r','LineWidth',1);
    scatter(t(ii),vC(2,ii),25,'o','MarkerEdgeColor','g','MarkerFaceColor','g','LineWidth',1);
    scatter(t(ii),vC(3,ii),25,'o','MarkerEdgeColor','c','MarkerFaceColor','c','LineWidth',1);
    scatter(t(ii),abs_vC(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_{Cx}','\itv_{Cy}','\itv_{Cz}','\itv_C','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
    
    % 绘制C点加速度曲线
    figure(3);
    
    set (gcf,'Position',[1200,200,560,420], 'color','w')
    
    % 绘制C点加速度
    plot(t,aC(1,:),'r','linewidth',2);
    hold on;
    xlim([0 t0*1.1]);
    plot(t,aC(2,:),'g','linewidth',2);
    plot(t,aC(3,:),'c','linewidth',2);
    plot(t,abs_aC,'k','linewidth',2);
    
    %绘制曲线上的散点
    scatter(t(ii),aC(1,ii),25,'o','MarkerEdgeColor','r','MarkerFaceColor','r','LineWidth',1);
    scatter(t(ii),aC(2,ii),25,'o','MarkerEdgeColor','g','MarkerFaceColor','g','LineWidth',1);
    scatter(t(ii),aC(3,ii),25,'o','MarkerEdgeColor','c','MarkerFaceColor','c','LineWidth',1);
    scatter(t(ii),abs_aC(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_{Cx}','\ita_{Cy}','\ita_{Cz}','\ita_C','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
    
    pause(0.01);
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 原点位置，3行1列向量
% x x轴长度
% y y轴长度
% z z轴长度
% eul 313欧拉角
% x_name x轴标注
% y_name y轴标注
% z_name z轴标注
% color 颜色
% st 字体大小
function descartes(xyz,x,y,z,eul,x_name,y_name,z_name,color,st)

   rotm = myeul2rotm(eul); % 旋转矩阵
   
   x=rotm*[x,0,0]'+xyz;
   y=rotm*[0,y,0]'+xyz;
   z=rotm*[0,0,z]'+xyz;
   
   plot3([xyz(1),x(1)],[xyz(2),x(2)],[xyz(3),x(3)],color,'linewidth',1);
   hold on
   plot3([xyz(1),z(1)],[xyz(2),z(2)],[xyz(3),z(3)],color,'linewidth',1);
   plot3([xyz(1),y(1)],[xyz(2),y(2)],[xyz(3),y(3)],color,'linewidth',1);
   text(x(1),x(2),x(3),x_name,'FontSize',st,'Fontname','Times New Roman')
   text(y(1),y(2),y(3),y_name,'FontSize',st,'Fontname','Times New Roman')
   text(z(1),z(2),z(3),z_name,'FontSize',st,'Fontname','Times New Roman')
   
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