clc
clear all
%%
T_n=50; % natural period
dt=0.05; % time step size
t=0:dt:T_n; % total length of time
n=length(t)-1; % number of time steps
Tol2=1e-8;
Tol1=1e-8; % convergence criteria
%% Determine which special case to use: constant avg. vs. linear
if dt/T_n<=0.551 % Use linear accel. method - closest to theoretical
gamma=0.5;
beta=1/6;
else % Use constant avg. accel. method - unconditionally stable
gamma=0.5;
beta=0.25;
end
%% Establish system properties
omega1=0.5; % natural angular frequency
omega2=0.8;
M=[1, 0; 0 , 2]; % mass
K1=3;
K2=2;
C1=0.1;
C2=0.2; % damping constant
%% Input excitation function
P=zeros(2,n);
P(1,(t<=T_n))=((11*sin(t(t<=T_n)/2))/4 - (2*cos((4*t(t<=T_n))/5))/25 + cos(t(t<=T_n)/2).^2/40 - 3*sin((4*t(t<=T_n))/5).^3);
P(2,(t<=T_n))=(5*sin((4*t(t<=T_n))/5).^3 - 3*sin(t(t<=T_n)/2) - (32*sin((4*t(t<=T_n))/5))/25 - cos(t(t<=T_n)/2).^2/40 - (2*cos((4*t(t<=T_n))/5))/25);
%% Establish initial conditions @ i=1
U(:,1)=[0; 0]; % Initial displacement
V(:,1)=[0.5 ;0.8]; % Intial velocity
N(:,1)=@ (U, V, i) [C1*V(1,i)^2-C1*V(2,i)+K1*U(1,i)-K1*U(2,i)^3; -C1*V(1,i).^2+C1*V(2,i)-K1*U(1,i)+K1*U(2,i)^3+K2*U(2,i)^3+C2*V(2,i)];
A(:,1)=inv(M)*(P(:,1)-N(U, V, 1)); % initaial acceleration
%% Calculations for each time step, i=0,1,2,...,n
Gs=@ (u1, v1, ud1, i) u1(:,i)-ud1(:,i)+dt^2*beta*(M^(-1)*(P(:,i)-N(U, V, i)));
Hs=@ (u1, v1, vd1, i) v1(:,i)-vd1(:,i)+dt*gamma*(M^(-1)*(P(:,i)-N(U, V, i)));
Kt=@ (U, V, i) [K1, -3*K1*U(2,i)^2 ; -K1, 3*(K2+K1)*U(2,i)^2];
Ct=@ (U, V, i) [2*C1*V(1,i), -C1; -2*C1*V(1,i), (C1+C2)];
I=eye(2);
for i=1:1
Ud(:,i+1)=U(:, i)+V(:, i)*dt+dt^2*(1/2-beta)*A(:,i);
Vd(:,i+1)=V(:, i)+dt*(1-gamma)*A(:, i);
U(:,i+1)=Ud(:,i+1);
V(:,i+1)=Vd(:, i+1);
while ((abs(U(1,i+1)-U(1,i))>Tol1) || (abs(U(1,i+1)-U(1,i))>Tol1)||(abs(V(1,i+1)-V(1,i))>=Tol2)||(abs(V(2,i+1)-V(2,i))>=Tol2));
d(:,i+1)=[U(:,i+1);V(:,i+1)]+[(I-dt^2*beta*inv(M)*Kt(U(:,i+1), V(:,i+1), i)), (-dt^2*beta*inv(M)*Ct(U(:,i+1), V(:,i+1), i)); (-dt*gamma*inv(M)*Kt(U(:,i+1), V(:,i+1), i)), (I-dt*gamma*inv(M)*Ct(U(:,i+1), V(:,i+1), i))]\[Gs(U(:,i+1), V(:,i+1), Ud(:,i+1),1);Hs(U(:, i+1), V(:,i+1), Vd(:, i+1), 1)] ;
U(:,i+1)=[d(1,i+1) ;d(2,i+1)];
V(:,i+1)=[d(3,i+1) ;d(4,i+1)];
end
A(:,i+1)=M^-1*(P(:,i+1)-N(U(:,i+1), V(:,i+1), 1));
end
