void DenseGenMatrix::CholeskySolveVector(DenseVector& b) const
{
DBG_ASSERT(NRows()==NCols());
DBG_ASSERT(b.Dim()==NRows());
DBG_ASSERT(initialized_);
Number* bvalues = b.Values();
IpLapackDpotrs(NRows(), 1, values_, NRows(), bvalues, b.Dim());
}
void DenseGenMatrix::LUSolveVector(DenseVector& b) const
{
DBG_ASSERT(NRows()==NCols());
DBG_ASSERT(b.Dim()==NRows());
DBG_ASSERT(initialized_);
DBG_ASSERT(factorization_==LU);
Number* bvalues = b.Values();
IpLapackDgetrs(NRows(), 1, values_, NRows(), pivot_, bvalues, b.Dim());
}
bool DenseGenMatrix::ComputeEigenVectors(const DenseSymMatrix& M,
DenseVector& Evalues)
{
Index dim = M.Dim();
DBG_ASSERT(Evalues.Dim()==dim);
DBG_ASSERT(NRows()==dim);
DBG_ASSERT(NCols()==dim);
// First we copy the content of the matrix into Q
const Number* Mvalues = M.Values();
for (Index j=0; j<dim; j++) {
for (Index i=j; i<dim; i++) {
values_[i+j*dim] = Mvalues[i+j*dim];
}
}
bool compute_eigenvectors = true;
Number* Evals = Evalues.Values();
Index info;
IpLapackDsyev(compute_eigenvectors, dim, values_,
dim, Evals, info);
initialized_ = (info==0);
ObjectChanged();
return (info==0);
}
void DenseGenMatrix::ScaleColumns(const DenseVector& scal_vec)
{
DBG_ASSERT(scal_vec.Dim() == NCols());
DBG_ASSERT(initialized_);
const Number* scal_values = scal_vec.Values();
for (Index j=0; j<NCols(); j++) {
IpBlasDscal(NRows(), scal_values[j], &values_[j*NRows()], 1);
}
ObjectChanged();
}
void DenseSymMatrix::SpecialAddForLMSR1(const DenseVector& D,
const DenseGenMatrix& L)
{
const Index dim = Dim();
DBG_ASSERT(initialized_);
DBG_ASSERT(dim==D.Dim());
DBG_ASSERT(dim==L.NRows());
DBG_ASSERT(dim==L.NCols());
// First add the diagonal matrix
const Number* Dvalues = D.Values();
for (Index i=0; i<dim; i++) {
values_[i+i*dim] += Dvalues[i];
}
// Now add the strictly-lower triagular matrix L and its transpose
const Number* Lvalues = L.Values();
for (Index j=0; j<dim; j++) {
for (Index i=j+1; i<dim; i++) {
values_[i+j*dim] += Lvalues[i+j*dim];
}
}
ObjectChanged();
}
/** Bi-conjugate gradient stabilized method. */
int BiCGSTABPrecondSolve( const SparseMatrix &A, const DenseVector &b, DenseVector &x, const IPreconditioner &M, float epsilon ) {
piDebugCheck( A.IsSquare() );
piDebugCheck( A.Width() == b.Dim() );
piDebugCheck( A.Width() == x.Dim() );
int i = 0;
const int D = A.Width();
const int i_max = D;
// const int i_max = 1000;
float resid;
float rho_1 = 0;
float rho_2 = 0;
float alpha = 0;
float beta = 0;
float omega = 0;
DenseVector p(D);
DenseVector phat(D);
DenseVector s(D);
DenseVector shat(D);
DenseVector t(D);
DenseVector v(D);
DenseVector r(D);
DenseVector rtilde(D);
DenseVector tmp(D);
// r = b - A·x;
A.Product( x, tmp );
r.Sub( b, tmp );
// rtilde = r
rtilde.Set( r );
float normb = b.Norm();
if( normb == 0.0 ) normb = 1;
// test convergence
resid = r.Norm() / normb;
if( resid < epsilon ) {
// method converges?
return 0;
}
while( i<i_max ) {
i++;
rho_1 = DenseVectorDotProduct( rtilde, r );
if( rho_1 == 0 ) {
// method fails
return -i;
}
if( i == 1 ) {
p.Set( r );
}
else {
beta = (rho_1 / rho_2) * (alpha / omega);
// p = r + beta * (p - omega * v);
p.Mad( p, v, -omega );
p.Mad( r, p, beta );
}
//phat = M.solve(p);
//phat.Set( p );
M.Precond( &phat, p );
//v = A * phat;
A.Product( phat, v );
alpha = rho_1 / DenseVectorDotProduct( rtilde, v );
// s = r - alpha * v;
s.Mad( r, v, -alpha );
resid = s.Norm() / normb;
//printf( "--- Iteration %d: residual = %f\n", i, resid );
if( resid < epsilon ) {
// x += alpha * phat;
x.Mad( x, phat, alpha );
return i;
}
//shat = M.solve(s);
//shat.Set( s );
M.Precond( &shat, s );
//t = A * shat;
A.Product( shat, t );
omega = DenseVectorDotProduct( t, s ) / DenseVectorDotProduct( t, t );
// x += alpha * phat + omega * shat;
x.Mad( x, shat, omega );
x.Mad( x, phat, alpha );
//r = s - omega * t;
r.Mad( s, t, -omega );
rho_2 = rho_1;
resid = r.Norm() / normb;
if( resid < epsilon ) {
return i;
}
if( omega == 0 ) {
return -i; // ???
}
}
return i;
}
/** Bi-conjugate gradient method. */
MATHLIB_API int BiConjugateGradientSolve( const SparseMatrix &A, const DenseVector &b, DenseVector &x, float epsilon ) {
piDebugCheck( A.IsSquare() );
piDebugCheck( A.Width() == b.Dim() );
piDebugCheck( A.Width() == x.Dim() );
int i = 0;
const int D = A.Width();
const int i_max = 4 * D;
float resid;
float rho_1 = 0;
float rho_2 = 0;
float alpha;
float beta;
DenseVector r(D);
DenseVector rtilde(D);
DenseVector p(D);
DenseVector ptilde(D);
DenseVector q(D);
DenseVector qtilde(D);
DenseVector tmp(D); // temporal vector.
// r = b - A·x;
A.Product( x, tmp );
r.Sub( b, tmp );
// rtilde = r
rtilde.Set( r );
// p = r;
p.Set( r );
// ptilde = rtilde
ptilde.Set( rtilde );
float normb = b.Norm();
if( normb == 0.0 ) normb = 1;
// test convergence
resid = r.Norm() / normb;
if( resid < epsilon ) {
// method converges?
return 0;
}
while( i < i_max ) {
i++;
rho_1 = DenseVectorDotProduct( r, rtilde );
if( rho_1 == 0 ) {
// method fails.
return -i;
}
if (i == 1) {
p.Set( r );
ptilde.Set( rtilde );
}
else {
beta = rho_1 / rho_2;
// p = r + beta * p;
p.Mad( r, p, beta );
// ptilde = ztilde + beta * ptilde;
ptilde.Mad( rtilde, ptilde, beta );
}
// q = A * p;
A.Product( p, q );
// qtilde = A^t * ptilde;
A.TransProduct( ptilde, qtilde );
alpha = rho_1 / DenseVectorDotProduct( ptilde, q );
// x += alpha * p;
x.Mad( x, p, alpha );
// r -= alpha * q;
r.Mad( r, q, -alpha );
// rtilde -= alpha * qtilde;
rtilde.Mad( rtilde, qtilde, -alpha );
rho_2 = rho_1;
// test convergence
resid = r.Norm() / normb;
if( resid < epsilon ) {
// method converges
return i;
}
}
return i;
}
