%%%% Truss 2D FEM sample 20251205 for MATLAB
clear all;
%%% DOF definitions:
NDOF=2;
%%% Node definitions:
L=1000.0;
numnp=4;
node(1,:)=[   0         0    ];
node(2,:)=[-1.5*L sqrt(3)/2*L];
node(3,:)=[ 0.5*L sqrt(3)/2*L];
node(4,:)=[   0   sqrt(3)/2*L];
fprintf("///node :: numnp %d\n",numnp);
for i=1:numnp
	fprintf("%d %d %d\n",i,node(i,1),node(i,2));
end
%%% Element definitions:
nume=3;
element(1,:)=[ 1    2];
element(2,:)=[ 1    3];
element(3,:)=[ 1    4];
fprintf("///element :: nume=%d\n",nume);
for i=1:nume
	fprintf("%d %d %d\n",i,element(i,1),element(i,2))
end
%%% Material definitions:
E=100e3;
A=100;
fprintf("///material :: E=%e, A=%e\n",E,A);
%%% SPC definitions:
nspc=6;
spc(1,:)=[2 1];
spc(2,:)=[2 2];
spc(3,:)=[3 1];
spc(4,:)=[3 2];
spc(5,:)=[4 1];
spc(6,:)=[4 2];
fprintf("///spc :: nspc=%d\n",nspc);
for i=1:nspc
	fprintf("%d %d %d\n",i,spc(i,1),spc(i,2))
end
%%% load definitions:
ncload=1;
cload(1,:)=[1 2];vcload(1)=-10000;
fprintf("///cload :: ncload=%d\n",ncload);
for i=1:ncload
	fprintf("%d %d %d %e\n",i,cload(i,1),cload(i,2),vcload(i))
end
%%% Global stiffness
KG=zeros(NDOF*numnp,NDOF*numnp);
%%% Loop over elements
for ie=1:nume
%%% Local elememt connectivity
	n1=element(ie,1);
	n2=element(ie,2);
%%% Local elememt stiffness element
	kL=kq_t2d2(node(n1,:),node(n2,:),E,A);
%%% Add element 1 to Global stiffness 
	KG(2*n1-1,2*n1-1)=KG(2*n1-1,2*n1-1)+kL(1,1);
	KG(2*n1-1,2*n1  )=KG(2*n1-1,2*n1  )+kL(1,2);
	KG(2*n1-1,2*n2-1)=KG(2*n1-1,2*n2-1)+kL(1,3);
	KG(2*n1-1,2*n2  )=KG(2*n1-1,2*n2  )+kL(1,4);
	KG(2*n1  ,2*n1-1)=KG(2*n1  ,2*n1-1)+kL(2,1);
	KG(2*n1  ,2*n1  )=KG(2*n1  ,2*n1  )+kL(2,2);
	KG(2*n1  ,2*n2-1)=KG(2*n1  ,2*n2-1)+kL(2,3);
	KG(2*n1  ,2*n2  )=KG(2*n1  ,2*n2  )+kL(2,4);
	KG(2*n2-1,2*n1-1)=KG(2*n2-1,2*n1-1)+kL(3,1);
	KG(2*n2-1,2*n1  )=KG(2*n2-1,2*n1  )+kL(3,2);
	KG(2*n2-1,2*n2-1)=KG(2*n2-1,2*n2-1)+kL(3,3);
	KG(2*n2-1,2*n2  )=KG(2*n2-1,2*n2  )+kL(3,4);
	KG(2*n2  ,2*n1-1)=KG(2*n2  ,2*n1-1)+kL(4,1);
	KG(2*n2  ,2*n1  )=KG(2*n2  ,2*n1  )+kL(4,2);
	KG(2*n2  ,2*n2-1)=KG(2*n2  ,2*n2-1)+kL(4,3);
	KG(2*n2  ,2*n2  )=KG(2*n2  ,2*n2  )+kL(4,4);
end
%%% constrained conditions
count=1;
for i=1:nspc
	npos=2*(spc(i,1)-1)+spc(i,2);
	row(count,:)=KG(npos,:);
	KG(npos,:)=0;KG(:,npos)=0;KG(npos,npos)=1;
	count=count+1;
end
%%% Load vector
F=zeros(NDOF*numnp,1);
for i=1:ncload
	npos=2*(cload(i,1)-1)+cload(i,2);
	F(npos)=vcload(i);
end
%%% computation 
U=KG\F;
fprintf("//Result displacement\n")
for i=1:numnp
	fprintf("%d %e %e\n",i,U(2*i-1),U(2*i))
end
%% post processing :: stress
fprintf("Element stress\n")
for ie=1:nume
	n1=element(ie,1);
	n2=element(ie,2);
	u1(1)=U(2*n1-1);
	u1(2)=U(2*n1);
	u2(1)=U(2*n2-1);
	u2(2)=U(2*n2);
	e_stress(ie)=rcv_t2d2(node(n1,:),node(n2,:),E,A,u1,u2);
	fprintf("%d %e\n",ie,e_stress(ie))
end
%% post processing :: reaction force
fprintf("Reaction force\n")
count=1;
for i=1:nspc
	f=0.0;
	for k=1:2*numnp
		f=f+row(count,k)*U(k);
	end
	count=count+1;
	fprintf("%d %d %e\n",spc(i,1),spc(i,2),f)
end
%%% for visualization
fid=fopen('vtk.vtk','w');
writeVTKtruss2d2(fid,NDOF,numnp,nume,node,element,U,e_stress);
fclose(fid);
%%% finished
%%%% Define function :: T2D2
function stf=kq_t2d2(p1,p2,ee,aa)
	v(1)=p2(1)-p1(1);
	v(2)=p2(2)-p1(2);
	length=sqrt( v(1)^2 + v(2)^2 );
	c=v(1)/length;
	s=v(2)/length;
	k=ee*aa/length;
	stf(1,1)= k*c*c;
	stf(1,2)= k*c*s;
	stf(1,3)=-k*c*c;
	stf(1,4)=-k*s*c;
	stf(2,1)= k*c*s;
	stf(2,2)= k*s*s;
	stf(2,3)=-k*s*c;
	stf(2,4)=-k*s*s;
	stf(3,1)=-k*c*c;
	stf(3,2)=-k*c*s;
	stf(3,3)= k*c*c;
	stf(3,4)= k*c*s;
	stf(4,1)=-k*c*s;
	stf(4,2)=-k*s*s;
	stf(4,3)= k*c*s;
	stf(4,4)= k*s*s;
end
%%% stress recovery
function sgm=rcv_t2d2(p1,p2,ee,aa,u1,u2)
	v(1)=p2(1)-p1(1);
	v(2)=p2(2)-p1(2);
	length=sqrt( v(1)^2 + v(2)^2 );
	c=v(1)/length;
	s=v(2)/length;
	sgm=ee/length*(-c*u1(1)-s*u1(2)+c*u2(1)+s*u2(2));
end
%%% VTK :: writeVTK
function writeVTKtruss2d2(fid,NDOF,numnp,nume,node,element,U,e_stress)
%%% VTK_CELL_TYPE 
	vtk_cell_type=3;
%%% NVARS
	NVARS=1; 
	vname=char("S11");
%%% NN
	NN=2;
%%% Header 
	fprintf(fid,"# vtk DataFile Version 2.0\n");
%% fprintf(fid,"%s\n",program_name());
	fprintf(fid,"vtk.vtk\n");
	fprintf(fid,"ASCII\n");
%%% Node 
	fprintf(fid,"DATASET UNSTRUCTURED_GRID\n");
	fprintf(fid,"POINTS %d float\n",numnp);
	for i=1:numnp
		xc(1)=0.0;xc(2)=0.0;xc(3)=0.0;
		for j=1:NN
			xc(j)=node(i,j);
		end
		fprintf(fid,"%e %e %e\n",xc(1),xc(2),xc(3));
	end
%%% Element
	fprintf(fid,"CELLS %d %d\n",nume,nume*(1+NN));
	for i=1:nume
		for j=1:NN
			nid(j)=element(i,j);
		end
		fprintf(fid,"%d ",NN);
		for j=1:NN
			fprintf(fid,"%d ",nid(j)-1);
		end
		fprintf(fid,"\n");
	end
%%% type
	fprintf(fid,"CELL_TYPES %d\n",nume);
	for i=1:nume
		fprintf(fid,"%d\n",vtk_cell_type);
	end
%%% Result for NODE
	fprintf(fid,"POINT_DATA %d\n",numnp);
%%% Deformation 
	fprintf(fid,"VECTORS DISPLACEMENT float\n");
	for i=1:numnp
		fprintf(fid," %e %e 0.0\n",U(2*i-1),U(2*i));
	end
%%% Result for ELEMENT
	fprintf(fid,"CELL_DATA %d\n",nume);
%%% Element Stress 
	for i=1:NVARS
		fprintf(fid,"SCALARS %s float 1\n",vname(i,:));
		fprintf(fid,"LOOKUP_TABLE default\n");
		for j=1:nume
			fprintf(fid,"%e\n",e_stress(j));
		end
	end
end
