Improvements

Pseudoinverse for polynomial matrix in bicgstab(l) to improve stability
(as in petsc);
Added RowMerger class for sum of sparse rows.

autodiff.cpp

@@ -19,7 +19,7 @@ namespace INMOST
 	void Automatizator::DerivativeFill(expr & var, INMOST_DATA_ENUM_TYPE element, INMOST_DATA_ENUM_TYPE parent, Solver::Row & entries, INMOST_DATA_REAL_TYPE multval, void * user_data)
 	{
 		INMOST_DATA_ENUM_TYPE voffset = var.values_offset(element), doffset = var.derivatives_offset(element);
-		INMOST_DATA_ENUM_TYPE k = var.data.size()-1;
+		INMOST_DATA_ENUM_TYPE k = static_cast<INMOST_DATA_ENUM_TYPE>(var.data.size()-1);
 		Storage e = Storage(m,var.current_stencil[element].first);
 		INMOST_DATA_REAL_TYPE lval, rval, ret;
 		var.values[doffset+k] = multval;

examples/MatSolve/main.cpp

@@ -94,7 +94,7 @@ int main(int argc, char ** argv)
 			t = Timer();
 			success = s.Solve(b,x); // Solve the linear system with the previously computted preconditioner
 			BARRIER
-			if( !rank ) std::cout << "solver: " << Timer() - t << std::endl;
+			if( !rank ) std::cout << "solver: " << Timer() - t << "\t\t\t" << std::endl;
 			iters = s.Iterations(); // Get the number of iterations performed
 			resid = s.Residual();   // Get the final residual achieved
 			reason = s.GetReason();
@@ -125,9 +125,9 @@ int main(int argc, char ** argv)
 			double temp[2] = {aresid,bresid}, recv[2] = {aresid,bresid};
 #if defined(USE_MPI)
 			MPI_Reduce(temp,recv,2,MPI_DOUBLE,MPI_SUM,0,MPI_COMM_WORLD);
-			if( info.GetRank() == 0 ) std::cout << "||Ax-b|| " << sqrt(recv[0]) << " ||b|| " << sqrt(recv[1]) << " ||Ax-b||/||b|| " << sqrt(recv[0]/recv[1]) << std::endl;
+			if( info.GetRank() == 0 ) std::cout << "||Ax-b|| " << sqrt(recv[0]) << " ||b|| " << sqrt(recv[1]) << " ||Ax-b||/||b|| " << sqrt(recv[0]/(recv[1]+1.0e-100)) << std::endl;
 #endif
-			realresid = sqrt(recv[0]/recv[1]);
+			realresid = sqrt(recv[0]/(recv[1]+1.0e-100));
 			//realresid = sqrt(realresid);
 
 			info.RestoreVector(x);

inmost_autodiff.h

@@ -156,7 +156,7 @@ namespace INMOST
 		__INLINE INMOST_DATA_REAL_TYPE                                   GetStaticValue(const Storage & e, INMOST_DATA_ENUM_TYPE ind, INMOST_DATA_ENUM_TYPE comp = 0) { return e->RealArray(GetStaticValueTag(ind))[comp]; }
 		__INLINE bool                                                    isStaticValid(const Storage & e, INMOST_DATA_ENUM_TYPE ind) { MarkerType mask = GetStaticMask(ind); return mask == 0 || e->GetMarker(mask); }
 #if defined(NEW_VERSION)
-		INMOST_DATA_REAL_TYPE                                            Evaluate(expr & var, const const Storage & e, void * user_data);
+		INMOST_DATA_REAL_TYPE                                            Evaluate(expr & var, const Storage & e, void * user_data);
 		INMOST_DATA_REAL_TYPE                                            Derivative(expr & var, const Storage & e, Solver::Row & out, Storage::real multiply, void * user_data);
 #else
 		INMOST_DATA_REAL_TYPE                                            Evaluate(const expr & var, const Storage & e, void * user_data);
@@ -547,17 +547,17 @@ namespace INMOST
 		expr(const expr * l, const expr * r) :data() { data.push_back(expr_data(l,r)); }
 		expr(INMOST_DATA_REAL_TYPE val) : data() { data.push_back(expr_data(val)); }
 		expr(INMOST_DATA_ENUM_TYPE op, INMOST_DATA_ENUM_TYPE comp) : data() { data.push_back(expr_data(op, comp, ENUMUNDEF)); }
-		expr(INMOST_DATA_ENUM_TYPE op, const expr & operand) : data(operand.data) { relink_data();  data.push_back(expr_data(op, data.size() - 1, ENUMUNDEF)); }
-		expr(const expr & operand, const expr & multiplyer) : data(operand.data) { relink_data();  data.push_back(expr_data(AD_VAL, data.size() - 1, multiplyer)); }
+		expr(INMOST_DATA_ENUM_TYPE op, const expr & operand) : data(operand.data) { relink_data();  data.push_back(expr_data(op, static_cast<INMOST_DATA_ENUM_TYPE>(data.size() - 1), ENUMUNDEF)); }
+		expr(const expr & operand, const expr & multiplyer) : data(operand.data) { relink_data();  data.push_back(expr_data(AD_VAL, static_cast<INMOST_DATA_ENUM_TYPE>(data.size() - 1), multiplyer)); }
 		expr(const expr & cond, const expr & if_true, const expr & if_false) :data(cond.data)
 		{ 
 			relink_data();  
-			data.push_back(expr_data(data.size() - 1, expr_data(&if_true, &if_false))); 
+			data.push_back(expr_data(static_cast<INMOST_DATA_ENUM_TYPE>(data.size() - 1), expr_data(&if_true, &if_false))); 
 		}
 		expr(INMOST_DATA_ENUM_TYPE op, const expr & l, const expr & r) :data(l.data) 
 		{
 			relink_data();
-			INMOST_DATA_ENUM_TYPE lp = data.size() - 1;
+			INMOST_DATA_ENUM_TYPE lp = static_cast<INMOST_DATA_ENUM_TYPE>(data.size() - 1);
 			INMOST_DATA_ENUM_TYPE rp = merge_data(r.data); 
 			data.push_back(expr_data(op, lp, rp));
 		}

inmost_solver.h

@@ -225,12 +225,15 @@ namespace INMOST
 				ret.second = val;
 				return ret;
 			}
-			
+
+		private:
 			typedef dynarray<entry,16> Entries; //replace later with more memory-efficient chunk_array, with first chunk in stack
 			//typedef array<entry> Entries;
 			//typedef std::vector<entry> Entries;
 			//typedef sparse_data<INMOST_DATA_ENUM_TYPE,INMOST_DATA_REAL_TYPE> Entries;
 			//typedef Entries::pair entry; //for sparse_data
+			
+		public:
 			typedef Entries::iterator iterator;
 			typedef Entries::const_iterator const_iterator;
 			typedef Entries::reverse_iterator reverse_iterator;
@@ -348,6 +351,9 @@ namespace INMOST
 			/// @param size New size of the row.
 			void                    Resize(INMOST_DATA_ENUM_TYPE size) {data.resize(size);}
 		};
+
+
+		
 		
 		/// Class to store the distributed sparse matrix by compressed rows.
 		/// The format used to store sparse matrix is analogous to Compressed Row Storage format (CRS).
@@ -428,6 +434,86 @@ namespace INMOST
 			std::string          GetName() {return name;}
 			//~ friend class Solver;
 		};
+
+		/// This class may be used to sum multiple sparse rows.
+		/// \warning
+		/// In parallel column indices of the matrix may span wider then 
+		/// local row indices, to prevent any problem you are currently
+		/// advised to set total size of the matrix as interval of the
+		/// RowMerger. In future this may change, see todo 2 below.
+		/// \todo
+		/// 1. Add iterators over entries.
+		/// 2. Implement multiple intervals for distributed computation,
+		///    then in parallel the user may specify additional range of indexes
+		///    for elements that lay on the borders between each pair of processors.
+		class RowMerger
+		{
+		public:
+			static const INMOST_DATA_ENUM_TYPE EOL = ENUMUNDEF-1; ///< End of linked list.
+			static const INMOST_DATA_ENUM_TYPE UNDEF = ENUMUNDEF; ///< Value not defined in linked list.
+		private:
+			bool Sorted; ///< Contents of linked list should be sorted.
+			INMOST_DATA_ENUM_TYPE Nonzeros; ///< Number of nonzero in linked list.
+			interval< INMOST_DATA_ENUM_TYPE, Row::entry > LinkedList; ///< Storage for linked list.
+		public:
+			/// Default constructor without size specfied.
+			RowMerger();
+			/// Constructor with size specified.
+			/// @param interval_begin First index in linked list.
+			/// @param interval_end Last index in linked list.
+			/// @param Sorted Result should be sorted.
+			RowMerger(INMOST_DATA_ENUM_TYPE interval_begin, INMOST_DATA_ENUM_TYPE interval_end, bool Sorted = true);
+			/// Constructor that gets sizes from matrix
+			/// @param A Matrix to get sizes from.
+			/// @param Sorted Result should be sorted.
+			RowMerger(Matrix & A, bool Sorted = true);
+			/// Destructor.
+			~RowMerger();
+			/// Resize linked list for new interval.
+			/// \warning
+			/// All contents of linked list will be lost after resize.
+			/// This behavior may be changed in future.
+			/// @param interval_begin First index in linked list.
+			/// @param interval_end Last index in linked list.
+			/// @param Sorted Result should be sorted.
+			void Resize(INMOST_DATA_ENUM_TYPE interval_begin, INMOST_DATA_ENUM_TYPE interval_end, bool Sorted = true);
+			/// Resize linked list for new matrix.
+			/// \warning
+			/// All contents of linked list will be lost after resize.
+			/// This behavior may be changed in future.
+			/// @param A Matrix to get sizes from.
+			/// @param Sorted Result should be sorted.
+			void Resize(Matrix & A, bool Sorted = true);
+			/// Clear linked list.
+			void Clear();
+			/// Add a row with a coefficient into empty linked list.
+			/// This routing should be a bit faster then Solver::RowMerger::AddRow
+			/// for empty linked list. It may result in an unexpected behavior
+			/// for non-empty linked list, asserts will fire in debug mode.
+			/// @param coef Coefficient to multiply row values.
+			/// @param r A row to be added.
+			/// @param PreSortRow Sort values of the row before adding. Will be activated only for sorted linked lists.
+			void PushRow(INMOST_DATA_REAL_TYPE coef, Row & r, bool PreSortRow = false);
+			/// Add a row with a coefficient into non-empty linked list.
+			/// Use Solver::RowMerger::PushRow for empty linked list.
+			/// @param coef Coefficient to multiply row values.
+			/// @param r A row to be added.
+			/// @param PreSortRow Sort values of the row before adding. Will be activated only for sorted linked lists.
+			void AddRow(INMOST_DATA_REAL_TYPE coef, Row & r, bool PreSortRow = false);
+			/// Multiply all entries of linked list by a coefficient.
+			/// @param coef A coefficient for multiplication.
+			void Multiply(INMOST_DATA_REAL_TYPE coef);
+			/// Place entries from linked list into row.
+			/// \warning
+			/// All contents of the row will be overwritten.
+			/// If you want contents of the row to be added
+			/// use AddRow with this row in advance.
+			/// @param r A row to be filled.
+			void RetriveRow(Row & r);
+			//INMOST_DATA_REAL_TYPE ScalarProd(RowMerger & other);
+			/// Get current number of nonzeros from linked list.
+			INMOST_DATA_ENUM_TYPE Size() {return Nonzeros;}
+		};
 		
 	private:
 		static bool is_initialized, is_finalized;

solver.cpp

@@ -94,6 +94,126 @@ namespace INMOST
 #endif
 	}
 
+	Solver::RowMerger::RowMerger() : Sorted(true), Nonzeros(0) {}
+
+	Solver::RowMerger::RowMerger(INMOST_DATA_ENUM_TYPE interval_begin, INMOST_DATA_ENUM_TYPE interval_end, bool Sorted) 
+				: Sorted(Sorted), Nonzeros(0), LinkedList(interval_begin,interval_end+1,Row::make_entry(UNDEF,0.0)) 
+	{
+		LinkedList.begin()->first = EOL;
+	}
+
+	void Solver::RowMerger::Resize(INMOST_DATA_ENUM_TYPE interval_begin, INMOST_DATA_ENUM_TYPE interval_end, bool _Sorted) 
+	{
+		LinkedList.set_interval_beg(interval_begin);
+		LinkedList.set_interval_end(interval_end+1);
+		std::fill(LinkedList.begin(),LinkedList.end(),Row::make_entry(UNDEF,0.0));
+		LinkedList.begin()->first = EOL;
+		Nonzeros = 0;
+		Sorted = _Sorted;
+	}
+
+	void Solver::RowMerger::Resize(Matrix & A, bool _Sorted)
+	{
+		INMOST_DATA_ENUM_TYPE mbeg, mend;
+		A.GetInterval(mbeg,mend);
+		Resize(mbeg,mend,_Sorted);
+	}
+
+	Solver::RowMerger::RowMerger(Matrix & A, bool Sorted) : Sorted(Sorted), Nonzeros(0)
+	{
+		INMOST_DATA_ENUM_TYPE mbeg, mend;
+		A.GetInterval(mbeg,mend);
+		LinkedList.set_interval_beg(mbeg);
+		LinkedList.set_interval_end(mend+1);
+		std::fill(LinkedList.begin(),LinkedList.end(),Row::make_entry(UNDEF,0.0));
+		LinkedList.begin()->first = EOL;
+	}
+
+	Solver::RowMerger::~RowMerger() {}
+
+	void Solver::RowMerger::Clear()
+	{
+		INMOST_DATA_ENUM_TYPE i = LinkedList.begin()->first, j;
+		LinkedList.begin()->first = EOL;
+		while( i != EOL )
+		{
+			j = LinkedList[i].first;
+			LinkedList[i].first = UNDEF; 
+			i = j;
+		}
+		Nonzeros = 0;
+	}
+
+	void Solver::RowMerger::PushRow(INMOST_DATA_REAL_TYPE coef, Row & r, bool PreSortRow)
+	{
+		if( Sorted && PreSortRow ) std::sort(r.Begin(),r.End());
+		assert(Nonzeros == 0); //Linked list should be empty
+		assert(LinkedList.begin()->first == EOL); //again check that list is empty
+		INMOST_DATA_ENUM_TYPE index = LinkedList.get_interval_beg();
+		Row::iterator it = r.Begin(), jt;
+		while( it != r.End() )
+		{
+			LinkedList[index].first = it->first+1;
+			LinkedList[it->first+1].first = EOL;
+			LinkedList[it->first+1].second = it->second*coef;
+			index = it->first+1;
+			++Nonzeros;
+			jt = it;
+			++it;
+			assert(!Sorted || it == r.End() || jt->first < it->first);
+		}
+	}
+
+	void Solver::RowMerger::AddRow(INMOST_DATA_REAL_TYPE coef, Row & r, bool PreSortRow)
+	{
+		if( Sorted && PreSortRow ) std::sort(r.Begin(),r.End());
+		INMOST_DATA_ENUM_TYPE index = LinkedList.get_interval_beg(), next;
+		Row::iterator it = r.Begin(), jt;
+		while( it != r.End() )
+		{
+			if( LinkedList[it->first+1].first != UNDEF )
+				LinkedList[it->first+1].second += coef*it->second;
+			else if( Sorted )
+			{
+				next = index;
+				while(next < it->first+1)
+				{
+					index = next;
+					next = LinkedList[index].first;
+				}
+				assert(index < it->first+1);
+				assert(it->first+1 < next);
+				LinkedList[index].first = it->first+1;
+				LinkedList[it->first+1].first = next;
+				LinkedList[it->first+1].second = coef*it->second;
+				++Nonzeros;
+			}
+			else
+			{
+				LinkedList[it->first+1].first = LinkedList[index].first;
+				LinkedList[it->first+1].second = coef*it->second;
+				LinkedList[index].first = it->first+1;
+				++Nonzeros;
+			}
+			jt = it;
+			++it;
+			assert(!Sorted || it == r.End() || jt->first < it->first);
+		}
+	}
+
+	void Solver::RowMerger::RetriveRow(Row & r)
+	{
+		r.Resize(Nonzeros);
+		INMOST_DATA_ENUM_TYPE i = LinkedList.begin()->first, k = 0;
+		while( i != EOL )
+		{
+			r.GetIndex(k) = i-1;
+			r.GetValue(k) = LinkedList[i].second;
+			i = LinkedList[i].first;
+			++k;
+		}
+	}
+
 	void Solver::OrderInfo::PrepareMatrix(Matrix & m, INMOST_DATA_ENUM_TYPE overlap)
 	{
 		have_matrix = true;

solver_bcgsl.hpp

@@ -2,103 +2,309 @@
 #ifndef __SOLVER_BCGS__
 #define __SOLVER_BCGS__
 
-//TODO:
-// comply solvers with Method prototype, after TODO in solver_prototypes.hpp is done
-
+//\todo
+// 1. comply solvers with Method prototype, after TODO in solver_prototypes.hpp is done
+// 2. Implement tricks from Read/solver/bcgsl/download.pdf with convex update and true residual correction
+// 3. Detect numerical accuracy breakdown - when preconditioned residual is too far from true residual (probably 2 will fix).
 
 #include "inmost_solver.h"
 
+#define PSEUDOINVERSE  // same trick as in petsc with pseudoinverse
+//#define USE_LAPACK_SVD // use lapack's dgesvd routine instead of built-in svdnxn
+
 //#if !defined(NDEBUG)
 #define REPORT_RESIDUAL
 //#endif
 
 namespace INMOST
 {
-	
-int solvenxn(INMOST_DATA_REAL_TYPE * A, INMOST_DATA_REAL_TYPE * x, INMOST_DATA_REAL_TYPE * b, int n, int * order)
-{
-	INMOST_DATA_REAL_TYPE temp, max;
-	int temp2;
-	for(int i = 0; i < n; i++) order[i] = i;
-	for(int i = 0; i < n; i++)
+	//lapack svd
+#if defined(PSEUDOINVERSE)
+#if defined(USE_LAPACK_SVD)
+	extern "C"
+	{
+		void dgesvd_(char*,char*,int*,int*,double*,int*,double*,double*,int*,double*,int*,double*,int*,int*);
+	}
+#else // SVD adopted from http://stackoverflow.com/questions/3856072/svd-implementation-c answer by Dhairya Malhotra
+	void GivensL(INMOST_DATA_REAL_TYPE * S, const int N, int m, INMOST_DATA_REAL_TYPE a, INMOST_DATA_REAL_TYPE b)
+	{
+		INMOST_DATA_REAL_TYPE r = sqrt(a*a+b*b);
+		INMOST_DATA_REAL_TYPE c = a/r;
+		INMOST_DATA_REAL_TYPE s = -b/r;
+		for(int i=0;i<N;i++)
+		{
+			INMOST_DATA_REAL_TYPE S0 = S[(m+0)*N+i];
+			INMOST_DATA_REAL_TYPE S1 = S[(m+1)*N+i];
+			S[(m+0)*N + i] += S0*(c-1);
+			S[(m+0)*N + i] += S1*( -s);
+			S[(m+1)*N + i] += S0*(s  );
+			S[(m+1)*N + i] += S1*(c-1);
+		}
+	}
+
+	void GivensR(INMOST_DATA_REAL_TYPE * S, const int N, int m, INMOST_DATA_REAL_TYPE a, INMOST_DATA_REAL_TYPE b)
 	{
-		int maxk = i, maxq = i;
-		max = fabs(A[maxk*n+maxq]);
-		//Find best pivot
-		for(int q = i; q < n; q++) // over columns
+		INMOST_DATA_REAL_TYPE r = sqrt(a*a+b*b);
+		INMOST_DATA_REAL_TYPE c = a/r;
+		INMOST_DATA_REAL_TYPE s = -b/r;
+		for(int i=0;i<N;i++)
 		{
-			for(int k = i; k < n; k++) // over rows
+			INMOST_DATA_REAL_TYPE S0 = S[i*N+(m+0)];
+			INMOST_DATA_REAL_TYPE S1 = S[i*N+(m+1)];
+			S[i*N+(m+0)] += S0*(c-1);
+			S[i*N+(m+0)] += S1*( -s);
+			S[i*N+(m+1)] += S0*(s  );
+			S[i*N+(m+1)] += S1*(c-1);
+		}
+	}
+
+	void svdnxn(INMOST_DATA_REAL_TYPE * A, INMOST_DATA_REAL_TYPE * U, INMOST_DATA_REAL_TYPE * S,  INMOST_DATA_REAL_TYPE * V, const int N)
+	{
+		memset(S,0,sizeof(INMOST_DATA_REAL_TYPE)*N*N);
+		memset(U,0,sizeof(INMOST_DATA_REAL_TYPE)*N*N);
+		memset(V,0,sizeof(INMOST_DATA_REAL_TYPE)*N*N);
+		for(int i=0;i<N;i++)
+		{
+			for(int j=0;j<N;j++)
+				S[i*N+j]=A[i*N+j];
+			U[i*N+i] = 1;
+			V[i*N+i] = 1;
+		}
+		INMOST_DATA_REAL_TYPE eps = -1;
+		{ // Bi-diagonalization
+			std::vector<INMOST_DATA_REAL_TYPE> house_vec(N);
+			for(int i=0;i<N;i++)
 			{
-				if( fabs(A[k*n+q]) > max )
+				// Column Householder
+				{
+					INMOST_DATA_REAL_TYPE x1= S[i*N+i];
+					if(x1<0) x1=-x1;
+
+					INMOST_DATA_REAL_TYPE x_inv_norm=0;
+					for(int j=i;j<N;j++) x_inv_norm+= S[j*N+i]*S[j*N+i];
+					x_inv_norm=1/sqrt(x_inv_norm);
+
+					INMOST_DATA_REAL_TYPE alpha=sqrt(1+x1*x_inv_norm);
+					INMOST_DATA_REAL_TYPE beta=x_inv_norm/alpha;
+
+					house_vec[i]=-alpha;
+					for(int j=i+1;j<N;j++) house_vec[j]=-beta*S[j*N+i];
+					if(S[i*N+i]<0) for(int j=i+1;j<N;j++) house_vec[j]=-house_vec[j];
+				}
+				
+				for(int k=i;k<N;k++)
+				{
+					INMOST_DATA_REAL_TYPE dot_prod=0;
+					for(int j=i;j<N;j++) dot_prod+=S[j*N+k]*house_vec[j];
+					
+					for(int j=i;j<N;j++) S[j*N+k]-=dot_prod*house_vec[j];
+				}
+				
+				for(int k=0;k<N;k++)
+				{
+					INMOST_DATA_REAL_TYPE dot_prod=0;
+					for(int j=i;j<N;j++) dot_prod+=U[k*N+j]*house_vec[j];
+					for(int j=i;j<N;j++) U[k*N+j]-=dot_prod*house_vec[j];
+				}
+
+				// Row Householder
+				if(i>=N-1) continue;
+
+				{
+					INMOST_DATA_REAL_TYPE x1=S[i*N+(i+1)];
+					if(x1<0) x1=-x1;
+
+					INMOST_DATA_REAL_TYPE x_inv_norm=0;
+					for(int j=i+1;j<N;j++) x_inv_norm+=S[i*N+j]*S[i*N+j];
+					x_inv_norm=1/sqrt(x_inv_norm);
+
+					INMOST_DATA_REAL_TYPE alpha=sqrt(1+x1*x_inv_norm);
+					INMOST_DATA_REAL_TYPE beta=x_inv_norm/alpha;
+
+					house_vec[i+1]=-alpha;
+					for(int j=i+2;j<N;j++) house_vec[j]=-beta*S[i*N+j];
+					if(S[i*N+(i+1)]<0) for(int j=i+2;j<N;j++) house_vec[j]=-house_vec[j];
+				}
+				
+				for(int k=i;k<N;k++)
 				{
-					max = fabs(A[k*n+q]);
-					maxk = k;
-					maxq = q;
+					INMOST_DATA_REAL_TYPE dot_prod=0;
+					for(int j=i+1;j<N;j++) dot_prod+=S[k*N+j]*house_vec[j];
+					for(int j=i+1;j<N;j++) S[k*N+j] -= dot_prod*house_vec[j];
+				}
+				
+				for(int k=0;k<N;k++)
+				{
+					INMOST_DATA_REAL_TYPE dot_prod=0;
+					for(int j=i+1;j<N;j++) dot_prod+=V[j*N+k]*house_vec[j];
+					for(int j=i+1;j<N;j++) V[j*N+k]-=dot_prod*house_vec[j];
 				}
 			}
 		}
-		//Exchange rows
-		if( maxk != i ) 
+
+		int k0=0;
+		if(eps<0)
 		{
-			for(int q = 0; q < n; q++)
-			{
-				temp = A[maxk*n+q];
-				A[maxk*n+q] = A[i*n+q];
-				A[i*n+q] = temp;
+			eps=1.0;
+			while(eps+(INMOST_DATA_REAL_TYPE)1.0>1.0) eps*=0.5;
+			eps*=64.0;
+		}
+		while(k0<N-1)
+		{ // Diagonalization
+			INMOST_DATA_REAL_TYPE S_max=0.0;
+			for(int i=0;i<N;i++) S_max=(S_max > S[i*N+i] ? S_max : S[i*N+i]);
+			while(k0<N-1 && fabs(S[k0*N+(k0+1)])<=eps*S_max) k0++;
+			int k=k0;
+			int n=k0+1;
+			while(n<N && fabs(S[(n-1)*N+n])>eps*S_max) n++;
+
+			INMOST_DATA_REAL_TYPE mu=0;
+			{ // Compute mu
+				INMOST_DATA_REAL_TYPE C[2][2];
+				C[0][0]=S[(n-2)*N+(n-2)]*S[(n-2)*N+(n-2)]+S[(n-3)*N+(n-2)]*S[(n-3)*N+(n-2)]; 
+				C[0][1]=S[(n-2)*N+(n-2)]*S[(n-2)*N+(n-1)];
+				C[1][0]=S[(n-2)*N+(n-2)]*S[(n-2)*N+(n-1)]; 
+				C[1][1]=S[(n-1)*N+(n-1)]*S[(n-1)*N+(n-1)]+S[(n-2)*N+(n-1)]*S[(n-2)*N+(n-1)];
+				INMOST_DATA_REAL_TYPE b =-(C[0][0]+C[1][1])/2;
+				INMOST_DATA_REAL_TYPE c =  C[0][0]*C[1][1] - C[0][1]*C[1][0];
+				INMOST_DATA_REAL_TYPE d = sqrt(b*b-c);
+				INMOST_DATA_REAL_TYPE lambda1 = -b+d;
+				INMOST_DATA_REAL_TYPE lambda2 = -b-d;
+				INMOST_DATA_REAL_TYPE d1 = lambda1-C[1][1]; d1 = (d1<0?-d1:d1);
+				INMOST_DATA_REAL_TYPE d2 = lambda2-C[1][1]; d2 = (d2<0?-d2:d2);
+				mu = (d1<d2?lambda1:lambda2);
 			}
-			//exchange rhs
+
+			INMOST_DATA_REAL_TYPE alpha = S[k*N+k] * S[k*N+k] - mu;
+			INMOST_DATA_REAL_TYPE beta  = S[k*N+k] * S[k*N+(k+1)];
+
+			for(;k<N-1;k++)
 			{
-				temp = b[maxk];
-				b[maxk] = b[i];
-				b[i] = temp;
+				GivensR(S,N,k,alpha,beta);
+				GivensL(V,N,k,alpha,beta);
+
+				alpha = S[k*N+k];
+				beta  = S[(k+1)*N+k];
+				GivensL(S,N,k,alpha,beta);
+				GivensR(U,N,k,alpha,beta);
+
+				alpha = S[k*N+(k+1)];
+				beta  = S[k*N+(k+2)];
 			}
 		}
-		//Exchange columns
-		if( maxq != i ) 
+		
+		for(int i=0;i<N;i++)
 		{
-			for(int k = 0; k < n; k++)
+			INMOST_DATA_REAL_TYPE temp;
+			for(int j=i+1;j<N;j++)
 			{
-				temp = A[k*n+maxq];
-				A[k*n+maxq] = A[k*n+i];
-				A[k*n+i] = temp;
-			}
-			//remember order in sol
-			{
-				temp2 = order[maxq];
-				order[maxq] = order[i];
-				order[i] = temp2;
+				temp = U[i+N*j];
+				U[i+N*j] = U[j+N*i];
+				U[j+N*i] = temp;
+
+				temp = V[i+N*j];
+				V[i+N*j] = V[j+N*i];
+				V[j+N*i] = temp;
 			}
 		}
-		if( fabs(b[i]/A[i*n+i]) > 1.0e+100 )
-			return i+1;
 		
-		for(int k = i+1; k < n; k++)
-		{
-			A[i*n+k] /= A[i*n+i];
-			A[k*n+i] /= A[i*n+i];
-		}
-		for(int k = i+1; k < n; k++)
-		for(int q = i+1; q < n; q++)
-		{
-			A[k*n+q] -= A[k*n+i] * A[i*n+i] * A[i*n+q];
-		}
-		for(int j = i+1; j < n; j++) //iterate over columns of L
+
+		for(int i=0;i<N;i++) if( S[i*N+i] < 0.0 )
 		{
-			b[j] -= b[i] * A[j*n+i];
+			for(int j=0;j<N;j++)
+			{
+				U[j+N*i] *= -1;
+			}
+			S[i*N+i] *= -1;
 		}
-		b[i] /= A[i*n+i];
+		
 	}
-
-	for(int i = n-1; i >= 0; i--) //iterate over rows of U
-		for(int j = i+1; j < n; j++)