static mxArray * McomputeH(int nargout_,
const mxArray * Uc,
const mxArray * Vc,
const mxArray * E_z,
const mxArray * E_zz,
const mxArray * RR) {
double **Ucc, **Vcc, **E_zc, **E_zzc, **RRc, **KK1c, **KK2c, **kk3c, **P_tc, **R_tc, **zz_hat_tc, **Tmp1c, **Tmp2c, **vecH_hatc=NULL,
delta_err, epsilon;
int tc, Fc, Nc, junk1, junk2, kc, ic, jc, ii, jj, kk, ll, i_max, **sparsityMap;
mxArray * vecH_hat = NULL;
mxArray * KK1 = NULL;
mxArray * R_t = NULL;
mxArray * P_t = NULL;
mxArray * t = NULL;
mxArray * KK2 = NULL;
mxArray * F = NULL;
mxArray * N = NULL;
mxArray * k = NULL;
#if MYDEBUG
#define DEBUGFILENAME ".\\debug.mat"
IO_MLabCreateFile(DEBUGFILENAME);
#endif
Ucc = getMxArray(Uc, &Fc, &Nc);
#if MYDEBUG
IO_MLabWriteDoubleImg(DEBUGFILENAME, "Ucc", Ucc[0], Fc, Nc);
#endif
Vcc = getMxArray(Vc, &junk1, &junk2);
#if MYDEBUG
IO_MLabWriteDoubleImg(DEBUGFILENAME, "Vcc", Vcc[0], Fc, Nc);
#endif
E_zc = getMxArray(E_z, &kc, &junk1);
#if MYDEBUG
IO_MLabWriteDoubleImg(DEBUGFILENAME, "E_zc", E_zc[0], kc, Fc);
#endif
E_zzc = getMxArray(E_zz, &junk1, &junk2);
#if MYDEBUG
IO_MLabWriteDoubleImg(DEBUGFILENAME, "E_zzc", E_zzc[0], kc*Fc, kc);
#endif
RRc = getMxArray(RR, &junk1, &junk2);
#if MYDEBUG
IO_MLabWriteDoubleImg(DEBUGFILENAME, "RRc", RRc[0], 2*Fc, 3);
#endif
/*
* KK2 = zeros(3*N*(k+1), 3*N*(k+1));
*/
KK2c = createMatrix(3*Nc*(kc+1), 3*Nc*(kc+1));
/*
* KK3 = zeros(3*N, k+1);
*/
kk3c = createMatrix(1,3*Nc*(kc+1));
P_tc = createMatrix(1, 2*Nc);
zz_hat_tc = createMatrix(kc+1, kc+1);
R_tc = createMatrix(2, 3);
KK1c = createMatrix(2*Nc, 3*Nc);
Tmp1c = createMatrix(3*Nc, 3*Nc);
Tmp2c = createMatrix(1, 3*Nc);
for(tc=0; tc<Fc; tc++)
{
/*
* P_t = [Uc(t,:); Vc(t,:)];
*/
memset(P_tc[0], 0, sizeof(double)*2*Nc);
for(jc=0; jc<Nc; jc++)
{
P_tc[0][2*jc] = Ucc[tc][jc];
P_tc[0][2*jc+1] = Vcc[tc][jc];
}
#if MYDEBUG
IO_MLabWriteDoubleImg(DEBUGFILENAME, "P_tc", P_tc[0], 1, 2*Nc);
#endif
/*
* zz_hat_t = [1 E_z(:,t)'; E_z(:,t) E_zz((t-1)*k+1:t*k,:)];
*/
memset(zz_hat_tc[0], 0, sizeof(double)*(kc+1)*(kc+1));
zz_hat_tc[0][0] = 1;
for(jc=1; jc<kc+1; jc++)
{
zz_hat_tc[jc][0] = E_zc[jc-1][tc];
zz_hat_tc[0][jc] = E_zc[jc-1][tc];
}
for(ic=1; ic<kc+1; ic++)
{
for(jc=1; jc<kc+1; jc++)
{
zz_hat_tc[ic][jc] = E_zzc[tc*kc + (ic-1)][jc-1];
}
}
#if MYDEBUG
IO_MLabWriteDoubleImg(DEBUGFILENAME, "zz_hat_tc", zz_hat_tc[0], kc+1, kc+1);
#endif
/*
* R_t = [RR(t, :); RR(t+F, :)];
*/
for(jc=0; jc<3; jc++)
{
R_tc[0][jc] = RRc[tc][jc];
R_tc[1][jc] = RRc[tc+Fc][jc];
}
/*
* KK1 = kron(speye(N), R_t);
*/
memset(KK1c[0], 0, sizeof(double)*2*Nc*3*Nc);
for(ic=0; ic<Nc; ic++)
{
KK1c[ic*2][ic*3] = R_tc[0][0];
KK1c[ic*2][ic*3+1] = R_tc[0][1];
KK1c[ic*2][ic*3+2] = R_tc[0][2];
KK1c[ic*2+1][ic*3] = R_tc[1][0];
KK1c[ic*2+1][ic*3+1] = R_tc[1][1];
KK1c[ic*2+1][ic*3+2] = R_tc[1][2];
}
#if MYDEBUG
IO_MLabWriteDoubleImg(DEBUGFILENAME, "KK1c", KK1c[0], 2*Nc, 3*Nc);
#endif
/* computes KK1'*KK1 */
memset(Tmp1c[0], 0, sizeof(double)*3*Nc*3*Nc);
Tmp1c[0][0] = KK1c[0][0]*KK1c[0][0] + KK1c[1][0]*KK1c[1][0];
Tmp1c[0][1] = KK1c[0][0]*KK1c[0][1] + KK1c[1][0]*KK1c[1][1];
Tmp1c[0][2] = KK1c[0][0]*KK1c[0][2] + KK1c[1][0]*KK1c[1][2];
Tmp1c[1][0] = KK1c[0][1]*KK1c[0][0] + KK1c[1][1]*KK1c[1][0];
Tmp1c[1][1] = KK1c[0][1]*KK1c[0][1] + KK1c[1][1]*KK1c[1][1];
Tmp1c[1][2] = KK1c[0][1]*KK1c[0][2] + KK1c[1][1]*KK1c[1][2];
Tmp1c[2][0] = KK1c[0][2]*KK1c[0][0] + KK1c[1][2]*KK1c[1][0];
Tmp1c[2][1] = KK1c[0][2]*KK1c[0][1] + KK1c[1][2]*KK1c[1][1];
Tmp1c[2][2] = KK1c[0][2]*KK1c[0][2] + KK1c[1][2]*KK1c[1][2];
for(ic=1; ic<Nc; ic++)
{
Tmp1c[ic*3][ic*3] = Tmp1c[0][0];
Tmp1c[ic*3][ic*3+1] = Tmp1c[0][1];
Tmp1c[ic*3][ic*3+2] = Tmp1c[0][2];
Tmp1c[ic*3+1][ic*3] = Tmp1c[1][0];
Tmp1c[ic*3+1][ic*3+1] = Tmp1c[1][1];
Tmp1c[ic*3+1][ic*3+2] = Tmp1c[1][2];
Tmp1c[ic*3+2][ic*3] = Tmp1c[2][0];
Tmp1c[ic*3+2][ic*3+1] = Tmp1c[2][1];
Tmp1c[ic*3+2][ic*3+2] = Tmp1c[2][2];
}
#if MYDEBUG
IO_MLabWriteDoubleImg(DEBUGFILENAME, "Tmp1c", Tmp1c[0], 3*Nc, 3*Nc);
#endif
/*
* KK2 = KK2 + kron(zz_hat_t', KK1'*KK1);
*/
/*for(ii=0; ii<(kc+1); ii++)
for(kk=0; kk<(3*Nc); kk++)
for(jj=0; jj<(kc+1); jj++)
for(ll=0; ll<(3*Nc); ll++)
KK2c[(3*Nc)*ii+kk][(3*Nc)*jj+ll] += zz_hat_tc[ii][jj]*Tmp1c[kk][ll];*/
for(ii=0; ii<(kc+1); ii++)
for(kk=0; kk<Nc; kk++)
for(jj=0; jj<(kc+1); jj++)
{
KK2c[(3*Nc)*ii+kk*3][(3*Nc)*jj+kk*3] += zz_hat_tc[ii][jj]*Tmp1c[kk*3][kk*3];
KK2c[(3*Nc)*ii+kk*3][(3*Nc)*jj+kk*3+1] += zz_hat_tc[ii][jj]*Tmp1c[kk*3][kk*3+1];
KK2c[(3*Nc)*ii+kk*3][(3*Nc)*jj+kk*3+2] += zz_hat_tc[ii][jj]*Tmp1c[kk*3][kk*3+2];
KK2c[(3*Nc)*ii+kk*3+1][(3*Nc)*jj+kk*3] += zz_hat_tc[ii][jj]*Tmp1c[kk*3+1][kk*3];
KK2c[(3*Nc)*ii+kk*3+1][(3*Nc)*jj+kk*3+1] += zz_hat_tc[ii][jj]*Tmp1c[kk*3+1][kk*3+1];
KK2c[(3*Nc)*ii+kk*3+1][(3*Nc)*jj+kk*3+2] += zz_hat_tc[ii][jj]*Tmp1c[kk*3+1][kk*3+2];
KK2c[(3*Nc)*ii+kk*3+2][(3*Nc)*jj+kk*3] += zz_hat_tc[ii][jj]*Tmp1c[kk*3+2][kk*3];
KK2c[(3*Nc)*ii+kk*3+2][(3*Nc)*jj+kk*3+1] += zz_hat_tc[ii][jj]*Tmp1c[kk*3+2][kk*3+1];
KK2c[(3*Nc)*ii+kk*3+2][(3*Nc)*jj+kk*3+2] += zz_hat_tc[ii][jj]*Tmp1c[kk*3+2][kk*3+2];
}
#if MYDEBUG
IO_MLabWriteDoubleImg(DEBUGFILENAME, "KK2c", KK2c[0], 3*Nc*(kc+1), 3*Nc*(kc+1));
#endif
/*
* KK3 = KK3 + KK1'* P_t(:) * [1 E_z(:,t)'];
*/
/* computes KK1'*P_t(:) */
memset(Tmp2c[0], 0, sizeof(double)*3*Nc);
for(ic=0; ic<Nc; ic++)
{
Tmp2c[0][3*ic] = KK1c[2*ic][3*ic]*P_tc[0][2*ic] + KK1c[2*ic+1][3*ic]*P_tc[0][2*ic+1];
Tmp2c[0][3*ic+1] = KK1c[2*ic][3*ic+1]*P_tc[0][2*ic] + KK1c[2*ic+1][3*ic+1]*P_tc[0][2*ic+1];
Tmp2c[0][3*ic+2] = KK1c[2*ic][3*ic+2]*P_tc[0][2*ic] + KK1c[2*ic+1][3*ic+2]*P_tc[0][2*ic+1];
}
#if MYDEBUG
IO_MLabWriteDoubleImg(DEBUGFILENAME, "Tmp2c", Tmp2c[0], 1, 3*Nc);
#endif
for(ic=0; ic<3*Nc; ic++)
{
/*KK3c[ic][0] = KK3c[ic][0] + Tmp2c[0][ic];*/
kk3c[0][ic] = kk3c[0][ic] + Tmp2c[0][ic];
for(jc=1; jc<(kc+1); jc++)
/*KK3c[ic][jc] = KK3c[ic][jc] + Tmp2c[0][ic]*E_zc[jc-1][tc];*/
kk3c[0][ic + jc*(3*Nc)] = kk3c[0][ic + jc*(3*Nc)] + Tmp2c[0][ic]*E_zc[jc-1][tc];
}
#if MYDEBUG
IO_MLabWriteDoubleImg(DEBUGFILENAME, "kk3c", kk3c[0], 1, 3*Nc*(kc+1));
#endif
//printf("debug\n");
}
freeMatrix(Ucc);
freeMatrix(Vcc);
freeMatrix(E_zc);
freeMatrix(E_zzc);
freeMatrix(RRc);
freeMatrix(KK1c);
freeMatrix(P_tc);
freeMatrix(R_tc);
freeMatrix(zz_hat_tc);
freeMatrix(Tmp1c);
freeMatrix(Tmp2c);
vecH_hatc = createMatrix(1, 3*Nc*(kc+1));
for(ic=0; ic<3*Nc; ic++)
vecH_hatc[0][ic] = (double) rand() / (double) RAND_MAX;
epsilon = 0.000000001;
i_max = 1000;
//sparsityMap = createIntMatrix(3*Nc*(kc+1), 3*Nc*(kc+1)+1);
//getSparsityMap(3*Nc*(kc+1), 3*Nc*(kc+1), KK2c, sparsityMap);
//delta_err = conjgradient(3*Nc*(kc+1), KK2c, kk3c, i_max, epsilon, vecH_hatc);
delta_err = conjgradient2(3*Nc*(kc+1), KK2c, kk3c, i_max, epsilon, kc, vecH_hatc);
if (delta_err>0.01)
printf("Large CG error\n");
vecH_hat = mxCreateDoubleMatrix(3*Nc*(kc+1), 1, mxREAL);
memcpy(mxGetPr(vecH_hat), vecH_hatc[0], sizeof(double)*3*Nc*(kc+1));
freeMatrix(KK2c);
freeMatrix(kk3c);
freeMatrix(vecH_hatc);
return vecH_hat;
}
void copy(const std::string& variableName, std::vector<T>& dataVec)
{
parseVariableName(variableName);
getMxArray(variableName);
copyFromMxArray(dataVec);
}
