%% 《理论力学(第3版)》（李俊峰，张雄编著）第72页：例3-12
%% 显示动圆锥轨迹及角速度、角加速度
clear;
close;

%% 输入参数

omega_1 = 5;  % w_1 角速度大小 rad/s
omega_2 = 3;  % w_2 角速度大小 rad/s

r_up = 9;     % 上齿轮半径
r_mid = 3;    % 中齿轮半径
r_down = 9;   % 下齿轮半径

t0 = 1;  % 时间  s
do = [-1,-1,0.5]; % 观察方向
mode = 1;   % 是否生成视频文件，1，生成，0，不生成

name = 'Example_3_12';  % 视频文件名

%% 时间

t=0:t0/100:t0;

%% 下齿轮计算

%计算下齿轮锥面坐标
h_down = r_down;  % 固定圆锥高度
z0_down = (0:h_down/100:h_down/7)';
x0_down = r_down*(1-z0_down/h_down);
alpha = 0:2*pi/100:2*pi;
alpha(end) =2 *pi;
y0_down = x0_down.*sin(alpha);
x0_down = x0_down.*cos(alpha);
z0_down = repmat(z0_down,1,numel(alpha))-h_down+(r_down-r_mid);

% 计算下齿轮底面坐标
z0_down_base_down = (0:h_down/100:h_down)';
r0_down_base_down = r_down*(1-z0_down_base_down/h_down);
y0_down_base_down = r0_down_base_down.*sin(alpha);
x0_down_base_down = r0_down_base_down.*cos(alpha);
z0_down_base_down = z0_down_base_down.*0;
z0_down_base_down = repmat(z0_down_base_down,1,numel(alpha))-h_down+(r_down-r_mid);

% 计算下齿轮底面轮廓坐标
z0_down_base_outline_down = 0;
r0_down_base_outline_down = r_down;
y0_down_base_outline_down = r0_down_base_outline_down.*sin(alpha);
x0_down_base_outline_down = r0_down_base_outline_down.*cos(alpha);
z0_down_base_outline_down = repmat(z0_down_base_outline_down,1,numel(alpha))-h_down+(r_down-r_mid);

% 计算下齿轮顶面坐标
z0_down_base_up = (h_down/7:h_down/100:h_down)';
r0_down_base_up = r_down*(1-z0_down_base_up/h_down);
y0_down_base_up = r0_down_base_up.*sin(alpha);
x0_down_base_up = r0_down_base_up.*cos(alpha);
z0_down_base_up = z0_down_base_up.*0 + h_down/7;
z0_down_base_up = repmat(z0_down_base_up,1,numel(alpha))-h_down+(r_down-r_mid);

% 计算下齿轮顶面轮廓坐标
z0_down_base_outline_up = h_down/7;
r0_down_base_outline_up = r_down*(1-z0_down_base_outline_up/h_down);
y0_down_base_outline_up = r0_down_base_outline_up.*sin(alpha);
x0_down_base_outline_up = r0_down_base_outline_up.*cos(alpha);
z0_down_base_outline_up = repmat(z0_down_base_outline_up,1,numel(alpha))-h_down+(r_down-r_mid);

%% 中齿轮计算

%计算中齿轮锥面坐标
h_mid = r_mid;  % 旋转圆锥高度
z0_mid = (0:h_mid/100:h_mid/2)';
x0_mid = r_mid*(1-z0_mid/h_mid);
alpha=0:2*pi/100:2*pi;
alpha(end)=2*pi;
y0_mid = x0_mid.*sin(alpha);
x0_mid = x0_mid.*cos(alpha);
z0_mid = repmat(z0_mid,1,numel(alpha))-h_mid-(r_down-h_mid);
axis_mid = [1,0,0]';  % 初始旋转轴矢量
angle_mid = pi/2;  % 绕轴旋转角度

% 计算中齿轮底面坐标
z0_mid_base_down = (0:h_mid/100:h_mid)';
r0_mid_base_down = r_mid*(1-z0_mid_base_down/h_mid);
y0_mid_base_down = r0_mid_base_down.*sin(alpha);
x0_mid_base_down = r0_mid_base_down.*cos(alpha);
z0_mid_base_down = z0_mid_base_down.*0;
z0_mid_base_down = repmat(z0_mid_base_down,1,numel(alpha))-h_mid-(r_down-h_mid);

% 计算中齿轮底面轮廓坐标
z0_mid_base_outline_down = 0;
r0_mid_base_outline_down = r_mid;
y0_mid_base_outline_down = r0_mid_base_outline_down.*sin(alpha);
x0_mid_base_outline_down = r0_mid_base_outline_down.*cos(alpha);
z0_mid_base_outline_down = repmat(z0_mid_base_outline_down,1,numel(alpha))-h_mid-(r_down-h_mid);

% 计算中齿轮轴
coord0_mid_axis(1,1:3) = 0;
coord0_mid_axis(2,1:3) = [0,0,-h_mid-(r_down-h_mid)];

% 计算中齿轮顶面坐标
z0_mid_base_up = (h_mid/2:h_mid/100:h_mid)';
r0_mid_base_up = r_mid*(1-z0_mid_base_up/h_mid);
y0_mid_base_up = r0_mid_base_up.*sin(alpha);
x0_mid_base_up = r0_mid_base_up.*cos(alpha);
z0_mid_base_up = z0_mid_base_up.*0 + h_mid/2;
z0_mid_base_up = repmat(z0_mid_base_up,1,numel(alpha))-h_mid-(r_down-h_mid);

% 计算中齿轮顶面轮廓坐标
z0_mid_base_outline_up = h_mid/2;
r0_mid_base_outline_up = r_mid*(1-z0_mid_base_outline_up/h_mid);
y0_mid_base_outline_up = r0_mid_base_outline_up.*sin(alpha);
x0_mid_base_outline_up = r0_mid_base_outline_up.*cos(alpha);
z0_mid_base_outline_up = repmat(z0_mid_base_outline_up,1,numel(alpha))-h_mid-(r_down-h_mid);


%% 上齿轮计算

%计算上齿轮锥面坐标
h_up = r_up;  % 上齿轮高度
z0_up = (0:h_up/100:h_up/7)';
x0_up = r_up*(1-z0_up/h_up);
alpha = 0:2*pi/100:2*pi;
alpha(end) =2 *pi;
y0_up = x0_up.*sin(alpha);
x0_up = x0_up.*cos(alpha);
z0_up = repmat(z0_up,1,numel(alpha))-h_up+(r_up-r_mid);

% 计算上齿轮底面坐标
z0_up_base_down = (0:h_up/100:h_up)';
r0_up_base_down = r_up*(1-z0_up_base_down/h_up);
y0_up_base_down = r0_up_base_down.*sin(alpha);
x0_up_base_down = r0_up_base_down.*cos(alpha);
z0_up_base_down = z0_up_base_down.*0;
z0_up_base_down = repmat(z0_up_base_down,1,numel(alpha))-h_up+(r_up-r_mid);

% 计算上齿轮底面轮廓坐标
z0_up_base_outline_down = 0;
r0_up_base_outline_down = r_up;
y0_up_base_outline_down = r0_up_base_outline_down.*sin(alpha);
x0_up_base_outline_down = r0_up_base_outline_down.*cos(alpha);
z0_up_base_outline_down = repmat(z0_up_base_outline_down,1,numel(alpha))-h_up+(r_up-r_mid);

% 计算上齿轮顶面坐标
z0_up_base_up = (h_up/7:h_up/100:h_up)';
r0_up_base_up = r_up*(1-z0_up_base_up/h_up);
y0_up_base_up = r0_up_base_up.*sin(alpha);
x0_up_base_up = r0_up_base_up.*cos(alpha);
z0_up_base_up = z0_up_base_up.*0 + h_down/7;
z0_up_base_up = repmat(z0_up_base_up,1,numel(alpha))-h_up+(r_up-r_mid);

% 计算上齿轮顶面轮廓坐标
z0_up_base_outline_up = h_up/7;
r0_up_base_outline_up = r_up*(1-z0_up_base_outline_up/h_up);
y0_up_base_outline_up = r0_up_base_outline_up.*sin(alpha);
x0_up_base_outline_up = r0_up_base_outline_up.*cos(alpha);
z0_up_base_outline_up = repmat(z0_up_base_outline_up,1,numel(alpha))-h_up+(r_up-r_mid);

% 计算固定齿轮轴
 coord_down_axis(1,1:3) = [0,0,h_up-(r_up-r_mid)];
 coord_down_axis(2,1:3) = [0,0,-h_down+(r_down-r_mid)];
 
%% 计算固定点
omega_4 = (omega_1 + omega_2)/2;
omega_3 = r_up/r_mid*(omega_2-omega_4);
coord_up_fixedpoint = zeros(3,length(t));
coord_mid_fixedpoint = zeros(3,length(t));
coord_down_fixedpoint = zeros(3,length(t));
for ii = 1:length(t)
    % 上齿轮点
    alpha_up = -omega_1*t(ii);
    coord_up_fixedpoint(1,ii) = r_up.*sin(alpha_up);
    coord_up_fixedpoint(2,ii) = r_up.*cos(alpha_up);
    coord_up_fixedpoint(3,ii) = h_up-(r_up-r_mid);
    
    % 中齿轮点
    alpha_mid = -omega_3*t(ii)+omega_4*t(ii); % 当前自转角度
    theta_mid = omega_4*t(ii);  % 当前公转角度
    z0_point = -h_mid-(r_down-h_mid);
    x0_point = r_mid.*cos(alpha_mid);
    y0_point = r_mid.*sin(alpha_mid);
    axis_rot = [cos(theta_mid),sin(theta_mid),0]';  % 当前旋转轴矢量
    R = myrotate(pi/2,axis_rot);
    xyz_mid_fixedpoint = R*[x0_point;y0_point;z0_point];
    coord_mid_fixedpoint(1,ii) = xyz_mid_fixedpoint(1);
    coord_mid_fixedpoint(2,ii) = xyz_mid_fixedpoint(2);
    coord_mid_fixedpoint(3,ii) = xyz_mid_fixedpoint(3);  
    
    % 下齿轮点
    alpha_down = -omega_2*t(ii);
    coord_down_fixedpoint(1,ii) = r_up.*sin(alpha_down);
    coord_down_fixedpoint(2,ii) = r_up.*cos(alpha_down);
    coord_down_fixedpoint(3,ii) = -h_down+(r_down-r_mid);

end

%% 绘制

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

% 预分配一个数组 M 来存储影片帧
loops = length(t);
M1(loops) = struct('cdata',[],'colormap',[]);

for ii = 1:length(t)
    
    % 绘制锥齿轮滚动
    % 绘制下固定齿轮
    figure(1)
    set (gcf,'Position',[600,200,560,420], 'color','w');
    surf(x0_down,y0_down,z0_down,'FaceColor',[0.5 0.5 0.5],'EdgeColor',[0.5,0.5,0.5]);  
    hold on
    set(gca,'xtick',[],'ytick',[],'ztick',[],'xcolor','w','ycolor','w','zcolor','w');
    surf(x0_down_base_down,y0_down_base_down,z0_down_base_down,'FaceColor',[0.4 0.4 0.4],'EdgeColor',[0.5,0.5,0.5]); 
    plot3(x0_down_base_outline_down,y0_down_base_outline_down,z0_down_base_outline_down,'k','linewidth',1); 
    surf(x0_down_base_up,y0_down_base_up,z0_down_base_up,'FaceColor',[0.4 0.4 0.4],'EdgeColor',[0.5,0.5,0.5]); 
    plot3(x0_down_base_outline_up,y0_down_base_outline_up,z0_down_base_outline_up,'k','linewidth',1); 
    plot3(coord_down_axis(:,1),coord_down_axis(:,2),coord_down_axis(:,3),'k','linewidth',2);

    % 绘制上固定齿轮
    surf(x0_up,y0_up,-z0_up,'FaceColor',[0.5 0.5 0.5],'EdgeColor',[0.5,0.5,0.5]);  
    hold on
    surf(x0_up_base_down,y0_up_base_down,-z0_up_base_down,'FaceColor',[0.4 0.4 0.4],'EdgeColor',[0.5,0.5,0.5]); 
    plot3(x0_up_base_outline_down,y0_up_base_outline_down,-z0_up_base_outline_down,'k','linewidth',1); 
    surf(x0_up_base_up,y0_up_base_up,-z0_up_base_up,'FaceColor',[0.4 0.4 0.4],'EdgeColor',[0.5,0.5,0.5]); 
    plot3(x0_up_base_outline_up,y0_up_base_outline_up,-z0_up_base_outline_up,'k','linewidth',1); 
    
    theta = omega_4 * t(ii);  % 旋转轴转动角度
    axis_mid = [cos(theta),sin(theta),0]';   % 当前旋转轴矢量
    R = myrotate(angle_mid,axis_mid);  % 旋转矩阵
    
    xyz_cone = [x0_mid(:)';y0_mid(:)';z0_mid(:)'];
    xyz_cone_base_up = [x0_mid_base_up(:)';y0_mid_base_up(:)';z0_mid_base_up(:)'];
    xyz_cone_base_down = [x0_mid_base_down(:)';y0_mid_base_down(:)';z0_mid_base_down(:)'];
    xyz_cone_base_outline_up = [x0_mid_base_outline_up(:)';y0_mid_base_outline_up(:)';z0_mid_base_outline_up(:)'];
    xyz_cone_base_outline_down = [x0_mid_base_outline_down(:)';y0_mid_base_outline_down(:)';z0_mid_base_outline_down(:)'];
    xyz_cone = R*xyz_cone;
    xyz_cone_base_up = R*xyz_cone_base_up;
    xyz_cone_base_down = R*xyz_cone_base_down;
    xyz_cone_base_outline_up = R*xyz_cone_base_outline_up;
    xyz_cone_base_outline_down = R*xyz_cone_base_outline_down;
    x_mid = reshape(xyz_cone(1,:)',size(x0_mid,1),size(x0_mid,2));
    y_mid = reshape(xyz_cone(2,:)',size(y0_mid,1),size(y0_mid,2));
    z_mid = reshape(xyz_cone(3,:)',size(z0_mid,1),size(z0_mid,2));
    surf(x_mid,y_mid,z_mid,'FaceColor',[0.5 0.5 0.5],'EdgeColor',[0.5,0.5,0.5]);   % 绘制旋转齿轮锥面

    x_mid_base_down = reshape(xyz_cone_base_down(1,:)',size(x0_mid_base_down,1),size(x0_mid_base_down,2));
    y_mid_base_down = reshape(xyz_cone_base_down(2,:)',size(y0_mid_base_down,1),size(y0_mid_base_down,2));
    z_mid_base_down = reshape(xyz_cone_base_down(3,:)',size(z0_mid_base_down,1),size(z0_mid_base_down,2));
    surf(x_mid_base_down,y_mid_base_down,z_mid_base_down,'FaceColor',[0.5 0.5 0.5],'EdgeColor',[0.5,0.5,0.5]);    % 绘制旋转齿轮底面

    x_mid_base_up = reshape(xyz_cone_base_up(1,:)',size(x0_mid_base_up,1),size(x0_mid_base_up,2));
    y_mid_base_up = reshape(xyz_cone_base_up(2,:)',size(y0_mid_base_up,1),size(y0_mid_base_up,2));
    z_mid_base_up = reshape(xyz_cone_base_up(3,:)',size(z0_mid_base_up,1),size(z0_mid_base_up,2));
    surf(x_mid_base_up,y_mid_base_up,z_mid_base_up,'FaceColor',[0.5 0.5 0.5],'EdgeColor',[0.5,0.5,0.5]);     % 绘制旋转齿轮顶面

    x_mid_base_outline_down = reshape(xyz_cone_base_outline_down(1,:)',size(x0_mid_base_outline_down,1),size(x0_mid_base_outline_down,2));
    y_mid_base_outline_down = reshape(xyz_cone_base_outline_down(2,:)',size(y0_mid_base_outline_down,1),size(y0_mid_base_outline_down,2));
    z_mid_base_outline_down = reshape(xyz_cone_base_outline_down(3,:)',size(z0_mid_base_outline_down,1),size(z0_mid_base_outline_down,2));
    plot3(x_mid_base_outline_down,y_mid_base_outline_down,z_mid_base_outline_down,'k','linewidth',1);  % 绘制旋转齿轮底面轮廓

    x_mid_base_outline_up = reshape(xyz_cone_base_outline_up(1,:)',size(x0_mid_base_outline_up,1),size(x0_mid_base_outline_up,2));
    y_mid_base_outline_up = reshape(xyz_cone_base_outline_up(2,:)',size(y0_mid_base_outline_up,1),size(y0_mid_base_outline_up,2));
    z_mid_base_outline_up = reshape(xyz_cone_base_outline_up(3,:)',size(z0_mid_base_outline_up,1),size(z0_mid_base_outline_up,2));
    plot3(x_mid_base_outline_up,y_mid_base_outline_up,z_mid_base_outline_up,'k','linewidth',1);  % 绘制旋转齿轮顶面轮廓
    
    coord_mid_axis = R*coord0_mid_axis';
    plot3(coord_mid_axis(1,:),coord_mid_axis(2,:),coord_mid_axis(3,:),'k','linewidth',2); % 绘制旋转齿轮轴
    
    scatter3(coord_up_fixedpoint(1,ii),coord_up_fixedpoint(2,ii),coord_up_fixedpoint(3,ii),'o','MarkerEdgeColor','b','MarkerFaceColor','b','LineWidth',1);  
    scatter3(coord_down_fixedpoint(1,ii),coord_down_fixedpoint(2,ii),coord_down_fixedpoint(3,ii),'o','MarkerEdgeColor','b','MarkerFaceColor','b','LineWidth',1);  
    scatter3(coord_mid_fixedpoint(1,ii),coord_mid_fixedpoint(2,ii),coord_mid_fixedpoint(3,ii),'o','MarkerEdgeColor','b','MarkerFaceColor','b','LineWidth',1);  % 绘制旋转齿轮上点
    plot3(coord_mid_fixedpoint(1,1:ii),coord_mid_fixedpoint(2,1:ii),coord_mid_fixedpoint(3,1:ii),'r');  % 绘制旋转齿轮上点轨迹
     
     axis('equal');
     axis([-r_down-r_mid r_down+r_mid -r_down-r_mid r_down+r_mid]);
     
     title(strcat('\omega_1=',num2str(omega_1),' rad/s',' \omega_3=',num2str(omega_3),' rad/s',' \omega_4=',num2str(omega_4),' rad/s',' \omega_2=',num2str(omega_2),' rad/s'));
     
     view(do);  % 调整视角
     hold off
    
    % 输出视频
    if mode == 1
        currFrame  =  getframe(gcf);
        M1(ii) = currFrame;
        writeVideo(aviobj1,currFrame);
    end
    
    
end

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

    % 绘制运动动画
    figure(1);
    clf;
    set(gca,'xtick',[],'ytick',[],'xcolor','w','ycolor','w');
    movie(gcf,M1) ;

end


%% 函数定义
%绕矢量r旋转theta函数
%theta：旋转角度
%r：轴矢量，单位列向量
%R：旋转矩阵，方阵
function R = myrotate(theta,r) 
    I=diag(ones(1,3));
    R=cos(theta)*(I-r*r')+sin(theta)*skewt(r)+r*r';
end

%角速度伪矢量变为张量
%w：角速度矢量，列向量
%wt：角速度张量，方阵
function wt = skewt(w) 
    wt=[0,-w(3),w(2);w(3),0,-w(1);-w(2),w(1),0];
end