54 lines
2.0 KiB
Fortran
54 lines
2.0 KiB
Fortran
C $Id: MATIN4.f,v 1.1 2009/06/09 21:51:51 daven Exp $
|
|
SUBROUTINE MATIN4 (A)
|
|
C-----------------------------------------------------------------------
|
|
C invert 4x4 matrix A(4,4) in place with L-U decomposition (mjp, old...)
|
|
C-----------------------------------------------------------------------
|
|
IMPLICIT NONE
|
|
REAL*8 A(4,4)
|
|
C---SETUP L AND U
|
|
A(2,1) = A(2,1)/A(1,1)
|
|
A(2,2) = A(2,2)-A(2,1)*A(1,2)
|
|
A(2,3) = A(2,3)-A(2,1)*A(1,3)
|
|
A(2,4) = A(2,4)-A(2,1)*A(1,4)
|
|
A(3,1) = A(3,1)/A(1,1)
|
|
A(3,2) = (A(3,2)-A(3,1)*A(1,2))/A(2,2)
|
|
A(3,3) = A(3,3)-A(3,1)*A(1,3)-A(3,2)*A(2,3)
|
|
A(3,4) = A(3,4)-A(3,1)*A(1,4)-A(3,2)*A(2,4)
|
|
A(4,1) = A(4,1)/A(1,1)
|
|
A(4,2) = (A(4,2)-A(4,1)*A(1,2))/A(2,2)
|
|
A(4,3) = (A(4,3)-A(4,1)*A(1,3)-A(4,2)*A(2,3))/A(3,3)
|
|
A(4,4) = A(4,4)-A(4,1)*A(1,4)-A(4,2)*A(2,4)-A(4,3)*A(3,4)
|
|
C---INVERT L
|
|
A(4,3) = -A(4,3)
|
|
A(4,2) = -A(4,2)-A(4,3)*A(3,2)
|
|
A(4,1) = -A(4,1)-A(4,2)*A(2,1)-A(4,3)*A(3,1)
|
|
A(3,2) = -A(3,2)
|
|
A(3,1) = -A(3,1)-A(3,2)*A(2,1)
|
|
A(2,1) = -A(2,1)
|
|
C---INVERT U
|
|
A(4,4) = 1.D0/A(4,4)
|
|
A(3,4) = -A(3,4)*A(4,4)/A(3,3)
|
|
A(3,3) = 1.D0/A(3,3)
|
|
A(2,4) = -(A(2,3)*A(3,4)+A(2,4)*A(4,4))/A(2,2)
|
|
A(2,3) = -A(2,3)*A(3,3)/A(2,2)
|
|
A(2,2) = 1.D0/A(2,2)
|
|
A(1,4) = -(A(1,2)*A(2,4)+A(1,3)*A(3,4)+A(1,4)*A(4,4))/A(1,1)
|
|
A(1,3) = -(A(1,2)*A(2,3)+A(1,3)*A(3,3))/A(1,1)
|
|
A(1,2) = -A(1,2)*A(2,2)/A(1,1)
|
|
A(1,1) = 1.D0/A(1,1)
|
|
C---MULTIPLY (U-INVERSE)*(L-INVERSE)
|
|
A(1,1) = A(1,1)+A(1,2)*A(2,1)+A(1,3)*A(3,1)+A(1,4)*A(4,1)
|
|
A(1,2) = A(1,2)+A(1,3)*A(3,2)+A(1,4)*A(4,2)
|
|
A(1,3) = A(1,3)+A(1,4)*A(4,3)
|
|
A(2,1) = A(2,2)*A(2,1)+A(2,3)*A(3,1)+A(2,4)*A(4,1)
|
|
A(2,2) = A(2,2)+A(2,3)*A(3,2)+A(2,4)*A(4,2)
|
|
A(2,3) = A(2,3)+A(2,4)*A(4,3)
|
|
A(3,1) = A(3,3)*A(3,1)+A(3,4)*A(4,1)
|
|
A(3,2) = A(3,3)*A(3,2)+A(3,4)*A(4,2)
|
|
A(3,3) = A(3,3)+A(3,4)*A(4,3)
|
|
A(4,1) = A(4,4)*A(4,1)
|
|
A(4,2) = A(4,4)*A(4,2)
|
|
A(4,3) = A(4,4)*A(4,3)
|
|
RETURN
|
|
END
|