double NonLocalPotential::energy(bool compute_hpsi, SlaterDet& dsd,
bool compute_forces, vector<vector<double> >& fion,
bool compute_stress, valarray<double>& sigma_enl)
{
const bool compute_anl = false;
const vector<double>& occ = sd_.occ();
const int ngwl = basis_.localsize();
// define atom block size
const int na_block_size = 32;
valarray<double> gr(na_block_size*ngwl); // gr[ig+ia*ngwl]
valarray<double> cgr(na_block_size*ngwl); // cgr[ig+ia*ngwl]
valarray<double> sgr(na_block_size*ngwl); // sgr[ig+ia*ngwl]
vector<vector<double> > tau;
atoms_.get_positions(tau);
double enl = 0.0;
double tsum[6] = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
if ( nspnl == 0 ) return 0.0;
const double omega = basis_.cell().volume();
assert(omega != 0.0);
const double omega_inv = 1.0 / omega;
for ( int is = 0; is < nsp; is++ )
{
if ( npr[is] > 0 ) // species is is non-local
{
if ( compute_anl )
{
// define number of atom blocks
const int na_blocks = na[is] / na_block_size +
( na[is] % na_block_size == 0 ? 0 : 1 );
valarray<double> anl_loc(npr[is]*na_block_size*2*ngwl);
const int nstloc = sd_.nstloc();
// fnl_loc[ipra][n]
valarray<double> fnl_loc(npr[is]*na_block_size*nstloc);
valarray<double> fnl_buf(npr[is]*na_block_size*nstloc);
for ( int ia_block = 0; ia_block < na_blocks; ia_block++ )
{
// process projectors of atoms in block ia_block
const int iastart = ia_block * na_block_size;
const int iaend = (ia_block+1) * na_block_size < na[is] ?
(ia_block+1) * na_block_size :
na[is];
const int ia_block_size = iaend - iastart;
// compute cgr[is][ia][ig], sgr[is][ia][ig]
int k = 3;
double mone = -1.0, zero = 0.0;
char cn='n';
// next line: const cast is ok since dgemm_ does not modify argument
double* gx = const_cast<double*>(basis_.gx_ptr(0));
dgemm(&cn,&cn,(int*)&ngwl,(int*)&ia_block_size,&k,&mone,
gx,(int*)&ngwl, &tau[is][3*iastart],&k,
&zero,&gr[0],(int*)&ngwl);
int len = ia_block_size * ngwl;
#if AIX || BGL
vsincos(&sgr[is][0],&cgr[is][0],&gr[0],&len);
#else
for ( int i = 0; i < len; i++ )
{
const double arg = gr[i];
sgr[i] = sin(arg);
cgr[i] = cos(arg);
}
#endif
// compute anl_loc
for ( int ipr = 0; ipr < npr[is]; ipr++ )
{
// twnl[is][ig+ngwl*ipr]
const double * t = &twnl[is][ngwl*ipr];
const int l = lproj[is][ipr];
// anl_loc[ig+ipra*ngwl]
double * a = &anl_loc[ipr*ia_block_size*ngwl];
if ( l == 0 )
{
for ( int ia = 0; ia < ia_block_size; ia++ )
{
for ( int ig = 0; ig < ngwl; ig++ )
{
a[ig+ia*ngwl] = t[ig] * cgr[ig+ia*ngwl];
a[ig+1+ia*ngwl] = t[ig] * sgr[ig+ia*ngwl];
}
}
}
else if ( l == 1 )
{
for ( int ia = 0; ia < ia_block_size; ia++ )
{
for ( int ig = 0; ig < ngwl; ig++ )
{
/* Next line: -i * eigr */
/* -i * (a+i*b) = b - i*a */
a[ig+ia*ngwl] = t[ig] * sgr[ig+ia*ngwl];
a[ig+1+ia*ngwl] = -t[ig] * cgr[ig+ia*ngwl];
}
}
}
else if ( l == 2 )
{
for ( int ia = 0; ia < ia_block_size; ia++ )
{
for ( int ig = 0; ig < ngwl; ig++ )
{
// Next line: (-) sign for -eigr
a[ig+ia*ngwl] = -t[ig] * cgr[ig+ia*ngwl];
a[ig+1+ia*ngwl] = -t[ig] * sgr[ig+ia*ngwl];
}
}
}
} // ipr
// array anl_loc is complete
// compute fnl[npra][nstloc] = anl^T * c
double one=1.0;
char ct='t';
int twongwl = 2 * ngwl;
int nprnaloc = ia_block_size * npr[is];
const complex<double>* c = sd_.c().cvalptr();
dgemm(&ct,&cn,&nprnaloc,(int*)&nstloc,&twongwl,&one,
&anl_loc[0],&twongwl, (double*)c, &twongwl,
&zero,&fnl_loc[0],&nprnaloc);
// correct for double counting if ctxt_.myrow() == 0
if ( ctxt_.myrow() == 0 )
{
// rank-one update
// dger(m,n,alpha,x,incx,y,incy,a,lda);
// a += alpha * x * transpose(y)
// x = first row of anl_loc
// y^T = first row of c
double alpha = -0.5;
dger(&nprnaloc,(int*)&nstloc,&alpha,&anl_loc[0],&twongwl,
(double*)c,&twongwl,&fnl_loc[0],&nprnaloc);
}
// Allreduce fnl partial sum
MPI_Comm basis_comm = basis_.context().comm();
double fnl_size = nprnaloc*nstloc;
MPI_Allreduce(&fnl_loc[0],&fnl_buf[0],fnl_size,
MPI_DOUBLE,MPI_SUM,basis_comm);
// factor 2.0 in next line is: counting G, -G
fnl_loc = 2.0 * fnl_buf;
// accumulate Enl contribution
const int nbase = ctxt_.mycol() * sd_.c().nb();
for ( int ipr = 0; ipr < npr[is]; ipr++ )
{
const double fac = wt[is][ipr] * omega_inv;
for ( int n = 0; n < nstloc; n++ )
{
const double facn = fac * occ[n + nbase];
for ( int ia = 0; ia < ia_block_size; ia++ )
{
const int i = ia + ipr*ia_block_size + n * nprnaloc;
cout << "fnl_loc[ipr=" << ipr << ",ia=" << ia
<< ",n=" << n << "]: " << fnl_loc[i] << endl;
const double tmp = fnl_loc[i];
enl += facn * tmp * tmp;
fnl_loc[i] = fac * tmp;
}
}
}
if ( compute_hpsi )
{
// compute cp += anl * fnl
complex<double>* cp = dsd.c().valptr();
dgemm(&cn,&cn,&twongwl,(int*)&nstloc,&nprnaloc,&one,
&anl_loc[0],&twongwl, &fnl_loc[0],&nprnaloc,
&one,(double*)cp, &twongwl);
}
assert(compute_forces==false);
assert(compute_stress==false);
} // ia_block
}
else
{
// compute fnl
// block distribution for fnl: same as SlaterDet for nst
DoubleMatrix fnl(ctxt_,anl[is]->n(),sd_.c().n(),
anl[is]->nb(),sd_.c().nb());
const DoubleMatrix c_proxy(sd_.c());
tmap["fnl_gemm"].start();
fnl.gemm('t','n',2.0,*anl[is],c_proxy,0.0);
tmap["fnl_gemm"].stop();
// correct for double counting of G=0 components
// rank-1 update using first row of *anl[is] and c_proxy
fnl.ger(-1.0,*anl[is],0,c_proxy,0);
cout << fnl << endl;
// compute the non-local energy
// multiply fnl[ipra+nprna*n] by fac = wt[is][ipr] * omega_inv;
// block sizes: npr*nalocmax x c().nb()
// loop over local array
double*f = fnl.valptr(0);
const int mb = fnl.mb();
const int nb = fnl.nb();
const int mloc = fnl.mloc();
for ( int li=0; li < fnl.mblocks(); li++)
{
const int mbs = fnl.mbs(li);
for ( int lj=0; lj < fnl.nblocks(); lj++)
{
const int nbs = fnl.nbs(lj);
for ( int ii=0; ii < mbs; ii++)
{
assert(mbs%npr[is]==0);
// mbs/npr[is] is the number of atoms in the block li
const int ipr = ii / (mbs/npr[is]);
const double fac = wt[is][ipr] * omega_inv;
for ( int jj=0; jj < nbs; jj++)
{
// global index: i(li,ii), j(lj,jj)
const int nglobal = fnl.j(lj,jj);
const double facn = fac * occ[nglobal];
const int iii = ii+li*mb;
const int jjj = jj+lj*nb;
const double tmp = f[iii+mloc*jjj];
enl += facn * tmp * tmp;
f[iii+mloc*jjj] = fac * tmp;
}
}
}
}
if ( compute_hpsi )
{
tmap["enl_hpsi"].start();
// Apply operator to electronic states and accumulate in dsd
DoubleMatrix cp_proxy(dsd.c());
cp_proxy.gemm('n','n',1.0,*anl[is],fnl,1.0);
tmap["enl_hpsi"].stop();
}
// ionic forces
if ( compute_forces )
{
tmap["enl_fion"].start();
double *tmpfion = new double[3*na[is]];
for ( int i = 0; i < 3*na[is]; i++ )
tmpfion[i] = 0.0;
DoubleMatrix danl(ctxt_,anl[is]->m(),anl[is]->n(),
anl[is]->mb(),anl[is]->nb());
DoubleMatrix dfnl(ctxt_,fnl.m(),fnl.n(),fnl.mb(),fnl.nb());
const int ngwl = basis_.localsize();
for ( int j = 0; j < 3; j++ )
{
const double *const gxj = basis_.gx_ptr(j);
for ( int ipr = 0; ipr < npr[is]; ipr++ )
{
const int l = lproj[is][ipr];
// twnl[is][ig+ngwl*ipr]
const double *t = &twnl[is][ngwl*ipr];
for ( int ia = 0; ia < naloc[is]; ia++ )
{
// danl[ig+ipra*ngwl]
// index = ig+cmloc_anl*(ia+nais*ipr), ig=0
const int ipra = ia+naloc[is]*ipr;
double *da = danl.valptr(2*(sd_.c().mloc()*ipra));
const double *c = &cosgr[is][ia*ngwl];
const double *s = &singr[is][ia*ngwl];
if ( l == 0 )
{
for ( int ig = 0; ig < ngwl; ig++ )
{
const double tt = gxj[ig] * t[ig];
// Next lines: -i * ( a + ib ) = b - ia
*da++ = tt * *s++;
*da++ = -tt * *c++;
}
}
else if ( l == 1 )
{
for ( int ig = 0; ig < ngwl; ig++ )
{
// Next lines: (-i)**2 * ( a + ib ) = - a - ib
const double tt = - gxj[ig] * t[ig];
*da++ = tt * *c++;
*da++ = tt * *s++;
}
}
else if ( l == 2 )
{
for ( int ig = 0; ig < ngwl; ig++ )
{
// Next lines: (-i) * - ( a + ib ) = i*(a+ib) = - b + ia
const double tt = gxj[ig] * t[ig];
*da++ = -tt * *s++;
*da++ = tt * *c++;
}
}
} // ia
} // ipr
// compute dfnl
const DoubleMatrix c_proxy(sd_.c());
dfnl.gemm('t','n',2.0,danl,c_proxy,0.0);
// Note: no need to correct for double counting of the
// G=0 component which is always zero
// non-local forces
// loop over local array
// block sizes: npr*nalocmax x c().nb()
const double*f = fnl.valptr(0);
const double*df = dfnl.valptr(0);
const int mloc = fnl.mloc();
const int mb = fnl.mb();
const int nb = fnl.nb();
for ( int li=0; li < fnl.mblocks(); li++)
{
// find index of first atom in block li
const int ia_first = nalocmax[is] *
( li * fnl.context().nprow() + fnl.context().myrow() );
const int mbs = fnl.mbs(li);
for ( int lj=0; lj < fnl.nblocks(); lj++)
{
const int nbs = fnl.nbs(lj);
for ( int ii=0; ii < mbs; ii++)
{
// ia_local: index of atom within block li
const int ia_local = ii % ( mbs / npr[is] );
const int ia_global = ia_local + ia_first;
assert(3*ia_global+j < 3*na[is]);
for ( int jj=0; jj < nbs; jj++)
{
const int nglobal = fnl.j(lj,jj);
// Factor 2.0 in next line from derivative of |Fnl|^2
const double facn = 2.0 * occ[nglobal];
const int iii = ii+li*mb;
const int jjj = jj+lj*nb;
tmpfion[3*ia_global+j] -= facn *
f[iii+mloc*jjj] * df[iii+mloc*jjj];
}
}
}
}
} // j
ctxt_.dsum(3*na[is],1,tmpfion,3*na[is]);
for ( int ia = 0; ia < na[is]; ia++ )
{
fion[is][3*ia+0] += tmpfion[3*ia];
fion[is][3*ia+1] += tmpfion[3*ia+1];
fion[is][3*ia+2] += tmpfion[3*ia+2];
}
delete [] tmpfion;
tmap["enl_fion"].stop();
} // compute_forces
if ( compute_stress )
{
const int ngwl = basis_.localsize();
DoubleMatrix danl(ctxt_,anl[is]->m(),anl[is]->n(),
anl[is]->mb(),anl[is]->nb());
DoubleMatrix dfnl(ctxt_,fnl.m(),fnl.n(),fnl.mb(),fnl.nb());
for ( int ij = 0; ij < 6; ij++ )
{
int ipr = 0;
while ( ipr < npr[is] )
{
const int l = lproj[is][ipr];
if ( l == 0 )
{
// dtwnl[is][ipr][ij][ngwl]
// index = ig + ngwl * ( ij + 6 * ipr))
// ipr = iquad + nquad[is] * ilm, where ilm = 0
const double *const dt0 = &dtwnl[is][ngwl*(ij+6*ipr)];
for ( int ia = 0; ia < naloc[is]; ia++ )
{
const int ipra0 = ia+naloc[is]*ipr;
double *da0 = danl.valptr(2*(sd_.c().mloc()*ipra0));
const double *c = &cosgr[is][ia*ngwl];
const double *s = &singr[is][ia*ngwl];
for ( int ig = 0; ig < ngwl; ig++ )
{
const double d0 = dt0[ig];
// danl[is][ipr][iquad][ia][ig].re =
// dtwnl[is][ipr][iquad][j][ig] * cosgr[is][ia][ig]
*da0++ = *c++ * d0;
// danl[is][ipr][iquad][ia][ig].im =
// dtwnl[is][ipr][iquad][j][ig] * singr[is][ia][ig]
*da0++ = *s++ * d0;
}
}
}
else if ( l == 1 )
{
const int ipr1 = ipr;
const int ipr2 = ipr + 1;
const int ipr3 = ipr + 2;
// dtwnl[is][ipr][ij][ngwl]
// index = ig + ngwl * ( ij + 6 * iprx ))
const double *dt1 = &dtwnl[is][ngwl*(ij+6*ipr1)];
const double *dt2 = &dtwnl[is][ngwl*(ij+6*ipr2)];
const double *dt3 = &dtwnl[is][ngwl*(ij+6*ipr3)];
for ( int ia = 0; ia < naloc[is]; ia++ )
{
const int ipra1 = ia+naloc[is]*ipr1;
const int ipra2 = ia+naloc[is]*ipr2;
const int ipra3 = ia+naloc[is]*ipr3;
double *da1 = danl.valptr(2*(sd_.c().mloc()*ipra1));
double *da2 = danl.valptr(2*(sd_.c().mloc()*ipra2));
double *da3 = danl.valptr(2*(sd_.c().mloc()*ipra3));
const double *c = &cosgr[is][ia*ngwl];
const double *s = &singr[is][ia*ngwl];
for ( int ig = 0; ig < ngwl; ig++ )
{
const double d1 = dt1[ig];
const double d2 = dt2[ig];
const double d3 = dt3[ig];
// Next line: (-i)^l factor is -i
// Next line: -i * eigr
// -i * (a+i*b) = b - i*a
const double tc = -*c++; // -cosgr[is][ia][ig]
const double ts = *s++; // singr[is][ia][ig]
*da1++ = d1 * ts;
*da1++ = d1 * tc;
*da2++ = d2 * ts;
*da2++ = d2 * tc;
*da3++ = d3 * ts;
*da3++ = d3 * tc;
}
}
}
else if ( l == 2 )
{
const int ipr4 = ipr;
const int ipr5 = ipr + 1;
const int ipr6 = ipr + 2;
const int ipr7 = ipr + 3;
const int ipr8 = ipr + 4;
// dtwnl[is][ipr][iquad][ij][ngwl]
// index = ig + ngwl * ( ij + 6 * ( iquad + nquad[is] * ipr ))
const double *dt4 = &dtwnl[is][ngwl*(ij+6*ipr4)];
const double *dt5 = &dtwnl[is][ngwl*(ij+6*ipr5)];
const double *dt6 = &dtwnl[is][ngwl*(ij+6*ipr6)];
const double *dt7 = &dtwnl[is][ngwl*(ij+6*ipr7)];
const double *dt8 = &dtwnl[is][ngwl*(ij+6*ipr8)];
for ( int ia = 0; ia < naloc[is]; ia++ )
{
const int ipra4 = ia+naloc[is]*ipr4;
const int ipra5 = ia+naloc[is]*ipr5;
const int ipra6 = ia+naloc[is]*ipr6;
const int ipra7 = ia+naloc[is]*ipr7;
const int ipra8 = ia+naloc[is]*ipr8;
double *da4 = danl.valptr(2*(sd_.c().mloc()*ipra4));
double *da5 = danl.valptr(2*(sd_.c().mloc()*ipra5));
double *da6 = danl.valptr(2*(sd_.c().mloc()*ipra6));
double *da7 = danl.valptr(2*(sd_.c().mloc()*ipra7));
double *da8 = danl.valptr(2*(sd_.c().mloc()*ipra8));
const double *c = &cosgr[is][ia*ngwl];
const double *s = &singr[is][ia*ngwl];
for ( int ig = 0; ig < ngwl; ig++ )
{
const double d4 = dt4[ig];
const double d5 = dt5[ig];
const double d6 = dt6[ig];
const double d7 = dt7[ig];
const double d8 = dt8[ig];
// Next lines: (-i)^2 * ( a + ib ) = - ( a + ib )
const double tc = -*c++;
const double ts = -*s++;
*da4++ = d4 * tc;
*da4++ = d4 * ts;
*da5++ = d5 * tc;
*da5++ = d5 * ts;
*da6++ = d6 * tc;
*da6++ = d6 * ts;
*da7++ = d7 * tc;
*da7++ = d7 * ts;
*da8++ = d8 * tc;
*da8++ = d8 * ts;
}
}
}
else
{
assert(false);
} // l
ipr += 2*l+1;
} // while ipr
// compute dfnl
const DoubleMatrix c_proxy(sd_.c());
dfnl.gemm('t','n',2.0,danl,c_proxy,0.0);
// Note: no need to correct for double counting of the
// G=0 component which is always zero
// partial contributions to the stress sigma_ij
// Note: fnl was already premultiplied by the factor
// fac = wt[is][ipr][iquad] * omega_inv;
const double *const f = fnl.cvalptr(0);
const double *const df = dfnl.cvalptr(0);
const int mb = fnl.mb();
const int nb = fnl.nb();
const int mloc = fnl.mloc();
for ( int li=0; li < fnl.mblocks(); li++)
{
const int mbs = fnl.mbs(li);
for ( int lj=0; lj < fnl.nblocks(); lj++)
{
const int nbs = fnl.nbs(lj);
for ( int ii=0; ii < mbs; ii++)
{
for ( int jj=0; jj < nbs; jj++)
{
// global index: i(li,ii), j(lj,jj)
const int nglobal = fnl.j(lj,jj);
const double facn = 2.0 * occ[nglobal];
const int iii = ii+li*mb;
const int jjj = jj+lj*nb;
const double tmp = f[iii+mloc*jjj];
const double dtmp = df[iii+mloc*jjj];
tsum[ij] += facn * tmp * dtmp;
}
}
}
}
} // ij
} // compute_stress
} // compute_anl
} // npr[is]>0
} // is
ctxt_.dsum(1,1,&enl,1);
sigma_enl = 0.0;
if ( compute_stress )
{
ctxt_.dsum(6,1,&tsum[0],6);
sigma_enl[0] = ( enl + tsum[0] ) * omega_inv;
sigma_enl[1] = ( enl + tsum[1] ) * omega_inv;
sigma_enl[2] = ( enl + tsum[2] ) * omega_inv;
sigma_enl[3] = + tsum[3] * omega_inv;
sigma_enl[4] = + tsum[4] * omega_inv;
sigma_enl[5] = + tsum[5] * omega_inv;
}
return enl;
}
/* Function Definitions */
static void b_eml_lusolve(const emlrtStack *sp, const emxArray_real_T *A,
emxArray_real_T *B)
{
emxArray_real_T *b_A;
int32_T i58;
int32_T iy;
emxArray_int32_T *ipiv;
int32_T info;
int32_T i59;
int32_T b;
int32_T j;
int32_T mmj;
int32_T c;
ptrdiff_t n_t;
ptrdiff_t incx_t;
double * xix0_t;
int32_T ix;
boolean_T overflow;
int32_T k;
real_T temp;
int32_T i60;
boolean_T b_c;
ptrdiff_t m_t;
ptrdiff_t incy_t;
ptrdiff_t lda_t;
double * alpha1_t;
double * Aia0_t;
double * Aiy0_t;
char_T DIAGA;
char_T TRANSA;
char_T UPLO;
char_T SIDE;
emlrtStack st;
emlrtStack b_st;
emlrtStack c_st;
emlrtStack d_st;
emlrtStack e_st;
emlrtStack f_st;
emlrtStack g_st;
emlrtStack h_st;
emlrtStack i_st;
st.prev = sp;
st.tls = sp->tls;
b_st.prev = &st;
b_st.tls = st.tls;
c_st.prev = &b_st;
c_st.tls = b_st.tls;
d_st.prev = &c_st;
d_st.tls = c_st.tls;
e_st.prev = &d_st;
e_st.tls = d_st.tls;
f_st.prev = &e_st;
f_st.tls = e_st.tls;
g_st.prev = &f_st;
g_st.tls = f_st.tls;
h_st.prev = &g_st;
h_st.tls = g_st.tls;
i_st.prev = &h_st;
i_st.tls = h_st.tls;
emlrtHeapReferenceStackEnterFcnR2012b(sp);
emxInit_real_T(sp, &b_A, 2, &ob_emlrtRTEI, true);
st.site = &ib_emlrtRSI;
b_st.site = &lb_emlrtRSI;
c_st.site = &nb_emlrtRSI;
d_st.site = &ob_emlrtRSI;
i58 = b_A->size[0] * b_A->size[1];
b_A->size[0] = A->size[0];
b_A->size[1] = A->size[1];
emxEnsureCapacity(&d_st, (emxArray__common *)b_A, i58, (int32_T)sizeof(real_T),
&ob_emlrtRTEI);
iy = A->size[0] * A->size[1];
for (i58 = 0; i58 < iy; i58++) {
b_A->data[i58] = A->data[i58];
}
b_emxInit_int32_T(&d_st, &ipiv, 2, &ob_emlrtRTEI, true);
e_st.site = &qb_emlrtRSI;
f_st.site = &rb_emlrtRSI;
g_st.site = &sb_emlrtRSI;
h_st.site = &tb_emlrtRSI;
eml_signed_integer_colon(&h_st, muIntScalarMin_sint32(A->size[1], A->size[1]),
ipiv);
info = 0;
if (A->size[1] < 1) {
} else {
i59 = A->size[1] - 1;
b = muIntScalarMin_sint32(i59, A->size[1]);
e_st.site = &pb_emlrtRSI;
for (j = 1; j <= b; j++) {
mmj = A->size[1] - j;
c = (j - 1) * (A->size[1] + 1) + 1;
e_st.site = &if_emlrtRSI;
f_st.site = &yb_emlrtRSI;
if (mmj + 1 < 1) {
iy = -1;
} else {
g_st.site = &ac_emlrtRSI;
h_st.site = &ac_emlrtRSI;
n_t = (ptrdiff_t)(mmj + 1);
h_st.site = &ac_emlrtRSI;
incx_t = (ptrdiff_t)(1);
i58 = b_A->size[0] * b_A->size[1];
xix0_t = (double *)(&b_A->data[emlrtDynamicBoundsCheckFastR2012b(c, 1,
i58, &je_emlrtBCI, &g_st) - 1]);
incx_t = idamax(&n_t, xix0_t, &incx_t);
iy = (int32_T)incx_t - 1;
}
if (b_A->data[(c + iy) - 1] != 0.0) {
if (iy != 0) {
ipiv->data[j - 1] = j + iy;
e_st.site = &jf_emlrtRSI;
f_st.site = &bc_emlrtRSI;
g_st.site = &cc_emlrtRSI;
ix = j;
iy += j;
h_st.site = &dc_emlrtRSI;
overflow = (A->size[1] > 2147483646);
if (overflow) {
i_st.site = &db_emlrtRSI;
check_forloop_overflow_error(&i_st);
}
for (k = 1; k <= A->size[1]; k++) {
i58 = b_A->size[0] * b_A->size[1];
temp = b_A->data[emlrtDynamicBoundsCheckFastR2012b(ix, 1, i58,
&le_emlrtBCI, &g_st) - 1];
i58 = b_A->size[0] * b_A->size[1];
i60 = b_A->size[0] * b_A->size[1];
b_A->data[emlrtDynamicBoundsCheckFastR2012b(ix, 1, i58, &le_emlrtBCI,
&g_st) - 1] = b_A->data[emlrtDynamicBoundsCheckFastR2012b(iy, 1,
i60, &le_emlrtBCI, &g_st) - 1];
i58 = b_A->size[0] * b_A->size[1];
b_A->data[emlrtDynamicBoundsCheckFastR2012b(iy, 1, i58, &le_emlrtBCI,
&g_st) - 1] = temp;
ix += A->size[1];
iy += A->size[1];
}
}
iy = c + mmj;
e_st.site = &kf_emlrtRSI;
if (c + 1 > iy) {
b_c = false;
} else {
b_c = (iy > 2147483646);
}
if (b_c) {
f_st.site = &db_emlrtRSI;
check_forloop_overflow_error(&f_st);
}
for (k = c; k + 1 <= iy; k++) {
b_A->data[k] /= b_A->data[c - 1];
}
} else {
info = j;
}
iy = A->size[1] - j;
e_st.site = &lf_emlrtRSI;
f_st.site = &ec_emlrtRSI;
g_st.site = &fc_emlrtRSI;
if ((mmj < 1) || (iy < 1)) {
} else {
h_st.site = &gc_emlrtRSI;
temp = -1.0;
m_t = (ptrdiff_t)(mmj);
n_t = (ptrdiff_t)(iy);
incx_t = (ptrdiff_t)(1);
incy_t = (ptrdiff_t)(A->size[1]);
lda_t = (ptrdiff_t)(A->size[1]);
alpha1_t = (double *)(&temp);
i58 = b_A->size[0] * b_A->size[1];
i60 = (c + A->size[1]) + 1;
Aia0_t = (double *)(&b_A->data[emlrtDynamicBoundsCheckFastR2012b(i60, 1,
i58, &ke_emlrtBCI, &h_st) - 1]);
i58 = b_A->size[0] * b_A->size[1];
xix0_t = (double *)(&b_A->data[emlrtDynamicBoundsCheckFastR2012b(c + 1,
1, i58, &ke_emlrtBCI, &h_st) - 1]);
i58 = b_A->size[0] * b_A->size[1];
i60 = c + A->size[1];
Aiy0_t = (double *)(&b_A->data[emlrtDynamicBoundsCheckFastR2012b(i60, 1,
i58, &ke_emlrtBCI, &h_st) - 1]);
dger(&m_t, &n_t, alpha1_t, xix0_t, &incx_t, Aiy0_t, &incy_t, Aia0_t,
&lda_t);
}
}
if ((info == 0) && (!(b_A->data[(A->size[1] + b_A->size[0] * (A->size[1] - 1))
- 1] != 0.0))) {
info = A->size[1];
}
}
if (info > 0) {
b_st.site = &mb_emlrtRSI;
warn_singular(&b_st);
}
b_st.site = &yf_emlrtRSI;
for (iy = 0; iy + 1 < A->size[1]; iy++) {
if (ipiv->data[iy] != iy + 1) {
temp = B->data[iy];
B->data[iy] = B->data[ipiv->data[iy] - 1];
B->data[ipiv->data[iy] - 1] = temp;
}
}
emxFree_int32_T(&ipiv);
b_st.site = &ag_emlrtRSI;
c_st.site = &ic_emlrtRSI;
if (A->size[1] < 1) {
} else {
d_st.site = &jc_emlrtRSI;
temp = 1.0;
DIAGA = 'U';
TRANSA = 'N';
UPLO = 'L';
SIDE = 'L';
e_st.site = &jc_emlrtRSI;
m_t = (ptrdiff_t)(A->size[1]);
e_st.site = &jc_emlrtRSI;
n_t = (ptrdiff_t)(1);
e_st.site = &jc_emlrtRSI;
lda_t = (ptrdiff_t)(A->size[1]);
e_st.site = &jc_emlrtRSI;
incx_t = (ptrdiff_t)(A->size[1]);
i58 = b_A->size[0] * b_A->size[1];
emlrtDynamicBoundsCheckFastR2012b(1, 1, i58, &ie_emlrtBCI, &d_st);
Aia0_t = (double *)(&b_A->data[0]);
xix0_t = (double *)(&B->data[0]);
alpha1_t = (double *)(&temp);
dtrsm(&SIDE, &UPLO, &TRANSA, &DIAGA, &m_t, &n_t, alpha1_t, Aia0_t, &lda_t,
xix0_t, &incx_t);
}
b_st.site = &bg_emlrtRSI;
c_st.site = &ic_emlrtRSI;
if (A->size[1] < 1) {
} else {
d_st.site = &jc_emlrtRSI;
temp = 1.0;
DIAGA = 'N';
TRANSA = 'N';
UPLO = 'U';
SIDE = 'L';
e_st.site = &jc_emlrtRSI;
m_t = (ptrdiff_t)(A->size[1]);
e_st.site = &jc_emlrtRSI;
n_t = (ptrdiff_t)(1);
e_st.site = &jc_emlrtRSI;
lda_t = (ptrdiff_t)(A->size[1]);
e_st.site = &jc_emlrtRSI;
incx_t = (ptrdiff_t)(A->size[1]);
i58 = b_A->size[0] * b_A->size[1];
emlrtDynamicBoundsCheckFastR2012b(1, 1, i58, &ie_emlrtBCI, &d_st);
Aia0_t = (double *)(&b_A->data[0]);
xix0_t = (double *)(&B->data[0]);
alpha1_t = (double *)(&temp);
dtrsm(&SIDE, &UPLO, &TRANSA, &DIAGA, &m_t, &n_t, alpha1_t, Aia0_t, &lda_t,
xix0_t, &incx_t);
}
emxFree_real_T(&b_A);
emlrtHeapReferenceStackLeaveFcnR2012b(sp);
}
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
// Allocate space.
double *Bbar, *delta_tilda_k, *delta_k;
double *tmp_stage, *tmp_stage_b;
double a, b;
mwSignedIndex p;
mwSignedIndex i;
#ifdef SUPERSAFE
mxArray* Bbar_copy = mxDuplicateArray(prhs[0]); // Safe
#else
mxArray* Bbar_copy = (prhs[0]); // Fast and Dangerous
#endif
// Read Input
p = mxGetM(prhs[0]);
Bbar = mxGetPr(Bbar_copy);
a = *(mxGetPr(prhs[1]));
b = *(mxGetPr(prhs[2]));
delta_tilda_k = mxGetPr(prhs[3]);
delta_k = mxGetPr(prhs[4]);
/* mexPrintf("a %f b %f Bbar(1,1) %f Bbar(1,3) %f", a, b, Bbar[0], Bbar[2]); */
/* print_arr(delta_tilda_k, p, "delta_tilda_k"); */
/* print_arr(delta_k, p, "delta_k"); */
#ifdef SAFE
tmp_stage = mxCalloc(p , sizeof(double));
tmp_stage_b = mxCalloc(p , sizeof(double));
#else
tmp_stage = mxGetPr(prhs[5]);
tmp_stage_b = mxGetPr(prhs[6]);
#endif
/* Stage 1: Bbar is symmetric.
*/
// tmp_stage = Bbar * delta_tilda_k;
dsymv("U", &p, &one, Bbar, &p, delta_tilda_k, &inc, &zero, tmp_stage, &inc);
// tmp_deno = a + b * (delta_k' * tmp_stage);
double tmp_deno = a + b * ddot(&p, delta_k, &inc, tmp_stage, &inc);
// tmp_stage_b = Bbar' * delta_k; == Bbar * delta_k (since Bbar is symmetric)
dsymv("U", &p, &one, Bbar, &p, delta_k, &inc, &zero, tmp_stage_b, &inc);
// Bbar = Bbar + (-b/tmp_deno) * tmp_stage * tmp_stage_b';
double b_by_tmp = -b / tmp_deno;
dger(&p, &p, &b_by_tmp, tmp_stage, &inc, tmp_stage_b, &inc, Bbar, &p);
/* Stage 2: Bbar is no longer symmetric.
Need to use dgemv instead of dsymv.
*/
// tmp_stage = Bbar * delta_k;
dgemv("N", &p, &p, &one, Bbar, &p, delta_k, &inc, &zero, tmp_stage, &inc);
// tmp_deno = a + b * (delta_tilda_k' * tmp_stage);
tmp_deno = a + b * ddot(&p, delta_tilda_k, &inc, tmp_stage, &inc);
// tmp_stage_b = Bbar' * delta_tilda_k;
dgemv("T", &p, &p, &one, Bbar, &p, delta_tilda_k, &inc, &zero, tmp_stage_b, &inc);
// Bbar = Bbar + (-b/tmp_deno) * tmp_stage * tmp_stage_b';
b_by_tmp = -b / tmp_deno;
dger(&p, &p, &b_by_tmp, tmp_stage, &inc, tmp_stage_b, &inc, Bbar, &p);
// In place edit Bbar and return the value.
plhs[0] = Bbar_copy;
#ifdef SAFE
mxFree(tmp_stage);
mxFree(tmp_stage_b);
#endif
}
