Commit 82230d19 authored by Kirill Terekhov's avatar Kirill Terekhov
Browse files

Improvements

Pseudoinverse for polynomial matrix in bicgstab(l) to improve stability
(as in petsc);
Added RowMerger class for sum of sparse rows.
parent 6177cfc7
......@@ -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;
......
......@@ -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);
......
......@@ -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));
}
......
......@@ -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;
......
......@@ -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;
......
......@@ -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++)
#endif //USE_LAPACK_SVD