2 files changed, 31 insertions, 0 deletions

diff --git a/lib/Numeric/LinearAlgebra/LAPACK/lapack-aux.c b/lib/Numeric/LinearAlgebra/LAPACK/lapack-aux.c
index 310f6ee..de3cc98 100644
--- a/lib/Numeric/LinearAlgebra/LAPACK/lapack-aux.c
+++ b/lib/Numeric/LinearAlgebra/LAPACK/lapack-aux.c
@@ -20,6 +20,9 @@
 #define OK return 0;
 #endif
 
+#define TRACEMAT(M) {int q; printf(" %d x %d: ",M##r,M##c); \
+                     for(q=0;q<M##r*M##c;q++) printf("%.1f ",M##p[q]); printf("\n");}
+
 #define CHECK(RES,CODE) MACRO(if(RES) return CODE;)
 
 #define BAD_SIZE 2000
@@ -814,3 +817,29 @@ int lu_l_C(KCMAT(a), DVEC(ipiv), CMAT(r)) {
     free(auxipiv);
     OK
 }
+
+
+//////////////////// LU substitution /////////////////////////
+char charN = 'N';
+
+int luS_l_R(DMAT(a), KDVEC(ipiv), KDMAT(b), DMAT(x)) {
+  integer m = ar;
+  integer n = ac;
+  integer mrhs = br;
+  integer nrhs = bc;
+
+  REQUIRES(m==n && m==mrhs && m==ipivn,BAD_SIZE);
+  integer* auxipiv = (integer*)malloc(n*sizeof(integer));
+  int k;
+  for (k=0; k<n; k++) {
+    auxipiv[k] = (integer)ipivp[k];
+  }
+  integer res;
+  memcpy(xp,bp,mrhs*nrhs*sizeof(double));
+  integer ldb = mrhs;
+  integer lda = n;
+  dgetrs_ (&charN,&n,&nrhs,ap,&lda,auxipiv,xp,&ldb,&res);
+  CHECK(res,res);
+  free(auxipiv);
+  OK
+}
diff --git a/lib/Numeric/LinearAlgebra/LAPACK/lapack-aux.h b/lib/Numeric/LinearAlgebra/LAPACK/lapack-aux.h
index 79e52be..e98e9dc 100644
--- a/lib/Numeric/LinearAlgebra/LAPACK/lapack-aux.h
+++ b/lib/Numeric/LinearAlgebra/LAPACK/lapack-aux.h
@@ -84,3 +84,5 @@ int schur_l_C(KCMAT(a), CMAT(u), CMAT(s));
 
 int lu_l_R(KDMAT(a), DVEC(ipiv), DMAT(r));
 int lu_l_C(KCMAT(a), DVEC(ipiv), CMAT(r));
+
+int luS_l_R(DMAT(a), KDVEC(ipiv), KDMAT(b), DMAT(x));