Initial commit

1 files changed, 143 insertions, 0 deletions

diff --git a/sys/mwcs/mwlu.x b/sys/mwcs/mwlu.x
new file mode 100644
index 00000000..f6a606f1
--- /dev/null
+++ b/sys/mwcs/mwlu.x
@@ -0,0 +1,143 @@
+# Copyright(c) 1986 Association of Universities for Research in Astronomy Inc.
+
+include	<mach.h>
+
+# MULU -- Matrix utilities for MWCS.
+#
+#	mw_ludecompose		performs LU decomposition of a square matrix
+#	mw_lubacksub		performs backsubstitution to solve a system
+#
+# These routines are derived from routines in the book Numerical Recipes,
+# Press et. al. 1986.
+
+
+# MW_LUDECOMPOSE -- Replace an NxN matrix A by the LU decomposition of a
+# rowwise permutation of the matrix.  The LU decomposed matrix A and the
+# permutation index IX are output.  The decomposition is performed in place.
+
+procedure mw_ludecompose (a, ix, ndim)
+
+double	a[ndim,ndim]		#U matrix to be inverted; inverted matrix
+int	ix[ndim]		#O vector describing row permutation
+int	ndim			#I dimension of square matrix
+
+pointer	sp, vv
+int	d, i, j, k, imax
+double	aamax, sum, dum
+
+begin
+	call smark (sp)
+	call salloc (vv, ndim, TY_DOUBLE)
+
+	# Keep track of the number of row interchanges, odd or even (not used).
+	d = 1
+
+	# Loop over rows to get implicit scaling information.
+	do i = 1, ndim {
+	    aamax = 0.0
+	    do j = 1, ndim
+		if (abs(a[i,j]) > aamax)
+		    aamax = abs(a[i,j])
+	    if (aamax == 0.0)
+		call error (1, "singular matrix")
+	    Memd[vv+i-1] = 1.0 / aamax
+	}
+
+	# Loop over columns using Crout's method.
+	do j = 1, ndim {
+	    do i = 1, j-1 {
+		sum = a[i,j]
+		do k = 1, i-1
+		    sum = sum - a[i,k] * a[k,j]
+		a[i,j] = sum
+	    }
+
+	    # Search for the largest pivot element.
+	    aamax = 0.0
+	    do i = j, ndim {
+		sum = a[i,j]
+		do k = 1, j-1
+		    sum = sum - a[i,k] * a[k,j]
+		a[i,j] = sum
+
+		# Figure of merit for the pivot.
+		dum = Memd[vv+i-1] * abs(sum)
+		if (dum >= aamax) {
+		    imax = i
+		    aamax = dum
+		}
+	    }
+
+	    # Do we need to interchange rows?
+	    if (j != imax) {
+		# Yes, do so...
+		do k = 1, ndim {
+		    dum = a[imax,k]
+		    a[imax,k] = a[j,k]
+		    a[j,k] = dum
+		}
+		d = -d
+		Memd[vv+imax-1] = Memd[vv+j-1]
+	    }
+
+	    ix[j] = imax
+	    if (a[j,j] == 0.0)
+		a[j,j] = EPSILOND
+
+	    # Divide by the pivot element.
+	    if (j != ndim) {
+		dum = 1.0 / a[j,j]
+		do i = j+1, ndim
+		    a[i,j] = a[i,j] * dum
+	    }
+	}
+
+	call sfree (sp)
+end
+
+
+# MW_LUBACKSUB -- Solves the set of N linear equations A*X=B.  Here A is input,
+# not as the matrix A but rather as its LU decomposition, determined by the
+# routine mw_ludecompose.  IX is input as the permutation vector as returned by
+# mw_ludecompose.  B is input as the right hand side vector B, and returns with
+# the solution vector X.
+
+procedure mw_lubacksub (a, ix, b, ndim)
+
+double	a[ndim,ndim]		#I LU decomposition of the matrix A
+int	ix[ndim]		#I permutation vector for A
+double	b[ndim]			#U rhs vector; solution vector
+int	ndim			#I dimension of system
+
+int	ii, ll, i, j
+double	sum
+
+begin
+	# Do the forward substitution, unscrambling the permutation as we
+	# go.  When II is set to a positive value, it will become the index
+	# of the first nonvanishing element of B.
+
+	ii = 0
+	do i = 1, ndim {
+	    ll = ix[i]
+	    sum = b[ll]
+	    b[ll] = b[i]
+
+	    if (ii != 0) {
+		do j = ii, i-1
+		    sum = sum - a[i,j] * b[j]
+	    } else if (sum != 0)
+		ii = i
+
+	    b[i] = sum
+	}
+
+	# Now do the backsubstitution.
+	do i = ndim, 1, -1 {
+	    sum = b[i]
+	    if (i < ndim)
+		do j = i+1, ndim
+		    sum = sum - a[i,j] * b[j]
+	    b[i] = sum / a[i,i]
+	}
+end