%% 《理论力学(第3版)》（李俊峰，张雄编著）第210页及211页：例6-12和例6-13
%% 绘制傅科摆的运动动画
clear;
close;

%% 输入参数

xy_v=[4,0,0,0];  % 初始时刻运动状态，变量格式为[x0,vx0,y0,vy0];
w = 2*pi/86164;  % 地球自转角速度rad/s
scale = 1000;  % 地球自转角速度放大倍数（用来凸显摆动平面的转动）
psi = 30;  % 纬度，角度制
g = 9.8;  % 重力加速度 m/s^2
l = 10; % 绳长 m
t = 30;  % 运动时间
do = [1,1,1]; % 观察方向
frame_num = 400; % 帧数
mode = 1;   % 是否生成视频文件，1，生成，0，不生成
name1 = 'Figure_6_12';  % 视频文件名
name2 = 'Figure_6_13';  % 视频文件名

%% 计算
w = w*scale;  %放大后的地球自转角速度
psi = psi/180*pi;
t = linspace(0,t,frame_num);
dtheta = -w*sin(psi);
theta = dtheta*t;

[t,xy]=ode45(@(t,x)fkb(t,x,w,psi,g,l),t,xy_v);  % 用ode45求解运动微分方程[d^x/dt^2-2*w*dy/dt*sin(psi)+g/l*x=0;d^y/dt^2+2*w*dx/dt*sin(psi)+g/l*y=0];

x = xy(:,1);
y = xy(:,3);
z = 0*x;


r_max = sqrt(max(x.^2+y.^2));  % 计算摆的最大运动范围
%% 绘制

if mode == 1
    
    aviobj1 = VideoWriter(name1);  % 新建视频文件，存储动画
    aviobj2 = VideoWriter(name2);
    open(aviobj1);  % 打开文件
    open(aviobj2);
    
    % 预分配一个数组 M 来存储影片帧
    loops = frame_num;
    M1(loops) = struct('cdata',[],'colormap',[]);
    M2(loops) = struct('cdata',[],'colormap',[]);
    
end

for ii = 1:1:frame_num
    % 三维图
    figure(1);
    plot3([0,x(ii)],[0,y(ii)],[l,z(ii)],'k','linewidth',1);  % 绘制绳子
    hold on;
    set(gcf,'Position',[200,200,560,420], 'color','w')
    
    scatter3(x(ii),y(ii),z(ii),25,'o','MarkerEdgeColor','k','MarkerFaceColor','k','LineWidth',1);  % 绘制小球
    plot3(x(1:ii),y(1:ii),z(1:ii),'k','linewidth',0.5);  % 绘制运动轨迹
    
    plot3([0,0],...
        [0,0],...
        [0,r_max],'k','linewidth',1);  % 绘制z轴
    plot3([0,r_max],...
        [0,0],...
        [0,0],'k','linewidth',1);  % 绘制x轴
    plot3([0,0],...
        [0,r_max],...
        [0,0],'k','linewidth',1);  % 绘制y轴
    
    set(gca,'xtick',[],'ytick',[],'ztick',[],'xcolor','w','ycolor','w','zcolor','w');
    axis('equal');
    axis([-r_max r_max -r_max r_max 0 r_max]);
    view(do);  % 调整视角
    hold off
    
    % 输出视频
    if mode == 1
        currFrame  =  getframe(gcf);
        M1(ii) = currFrame;
        writeVideo(aviobj1,currFrame);
    end
    
    % 二维图
    figure(2);
    plot([0,x(ii)],[0,y(ii)],'k','linewidth',1);  % 绘制绳子
    hold on;
    set(gcf,'Position',[800,200,560,420], 'color','w')
    
    scatter(x(ii),y(ii),25,'o','MarkerEdgeColor','k','MarkerFaceColor','k','LineWidth',1);  % 绘制小球
    plot(x(1:ii),y(1:ii),'k','linewidth',0.5);  % 绘制运动轨迹
    
%   plot([0,r_max],[0,0],'k','linewidth',1);  % 绘制x轴
%   arrow([r_max,0],[1,0],r_max/15,2,0,'k');
%   plot([0,0],[0,r_max],'k','linewidth',1);  % 绘制y轴
%   arrow([0,r_max],[0,1],r_max/15,2,0,'k');
    
    theta1 = linspace(0,2*pi,1000);
    x_c = r_max*cos(theta1);
    y_c = r_max*sin(theta1);
    plot(x_c,y_c,'--k','linewidth',0.5);  % 边界圆
    
    set(gca,'xtick',[],'ytick',[],'ztick',[],'xcolor','w','ycolor','w','zcolor','w');
    axis('equal');
    axis([-r_max-r_max/7 r_max+r_max/7 -r_max-r_max/7 r_max+r_max/7]);
    hold off
    
    % 输出视频
    if mode == 1
        currFrame  =  getframe(gcf);
        M2(ii) = currFrame;
        writeVideo(aviobj2,currFrame);
    end
end

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

%% 函数定义

% 运动方程
% t 运动时间
% x 变量, [x,dx/dt,y,dy/dt]
% w 地球自转角速度
% psi 纬度
% g 重力加速度
% l 绳长
function tt = fkb(t,x,w,psi,g,l)
    a = (2*w*sin(psi));
    b = g/l;
    tt = [x(2);
        a*x(4)-b*x(1);
        x(4);
        -a*x(2)-b*x(3)];
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