int chrono_guess(spinor * const trial, spinor * const phi, spinor ** const v, int index_array[],
const int _N, const int _n, const int V, matrix_mult f) {
int info = 0;
int i, j, N=_N, n=_n;
_Complex double s;
static int init_csg = 0;
static _Complex double *bn = NULL;
static _Complex double *G = NULL;
int max_N = 20;
if(N > 0) {
if(g_proc_id == 0 && g_debug_level > 1) {
printf("CSG: preparing trial vector \n");
fflush(stdout);
}
if(init_csg == 0) {
init_csg = 1;
bn = (_Complex double*) malloc(max_N*sizeof(_Complex double));
G = (_Complex double*) malloc(max_N*max_N*sizeof(_Complex double));
}
/* Construct an orthogonal basis */
for(j = n-1; j > n-2; j--) {
for(i = j-1; i > -1; i--) {
s = scalar_prod(v[index_array[j]], v[index_array[i]], V, 1);
assign_diff_mul(v[index_array[i]], v[index_array[j]], s, V);
if(g_debug_level > 2) {
s = scalar_prod(v[index_array[i]], v[index_array[j]], V, 1);
if(g_proc_id == 0) {
printf("CSG: <%d,%d> = %e +i %e \n", i, j, creal(s), cimag(s));fflush(stdout);
}
}
}
}
/* Generate "interaction matrix" V^\dagger f V */
/* We assume that f is hermitian */
/* Generate also the right hand side */
for (j = 0; j < n; j++){
f(trial, v[index_array[j]]);
/* Only the upper triangular part is stored */
for(i = 0; i < j+1; i++){
G[i*N + j] = scalar_prod(v[index_array[i]], trial, V, 1);
if(j != i) {
(G[j*N + i]) = conj(G[i*N + j]);
}
if(g_proc_id == 0 && g_debug_level > 2) {
printf("CSG: G[%d*N + %d]= %e + i %e \n", i, j, creal(G[i*N + j]), cimag(G[i*N + j]));
fflush(stdout);
}
}
/* The right hand side */
bn[j] = scalar_prod(v[index_array[j]], phi, V, 1);
}
/* Solver G y = bn for y and store it in bn */
LUSolve(n, G, N, bn);
/* Construct the new guess vector */
if(info == 0) {
mul(trial, bn[n-1], v[index_array[n-1]], V);
if(g_proc_id == 0 && g_debug_level > 2) {
printf("CSG: bn[%d] = %f %f\n", index_array[n-1], creal(bn[index_array[n-1]]), cimag(bn[index_array[n-1]]));
}
for(i = n-2; i > -1; i--) {
assign_add_mul(trial, v[index_array[i]], bn[i], V);
if(g_proc_id == 0 && g_debug_level > 2) {
printf("CSG: bn[%d] = %f %f\n", index_array[i], creal(bn[index_array[i]]), cimag(bn[index_array[i]]));
}
}
}
else {
assign(trial, phi, V);
}
if(g_proc_id == 0 && g_debug_level > 1) {
printf("CSG: done! n= %d N=%d \n", n, N);fflush(stdout);
}
}
else {
if(g_proc_id == 0 && g_debug_level > 1) {
printf("CSG: using zero trial vector \n");
fflush(stdout);
}
zero_spinor_field(trial, V);
}
return(info);
}
double _RKOCPfhg_implicitZ(Vector nlp_x, Vector nlp_h, Vector nlp_g) {
RKOCP r = thisRKOCP;
double phi = 0;
int i_node;
int i, j, k;
int m = r->nlp_m;
int p = r->nlp_p;
int n_nodes = r->n_nodes;
int n_parameters = r->n_parameters;
int n_states = r->n_states;
int n_controls = r->n_controls;
int n_inequality = r->ocp->n_inequality;
int n_initial = r->ocp->n_initial;
int n_terminal = r->ocp->n_terminal;
int only_compute_lte = r->only_compute_lte;
int rk_stages = r->rk_stages;
double **W = r->rk_w->e;
//double **rk_a = r->rk_a->e;
//double *rk_b = r->rk_b->e;
double *rk_c = r->rk_c->e;
//double *rk_e = r->rk_e->e;
Vector T = r->T;
Matrix Yw = r->Yw;
Vector yc = r->yc;
Vector uc = r->uc;
Vector pc = r->pc;
//Vector local_error = r->local_error;
//Vector lte = r->lte;
Vector Gamma = r->Gamma;
Vector G = r->G;
Vector Psi = r->Psi;
Matrix *Ydata = r->Ydata; // Ydata[i] => Ystage
Matrix *Udata = r->Udata;
Vector *Zstage = r->Kstage;
Matrix Ystage;
double ti;
// FIX: preallocate the variables
Vector y_old = VectorNew(n_states+1);
Vector *Z_old = NULL;
Z_old = VectorArrayNew(r->rk_stages, n_states+1);
Vector f = VectorNew(n_states+1);
Vector res = VectorNew(rk_stages*(n_states+1));
Vector dZ = VectorNew(rk_stages*(n_states+1));
Vector y = VectorNew(n_states+1);
Matrix L;// = MatrixNew(rk_stages*(n_states+1), rk_stages*(n_states+1));
Matrix U;// = MatrixNew(rk_stages*(n_states+1), rk_stages*(n_states+1));
int *P;// = (int *)malloc(rk_stages*(n_states+1)*sizeof(int));
if (only_compute_lte == NO) {
if (m > 0) {
VectorSetAllTo(nlp_h, 0.0);
}
if (p > 0) {
VectorSetAllTo(nlp_g, 0.0);
}
}
for (i = 0; i < n_parameters; i++) {
pc->e[i] = nlp_x->e[i];
}
for (j = 0; j < n_states; j++) {
Yw->e[0][j] = nlp_x->e[n_parameters+j];
y->e[j] = Yw->e[0][j];
}
Yw->e[0][n_states] = 0;
y->e[n_states] = 0;
if (n_controls > 0) {
_setUdata(r, nlp_x);
for (i = 0; i < n_controls; i++) {
uc->e[i] = nlp_x->e[n_parameters+n_states+i];
}
}
if (only_compute_lte == NO) {
// form Gamma(y(t_initial), p)
if (n_initial > 0) {
r->ocp->initial_constraints(y, pc, Gamma);
for (i = 0; i < n_initial; i++) {
nlp_h->e[i] = Gamma->e[i];
}
}
// form g(y,u,p,t_initial)
if (n_inequality > 0) {
r->ocp->inequality_constraints(y, uc, pc, T->e[0], G);
for (i = 0; i < n_inequality; i++) {
nlp_g->e[i] = G->e[i];
}
}
}
// integrate the ODEs using an implicit Runge-Kutta method
int MAX_NEWTON_ITER = 15;
int iter;
int err;
for (i_node = 0; i_node < (n_nodes-1); i_node++) {
L = r->Lnode[i_node];
U = r->Unode[i_node];
P = r->pLUnode[i_node];
// form H = (1/h) W (x) I - I (x) J
// get the L, U, p factors of H
double h = T->e[i_node+1] - T->e[i_node];
err = _formIRKmatrixZ(r, y, uc, pc, T->e[i_node], h, L, U, P);
if (err != 0) {
printf("IRK factor error: t = %e\n", T->e[i_node]);
//pause();
r->integration_fatal_error = YES;
if (m > 0) VectorSetAllTo(nlp_h, 0);
if (p > 0) VectorSetAllTo(nlp_g, 0);
return 0;
}
// initialize Kstage
// FIX: extrapolate previous result
if (i_node == 0 ) {
for (i = 0; i < rk_stages; i++) {
for (j = 0; j < n_states+1; j++) {
Zstage[i]->e[j] = 0;
}
}
} else {
// given y0, Z0, y1, tau = h1/h0, estimate Z1
// using a Lagrange interpolation formula
double tau;
tau = h / (T->e[i_node] - T->e[i_node-1]);
_estimateZ(r, y_old, Z_old, y, tau, Zstage);
}
Ystage = Ydata[i_node];
// Y_i = y + Z_i
// Ydot_i = (1/h) sum_{j = 1, s} w_{i,j} * Z_j
// set res_i = -Ydot_i
for (i = 0; i < rk_stages; i++) {
for (j = 0; j < n_states+1; j++) {
Ystage->e[i][j] = y->e[j] + Zstage[i]->e[j];
}
for (k = 0; k < n_states+1; k++) {
//Ydot[i]->e[k] = 0.0;
res->e[i*(n_states+1) + k] = 0;
}
for (j = 0; j < rk_stages; j++) {
for (k = 0; k < n_states+1; k++) {
//Ydot[i]->e[k] += W[i][j] * Zstage[j]->e[k] / h;
res->e[i*(n_states+1) + k] -= W[i][j] * Zstage[j]->e[k] / h;
}
}
}
for (iter = 0; iter < MAX_NEWTON_ITER; iter++) {
for (i = 0; i < rk_stages; i++) {
ti = T->e[i_node] + h*rk_c[i];
// get uc = u(t_i)
for (j = 0; j < n_controls; j++) {
uc->e[j] = Udata[i_node]->e[i][j];
}
// get yc = Y_i
for (j = 0; j < n_states+1; j++) {
yc->e[j] = Ystage->e[i][j];
}
// form f(Y_i, u_i, p, t_i)
f->e[n_states] = r->ocp->differential_equations(yc, uc, pc, ti, f);
// form res_i = Ydot_i - f(Y_i, u_i, p, t_i)
for (j = 0; j < n_states+1; j++) {
res->e[i*(n_states+1) + j] += f->e[j];
}
}
// solve H*dZ = -res = -(ydot - f(y,u,p,t))
err = LUSolve(L, U, P, res, dZ);
// Z = Z + dZ
for (i = 0; i < rk_stages; i++) {
for (j = 0; j < n_states+1; j++) {
Zstage[i]->e[j] += dZ->e[i*(n_states+1) + j];
}
}
// Y_i = y + Z_i
// Ydot_i = (1/h) sum_{j = 1, s} w_{i,j} * Z_j
// set res_i = -Ydot_i
for (i = 0; i < rk_stages; i++) {
for (j = 0; j < n_states+1; j++) {
Ystage->e[i][j] = y->e[j] + Zstage[i]->e[j];
}
for (k = 0; k < n_states+1; k++) {
//Ydot[i]->e[k] = 0.0;
res->e[i*(n_states+1) + k] = 0;
}
for (j = 0; j < rk_stages; j++) {
for (k = 0; k < n_states+1; k++) {
//Ydot[i]->e[k] += W[i][j] * Zstage[j]->e[k] / h;
res->e[i*(n_states+1) + k] -= W[i][j] * Zstage[j]->e[k] / h;
}
}
}
// test for convergence
double norm_dZ = VectorNorm(dZ);
if (norm_dZ <= 0.01*r->tolerance) {
break;
}
}
//printf("i_node: %d, iter: %d\n", i_node, iter);
// y_old = y
// Z_old = Z
for (j = 0; j < n_states+1; j++) {
for (i = 0; i < rk_stages; i++) {
Z_old[i]->e[j] = Zstage[i]->e[j];
}
y_old->e[j] = y->e[j];
}
// Assumes R-K methods with c[s] = 1
// form Yw[i_node+1] = Ystage_s
// update y = Yw[i_node+1]
for (j = 0; j < n_states+1; j++) {
Yw->e[i_node+1][j] = Ystage->e[rk_stages-1][j];
y->e[j] = Yw->e[i_node+1][j];
}
// update u = u[i_node+1];
int i_node1 = i_node+1;
if ((r->c_type == CONSTANT) && (i_node1 == (r->n_nodes-1))) {
i_node1 -= 1;
}
for (i = 0; i < n_controls; i++) {
uc->e[i] = nlp_x->e[n_parameters + n_states + i_node1*n_controls + i];
}
// compute g(y(t+h),u(t+h),p,t+h)
if (only_compute_lte == NO) {
if (n_inequality > 0) {
r->ocp->inequality_constraints(y, uc, pc, T->e[i_node+1], G);
for (i = 0; i < n_inequality; i++) {
nlp_g->e[i + (i_node+1) * n_inequality] = G->e[i];
}
}
}
// FIX: compute LTE
}
if (only_compute_lte == YES) {
return 0;
}
// form phi, Psi
phi = r->ocp->terminal_constraints(yc, pc, Psi);
for (i = 0; i < n_terminal; i++) {
nlp_h->e[i + n_initial] = Psi->e[i];
}
VectorDelete(y_old);
VectorArrayDelete(Z_old, r->rk_stages);
VectorDelete(f);
VectorDelete(res);
VectorDelete(y);
VectorDelete(dZ);
//MatrixDelete(L);
//MatrixDelete(U);
//free(P);
//pause();
return phi + Yw->e[n_nodes-1][n_states];
}
