spherical_pendulum_3D

% 球面摆运动模型
clc;clear;close all
% 模型参数、初值、数值计算
g=10;L=1.0;
thetad=15;%释放初始角度DEG
theta=thetad*pi/180;%释放初始角度
phid=90;
phi=phid*pi/180;
% 初值
dphi=0.8386*sqrt(g/L)/sin(theta);
dtheta=0.9*sqrt(g/L);
T=120;%最大计算时间
odeopts=odeset('RelTol',1e-10,'AbsTol',1e-10, ...
    'MaxStep',1e-2,'InitialStep',1e-4,...
    'Events',@(t,u) dyn_evns(t,u,L,theta));
[t,u] = ode45(@(t,u) pendum(t,u,g,L),...
    0:0.01:T,[theta phi dtheta dphi],odeopts);
% 质点位置
x=L.*sin(u(:,1)).*cos(u(:,2));
y=L.*sin(u(:,1)).*sin(u(:,2));
z=-L.*cos(u(:,1));tic
xc=0.05*cos(0:0.1:2*pi);yc=0.05*sin(0:0.1:2*pi);
%% 模型绘图
D=-1.2*L;
% 支架
f=figure;f.MenuBar='none';
plot3([0 0 0 0],[L L -L -L],[D 0 0 D],...
    '-','Color','k','LineWidth',2)
hold on
[X1,Y1,Z1]=line3d([0 L D],[0 L 0],0.02,30);
[X2,Y2,Z2]=line3d([0 -L D],[0 -L 0],0.02,30);
[X3,Y3,Z3]=line3d([0 L 0],[0 -L 0],0.02,30);
zhu1=surf(X1,Y1,Z1);zhu1.FaceColor='w';zhu1.EdgeColor='none';
zhu2=surf(X2,Y2,Z2);zhu2.FaceColor='w';zhu2.EdgeColor='none';
heng=surf(X3,Y3,Z3);heng.FaceColor='w';heng.EdgeColor='none';
qiu(0,L,0,0.02,10,'w');
qiu(0,-L,0,0.02,10,'w');
qiu(0,0,0,0.03,30,'w');
% 3D小球
[X,Y,Z]=sphere(50);
r=0.05;X=r*X;Y=r*Y;Z=r*Z;
sp=surf(X,Y,Z);
sp.FaceColor='w';
sp.EdgeColor='none';
% 3D绳子
n1=[x y z];n0=zeros(size(n1));
for ii=1:length(x)
  [ Xl(:,:,ii), ...
    Yl(:,:,ii), ...
    Zl(:,:,ii) ] = line3d(n1(ii,:),n0(ii,:),0.01,20);
end
sl=surf(Xl(:,:,1),Yl(:,:,1),Zl(:,:,1));
sl.FaceColor='w';
sl.EdgeColor='none';
sl.AmbientStrength=0.3;
% 固定点
plot3(0,0,0,'.','Color','#202020','markersize',20)
a=animatedline('LineStyle','-','LineWidth',2,'Color','#AAAAAA');
tracks=animatedline('Color','#D95319','LineWidth',2);
fill3(1.5*[L -L -L L],1.5*[L L -L -L],[D D D D],'w')
p=patch(xc,yc,zeros(size(xc))+D+0.001,10*16/255*[1 1 1]);
p.EdgeColor='none';
h = light;
h.Style='local';
axis equal;axis off;axis([-L L -L L D 0])
ax=gca;ax.Clipping='off';
ax.Projection = 'perspective';%透视
ax.CameraViewAngle = 10;%视野范围
ax.CameraTarget = [0 0 -0.7];%目标位置
dpi=1.25;scr=[1920 1080];res=[1280 960];
fig=gcf;fig.Color='k';
fig.Position = [(scr-res)/2,res]/dpi;% 居中显示
rotate3d on
%% 动画
mv = 1;% 是否录制视频，1录制，其它不录制
if mv == 1
v = VideoWriter('D:\Programs\MATLAB\球面摆3D.mp4','MPEG-4');
v.Quality = 100;v.FrameRate = 60;
open(v);
end
for n=1:3000
    sp.XData=X+x(n);
    sp.YData=Y+y(n);
    sp.ZData=Z+z(n);
    sl.XData=Xl(:,:,n);
    sl.YData=Yl(:,:,n);
    sl.ZData=Zl(:,:,n);
    p.XData=xc+x(n);p.YData=yc+y(n);
    addpoints(tracks,x(n),y(n),z(n));
    addpoints(a,x(n),y(n),D);
    ax.CameraPosition = [10*sin(t(n)/20) 10*cos(t(n)/20) 6];%相机位置
    drawnow% limitrate
    if mv==1;F=getframe(fig);writeVideo(v,F);end
end
for n=3001:3:length(t)
    sp.XData=X+x(n);
    sp.YData=Y+y(n);
    sp.ZData=Z+z(n);
    sl.XData=Xl(:,:,n);
    sl.YData=Yl(:,:,n);
    sl.ZData=Zl(:,:,n);
    p.XData=xc+x(n);p.YData=yc+y(n);
    addpoints(tracks,x(n),y(n),z(n));
    addpoints(a,x(n),y(n),D);
    ax.CameraPosition = [10*sin(t(n)/20) 10*cos(t(n)/20) 6];%相机位置
    drawnow% limitrate
    if mv==1;F=getframe(fig);writeVideo(v,F);end
end
if mv == 1;close(v);end
fprintf(strcat('绘图保存用时',num2str(toc,4),'秒\n'));
%% 函数
function du = pendum(~,u,g,L)
theta=u(1);phi=u(2);dtheta=u(3);dphi=u(4);
ddtheta=-g/L*sin(theta)+dphi^2*sin(theta)*cos(theta);
ddphi=-2*dtheta*dphi*cot(theta);
du=[dtheta dphi ddtheta ddphi]';
end
function [position,isterminal,direction] = dyn_evns(~,y,L,theta)
position = y(1)/L-sin(theta)+theta*1e-5;
isterminal = 0;
direction = 1;
end
function [X,Y,Z]=line3d(A,B,R,N)
n=(A-B)';C=[A;B];
a=cross([1 1 1]',n);
b=cross(a,n);
a=R*a/norm(a);b=R*b/norm(b);
t=linspace(0,2*pi,N);
r=a*sin(t)+b*cos(t);
X=C(:,1)+r(1,:);
Y=C(:,2)+r(2,:);
Z=C(:,3)+r(3,:);
end
function [X,Y,Z,s]=qiu(x,y,z,r,n,Color)
[X,Y,Z]=sphere(n);
X=r*X;Y=r*Y;Z=r*Z;
s=surf(X+x,Y+y,Z+z);
s.FaceColor=Color;
s.EdgeColor='none';
end