/* ------------------------------------------------------------------------------- */
double
residError (const MAT * A, double *x, const double *b, const double *z)
{
int i;
ptrdiff_t mBlas = A->m, nBlas = A->n, incx = 1, incy = 1;
double dbl_one = 1, minus_one = -1;
double *Ax = (double *) malloc (A->m * sizeof (double));
memcpy (Ax, b, A->m * sizeof (double)); // Ax = b;
for (i = 0; i < A->m; i++)
Ax[i] -= z[i];
// Ax = (b - z);
DGEMV ("N", &mBlas, &nBlas, &dbl_one, A->val, &mBlas, x, &incx, &minus_one,
Ax, &incy);
// Ax = A * x - b + z;
free (Ax);
return DNRM2 (&mBlas, Ax, &incx) / DNRM2 (&nBlas, x, &incx);
}
double residError (const MAT * A, double *x, const double *b, const double *z){
int i;
double *Ax = (double *) malloc (A->m * sizeof (double));
memcpy (Ax, z, A->m * sizeof (double)); // Ax = b;
for (i = 0; i < A->m; i++)
Ax[i] -= b[i];
// Here, Ax = (z - b);
myDGEMV(A, x, Ax);
// Now, compute norm of Ax = A * x - b + z;
double nrm = DNRM2 (A->m, Ax, 1);
free (Ax);
return nrm / DNRM2 (A->n, x, 1);
}
double ProtoMol::Lapack::dnrm2(int *n, double *x, int *incx) {
FAHCheckIn();
#if defined(HAVE_LAPACK)
return dnrm2_(n, x, incx);
#elif defined(HAVE_SIMTK_LAPACK)
return dnrm2_(*n, x, *incx);
#elif defined(HAVE_MKL_LAPACK)
return DNRM2(n, x, incx);
#else
THROW(std::string(__func__) + " not supported");
#endif
}
inline void
computeColNorms (const MAT * A, double *prob)
{
size_t n = A->n, j;
ptrdiff_t mBlas = A->m, incx = 1;
memset (prob, 0, n * sizeof (double));
for (j = 0; j < n; j++)
{
prob[j] = DNRM2 (&mBlas, (A->val + j * A->m), &incx);
prob[j] = pow (prob[j], 2);
}
};
double lsErrorSparse(const SMAT* A, const double* x, const double* b){
int i;
double error = 0.0;
double* residVector = (double*) malloc(A->m * sizeof(double));
for (i = 0 ; i < A->m; i++){
residVector[i] = - b[i];
}
// residVector <- A * x - b
myDGEMVSparse(A, x, residVector);
error = DNRM2(A->m, residVector, 1);
free(residVector);
return error;
}
void mexFunction(int nargout, mxArray *argout[], int nargin, const mxArray *argin[])
{
double tol;
double *A, *b, *L, *x, *u, *ut, *v, *vt, *d, *z;
double beta, alpha, normr;
double c, s, phibar, phi, nn;
double thet, rhot, rho;
double mbeta;
double *resvec, *xvec, *Atr, *r;
long m, n;
long maxit, it, i;
long int_zero = 0;
long int_one = 1;
double dbl_one = 1.0;
double dbl_mone = -1.0;
double dbl_zero = 0.0;
A = mxGetPr(argin[0]);
b = mxGetPr(argin[1]);
L = mxIsEmpty(argin[2]) ? NULL : mxGetPr(argin[2]);
m = mxGetM(argin[0]);
n = mxGetN(argin[0]);
tol = mxGetScalar(argin[3]) * DNRM2(&m, b, &int_one);
maxit = (int)mxGetScalar(argin[4]);
u = malloc(m * sizeof(double));
ut = malloc(m * sizeof(double));
v = malloc(n * sizeof(double));
vt = malloc(n * sizeof(double));
d = malloc(n * sizeof(double));
z = malloc(n * sizeof(double));
argout[0] = mxCreateDoubleMatrix(n, 1, mxREAL);
x = mxGetPr(argout[0]);
if (nargout > 2) {
argout[2] = mxCreateDoubleMatrix(maxit+1, 1, mxREAL);
resvec = mxGetPr(argout[2]);
argout[3] = mxCreateDoubleMatrix(maxit+1, 1, mxREAL);
xvec = mxGetPr(argout[3]);
r = malloc(m * sizeof(double));
memcpy(r, b, m * sizeof(double));
resvec[0] = DNRM2(&m, r, &int_one);
xvec[0] = DNRM2(&n, x, &int_one);
}
memset(x, 0, n * sizeof(double));
memset(d, 0, n * sizeof(double));
memcpy(u, b, m * sizeof(double));
if (L != NULL)
DTRSV("L", "N", "Not Unit", &m, L, &m, u, &int_one);
beta = DNRM2(&m, u, &int_one);
normr = beta;
scale(u, m, 1/beta);
c = 1; s = 0; phibar = beta;
memcpy(z, u, m * sizeof(double));
if (L != NULL)
DTRSV("L", "T", "Not Unit", &m, L, &m, z, &int_one);
DGEMV("T", &m, &n, &dbl_one, A, &m, z, &int_one, &dbl_zero, v, &int_one);
alpha = DNRM2(&n, v, &int_one);
scale(v, n, 1/alpha);
it = 0;
while (it < maxit) {
DGEMV("N", &m, &n, &dbl_one, A, &m, v, &int_one, &dbl_zero, ut, &int_one);
if (L != NULL)
DTRSV("L", "N", "Not Unit", &m, L, &m, ut, &int_one);
for (i = 0; i < m; i++)
u[i] = ut[i] - alpha * u[i];
beta = DNRM2(&m, u, &int_one);
scale(u, m, 1/beta);
thet = - s * alpha;
rhot = c * alpha;
rho = sqrt(rhot * rhot + beta * beta);
c = rhot / rho;
s = - beta / rho;
phi = c * phibar;
phibar = s * phibar;
for (i = 0; i < n; i++) {
d[i] = (v[i] - thet * d[i]) / rho;
x[i] = x[i] + phi * d[i];
}
it++;
if (nargout > 2) {
memcpy(r, b, m * sizeof(double));
DGEMV("N", &m, &n, &dbl_mone, A, &m, x, &int_one, &dbl_one, r, &int_one);
resvec[it] = DNRM2(&m, r, &int_one);
xvec[it] = DNRM2(&n, x, &int_one);
}
normr = fabs(s) * normr;
if (normr < tol)
break;
mbeta = -beta;
memcpy(z, u, m * sizeof(double));
if (L != NULL)
DTRSV("L", "T", "Not Unit", &m, L, &m, z, &int_one);
DGEMV("T", &m, &n, &dbl_one, A, &m, z, &int_one, &mbeta, v, &int_one);
alpha = DNRM2(&n, v, &int_one);
scale(v, n, 1/alpha);
}
if (nargout > 2){
mxSetM(argout[2] , it + 1);
mxSetM(argout[3] , it + 1);
}
if (nargout > 1)
argout[1] = mxCreateScalarDouble(it);
if (nn > tol)
mexPrintf("dense_lsqr: did not converge\n");
else
mexPrintf("dense_lsqr: converged at iteration %d\n", it);
free(u);
free(ut);
free(v);
free(d);
free(z);
if (nargout > 2)
free(r);
}
double
F77_NAME(dnrm2)(const int *n, const double *dx, const int *incx)
{
return DNRM2(n, dx, incx);
}
/* Interface to FORTRAN routine DNRM2. */
Number IpBlasDnrm2(Index size, const Number *x, Index incX)
{
ipfint n=size, INCX=incX;
return DNRM2(&n, x, &INCX);
}
void HLBFGS(int N, int M, double *x,
void EVALFUNC(int,double*,double*,double*,double*),
void EVALFUNC_H(int,double*,double*,double*,double*,HESSIAN_MATRIX&),
void USER_DEFINED_HLBFGS_UPDATE_H(int,int,double*,double*,double*,int,double*, int[]),
void NEWITERATION(int,int,double*,double*,double*,double*),
double PARAMETERS[], int INFO[] )
{
int T = INFO[6];
if ( N < 1 || M < 0 || T < -1 || INFO[4] < 1)
{
HLBFGS_MESSAGE(INFO[5]!=0, 0, PARAMETERS);
return;
}
//allocate mem
double *q = new double[N];
double *g = new double[N];
double *alpha = M<=0? 0: new double[M];
double *rho = M<=0? 0: new double[M];
double *s = M<=0? 0: new double[M*N];
double *y = M<=0? 0: new double[M*N];
double *prev_x = new double[N];
double *prev_g = new double[N];
double *diag = 0;
double *wa = new double[N];
double update_alpha = 1;
HESSIAN_MATRIX m_hessian(N);
if (INFO[3] == 1)
{
diag = new double[N];
for (int i = 0; i < N; i++)
{
diag[i] = 1.0;
}
}
double *prev_q_first_stage = 0;
double *prev_q_update = 0;
double scale = 0.0;
double cg_dginit = 0;
if (INFO[10] == 1)
{
if (INFO[11] == 1)
prev_q_first_stage = new double[N];
prev_q_update = new double[N];
}
//initialize
static int inc = 1;
INFO[1] = 0;
INFO[2] = 0;
double f = 0;
int maxfev = INFO[0], bound = 0, nfev = 0, cur_pos = 0, start = 0;
//line search parameters
double stp, ftol=PARAMETERS[0], xtol=PARAMETERS[1], gtol=PARAMETERS[2],
stpmin = PARAMETERS[3], stpmax = PARAMETERS[4];
int info, keep[20];
double gnorm, rkeep[40];
memset(rkeep, 0, sizeof(double)*40);
memset(keep, 0, sizeof(int)*20);
m_hessian.l_info.allocate_mem(N);
char task1='N';
char task2='T';
double prev_f;
//////////////////////////////////////////////////////////////////////////
do
{
if ( INFO[7] == 1 && ( (T==0) || ( INFO[2] % T == 0) ) )
{
//std::cout << "Generate Hessian\n";
EVALFUNC_H(N, x, INFO[2]==0?0:prev_x, &f, g, m_hessian);
HLBFGS_BUILD_HESSIAN_INFO(m_hessian, INFO);
}
else if (INFO[2] == 0)
{
EVALFUNC(N, x, 0, &f, g);
INFO[1]++;
}
if (INFO[2] > 0 && M > 0)
{
//compute s and y
start = cur_pos*N;
for (int i = 0; i < N; i++)
{
s[start+i] = x[i] - prev_x[i];
y[start+i] = g[i] - prev_g[i];
}
rho[cur_pos] = 1.0/DDOT(&N, &y[start], &inc, &s[start], &inc);
if (INFO[13] == 1)
{
update_alpha = 1.0 / (rho[cur_pos] * 6 * ( prev_f - f + DDOT(&N, g, &inc, &s[start], &inc) ) - 2.0);
}
else if (INFO[13] == 2)
{
update_alpha = 1.0 / (rho[cur_pos] * 2 * ( prev_f - f + DDOT(&N, g, &inc, &s[start], &inc) ) );
}
else if (INFO[13] == 3)
{
update_alpha = 1.0 / (1 + rho[cur_pos] * (6 *(prev_f - f) + 3*(DDOT(&N, g, &inc, &s[start], &inc)+DDOT(&N, prev_g, &inc, &s[start], &inc)) ) );
}
if (INFO[13] != 0)
{
if (update_alpha < 0.01)
{
update_alpha = 0.01;
}
else if (update_alpha > 100)
{
update_alpha = 100;
}
rho[cur_pos] *= update_alpha;
}
}
for (int i = 0; i < N; i++)
{
q[i] = -g[i];
}
if (INFO[2] > 0 && M > 0)
{
bound = INFO[2] > M ? M-1:INFO[2]-1;
HLBFGS_UPDATE_First_Step(N, M, q, s, y, rho, alpha, bound, cur_pos, INFO[2]);
}
if (INFO[10] == 0)
{
if (INFO[7] == 1)
{
dstrsol_(&N,m_hessian.l_info.l,m_hessian.l_info.ldiag,m_hessian.l_info.lcol_ptr,m_hessian.l_info.lrow_ind,q,&task1);
dstrsol_(&N,m_hessian.l_info.l,m_hessian.l_info.ldiag,m_hessian.l_info.lcol_ptr,m_hessian.l_info.lrow_ind,q,&task2);
}
else
{
USER_DEFINED_HLBFGS_UPDATE_H(N, M, q, s, y, cur_pos, diag, INFO);
}
}
else
{
if (INFO[7] == 1)
{
dstrsol_(&N,m_hessian.l_info.l,m_hessian.l_info.ldiag,m_hessian.l_info.lcol_ptr,m_hessian.l_info.lrow_ind,q,&task1);
CONJUGATE_GRADIENT_UPDATE(N, q, prev_q_update, prev_q_first_stage, INFO);
cg_dginit = -DDOT(&N, q, &inc, q, &inc);
dstrsol_(&N,m_hessian.l_info.l,m_hessian.l_info.ldiag,m_hessian.l_info.lcol_ptr,m_hessian.l_info.lrow_ind,q,&task2);
}
else
{
INFO[12] = 0;
USER_DEFINED_HLBFGS_UPDATE_H(N, M, q, s, y, cur_pos, INFO[3]==0 ? (&scale):diag, INFO);
if (INFO[3] == 0)
{
if (M > 0 && INFO[2] > 0 && scale != 1.0)
{
scale = std::sqrt(scale);
DSCAL(&N, &scale, q, &inc);
}
CONJUGATE_GRADIENT_UPDATE(N, q, prev_q_update, prev_q_first_stage, INFO);
cg_dginit = -DDOT(&N, q, &inc, q, &inc);
if (M > 0 && INFO[2] > 0 && scale != 1.0)
DSCAL(&N, &scale, q, &inc);
}
else
{
if (M > 0 && INFO[2] > 0)
{
//use prev_g as temporary array
for (int i = 0; i < N; i++)
{
prev_g[i] = std::sqrt(diag[i]);
q[i] *= prev_g[i];
}
}
CONJUGATE_GRADIENT_UPDATE(N, q, prev_q_update, prev_q_first_stage, INFO);
cg_dginit = -DDOT(&N, q, &inc, q, &inc);
if (M > 0 && INFO[2] > 0)
{
for (int i = 0; i < N; i++)
{
q[i] *= prev_g[i];
}
}
}
INFO[12] = 1;
}
}
if (INFO[2] > 0 && M > 0)
{
HLBFGS_UPDATE_Second_Step(N, M, q, s, y, rho, alpha, bound, cur_pos, INFO[2]);
cur_pos = (cur_pos+1)%M;
}
//store g and x
memcpy(prev_x, x, sizeof(double)*N);
memcpy(prev_g, g, sizeof(double)*N);
prev_f = f;
//linesearch, find new x
bool blinesearch = true;
if (INFO[2] == 0)
{
gnorm = DNRM2(&N, g, &inc);
//if(gnorm > 1)
stp = 1.0/gnorm;
//else
// stp = 1;
}
else
{
stp = 1;
}
info = 0;
do
{
MCSRCH(&N, x, &f, g, q, &stp, &ftol, >ol, &xtol, &stpmin, &stpmax, &maxfev, &info, &nfev, wa, keep, rkeep, INFO[10] == 0?0:(&cg_dginit));
blinesearch =(info == -1);
if (blinesearch)
{
EVALFUNC(N, x, prev_x, &f, g);
INFO[1]++;
}
if (INFO[9] == 1 && prev_f > f) //modify line search to avoid too many function calls
{
info = 1;
break;
}
}
while (blinesearch);
gnorm = DNRM2(&N, g, &inc);
INFO[2]++;
NEWITERATION(INFO[2], INFO[1], x, &f, g, &gnorm);
double xnorm =DNRM2(&N, x, &inc);
xnorm = 1>xnorm?1:xnorm;
rkeep[2] = gnorm;
rkeep[8] = xnorm;
if (info != 1)
{
HLBFGS_MESSAGE(INFO[5]!=0, 1, PARAMETERS);
break;
}
if (gnorm/xnorm <= PARAMETERS[5])
{
HLBFGS_MESSAGE(INFO[5]!=0, 2, PARAMETERS);
break;
}
if (gnorm < PARAMETERS[6])
{
HLBFGS_MESSAGE(INFO[5]!=0, 3, PARAMETERS);
break;
}
if (stp < stpmin || stp > stpmax)
{
HLBFGS_MESSAGE(INFO[5]!=0, 4, PARAMETERS);
break;
}
if (INFO[2] > INFO[4])
{
HLBFGS_MESSAGE(INFO[5]!=0, 5, PARAMETERS);
break;
}
}
while (true);
//free mem
delete[] q;
delete[] g;
if (M > 0)
{
delete[] alpha;
delete[] rho;
delete[] s;
delete[] y;
}
delete[] prev_x;
delete[] prev_g;
delete[] wa;
if (diag)
delete[] diag;
if (prev_q_first_stage)
delete[] prev_q_first_stage;
if (prev_q_update)
delete[] prev_q_update;
}