# HG changeset patch
# User dbateman
# Date 1142532147 0
# Node ID a6bfe12c4c720b955d4096cf9221f13d5c2a6dda
# Parent  cbc323d109e4a9e893b9af5f4a6e58b17fe9f75d
[project @ 2006-03-16 18:02:27 by dbateman]

diff --git a/liboctave/sparse-dmsolve.cc b/liboctave/sparse-dmsolve.cc
new file mode 100644
--- /dev/null
+++ b/liboctave/sparse-dmsolve.cc
@@ -0,0 +1,507 @@
+/*
+
+Copyright (C) 2006 David Bateman
+
+Octave is free software; you can redistribute it and/or modify it
+under the terms of the GNU General Public License as published by the
+Free Software Foundation; either version 2, or (at your option) any
+later version.
+
+Octave is distributed in the hope that it will be useful, but WITHOUT
+ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License
+for more details.
+
+You should have received a copy of the GNU General Public License
+along with this program; see the file COPYING.  If not, write to the
+Free Software Foundation, Inc., 51 Franklin Street, Fifth Floor,
+Boston, MA 02110-1301, USA.
+
+*/
+
+#ifdef HAVE_CONFIG_H
+#include <config.h>
+#endif
+
+
+
+#include "ov-re-sparse.h"
+#include "ov-cx-sparse.h"
+#include "MArray2.h"
+#include "MSparse.h"
+#include "SparseQR.h"
+#include "SparseCmplxQR.h"
+#include "SparseType.h"
+#include "oct-sort.h"
+
+template <class T>
+static MSparse<T>
+dmsolve_extract (const MSparse<T> &A, const octave_idx_type *Pinv, 
+		const octave_idx_type *Q, octave_idx_type rst, 
+		octave_idx_type rend, octave_idx_type cst, 
+		octave_idx_type cend, octave_idx_type maxnz = -1,
+		bool lazy = false)
+{
+  octave_idx_type nz = (rend - rst) * (cend - cst);
+  maxnz = (maxnz < 0 ? A.nnz () : maxnz);
+  MSparse<T> B (rend - rst, cend - cst, (nz < maxnz ? nz : maxnz));
+  // Some sparse functions can support lazy indexing (where elements
+  // in the row are in no particular order), even though octave in 
+  // general can't. For those functions that can using it is a big 
+  // win here in terms of speed.
+  if (lazy)
+    {
+      nz = 0;
+      for (octave_idx_type j = cst ; j < cend ; j++)
+	{
+	  octave_idx_type qq = (Q ? Q [j] : j);
+	  B.xcidx (j - cst) = nz;
+	  for (octave_idx_type p = A.cidx(qq) ; p < A.cidx (qq+1) ; p++)
+	    {
+	      OCTAVE_QUIT;
+	      octave_idx_type r = (Pinv ? Pinv [A.ridx (p)] : A.ridx (p));
+	      if (r >= rst && r < rend)
+		{
+		  B.xdata (nz) = A.data (p);
+		  B.xridx (nz++) =  r - rst ;
+		}
+	    }
+	}
+      B.xcidx (cend - cst) = nz ;
+    }
+  else
+    {
+      OCTAVE_LOCAL_BUFFER (T, X, rend - rst);
+      octave_sort<octave_idx_type> sort;
+      octave_idx_type *ri = B.xridx();
+      nz = 0;
+      for (octave_idx_type j = cst ; j < cend ; j++)
+	{
+	  octave_idx_type qq = (Q ? Q [j] : j);
+	  B.xcidx (j - cst) = nz;
+	  for (octave_idx_type p = A.cidx(qq) ; p < A.cidx (qq+1) ; p++)
+	    {
+	      OCTAVE_QUIT;
+	      octave_idx_type r = (Pinv ? Pinv [A.ridx (p)] : A.ridx (p));
+	      if (r >= rst && r < rend)
+		{
+		  X [r-rst] = A.data (p);
+		  B.xridx (nz++) =  r - rst ;
+		}
+	    }
+	  sort.sort (ri + B.xcidx (j - cst), nz - B.xcidx (j - cst));
+	  for (octave_idx_type p = B.cidx (j - cst); p < nz; p++)
+	    B.xdata (p) = X [B.xridx (p)]; 
+	}
+      B.xcidx (cend - cst) = nz ;
+    }
+
+  return B;
+}
+
+#if !defined (CXX_NEW_FRIEND_TEMPLATE_DECL)
+static MSparse<double>
+dmsolve_extract (const MSparse<double> &A, const octave_idx_type *Pinv, 
+		const octave_idx_type *Q, octave_idx_type rst, 
+		octave_idx_type rend, octave_idx_type cst, 
+		octave_idx_type cend, octave_idx_type maxnz,
+		bool lazy);
+
+static MSparse<Complex>
+dmsolve_extract (const MSparse<Complex> &A, const octave_idx_type *Pinv, 
+		const octave_idx_type *Q, octave_idx_type rst, 
+		octave_idx_type rend, octave_idx_type cst, 
+		octave_idx_type cend, octave_idx_type maxnz,
+		bool lazy);
+#endif
+
+template <class T>
+static MArray2<T>
+dmsolve_extract (const MArray2<T> &m, const octave_idx_type *, 
+		 const octave_idx_type *, octave_idx_type r1, 
+		 octave_idx_type r2, octave_idx_type c1, 
+		 octave_idx_type c2)
+{
+  r2 -= 1;
+  c2 -= 1;
+  if (r1 > r2) { octave_idx_type tmp = r1; r1 = r2; r2 = tmp; }
+  if (c1 > c2) { octave_idx_type tmp = c1; c1 = c2; c2 = tmp; }
+
+  octave_idx_type new_r = r2 - r1 + 1;
+  octave_idx_type new_c = c2 - c1 + 1;
+
+  MArray2<T> result (new_r, new_c);
+
+  for (octave_idx_type j = 0; j < new_c; j++)
+    for (octave_idx_type i = 0; i < new_r; i++)
+      result.xelem (i, j) = m.elem (r1+i, c1+j);
+
+  return result;
+}
+
+#if !defined (CXX_NEW_FRIEND_TEMPLATE_DECL)
+static MArray2<double>
+dmsolve_extract (const MArray2<double> &m, const octave_idx_type *, 
+		 const octave_idx_type *, octave_idx_type r1, 
+		 octave_idx_type r2, octave_idx_type c1, 
+		 octave_idx_type c2)
+
+static MArray2<Complex>
+dmsolve_extract (const MArray2<Complex> &m, const octave_idx_type *, 
+		 const octave_idx_type *, octave_idx_type r1, 
+		 octave_idx_type r2, octave_idx_type c1, 
+		 octave_idx_type c2)
+#endif
+
+template <class T>
+static void
+dmsolve_insert (MArray2<T> &a, const MArray2<T> &b, const octave_idx_type *Q,
+	       octave_idx_type r, octave_idx_type c)
+{
+  T *ax = a.fortran_vec();
+  const T *bx = b.fortran_vec();
+  octave_idx_type anr = a.rows();
+  octave_idx_type nr = b.rows();
+  octave_idx_type nc = b.cols();
+  for (octave_idx_type j = 0; j < nc; j++)
+    {
+      octave_idx_type aoff = (c + j) * anr;
+      octave_idx_type boff = j * nr;
+      for (octave_idx_type i = 0; i < nr; i++)
+	{
+	  OCTAVE_QUIT;
+	  ax [Q [r + i] + aoff] = bx [i + boff];
+	}
+    }
+}
+
+#if !defined (CXX_NEW_FRIEND_TEMPLATE_DECL)
+static void
+dmsolve_insert (MArray2<double> &a, const MArray2<double> &b, 
+	       const octave_idx_type *Q, octave_idx_type r, octave_idx_type c);
+
+static void
+dmsolve_insert (MArray2<Complex> &a, const MArray2<Complex> &b,
+	       const octave_idx_type *Q, octave_idx_type r, octave_idx_type c);
+#endif
+
+template <class T>
+static void
+dmsolve_insert (MSparse<T> &a, const MSparse<T> &b, const octave_idx_type *Q,
+	       octave_idx_type r, octave_idx_type c)
+{
+  octave_idx_type b_rows = b.rows ();
+  octave_idx_type b_cols = b.cols ();
+  octave_idx_type nr = a.rows ();
+  octave_idx_type nc = a.cols ();
+
+  OCTAVE_LOCAL_BUFFER (octave_idx_type, Qinv, nr);
+  for (octave_idx_type i = 0; i < nr; i++)
+    Qinv [Q [i]] = i;
+
+  // First count the number of elements in the final array
+  octave_idx_type nel = a.xcidx(c) + b.nnz ();
+
+  if (c + b_cols < nc)
+    nel += a.xcidx(nc) - a.xcidx(c + b_cols);
+
+  for (octave_idx_type i = c; i < c + b_cols; i++)
+    for (octave_idx_type j = a.xcidx(i); j < a.xcidx(i+1); j++)
+      if (Qinv [a.xridx(j)] < r || Qinv [a.xridx(j)] >= r + b_rows)
+	nel++;
+
+  OCTAVE_LOCAL_BUFFER (T, X, nr);
+  octave_sort<octave_idx_type> sort;
+  MSparse<T> tmp (a);
+  a = MSparse<T> (nr, nc, nel);
+  octave_idx_type *ri = a.xridx();
+
+  for (octave_idx_type i = 0; i < tmp.cidx(c); i++)
+    {
+      a.xdata(i) = tmp.xdata(i);
+      a.xridx(i) = tmp.xridx(i);
+    }
+  for (octave_idx_type i = 0; i < c + 1; i++)
+    a.xcidx(i) = tmp.xcidx(i);
+
+  octave_idx_type ii = a.xcidx(c);
+
+  for (octave_idx_type i = c; i < c + b_cols; i++)
+    {
+      OCTAVE_QUIT;
+
+      for (octave_idx_type j = tmp.xcidx(i); j < tmp.xcidx(i+1); j++)
+	if (Qinv [tmp.xridx(j)] < r || 	Qinv [tmp.xridx(j)] >= r + b_rows)
+	  {
+	    X [tmp.xridx(j)] = tmp.xdata(j);
+	    a.xridx(ii++) = tmp.xridx(j);
+	  }
+
+      OCTAVE_QUIT;
+
+      for (octave_idx_type j = b.cidx(i-c); j < b.cidx(i-c+1); j++)
+	{
+	  X [Q [r + b.ridx(j)]] = b.data(j);
+	  a.xridx(ii++) = Q [r + b.ridx(j)];
+	}
+
+      sort.sort (ri + a.xcidx (i), ii - a.xcidx (i));
+      for (octave_idx_type p = a.xcidx (i); p < ii; p++)
+	a.xdata (p) = X [a.xridx (p)]; 
+      a.xcidx(i+1) = ii;
+    }
+
+  for (octave_idx_type i = c + b_cols; i < nc; i++)
+    {
+      for (octave_idx_type j = tmp.xcidx(i); j < tmp.cidx(i+1); j++)
+	{
+	  a.xdata(ii) = tmp.xdata(j);
+	  a.xridx(ii++) = tmp.xridx(j);
+	}
+      a.xcidx(i+1) = ii;
+    }
+}
+
+#if !defined (CXX_NEW_FRIEND_TEMPLATE_DECL)
+static void
+dmsolve_insert (MSparse<double> &a, const SparseMatrix &b, 
+	       const octave_idx_type *Q, octave_idx_type r, octave_idx_type c);
+
+static void
+dmsolve_insert (MSparse<Complex> &a, const MSparse<Complex> &b,
+	       const octave_idx_type *Q, octave_idx_type r, octave_idx_type c);
+#endif
+
+template <class T, class RT>
+static void
+dmsolve_permute (MArray2<RT> &a, const MArray2<T>& b, const octave_idx_type *p)
+{
+  octave_idx_type b_nr = b.rows ();
+  octave_idx_type b_nc = b.cols ();
+  const T *Bx = b.fortran_vec();
+  a.resize(b_nr, b_nc);
+  RT *Btx = a.fortran_vec();
+  for (octave_idx_type j = 0; j < b_nc; j++)
+    {
+      octave_idx_type off = j * b_nr;
+      for (octave_idx_type i = 0; i < b_nr; i++)
+	{
+	  OCTAVE_QUIT;
+	  Btx [p [i] + off] = Bx [ i + off];
+	}
+    }
+}
+
+#if !defined (CXX_NEW_FRIEND_TEMPLATE_DECL)
+static void
+dmsolve_permute (MArray2<double> &a, const MArray2<double>& b,
+		 const octave_idx_type *p);
+
+static void
+dmsolve_permute (MArray2<Complex> &a, const MArray2<double>& b,
+		 const octave_idx_type *p);
+
+static void
+dmsolve_permute (MArray2<Complex> &a, const MArray2<Complex>& b,
+		 const octave_idx_type *p);
+#endif
+
+template <class T, class RT>
+static void
+dmsolve_permute (MSparse<RT> &a, const MSparse<T>& b, const octave_idx_type *p)
+{
+  octave_idx_type b_nr = b.rows ();
+  octave_idx_type b_nc = b.cols ();
+  octave_idx_type b_nz = b.nnz ();
+  octave_idx_type nz = 0;
+  a = MSparse<RT> (b_nr, b_nc, b_nz);
+  octave_sort<octave_idx_type> sort;
+  octave_idx_type *ri = a.xridx();
+  OCTAVE_LOCAL_BUFFER (RT, X, b_nr);
+  a.xcidx(0) = 0;
+  for (octave_idx_type j = 0; j < b_nc; j++)
+    {
+      for (octave_idx_type i = b.cidx(j); i < b.cidx(j+1); i++)
+	{
+	  OCTAVE_QUIT;
+	  octave_idx_type r = p [b.ridx (i)];
+	  X [r] = b.data (i);
+	  a.xridx(nz++) = p [b.ridx (i)];
+	}
+      sort.sort (ri + a.xcidx (j), nz - a.xcidx (j));
+      for (octave_idx_type i = a.cidx (j); i < nz; i++)
+	{
+	  OCTAVE_QUIT;
+	  a.xdata (i) = X [a.xridx (i)]; 
+	}
+      a.xcidx(j+1) = nz;
+    }
+}
+
+#if !defined (CXX_NEW_FRIEND_TEMPLATE_DECL)
+static void
+dmsolve_permute (MSparse<double> &a, const MSparse<double>& b, 
+		 const octave_idx_type *p);
+
+static void
+dmsolve_permute (MSparse<Complex> &a, const MSparse<double>& b,
+		 const octave_idx_type *p);
+
+static void
+dmsolve_permute (MSparse<Complex> &a, const MSparse<Complex>& b,
+		 const octave_idx_type *p);
+#endif
+
+static void
+solve_singularity_warning (double)
+{
+  // Dummy singularity handler so that LU solver doesn't flag
+  // an error for numerically rank defficient matrices
+}
+
+template <class RT, class ST, class T>
+RT
+dmsolve (const ST &a, const T &b, octave_idx_type &info)
+{
+  octave_idx_type nr = a.rows ();
+  octave_idx_type nc = a.cols ();
+  octave_idx_type b_nr = b.rows ();
+  octave_idx_type b_nc = b.cols ();
+  RT retval;
+
+  if (nr < 1 || nc < 1 || nr != b_nr)
+    (*current_liboctave_error_handler)
+      ("matrix dimension mismatch in solution of minimum norm problem");
+  else
+    {
+      octave_idx_type nnz_remaining = a.nnz ();
+      CXSPARSE_DNAME () csm;
+      csm.m = nr;
+      csm.n = nc;
+      csm.x = NULL;
+      csm.nz = -1;
+      csm.nzmax = a.nnz ();
+      // Cast away const on A, with full knowledge that CSparse won't touch it.
+      // Prevents the methods below making a copy of the data.
+      csm.p = const_cast<octave_idx_type *>(a.cidx ());
+      csm.i = const_cast<octave_idx_type *>(a.ridx ());
+
+      CXSPARSE_DNAME (d) *dm = CXSPARSE_DNAME(_dmperm) (&csm);
+      octave_idx_type *p = dm->P;
+      octave_idx_type *q = dm->Q;
+      OCTAVE_LOCAL_BUFFER (octave_idx_type, pinv, nr);
+      for (octave_idx_type i = 0; i < nr; i++)
+	pinv [p [i]] = i;
+      RT btmp;
+      dmsolve_permute (btmp, b, pinv);
+      SparseType mtyp (SparseType::Full);
+      info = 0;
+      retval.resize (nc, b_nc);
+
+      // Leading over-determined block
+      if (dm->rr [2] < nr && dm->cc [3] < nc)
+	{
+	  ST m = dmsolve_extract (a, pinv, q, dm->rr [2], nr, dm->cc [3], nc, 
+				  nnz_remaining, true);
+	  nnz_remaining -= m.nnz();
+	  RT mtmp = 
+	    qrsolve (m, dmsolve_extract (btmp, NULL, NULL, dm->rr[2], b_nr, 0,
+					 b_nc), info);
+	  dmsolve_insert (retval, mtmp, q, dm->cc [3], 0);
+	  if (dm->rr [2] > 0 && !info && !error_state)
+	    {
+	      m = dmsolve_extract (a, pinv, q, 0, dm->rr [2], 
+				   dm->cc [3], nc, nnz_remaining, true);
+	      nnz_remaining -= m.nnz();
+	      RT ctmp = dmsolve_extract (btmp, NULL, NULL, 0, 
+					 dm->rr[2], 0, b_nc);
+	      btmp.insert (ctmp - m * mtmp, 0, 0);
+	    }
+	}
+      
+      // Structurally non-singular blocks
+      // XXX FIXME XXX Should use fine Dulmange-Mendelsohn decomposition here.
+      if (dm->rr [1] < dm->rr [2] && dm->cc [2] < dm->cc [3] && 
+	  !info && !error_state)
+	{
+	  ST m = dmsolve_extract (a, pinv, q, dm->rr [1], dm->rr [2], 
+				  dm->cc [2], dm->cc [3], nnz_remaining, false);
+	  nnz_remaining -= m.nnz();
+	  RT btmp2 = dmsolve_extract (btmp, NULL, NULL, dm->rr [1], dm->rr [2], 
+				      0, b_nc);
+	  double rcond = 0.0;
+	  RT mtmp = m.solve (mtyp, btmp2, info, rcond, 
+			     solve_singularity_warning);	
+	  if (info != 0)
+	    {
+	      info = 0;
+	      mtmp = qrsolve (m, btmp2, info);
+	    }
+
+	  dmsolve_insert (retval, mtmp, q, dm->cc [2], 0);
+	  if (dm->rr [1] > 0 && !info && !error_state)
+	    {
+	      m = dmsolve_extract (a, pinv, q, 0, dm->rr [1], dm->cc [2],
+				   dm->cc [3], nnz_remaining, true);
+	      nnz_remaining -= m.nnz();
+	      RT ctmp = dmsolve_extract (btmp, NULL, NULL, 0,
+					 dm->rr[1], 0, b_nc);
+	      btmp.insert (ctmp - m * mtmp, 0, 0);
+	    }
+	}
+
+      // Trailing under-determined block
+      if (dm->rr [1] > 0 && dm->cc [2] > 0 && !info && !error_state)
+	{
+	  ST m = dmsolve_extract (a, pinv, q, 0, dm->rr [1], 0, 
+				  dm->cc [2], nnz_remaining, true);
+	  RT mtmp = 
+	    qrsolve (m, dmsolve_extract(btmp, NULL, NULL, 0, dm->rr [1] , 0, 
+					b_nc), info);
+	  dmsolve_insert (retval, mtmp, q, 0, 0);
+	}
+
+      CXSPARSE_DNAME (_dfree) (dm);
+    }
+  return retval;
+}
+
+#if !defined (CXX_NEW_FRIEND_TEMPLATE_DECL)
+extern Matrix
+dmsolve (const SparseMatrix &a, const Matrix &b, 
+	 octave_idx_type &info);
+
+extern ComplexMatrix
+dmsolve (const SparseMatrix &a, const ComplexMatrix &b, 
+	 octave_idx_type &info);
+
+extern ComplexMatrix
+dmsolve (const SparseComplexMatrix &a, const Matrix &b, 
+	 octave_idx_type &info);
+
+extern ComplexMatrix
+dmsolve (const SparseComplexMatrix &a, const ComplexMatrix &b, 
+	 octave_idx_type &info);
+
+extern SparseMatrix
+dmsolve (const SparseMatrix &a, const SparseMatrix &b, 
+	 octave_idx_type &info);
+
+extern SparseComplexMatrix
+dmsolve (const SparseMatrix &a, const SparseComplexMatrix &b, 
+	 octave_idx_type &info);
+
+extern SparseComplexMatrix
+dmsolve (const SparseComplexMatrix &a, const SparseMatrix &b, 
+	 octave_idx_type &info);
+
+extern SparseComplexMatrix
+dmsolve (const SparseComplexMatrix &a, const SparseComplexMatrix &b, 
+	 octave_idx_type &info);
+#endif
+
+/*
+;;; Local Variables: ***
+;;; mode: C++ ***
+;;; End: ***
+*/