void updateF(arr u)
{
double v=u(0); // save the controls to easy access them
arr update_F(3, 3); // variable to create a updated Jacobian of f
// F = [(1, 0, -tau*v*cos(theta));
// (0, 1, tau*v*cos(theta));
// (0, 0, 1)]
// --> rows of F
update_F(0,0) = 1; // first row
update_F(0,1) = 0;
update_F(0,2) = -1 * tau * v * sin(mu(2));
update_F(1,0) = 0; // second row
update_F(1,1) = 1;
update_F(1,2) = tau * v * cos(mu(2));
update_F(2,0) = 0; // third row
update_F(2,1) = 0;
update_F(2,2) = 1;
// assign the updated Jacobian to F
F = update_F;
}
CvMat *Kalman::predict(unsigned long tick) {
update_F(tick);
predict_x(tick);
predict_P();
return x_pred;
}
// for SCF, J = K
//void fock_task(BasisSet_t basis, ERD_t erd, int ncpu_f, int num_dmat,
void fock_task(BasisSet_t basis, int ncpu_f, int num_dmat, int *shellptr, double *shellvalue,
int *shellid, int *shellrid, int *f_startind,
int *rowpos, int *colpos, int *rowptr, int *colptr,
double tolscr2, int startrow, int startcol,
int startM, int endM, int startP, int endP,
double **D1, double **D2, double **D3,
double *F1, double *F2, double *F3,
double *F4, double *F5, double *F6,
int ldX1, int ldX2, int ldX3,
int ldX4, int ldX5, int ldX6,
int sizeX1, int sizeX2, int sizeX3,
int sizeX4, int sizeX5, int sizeX6,
double *nitl, double *nsq)
{
int startMN = shellptr[startM];
int endMN = shellptr[endM + 1];
int startPQ = shellptr[startP];
int endPQ = shellptr[endP + 1];
#pragma omp parallel
{
// init
int nt = omp_get_thread_num ();
int nf = nt/ncpu_f;
double *F_MN = &(F1[nf * sizeX1 * num_dmat]);
double *F_PQ = &(F2[nf * sizeX2 * num_dmat]);
double *F_NQ = F3;
double *F_MP = &(F4[nf * sizeX4 * num_dmat]);
double *F_MQ = &(F5[nf * sizeX5 * num_dmat]);
double *F_NP = &(F6[nf * sizeX6 * num_dmat]);
double mynsq = 0.0;
double mynitl = 0.0;
#pragma omp for schedule(dynamic)
for (int i = startMN; i < endMN; i++) {
int M = shellrid[i];
int N = shellid[i];
double value1 = shellvalue[i];
int dimM = f_startind[M + 1] - f_startind[M];
int dimN = f_startind[N + 1] - f_startind[N];
int iX1M = f_startind[M] - f_startind[startrow];
int iX3M = rowpos[M];
int iXN = rowptr[i];
int iMN = iX1M * ldX1+ iXN;
int flag1 = (value1 < 0.0) ? 1 : 0;
for (int j = startPQ; j < endPQ; j++) {
int P = shellrid[j];
int Q = shellid[j];
if ((M > P && (M + P) % 2 == 1) ||
(M < P && (M + P) % 2 == 0))
continue;
if ((M == P) &&
((N > Q && (N + Q) % 2 == 1) ||
(N < Q && (N + Q) % 2 == 0)))
continue;
double value2 = shellvalue[j];
int dimP = f_startind[P + 1] - f_startind[P];
int dimQ = f_startind[Q + 1] - f_startind[Q];
int iX2P = f_startind[P] - f_startind[startcol];
int iX3P = colpos[P];
int iXQ = colptr[j];
int iPQ = iX2P * ldX2+ iXQ;
int iNQ = iXN * ldX3 + iXQ;
int iMP = iX1M * ldX4 + iX2P;
int iMQ = iX1M * ldX5 + iXQ;
int iNP = iXN * ldX6 + iX2P;
int iMP0 = iX3M * ldX3 + iX3P;
int iMQ0 = iX3M * ldX3 + iXQ;
int iNP0 = iXN * ldX3 + iX3P;
int flag3 = (M == P && Q == N) ? 0 : 1;
int flag2 = (value2 < 0.0) ? 1 : 0;
if (fabs(value1 * value2) >= tolscr2) {
int nints;
double *integrals;
mynsq += 1.0;
mynitl += dimM*dimN*dimP*dimQ;
nints=ComputeShellQuartet(basis,nt,M,N,P,Q,&integrals);
//CInt_computeShellQuartet(basis, erd, nt,
// M, N, P, Q, &integrals, &nints);
if (nints != 0) {
update_F(num_dmat, integrals, dimM, dimN, dimP, dimQ,
flag1, flag2, flag3,
iMN, iPQ, iMP, iNP, iMQ, iNQ,
iMP0, iMQ0, iNP0,
D1, D2, D3,
F_MN, F_PQ, F_NQ, F_MP, F_MQ, F_NP,
sizeX1, sizeX2, sizeX3, sizeX4, sizeX5, sizeX6,
ldX1, ldX2, ldX3, ldX4, ldX5, ldX6);
}
}
}
}
#pragma omp critical
{
*nitl += mynitl;
*nsq += mynsq;
}
} /* #pragma omp parallel */
}
