// Funciones Analisis por Elementos Finitos

function A=kmatriz(i,n,C,lm)
// Crea matriz A de rigidez de cada elemento
// i : Elemento
// n : Numero de nodos
// C : Matriz de conectividad para problemas de armaduras
// lm : Matriz de coseņos directores problemas de armaduras
//
// A = kmatriz(i,n) para elementos BEAM
// A = kamtriz(i,n,C,lm) para problemas de armaduras
//
// Creada por curso de elementos finitos B/2005

[%nargout,%nargin] = argn(0)

if %nargin == 2
    A=zeros(n,n);
    A(i,i)=1;
    A(i,i+1)=-1;
    A(i+1,i)=-1;
    A(i+1,i+1)=1;
else
    A=zeros(2*n,2*n);
    n2=C(i,2); n1=C(i,1);
    A(2*n1-1,2*n1-1)=lm(i,1)^2;
    A(2*n1-1,2*n1)=lm(i,1)*lm(i,2);
    A(2*n1-1,2*n2-1)=-lm(i,1)^2;
    A(2*n1-1,2*n2)=-lm(i,1)*lm(i,2);

    A(2*n1,2*n1-1)=lm(i,1)*lm(i,2);
    A(2*n1,2*n1)=lm(i,2)^2;
    A(2*n1,2*n2-1)=-lm(i,1)*lm(i,2);
    A(2*n1,2*n2)=-lm(i,2)^2;

    A(2*n2-1,2*n1-1)=-lm(i,1)^2;
    A(2*n2-1,2*n1)=-lm(i,1)*lm(i,2);
    A(2*n2-1,2*n2-1)=lm(i,1)^2;
    A(2*n2-1,2*n2)=lm(i,1)*lm(i,2);

    A(2*n2,2*n1-1)=-lm(i,1)*lm(i,2);
    A(2*n2,2*n1)=-lm(i,2)^2;
    A(2*n2,2*n2-1)=lm(i,1)*lm(i,2);
    A(2*n2,2*n2)=lm(i,2)^2;
end
endfunction
// ----------------------------------------
function K=kglobal(A,E,L,n,C,lm)
// K = kglobal(A,E,L)  Construye la matriz de rigidez global en elementos TRUSS
// K = kglobal(A,E,L,n,C,lm) Contruye la matriz de rigidez global para armaduras
//
// A : Vector de area de los elementos
// E : Vector de modulo de elasticidad
// L : Vector de longitud de los elementos
// n : Numero de nodos en armaduras
// C : Matriz de conectividad en armaduras
// lm : Matriz de cosenos directores en armaduras
// A, E y L debe tener la misma dimension
//
// Creada por curso de elementos finitos B/2005

[%nargout,%nargin] = argn(0)

if %nargin == 3
    n=length(A)+1; // Numero de GDL
    K=zeros(n,n);
    for i=1:n-1
        K=K+E(i)*A(i)/L(i)*kmatriz(i,n);
    end
else
    [e,c]=size(C);
    K=zeros(2*n,2*n);
    for i=1:e // Numero de Elementos
        K=K+E(i)*A(i)/L(i)*kmatriz(i,n,C,lm);
    end
end
endfunction
// ----------------------------------------
function F = fglobal(A,L,f,T,P,t)
// F = fglobal(A,L,f,T,P)  Construye el vector de carga global
//               en elementos TRUSS
//
// A : Vector de area de los elementos
// L : Vector de longitud de los elementos
// f : Vector de densidades de los elementos
// T : Vector de Fuerza de traccion de los elementos [F/long]
// P : Vector de cargas aplicados a cada nodo
// A, E y L debe tener la misma dimension
//
// F = fglobal(sp,Pc,u,p,G,t) Construye el vector de carga global
//         para elementos Bidimensionales - Traingulos DUC
// sp : Direccion de cargas puntuales
// Pc : Valor de carga en las direcciones sp
// u : Matriz nodos de carga distibuida
// p : Matriz de cargas disstribuidas en u
//   Cada fila representa la carga en cada nodo
//   p = [p1 p2] Para cargas perpendiculares al elemento
//   p = [p1x p1y p2x p2y] Para compoenentes de la carga en cada eje
// G : Matriz de coordenadas globales
// t : Espesor de la placa

// Creada por curso de elementos finitos B/2004


[%nargout,%nargin] = argn(0);

// Para elementos unidimensionales ----------------------------
if %nargin==5

  P = P.';
  n=size(A)+1; // Numero de GDL
  F=zeros(n(1),1);

  for i=1:n-1
   F = F + (A(i)*L(i)*f(i)/2 + T(i)*L(i)/2) * fvector(i,n);
  end
   F = F + P;
// Para elementos bidimensionales -----------------------------
elseif %nargin == 6

    sp=A; Pc=L; u=f; p=T; G=P;  // Redefinir las variables
  [n,z] = size(G);  // Numero de nodos

  // Para Cargas Distibuidas
  [k,m] = size(p); // Numero de columnas en p
  [i,j] = size(u);   // Determinar el numero de elementos con carga

  Td = zeros(2*n,1); // Inicializar vector de cargas

  for a = 1:i  // Hacer ciclo para cada elemento
   X1 = G(u(a,1),2);  X2 = G(u(a,2),2);
   Y1 = G(u(a,1),3);  Y2 = G(u(a,2),3);

   le12=norm(G(u(a,2),2:3)-G(u(a,1),2:3)); // Longitud del elemento

    if m==2 // Cargas perpendiculares
        c=(Y2-Y1)/le12;  s=(X1-X2)/le12;

        Tx1=-c*p(a,1);   Tx2=-c*p(a,2);
        Ty1=-s*p(a,1);   Ty2=-s*p(a,2);
    elseif m == 4  // Para cargas sobre los ejes
        Tx1 = p(a,1);  Tx2 = p(a,3);
        Ty1 = p(a,2);  Ty2 = p(a,4);
    end

  T=(t*le12)/6*[2*Tx1+Tx2 , 2*Ty1+Ty2 , Tx1+2*Tx2 , Ty1+2*Ty2];

  Te=zeros(2*n,1);
  Te(2*u(a,1)-1) = T(1);  Te(2*u(a,1)) = T(2);
  Te(2*u(a,2)-1) = T(3);  Te(2*u(a,2)) = T(4);

  Td = Td + Te;
  end

  // Para cargas punutales
  P=zeros(2*n,1);
    for i=1:length(sp)
        a=sp(i);
        P(a)=Pc(i);
    end

  // Suma total
  F = Td + P;
end
endfunction
// ----------------------------------------
function A = fvector(i,n)
// Crea vector de unos del elemento
// i : Elemento
// n : Grados de libertad
// creada por curso de elementos finitos B/2004

A=zeros(n,1);
A(i)=1;
A(i+1)=1;
endfunction
// ----------------------------------------
function [Km,Fm] = elimina(K,F,r,a)
// [Km,Fm] = elimina(K,F,p,a)  Aplica el metodo de eliminacion de restricciones
// para elementos unidimensionales (Cap 3)
//
// K : Matriz de rigidez Global [n,n]
// F : Matriz de Carga Global [n,1]
// p : Nodos de restriccion [1,r] (r es el numero de restricciones)
// a : Valor de la restriccion en cada nodo r [1,r]
// Km : Matriz de Rigidez Global modificada [n-r,n-r]
// Fm : Vector de Carga Global modificada [n-r,1]
//
// Creada Curso de Elementos Finitos B/2005

// 1. Almacene las p_ -esimas filas de K y F
Kp=K(r,:);
Fp=F(r);
//2. Borre la p_r - esimas filas y columnas de K y F
Km = K; Fm = F;
Kpm = Kp;
for j = 1:length(r)
    Km = elim(Km,r(j)-(j-1));
    Kpm = elim(Kpm,r(j)-(j-1),'col');
    Fm = elim(Fm,r(j)-(j-1),'row');
end
// Modifica la componente F
Fm = Fm - Kpm'*a';
// Solucion del sistema
// Q = inv(Km)*Fm;
endfunction
// ----------------------------------------
function B=elim(A,n,orient)
// Funcion que elimina fila y/o columna
// A : matriz de entrada
// B : matriz de salida
// n : posicion de fila y/o columna a eliminar
// orient : opcion de eliminacion
//   B = elim(A,n) -> Borra fila y columna n de A
//   B = elim(A,n,'row') -> Borra fila n de A
//   B = elim(A,n,'col') -> Borra columna n de A
//////// CREADA POR : CURSO ELEMENTOS FINITOS B/2004

[%nargout,%nargin] = argn(0)
if %nargin==3

select orient
case 'row' then
    A(n,:) = [];
case 'col' then
    A(:,n) = [];
else
    error(['La tercera entrada debe ser un string: ''row'' o ''col''']);
end

elseif %nargin==2
    //Eliminar fila y columna simultaneamente
    A(n,:) = [];
    A(:,n) = [];
else
    error(['Faltan o sobran argumentos']);
end
B = A;
endfunction
// ----------------------------------------
function [Km,Fm]=pena(K,F,r,a,C)
// Metodo de penalizacion para aplicar restricciones
// [Km,Fm]=pena(K,F,r,a,C)
// K : Matriz de rigidez global
// F : Vector de cargas global
// r : Vector de nodos(unidimensional) o direccion(estrucuturas) de restricciones
// a : Valor de las restricciones
// C : Penalizacion :  C=max(max(K))*1e4

Km=K; Fm=F;
for i=1:length(r)
    Km(r(i),r(i))=K(r(i),r(i))+C;
    Fm(r(i))=F(r(i))+C*a(i);
end
endfunction
// ----------------------------------------
function qe=qelemen(Qt,i,C)
// Desplazamientos locales
//
// Qt : Matiz global de desplazamiento
// i : elemento
// C : Matriz de conectividad, para elementos bidimensionales
//     o para armaduras
//
// qe = qelemen(Qt,i) Para elementos unidiemnsionales
// qe = qelemen(Qt,i,C) Para elementos bidimensionales
//                       sin importar el valor i
//
// Creada por curso de elementos finitos B/2004

[%nargout,%nargin] = argn(0);

if %nargin ==2 // Elementos TRUSS
    qe(1,1)=Qt(i);
    qe(2,1)=Qt(i+1);
else
    [m,n]=size(C);
    if n == 4 // Problemas Bidimensionales
        for i=1:m  // Numero de elementos
            N1=C(i,2);N2=C(i,3);N3=C(i,4);
            qe(1,i)=Qt(2*N1-1);
            qe(2,i)=Qt(2*N1);
            qe(3,i)=Qt(2*N2-1);
            qe(4,i)=Qt(2*N2);
            qe(5,i)=Qt(2*N3-1);
            qe(6,i)=Qt(2*N3);
        end
    elseif n == 2 // Problemas de armaduras
        n2 = C(i,2); n1=C(i,1);
        qe(:,1) = Qt(2*n1-1:2*n1);
        qe([3,4],1) = Qt(2*n2-1:2*n2);
    end
end
endfunction
// ----------------------------------------
function [le,lm] = cosenos(C,G)
// [le,lm] = COSENOS(C,G) Calcula la longitud y los cosenos directores 
//    de cada elemento para problemas de Armaduras
//
// le : Vector de longitudes de los elementos
// lm : Matriz de cosenos directores
// C : Matriz de conectividad
// G : Matriz de coordenadas globales
//
// Creada por Curso Elementos Finitos B/2005

[e,c]=size(C); // numero de elementos
for i=1:e
    n2=C(i,2); n1=C(i,1);
    le(i)=norm(G(n2,:)-G(n1,:)); // Longitud elementos
    plot([G(n1,1) G(n2,1)],[G(n1,2) G(n2,2)])
    // hold on
    lm(i,:)=(G(n2,:)-G(n1,:))/le(i); // Cosenos directores
end
endfunction
// ----------------------------------------

function [K , Be , De, Ae] = k2d(C,G,t,v,E,d)
// Calcula matriz de rigidez K para elementos bidimensionales
// [K, Be , De] = k2d(C,G,t,v,E,d)
//
// C: Matriz de conectividad del elemento. de e filas y 4 columnas
//   la primera es el elemento, 2,3 y 4 columna son los nodos    
// G: Matriz de coordenadas globales. De n filas y 3 columnas
//   la primera en el nodo, 2 y 3 coordenadas x - y del mismo
// t: Espesor de la placa
// v: Modulo de Poisson 
// E: Modulo de elasticad
// d: Opciones 1 para ESFUERZO PLANO, 2 DEFORMACION UNITARIA PLANA 
// K  : Matriz de rigidez global del sistema
// Be : Matriz de rigidez de los elementos
//%%%% Curso Elementos Finitos B/2004

n = size(G); 
K=zeros(2*n(1),2*n(1));
e=size(C);
for i=1:e(1)
disp('Elemento ' + string(i)), xpause(1e4)
// Nodos 
N1=C(i,2); N2=C(i,3); N3=C(i,4); 

// Coordenadas Nodos Elemento
x1=G(N1,2);  x2=G(N2,2); x3=G(N3,2);
y1=G(N1,3);  y2=G(N2,3); y3=G(N3,3);

J=[x1-x3 y1-y3;x2-x3 y2-y3]; // Jacobiano

Ae(i)=det(J)/2; // Area Elemento

// Matriz Deformacion unitaria
Be(:,:,i)=(1/det(J))*[y2-y3 0 y3-y1 0 y1-y2 0;0 x3-x2 0 x1-x3 0 x2-x1;x3-x2 y2-y3 x1-x3 y3-y1 x2-x1 y1-y2];

// dibujar el elemento
plot([x1 x2],[y1 y2])
plot([x2 x3],[y2 y3])
plot([x3 x1],[y3 y1])
//text(x1,y1,num2str(N1))
//text(x2,y2,num2str(N2))
//text(x3,y3,num2str(N3))
// pause


if d==1
    De=(E/(1-v^2))*[1 v 0;v 1 0;0 0 (1-v)/2];  // Esfuerzo Plano
else
    // Deformacion Unitaria Plana
    De=(E/((1-v)*(1-2*v)))*[1-v v 0;v 1-v 0;0 0 (1-v)/2]; 
end

// Matriz de rigidez elemento
Ke=t*Ae(i)*Be(:,:,i)'*De*Be(:,:,i);

KG=zeros(2*n(1),2*n(1));

KG(2*N1-1,2*N1-1) = Ke(1,1);  KG(2*N1,2*N1-1) = Ke(1,2);
KG(2*N2-1,2*N1-1) = Ke(1,3);  KG(2*N2,2*N1-1) = Ke(1,4);
KG(2*N3-1,2*N1-1) = Ke(1,5);  KG(2*N3,2*N1-1) = Ke(1,6);

KG(2*N1-1,2*N1) = Ke(2,1);   KG(2*N1,2*N1) = Ke(2,2);
KG(2*N2-1,2*N1) = Ke(2,3);   KG(2*N2,2*N1) = Ke(2,4);
KG(2*N3-1,2*N1) = Ke(2,5);   KG(2*N3,2*N1) = Ke(2,6);

KG(2*N1-1,2*N2-1) = Ke(3,1);  KG(2*N1,2*N2-1) = Ke(3,2);
KG(2*N2-1,2*N2-1) = Ke(3,3);  KG(2*N2,2*N2-1) = Ke(3,4);
KG(2*N3-1,2*N2-1) = Ke(3,5);  KG(2*N3,2*N2-1) = Ke(3,6);

KG(2*N1-1,2*N2) = Ke(4,1);  KG(2*N1,2*N2) = Ke(4,2);
KG(2*N2-1,2*N2) = Ke(4,3);  KG(2*N2,2*N2) = Ke(4,4);
KG(2*N3-1,2*N2) = Ke(4,5);  KG(2*N3,2*N2) = Ke(4,6);

KG(2*N1-1,2*N3-1) = Ke(5,1);  KG(2*N1,2*N3-1) = Ke(5,2);
KG(2*N2-1,2*N3-1) = Ke(5,3);  KG(2*N2,2*N3-1) = Ke(5,4);
KG(2*N3-1,2*N3-1) = Ke(5,5);  KG(2*N3,2*N3-1) = Ke(5,6);

KG(2*N1-1,2*N3) = Ke(6,1);  KG(2*N1,2*N3) = Ke(6,2);
KG(2*N2-1,2*N3) = Ke(6,3);  KG(2*N2,2*N3) = Ke(6,4);
KG(2*N3-1,2*N3) = Ke(6,5);  KG(2*N3,2*N3) = Ke(6,6);

K = K+KG;
end
endfunction

// --------------------------------------------------------
function [qe,sig,sigVM] = esf(Q,C,G,De,Be,d)
// Determina esfuerzos por cada elemento
//  para elementos bidimensionales
// [qe,sig,sigVM] = esf(Q,C,G,De,Be,d)
//
// qe : Desplazamiento de los nodos por cada elemento
// sig : Esfuerzo por cada elemento
// sigVM : Esfuerzo de VonMieses
// Q : Desplazamiento en cada nodo
// C : Conectividad
// G : Coordenadas Globales

e = size(C);  // Numero de elementos
// Encontrar los desplazamiento locales
qe = qelemen(Q,1,C);


// Determinar los esfuerzos en cada elemento
for i=1:e(1)
    sig(:,i)=De*Be(:,:,i)*qe(:,i);
end

// calcular esfuerzo de Von-Mieses
if d==1 // Esfuerzo plano
    I1=sig(1,:)+sig(2,:);
    I2=sig(1,:).*sig(2,:)-sig(3,:).^2;
else
    sigZ=v*(sig(1,:)+sig(2,:));
    I1=sig(1,:)+sig(2,:)+sigZ;
    I2=sig(1,:).*sig(2,:)+sig(1,:).*sigZ+sig(2,:).*sigZ-sig(3,:).^2;
end

sigVM=sqrt(I1.^2-3*I2);

// grafica de triangulos deformados

for i=1:e(1)
  X=G(C(i,[2:$]),2); X($+1) = X(1);
  Y=G(C(i,[2:$]),3); Y($+1) = Y(1);
  plot2d(X,Y,2)
  Xm=G(C(i,[2:$]),2)+Q(C(i,[2:$])*2-1);   Xm($+1) = Xm(1);
  Ym=G(C(i,[2:$]),3)+Q(C(i,[2:$])*2);   Ym($+1) = Ym(1);
  plot2d(Xm,Ym,5)
end

endfunction