Exemple #1
0
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);
}
Exemple #2
0
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];
}