N_pendulum_symbol

%% N摆的拉格朗日方程
%% ——用MATLAB求解

clc;clear
N=3;
syms a [1 N]
syms b [1 N]
syms c [1 N]
syms theta(t) [1 N]
syms t
% 几何约束
for k=1:N
    if k==1
        x(k)=sin(eval(['theta',num2str(k)]));
        y(k)=-cos(eval(['theta',num2str(k)]));

    else
    x(k)=x(k-1)+sin(eval(['theta',num2str(k)]));
    y(k)=y(k-1)-cos(eval(['theta',num2str(k)]));
    end
    E(k)=Ek(x(k),y(k),t);
end
% 势能、动能、拉格朗日量
V=sum(y);T=sum(E);L=T-V;
% 拉格朗日方程
for k=1:N
    D2(k)=diff(eval(['theta',num2str(k)]),t,2);
    D1(k)=diff(eval(['theta',num2str(k)]),t);
    eq(k)=diff(diff(L,D1(k)),t)-diff(L,eval(['theta',num2str(k)]));
end
% 获得动能函数，用以验证能量是否守恒
Tf=subs(T,[D1,theta],[c,b]);% 符号替换
Tf=simplify(Tf);
Tf=matlabFunction(Tf);
for k=1:2*N
    if k==1
        x=['u(',num2str(k),')'];
    else
        x=[x,',u(',num2str(k),')'];
    end
end
Tkf=eval(['@(u) Tf(',x,')']);
% 利用拉格朗日方程解出二阶导数表达式
eq=subs(eq,[D1,theta,D2],[c,b,a]);% 符号替换
eq=simplify(eq);% 简化方程
tic;D=solve(eq,a);toc% 求解
% 生成一阶导数的系数矩阵
Dn=[];
for k=1:N
    Dk=eval(['D.a',num2str(k)]);
    Dk=coeffs(Dk,c);
    Dn=[Dn;Dk];
end
tic;Dn=simplify(Dn);toc
% 转为函数脚本，以供数值计算使用
ddn=matlabFunction(Dn);
% 转化为向量为自变量的函数
for k=1:N
    if k==1
        x=['u(',num2str(k),')'];
    else
        x=[x,',u(',num2str(k),')'];
    end
end
f=eval(['@(u) ddn(',x,')']);
save func_N3k.mat f N Tkf
%%
function E=Ek(x,y,t)
% 动能函数
E=1/2*((diff(x,t))^2+(diff(y,t))^2);
end