;------------------------------------------------------------------
; FINITE ELEMENT (RODS) SOLVER
; Making constructions with rods, under constraint of gravity.
; 
; ARGUMENTS:
;
;  x0    X-xoordinates of nodes in relaxed body (NNODE-array)
;  y0    Y-xoordinates of nodes in relaxed body (NNODE-array)
;  x     X-xoordinates of nodes after earlier iterations (NNODE-array)
;  y     Y-xoordinates of nodes after earlier iterations (NNODE-array)
;  ixfix Indices of nodes for which the x-coordinate should be fixed
;  iyfix Indices of nodes for which the y-coordinate should be fixed
;  ii1   Index of left node of FE (NEL-array) 
;  ii2   Index of right node of FE (NEL-array) 
;  r     Radius of rod (NEL-array)
;  e     Elasticity of rod (NEL-array)
;  rho   Density of rod (NEL-array)
;  g     Gravitational constant (in y-direction)
;------------------------------------------------------------------
function fem2d,x0,y0,x,y,ixfix,iyfix,ii1,ii2,r,e,rho,g,$
               matrix=matrix,rhs=rhs,result=result
nnode = n_elements(x0)
if n_elements(y0) ne nnode then stop
if n_elements(x) ne nnode then stop
if n_elements(y) ne nnode then stop
nel = n_elements(ii1) 
if n_elements(ii2) ne nel then stop
if n_elements(r) ne nel then stop
if n_elements(e) ne nel then stop
if n_elements(rho) ne nel then stop
if n_elements(ixfix)+n_elements(iyfix) lt 3 then stop
;
; Make matrix and right-hand-side vector
;
matrix = dblarr(2*nnode,2*nnode)
rhs    = dblarr(2*nnode)
;
; Fill matrix
;
for iel=0,nel-1 do begin
    i1  = ii1[iel]
    i2  = ii2[iel]
    ;;
    ;; Calculate length, mass and K
    ;;
    l0  = sqrt((x0[i2]-x0[i1])^2+(y0[i2]-y0[i1])^2)
    a   = !dpi*r[iel]^2
    m   = a*l0*rho[iel]
    k   = a*e[iel]/l0
    ;;
    ;; Projection of K
    ;;
    l   = sqrt((x[i2]-x[i1])^2+(y[i2]-y[i1])^2)
    kxx = k*(x[i2]-x[i1])^2/l^2
    kxy = k*(x[i2]-x[i1])*(y[i2]-y[i1])/l^2
    kyy = k*(y[i2]-y[i1])^2/l^2
    ;;
    ;; Add to matrix
    ;;
    matrix[2*i1,2*i1]     = matrix[2*i1,2*i1]     + kxx
    matrix[2*i1,2*i1+1]   = matrix[2*i1,2*i1+1]   + kxy
    matrix[2*i1+1,2*i1]   = matrix[2*i1+1,2*i1]   + kxy
    matrix[2*i1+1,2*i1+1] = matrix[2*i1+1,2*i1+1] + kyy
    matrix[2*i2,2*i2]     = matrix[2*i2,2*i2]     + kxx
    matrix[2*i2,2*i2+1]   = matrix[2*i2,2*i2+1]   + kxy
    matrix[2*i2+1,2*i2]   = matrix[2*i2+1,2*i2]   + kxy
    matrix[2*i2+1,2*i2+1] = matrix[2*i2+1,2*i2+1] + kyy
    matrix[2*i1,2*i2]     = matrix[2*i1,2*i2]     - kxx
    matrix[2*i1,2*i2+1]   = matrix[2*i1,2*i2+1]   - kxy
    matrix[2*i1+1,2*i2]   = matrix[2*i1+1,2*i2]   - kxy
    matrix[2*i1+1,2*i2+1] = matrix[2*i1+1,2*i2+1] - kyy
    matrix[2*i2,2*i1]     = matrix[2*i2,2*i1]     - kxx
    matrix[2*i2,2*i1+1]   = matrix[2*i2,2*i1+1]   - kxy
    matrix[2*i2+1,2*i1]   = matrix[2*i2+1,2*i1]   - kxy
    matrix[2*i2+1,2*i1+1] = matrix[2*i2+1,2*i1+1] - kyy
    ;;
    ;; Add gravity to right-hand-side
    ;;
    rhs[2*i1+1] = rhs[2*i1+1] - 0.5*g*m
    rhs[2*i2+1] = rhs[2*i2+1] - 0.5*g*m
endfor
;
; Add the boundary conditions
;
for i=0,n_elements(ixfix)-1 do begin
    for ii=0,2*nnode-1 do matrix[2*ixfix[i],ii]=0.d0
    matrix[2*ixfix[i],2*ixfix[i]]=1.d0
    rhs[2*ixfix[i]]=0.d0
endfor
for i=0,n_elements(iyfix)-1 do begin
    for ii=0,2*nnode-1 do matrix[2*iyfix[i]+1,ii]=0.d0
    matrix[2*iyfix[i]+1,2*iyfix[i]+1]=1.d0
    rhs[2*iyfix[i]+1]=0.d0
endfor
;
; Solve the matrix equation
;
ludc,matrix,index,/column
result = lusol(matrix,index,rhs,/double,/column)
;
; Extract solution
;
dx = dblarr(nnode)
dy = dblarr(nnode)
for inode=0,nnode-1 do dx[inode]=result[2*inode]
for inode=0,nnode-1 do dy[inode]=result[2*inode+1]
;
return,{dx:dx,dy:dy}
end