% 已知物理参数
L1 = 5;L2 = 3;  %两摆杆长度和质量
m1 = 3;m2 = 5;
g = 9.80665;   % 物理重力加速度m/s^2


% 定义初始参数
% y0的第1参数，即摆1的初始角度；第2，即摆2的初始角度
% 第3，摆1的初始角速度；第4，摆2的初始角速度
theta1_0 = pi/2;
theta2_0 = pi/2;
%警告: 在 t=2.623401e+00 处失败。在时间 t 处，步长必须降至所允许的最小值(7.105427e-15)以下，才能达到积分容差要求。
tspan = [0,0.01,2];
doublePendulum(tspan,theta1_0,theta2_0,L1,L2,m1,m2,g);

function [T, Theta1, Theta2] = doublePendulum(tspan,theta1_0,theta2_0,L1,L2,m1,m2,g)
% 定义求解常微分方程所需的函数
    function dydt = doubleP(t,y)
        % 定义双摆模型方程
        d1 = m1+m2*L1^2/L2^2;
        d2 = m2*L1*L2*cos(y(1)-y(2))/L2^2;
        d3 = m2*L1*L2*sin(y(1)-y(2))*y(4)^2/L2;
        d4 = g/L2*sin(y(2)) + cos(y(1)-y(2))*y(3)^2/L2;
        dydt = [y(3); y(4); (m2*d4*d2-d3*m2*cos(y(1)-y(2)))/(d1-m2*d2*d2); (d1*d4-m2*d3*cos(y(1)-y(2)))/(L2*(d1-m2*d2*d2))];
    end
    % 定义初值和时间步长
    y0 = [theta1_0, theta2_0, 0, 0];
    options = odeset('RelTol',1e-10,'AbsTol',1e-10);

    % 调用ode45求解微分方程组
    [T,Y] = ode45(@doubleP,tspan,y0,options);
    
    % 计算摆的位置
    Theta1 = L1*sin(Y(:,1));
    Theta2 = L2*sin(Y(:,2))+Theta1;
    Theta1 = -L1*cos(Y(:,1));
    Theta2 = -L2*cos(Y(:,2))+Theta2;

    % 绘制双摆摆角随时间变化的图像
    plot(T,Y(:,1:2))
    legend('\theta_1','\theta_2')
    xlabel('时间 t')
    ylabel('摆角 \theta')
    title('双摆摆角随时间变化的图像')
end

% 运用双摆模型动画
% 初始角度
theta1 = pi/3;
theta2 = -pi/4;

% 初始速度
w1 = 0;
w2 = 0;

% 物理参数
g = 9.8;  % 重力加速度
l1 = 3;   % 第一根杆长度
l2 = 2;   % 第二根杆长度
m1 = 2;   % 第一个质点质量
m2 = 1;   % 第二个质点质量

% 仿真时间
tmax = 10;
dt = 0.01;
tspan = 0:dt:tmax;

% 初始状态
init = [theta1 w1 theta2 w2];

% 调用ode45求解微分方程
[t,y] = ode45(@(t,y)double_pend(t, y, m1, m2, l1, l2, g), tspan, init);

% 动画绘制
figure('Name', '双摆动画');
for i = 1:length(t)
    x1 = l1*sin(y(i, 1));
    y1 = -l1*cos(y(i, 1));
    x2 = x1+l2*sin(y(i, 3));
    y2 = y1-l2*cos(y(i, 3));
    plot([0;x1;x2], [0;y1;y2], '-o', 'LineWidth', 2);
    axis([-3 3 -3 1]);
    title(sprintf('双摆动画, t = %.2f', t(i)));
    grid on;
    drawnow;
end


% 使用 animatedline 函数来动态绘制出双摆的运动轨迹
% 要调用这个double_pend函数，需要传入若干个参数(见上)，包括时间 T，摆的位置 Theta1 与 Theta2，摆杆长度 L1 和 L2，以及每个时间步长的间隔时间 interval
function dydt = double_pend(t, y, m1, m2, l1, l2, g)
    theta1 = y(1);
    w1 = y(2);
    theta2 = y(3);
    w2 = y(4);

    dtheta1dt = w1;
    dw1dt = (m2*g*sin(theta2)*cos(theta1-theta2)-m2*l1*w1^2*sin(theta1-theta2)-...
        m2*l2*w2^2*sin(theta1-theta2)-(m1+m2)*g*sin(theta1))/((m1+m2)*l1-m2*l1*cos(theta1-theta2).^2);
    dtheta2dt = w2;
    dw2dt = ((m1+m2)*(g*sin(theta1)*cos(theta1-theta2)-l1*w1^2*sin(theta1-theta2)...
        -g*sin(theta2))-m2*l2*w2^2*sin(theta1-theta2)*cos(theta1-theta2))/((m1+m2)*l2-m2*l2*cos(theta1-theta2).^2);

    dydt = [dtheta1dt; dw1dt; dtheta2dt; dw2dt];
end