Exemplo n.º 1
0
int main( ) {
    float A[n][n] = {
        {2.0,  3.0,  1.0,  5.0},
        {6.0, 13.0,  5.0, 19.0},
        {2.0, 19.0, 10.0, 23.0},
        {4.0, 10.0, 11.0, 31.0}};
    float L[n][n], U[n][n];
    int i, j;
    
    lu_decomposition(A, L, U);
    
    for (i = 0; i < n; i++) {       
        printf("[ ");
        
        for (j = 0; j < n; j++)
            printf("%.2f ", L[i][j]);
        
        printf("] [ ");
        
        for (j = 0; j < n; j++)
            printf("%.2f ", U[i][j]);
        
        printf("]\n");
    }

    return 0;
}
Exemplo n.º 2
0
    void ImplicitSweeper<time>::setup(bool coarse)
    {
      pfasst::encap::EncapSweeper<time>::setup(coarse);

      auto const nodes = this->quadrature->get_nodes();
      auto const num_nodes = this->quadrature->get_num_nodes();

      if (this->quadrature->left_is_node()) {
        ML_CLOG(INFO, "Sweeper", "implicit sweeper shouldn't include left endpoint");
        throw ValueError("implicit sweeper shouldn't include left endpoint");
      }

      for (size_t m = 0; m < num_nodes; m++) {
        this->s_integrals.push_back(this->get_factory()->create(pfasst::encap::solution));
        this->fs_impl.push_back(this->get_factory()->create(pfasst::encap::function));
      }

      Matrix<time> QT = this->quadrature->get_q_mat().transpose();
      auto lu = lu_decomposition(QT);
      auto L = get<0>(lu);
      auto U = get<1>(lu);
      this->q_tilde = U.transpose();

      ML_CLOG(DEBUG, "Sweeper", "Q':" << endl << QT);
      ML_CLOG(DEBUG, "Sweeper", "L:" << endl << L);
      ML_CLOG(DEBUG, "Sweeper", "U:" << endl << U);
      ML_CLOG(DEBUG, "Sweeper", "LU:" << endl << L * U);
      ML_CLOG(DEBUG, "Sweeper", "q_tilde:" << endl << this->q_tilde);
    }
Exemplo n.º 3
0
// simultaneous linear equation 
void solute_SLE_with_n_variables(double coefficient_matrix[N][N], double right_hand_vector[N], double solution_vector[N]){
    double l_matrix[N][N] = {0};
    double u_matrix[N][N] = {0};
    double mid_solution_vector[N];

    lu_decomposition(coefficient_matrix, l_matrix, u_matrix);
    forward_substitution(l_matrix, right_hand_vector, mid_solution_vector);
    backward_substitution(u_matrix, mid_solution_vector, solution_vector);
}
Exemplo n.º 4
0
int main(){
  char *filename = "data_F.dat";
  int n_col=2;
  int n_row = 884;
  float *file_data = load_matrix(filename, n_row);
  float *data = malloc(n_row*n_col*sizeof(float));
  float *b = malloc(n_row*sizeof(float));
  float *theta = malloc(n_row*sizeof(float));
  make_data(file_data, data, b,theta, n_row);
  float *data_traspose = traspose(data,n_row,n_col);
  float *matrix = multiply(data_traspose,data, n_col,n_row,n_row,n_col);
  float *new_b = multiply(data_traspose,b, n_col, n_row, n_row, 1);
  float *U = malloc(n_col*n_col*sizeof(float));
  float *L = malloc(n_col*n_col*sizeof(float));
  lu_decomposition(matrix,new_b,U,L,n_col);
  float a1,a2;
  solve_upper_triangular(U,new_b,&a1,&a2);
  print_in_file(a1,a2);
  return 0;
}
Exemplo n.º 5
0
    static lu_pair<scalar> lu_decomposition(const Matrix<scalar>& A)
    {
      assert(A.rows() == A.cols());

      auto n = A.rows();

      Matrix<scalar> L = Matrix<scalar>::Zero(n, n);
      Matrix<scalar> U = Matrix<scalar>::Zero(n, n);

      if (A.rows() == 1) {

        L(0, 0) = 1.0;
        U(0, 0) = A(0,0);

      } else {

        // first row of U is first row of A
        auto U12 = A.block(0, 1, 1, n-1);

        // first column of L is first column of A / a11
        auto L21 = A.block(1, 0, n-1, 1) / A(0, 0);

        // remove first row and column and recurse
        auto A22  = A.block(1, 1, n-1, n-1);
        Matrix<scalar> tmp = A22 - L21 * U12;
        auto LU22 = lu_decomposition(tmp);

        L(0, 0) = 1.0;
        U(0, 0) = A(0, 0);
        L.block(1, 0, n-1, 1) = L21;
        U.block(0, 1, 1, n-1) = U12;
        L.block(1, 1, n-1, n-1) = get<0>(LU22);
        U.block(1, 1, n-1, n-1) = get<1>(LU22);

      }

      return lu_pair<scalar>(L, U);
    }
Exemplo n.º 6
0
    int
    lu_solver( const matrix<T1,D1,A1>&           A, 
               matrix<T2,D2,A2>&                 x, 
               const matrix<T3,D3,A3>&           b )
    {
        typedef matrix<T1,D1,A1>                 matrix_type;
        //typedef typename matrix_type::value_type value_type;
        typedef typename matrix_type::size_type  size_type;

        assert( A.row() == A.col() );
        assert( A.row() == b.row() );
        assert( b.col() == 1 );
        size_type const n = A.row();

        matrix_type L, U;
        // if lu decomposition failed, return 
        if ( lu_decomposition( A, L, U ) ) return 1;

        matrix_type Y;
        if( forward_substitution( L, Y, b ) ) return 1; // solve LY=b
        if( backward_substitution( U, x, Y )) return 1; //solve Ux=Y

        return 0;
    }
Exemplo n.º 7
0
myResult* simulate_implicit(Model_t *m, myResult *result, mySpecies *sp[], myParameter *param[], myCompartment *comp[], myReaction *re[], myRule *rule[], myEvent *event[], myInitialAssignment *initAssign[], myAlgebraicEquations *algEq, timeVariantAssignments *timeVarAssign, double sim_time, double dt, int print_interval, double *time, int order, int use_lazy_method, int print_amount, allocated_memory *mem){
  unsigned int i, j;
  int cycle;
  int error;
  int end_cycle = get_end_cycle(sim_time, dt);
  double reverse_time;
  double *value_time_p = result->values_time;
  double *value_sp_p = result->values_sp;
  double *value_param_p = result->values_param;
  double *value_comp_p = result->values_comp;
  double **coefficient_matrix = NULL;
  double *constant_vector = NULL;
  int *alg_pivot = NULL;
  double reactants_numerator, products_numerator;
  double min_value;
  double *init_val;
  /* for implicit */
  double **jacobian;
  int is_convergence = 0;
  double *b;
  double *pre_b;
  int    *p; /* for pivot selection */
  boolean flag;
  double delta = 1.0e-8;
  double tolerance = 1.0e-4; /* error tolerance of neuton method */
  unsigned int loop;
  double *delta_value;
  double k_next; /* speculated k value : k(t+1) */
  double *k_t;   /* k(t) */

  /* num of SBase objects */
  unsigned int num_of_species = Model_getNumSpecies(m);
  unsigned int num_of_parameters = Model_getNumParameters(m);
  unsigned int num_of_compartments = Model_getNumCompartments(m);
  unsigned int num_of_reactions = Model_getNumReactions(m);
  unsigned int num_of_rules = Model_getNumRules(m);
  unsigned int num_of_events = Model_getNumEvents(m);
  unsigned int num_of_initialAssignments = Model_getNumInitialAssignments(m);

  /* num of variables whose quantity is not a constant */
  unsigned int num_of_all_var_species = 0;
  unsigned int num_of_all_var_parameters = 0;
  unsigned int num_of_all_var_compartments = 0;
  unsigned int num_of_all_var_species_reference = 0;
  /* num of variables (which is NOT changed by assignment nor algebraic rule) */
  unsigned int num_of_var_species = 0;
  unsigned int num_of_var_parameters = 0;
  unsigned int num_of_var_compartments = 0;
  unsigned int num_of_var_species_reference = 0;
  unsigned int sum_num_of_vars;
  /* All variables (whose quantity is not a constant) */
  mySpecies **all_var_sp;           /* all variable species */
  myParameter **all_var_param;      /* all variable parameters */
  myCompartment **all_var_comp;     /* all variable compartments */
  mySpeciesReference **all_var_spr; /* all varialbe SpeciesReferences */
  /* variables (which is NOT changed by assignment nor algebraic rule) */
  mySpecies **var_sp;
  myParameter **var_param;
  myCompartment **var_comp;
  mySpeciesReference **var_spr;

  set_seed();

  check_num(num_of_species, num_of_parameters, num_of_compartments, num_of_reactions, &num_of_all_var_species, &num_of_all_var_parameters, &num_of_all_var_compartments, &num_of_all_var_species_reference, &num_of_var_species, &num_of_var_parameters, &num_of_var_compartments, &num_of_var_species_reference, sp, param, comp, re);

  /* create objects */
  all_var_sp = (mySpecies **)malloc(sizeof(mySpecies *) * num_of_all_var_species);
  all_var_param = (myParameter **)malloc(sizeof(myParameter *) * num_of_all_var_parameters);
  all_var_comp = (myCompartment **)malloc(sizeof(myCompartment *) * num_of_all_var_compartments);
  all_var_spr = (mySpeciesReference **)malloc(sizeof(mySpeciesReference *) * num_of_all_var_species_reference);
  var_sp = (mySpecies **)malloc(sizeof(mySpecies *) * num_of_var_species);
  var_param = (myParameter **)malloc(sizeof(myParameter *) * num_of_var_parameters);
  var_comp = (myCompartment **)malloc(sizeof(myCompartment *) * num_of_var_compartments);
  var_spr = (mySpeciesReference **)malloc(sizeof(mySpeciesReference *) * num_of_var_species_reference);
  /* mySpecies *all_var_sp[num_of_all_var_species]; */
  /* myParameter *all_var_param[num_of_all_var_parameters]; */
  /* myCompartment *all_var_comp[num_of_all_var_compartments]; */
  /* mySpeciesReference *all_var_spr[num_of_all_var_species_reference]; */
  /* mySpecies *var_sp[num_of_var_species]; */
  /* myParameter *var_param[num_of_var_parameters]; */
  /* myCompartment *var_comp[num_of_var_compartments]; */
  /* mySpeciesReference *var_spr[num_of_var_species_reference]; */

  create_calc_object_list(num_of_species, num_of_parameters, num_of_compartments, num_of_reactions, all_var_sp, all_var_param, all_var_comp, all_var_spr, var_sp, var_param, var_comp, var_spr, sp, param, comp, re);

  sum_num_of_vars = num_of_var_species + num_of_var_parameters +
                    num_of_var_compartments + num_of_var_species_reference;

  jacobian = (double**)malloc(sizeof(double*)*(sum_num_of_vars));
  for(i=0; i<sum_num_of_vars; i++){
    jacobian[i] = (double*)malloc(sizeof(double)*(sum_num_of_vars));
  }

  b = (double *)malloc(sizeof(double) * (sum_num_of_vars));
  pre_b = (double *)malloc(sizeof(double) * (sum_num_of_vars));
  p = (int *)malloc(sizeof(int) * (sum_num_of_vars));
  delta_value = (double *)malloc(sizeof(double) * (sum_num_of_vars));
  k_t = (double *)malloc(sizeof(double) * (sum_num_of_vars));
  /*
  double b[sum_num_of_vars];
  double pre_b[sum_num_of_vars];
  int p[sum_num_of_vars];
  double delta_value[sum_num_of_vars];
  double k_t[sum_num_of_vars];
  */

  if(algEq != NULL){
    coefficient_matrix = (double**)malloc(sizeof(double*)*(algEq->num_of_algebraic_variables));
    for(i=0; i<algEq->num_of_algebraic_variables; i++){
      coefficient_matrix[i] = (double*)malloc(sizeof(double)*(algEq->num_of_algebraic_variables));
    }
    constant_vector = (double*)malloc(sizeof(double)*(algEq->num_of_algebraic_variables));
    alg_pivot = (int*)malloc(sizeof(int)*(algEq->num_of_algebraic_variables));
  }

  PRG_TRACE(("Simulation for [%s] Starts!\n", Model_getId(m)));
  cycle = 0;

  /* initialize delay_val */
  initialize_delay_val(sp, num_of_species, param, num_of_parameters, comp, num_of_compartments, re, num_of_reactions, sim_time, dt, 0);

  /* calc temp value by assignment */
  for(i=0; i<num_of_all_var_species; i++){
    if(all_var_sp[i]->depending_rule != NULL && all_var_sp[i]->depending_rule->is_assignment){
      all_var_sp[i]->temp_value = calc(all_var_sp[i]->depending_rule->eq, dt, cycle, &reverse_time, 0);
    }
  }
  for(i=0; i<num_of_all_var_parameters; i++){
    if(all_var_param[i]->depending_rule != NULL && all_var_param[i]->depending_rule->is_assignment){
      all_var_param[i]->temp_value = calc(all_var_param[i]->depending_rule->eq, dt, cycle, &reverse_time, 0);
    }
  }
  for(i=0; i<num_of_all_var_compartments; i++){
    if(all_var_comp[i]->depending_rule != NULL && all_var_comp[i]->depending_rule->is_assignment){
      all_var_comp[i]->temp_value = calc(all_var_comp[i]->depending_rule->eq, dt, cycle, &reverse_time, 0);
    }
  }
  for(i=0; i<num_of_all_var_species_reference; i++){
    if(all_var_spr[i]->depending_rule != NULL && all_var_spr[i]->depending_rule->is_assignment){
      all_var_spr[i]->temp_value = calc(all_var_spr[i]->depending_rule->eq, dt, cycle, &reverse_time, 0);
    }
  }
  /* forwarding value */
  forwarding_value(all_var_sp, num_of_all_var_species, all_var_param, num_of_all_var_parameters, all_var_comp, num_of_all_var_compartments, all_var_spr, num_of_all_var_species_reference);

  /* initialize delay_val */
  initialize_delay_val(sp, num_of_species, param, num_of_parameters, comp, num_of_compartments, re, num_of_reactions, sim_time, dt, 0);

  /* calc InitialAssignment */
  calc_initial_assignment(initAssign, num_of_initialAssignments, dt, cycle, &reverse_time);

  /* initialize delay_val */
  initialize_delay_val(sp, num_of_species, param, num_of_parameters, comp, num_of_compartments, re, num_of_reactions, sim_time, dt, 0);

  /* rewriting for explicit delay */
  for(i=0; i<num_of_initialAssignments; i++){
    for(j=0; j<initAssign[i]->eq->math_length; j++){
      if(initAssign[i]->eq->number[j] == time){
        TRACE(("time is replaced with reverse time\n"));
        initAssign[i]->eq->number[j] = &reverse_time;
      }else if(initAssign[i]->eq->number[j] != NULL){
        init_val = (double*)malloc(sizeof(double));
        *init_val = *initAssign[i]->eq->number[j];
        mem->memory[mem->num_of_allocated_memory++] = init_val;
        initAssign[i]->eq->number[j] = init_val;
      }
    }
  }
  for(i=0; i<timeVarAssign->num_of_time_variant_assignments; i++){
    for(j=0; j<timeVarAssign->eq[i]->math_length; j++){
      if(timeVarAssign->eq[i]->number[j] == time){
        TRACE(("time is replaced with reverse time\n"));
        timeVarAssign->eq[i]->number[j] = &reverse_time;
      }else if(timeVarAssign->eq[i]->number[j] != NULL){
        init_val = (double*)malloc(sizeof(double));
        *init_val = *timeVarAssign->eq[i]->number[j];
        mem->memory[mem->num_of_allocated_memory++] = init_val;
        timeVarAssign->eq[i]->number[j] = init_val;
      }
    }
  }

  /* initialize delay_val */
  initialize_delay_val(sp, num_of_species, param, num_of_parameters, comp, num_of_compartments, re, num_of_reactions, sim_time, dt, 0);

  /* calc temp value by assignment */
  for(i=0; i<num_of_all_var_species; i++){
    if(all_var_sp[i]->depending_rule != NULL && all_var_sp[i]->depending_rule->is_assignment){
      all_var_sp[i]->temp_value = calc(all_var_sp[i]->depending_rule->eq, dt, cycle, &reverse_time, 0);
    }
  }
  for(i=0; i<num_of_all_var_parameters; i++){
    if(all_var_param[i]->depending_rule != NULL && all_var_param[i]->depending_rule->is_assignment){
      all_var_param[i]->temp_value = calc(all_var_param[i]->depending_rule->eq, dt, cycle, &reverse_time, 0);
    }
  }
  for(i=0; i<num_of_all_var_compartments; i++){
    if(all_var_comp[i]->depending_rule != NULL && all_var_comp[i]->depending_rule->is_assignment){
      all_var_comp[i]->temp_value = calc(all_var_comp[i]->depending_rule->eq, dt, cycle, &reverse_time, 0);
    }
  }
  for(i=0; i<num_of_all_var_species_reference; i++){
    if(all_var_spr[i]->depending_rule != NULL && all_var_spr[i]->depending_rule->is_assignment){
      all_var_spr[i]->temp_value = calc(all_var_spr[i]->depending_rule->eq, dt, cycle, &reverse_time, 0);
    }
  }
  /* forwarding value */
  forwarding_value(all_var_sp, num_of_all_var_species, all_var_param, num_of_all_var_parameters, all_var_comp, num_of_all_var_compartments, all_var_spr, num_of_all_var_species_reference);

  /* initialize delay_val */
  initialize_delay_val(sp, num_of_species, param, num_of_parameters, comp, num_of_compartments, re, num_of_reactions, sim_time, dt, 0);

  /* calc temp value algebraic by algebraic */
  if(algEq != NULL){
    if(algEq->num_of_algebraic_variables > 1){
      /* initialize pivot */
      for(i=0; i<algEq->num_of_algebraic_variables; i++){
        alg_pivot[i] = i;
      }
      for(i=0; i<algEq->num_of_algebraic_variables; i++){
        for(j=0; j<algEq->num_of_algebraic_variables; j++){
          coefficient_matrix[i][j] = calc(algEq->coefficient_matrix[i][j], dt, cycle, &reverse_time, 0);
          /* TRACE(("coefficient matrix[%d][%d] = %lf\n", i, j, coefficient_matrix[i][j])); */
        }
      }
      for(i=0; i<algEq->num_of_algebraic_variables; i++){
        constant_vector[i] = -calc(algEq->constant_vector[i], dt, cycle, &reverse_time, 0);
        /* TRACE(("constant vector[%d] = %lf\n", i, constant_vector[i])); */
      }
      /* LU decompostion */
      error = lu_decomposition(coefficient_matrix, alg_pivot, algEq->num_of_algebraic_variables);
      if(error == 0){/* failure in LU decomposition */
        return NULL;
      }
      /* forward substitution & backward substitution */
      lu_solve(coefficient_matrix, alg_pivot, algEq->num_of_algebraic_variables, constant_vector);
      /*       for(i=0; i<algEq->num_of_algebraic_variables; i++){ */
      /*  TRACE(("ans[%d] = %lf\n", i, constant_vector[i])); */
      /*       } */
      for(i=0; i<algEq->num_of_alg_target_sp; i++){
        algEq->alg_target_species[i]->target_species->temp_value = constant_vector[algEq->alg_target_species[i]->order];
      }    
      for(i=0; i<algEq->num_of_alg_target_param; i++){
        algEq->alg_target_parameter[i]->target_parameter->temp_value = constant_vector[algEq->alg_target_parameter[i]->order];
      }    
      for(i=0; i<algEq->num_of_alg_target_comp; i++){
        /* new code */
        for(j=0; j<algEq->alg_target_compartment[i]->target_compartment->num_of_including_species; j++){
          if(algEq->alg_target_compartment[i]->target_compartment->including_species[j]->is_concentration){
            algEq->alg_target_compartment[i]->target_compartment->including_species[j]->temp_value = algEq->alg_target_compartment[i]->target_compartment->including_species[j]->temp_value*algEq->alg_target_compartment[i]->target_compartment->temp_value/constant_vector[algEq->alg_target_compartment[i]->order];
          }
        }
       /* new code end */
        algEq->alg_target_compartment[i]->target_compartment->temp_value = constant_vector[algEq->alg_target_compartment[i]->order];
      }
    }else{
      if(algEq->target_species != NULL){
        algEq->target_species->temp_value = -calc(algEq->constant, dt, cycle, &reverse_time, 0)/calc(algEq->coefficient, dt, cycle, &reverse_time, 0);
      }
      if(algEq->target_parameter != NULL){
        algEq->target_parameter->temp_value = -calc(algEq->constant, dt, cycle, &reverse_time, 0)/calc(algEq->coefficient, dt, cycle, &reverse_time, 0);
      }
      if(algEq->target_compartment != NULL){
        /* new code */
        for(i=0; i<algEq->target_compartment->num_of_including_species; i++){
          if(algEq->target_compartment->including_species[i]->is_concentration){
            algEq->target_compartment->including_species[i]->temp_value = algEq->target_compartment->including_species[i]->temp_value*algEq->target_compartment->temp_value/(-calc(algEq->constant, dt, cycle, &reverse_time, 0)/calc(algEq->coefficient, dt, cycle, &reverse_time, 0));
          }
        }
       /* new code end */
        algEq->target_compartment->temp_value = -calc(algEq->constant, dt, cycle, &reverse_time, 0)/calc(algEq->coefficient, dt, cycle, &reverse_time, 0);
      }
    }
    /* forwarding value */
    forwarding_value(all_var_sp, num_of_all_var_species, all_var_param, num_of_all_var_parameters, all_var_comp, num_of_all_var_compartments, all_var_spr, num_of_all_var_species_reference);
  }

  /* initialize delay_val */
  initialize_delay_val(sp, num_of_species, param, num_of_parameters, comp, num_of_compartments, re, num_of_reactions, sim_time, dt, 1);

  /* cycle start */
  for(cycle=0; cycle<=end_cycle; cycle++){
    /* calculate unreversible fast reaction */
    for(i=0; i<num_of_reactions; i++){
      if(re[i]->is_fast && !re[i]->is_reversible){
        if(calc(re[i]->eq, dt, cycle, &reverse_time, 0) > 0){
          min_value = DBL_MAX;
          for(j=0; j<re[i]->num_of_reactants; j++){
            if(min_value > re[i]->reactants[j]->mySp->value/calc(re[i]->reactants[j]->eq, dt, cycle, &reverse_time, 0)){
              min_value = re[i]->reactants[j]->mySp->value/calc(re[i]->reactants[j]->eq, dt, cycle, &reverse_time, 0);
            }
          }
          for(j=0; j<re[i]->num_of_products; j++){
            if(!Species_getBoundaryCondition(re[i]->products[j]->mySp->origin)){
              re[i]->products[j]->mySp->value += calc(re[i]->products[j]->eq, dt, cycle, &reverse_time, 0)*min_value;
              re[i]->products[j]->mySp->temp_value = re[i]->products[j]->mySp->value;
            }
          }
          for(j=0; j<re[i]->num_of_reactants; j++){
            if(!Species_getBoundaryCondition(re[i]->reactants[j]->mySp->origin)){
              re[i]->reactants[j]->mySp->value -= calc(re[i]->reactants[j]->eq, dt, cycle, &reverse_time, 0)*min_value;
              re[i]->reactants[j]->mySp->temp_value = re[i]->reactants[j]->mySp->value;
            }
          }
        }
      }
    }
    /* calculate reversible fast reactioin */
    for(i=0; i<num_of_reactions; i++){
      if(re[i]->is_fast && re[i]->is_reversible){
        if(!(Species_getBoundaryCondition(re[i]->products[0]->mySp->origin) 
              && Species_getBoundaryCondition(re[i]->reactants[0]->mySp->origin))){
          products_numerator = calc(re[i]->products_equili_numerator, dt, cycle, &reverse_time, 0);
          reactants_numerator = calc(re[i]->reactants_equili_numerator, dt, cycle, &reverse_time, 0);
          if(products_numerator > 0 || reactants_numerator > 0){
            if(Species_getBoundaryCondition(re[i]->products[0]->mySp->origin)){
              re[i]->reactants[0]->mySp->value = (reactants_numerator/products_numerator)*re[i]->products[0]->mySp->value;
              re[i]->reactants[0]->mySp->temp_value = re[i]->reactants[0]->mySp->value;
            }else if(Species_getBoundaryCondition(re[i]->reactants[0]->mySp->origin)){
              re[i]->products[0]->mySp->value = (products_numerator/reactants_numerator)*re[i]->reactants[0]->mySp->value;
              re[i]->products[0]->mySp->temp_value = re[i]->products[0]->mySp->value;	    
            }else{
              re[i]->products[0]->mySp->value = (products_numerator/(products_numerator+reactants_numerator))*(re[i]->products[0]->mySp->temp_value+re[i]->reactants[0]->mySp->temp_value);
              re[i]->reactants[0]->mySp->value = (reactants_numerator/(products_numerator+reactants_numerator))*(re[i]->products[0]->mySp->temp_value+re[i]->reactants[0]->mySp->temp_value);
              re[i]->products[0]->mySp->temp_value = re[i]->products[0]->mySp->value;
              re[i]->reactants[0]->mySp->temp_value = re[i]->reactants[0]->mySp->value;
            }
          }
        }
      }
    }

    /* event */
    calc_event(event, num_of_events, dt, *time, cycle, &reverse_time);    

    /* substitute delay val */
    substitute_delay_val(sp, num_of_species, param, num_of_parameters, comp, num_of_compartments, re, num_of_reactions, cycle);

    /* progress */
    if(cycle%(int)(end_cycle/10) == 0){
      PRG_TRACE(("%3d %%\n", (int)(100*((double)cycle/(double)end_cycle))));
      PRG_TRACE(("\x1b[1A"));
      PRG_TRACE(("\x1b[5D"));
    }
    /* print result */
    if(cycle%print_interval == 0){
      /*  Time */
      *value_time_p = *time;
      value_time_p++;
      /*  Species */
      for(i=0; i<num_of_species; i++){
        /*         if(!(Species_getConstant(sp[i]->origin) && Species_getBoundaryCondition(sp[i]->origin))){ // XXX must remove this */
        if(print_amount){
          if(sp[i]->is_concentration){
            *value_sp_p = sp[i]->value*sp[i]->locating_compartment->value;
          }else{
            *value_sp_p = sp[i]->value;
          }
        }else{
          if(sp[i]->is_amount){
            *value_sp_p = sp[i]->value/sp[i]->locating_compartment->value;
          }else{
            *value_sp_p = sp[i]->value;
          }
        }
        value_sp_p++;
        /*         } */
      }
      /*  Parameter */
      for(i=0; i<num_of_parameters; i++){
        /*         if(!Parameter_getConstant(param[i]->origin)){ // XXX must remove this */
        *value_param_p = param[i]->value;
        /*         } */
        value_param_p++;
      }
      /*  Compartment */
      for(i=0; i<num_of_compartments; i++){
        /*         if(!Compartment_getConstant(comp[i]->origin)){ // XXX must remove this */
        *value_comp_p = comp[i]->value;
        /*         } */
        value_comp_p++;
      }
    }

    /* time increase */
    *time = (cycle+1)*dt;

    /* implicit method */
    /* define init value by Euler start */
    calc_k(all_var_sp, num_of_all_var_species, all_var_param, num_of_all_var_parameters, all_var_comp, num_of_all_var_compartments, all_var_spr, num_of_all_var_species_reference, re, num_of_reactions, rule, num_of_rules, cycle, dt, &reverse_time, 0, 1);

    /* preserve k(t) value */
    for(i=0; i<sum_num_of_vars; i++){
      if(i < num_of_var_species){
        k_t[i] = var_sp[i]->k[0];
      }else if(i < num_of_var_species+num_of_var_parameters){
        k_t[i] = var_param[i-num_of_var_species]->k[0];
      }else if(i < num_of_var_species+num_of_var_parameters+num_of_var_compartments){
        k_t[i] = var_comp[i-num_of_var_species-num_of_var_parameters]->k[0];
      }else{
        k_t[i] = var_spr[i-num_of_var_species-num_of_var_parameters-num_of_var_compartments]->k[0];
      }
    }

    calc_temp_value(all_var_sp, num_of_all_var_species, all_var_param, num_of_all_var_parameters, all_var_comp, num_of_all_var_compartments, all_var_spr, num_of_all_var_species_reference, dt, 0);
    /* define init value by Euler end */

    /* newton method */
    if(use_lazy_method){
      is_convergence = 0;
      for(i=0; i<sum_num_of_vars; i++){
        pre_b[i] = 0;
      }
    }
    flag = 1;
    while(flag){
      /* calc b */
      calc_k(var_sp, num_of_var_species, var_param, num_of_var_parameters, var_comp, num_of_var_compartments, var_spr, num_of_var_species_reference, re, num_of_reactions, rule, num_of_rules, cycle, dt, &reverse_time, 0, 0);
      for(i=0; i<num_of_var_species; i++){
        k_next = var_sp[i]->k[0];
        b[i] = calc_implicit_formula(order, var_sp[i]->temp_value, var_sp[i]->value, var_sp[i]->prev_val[0], var_sp[i]->prev_val[1], var_sp[i]->prev_val[2], k_next, k_t[i], var_sp[i]->prev_k[0], var_sp[i]->prev_k[1], dt);
      }
      for(i=0; i<num_of_var_parameters; i++){
        b[num_of_var_species+i] = calc_implicit_formula(order, var_param[i]->temp_value, var_param[i]->value, var_param[i]->prev_val[0], var_param[i]->prev_val[1], var_param[i]->prev_val[2], var_param[i]->k[0], k_t[num_of_var_species+i], var_param[i]->prev_k[0], var_param[i]->prev_k[1], dt);
      }
      for(i=0; i<num_of_var_compartments; i++){
        b[num_of_var_species+num_of_var_parameters+i] = calc_implicit_formula(order, var_comp[i]->temp_value, var_comp[i]->value, var_comp[i]->prev_val[0], var_comp[i]->prev_val[1], var_comp[i]->prev_val[2], var_comp[i]->k[0], k_t[num_of_var_species+num_of_var_parameters+i], var_comp[i]->prev_k[0], var_comp[i]->prev_k[1], dt);
      }
      for(i=0; i<num_of_var_species_reference; i++){
        b[num_of_var_species+num_of_var_parameters+num_of_var_compartments+i] = calc_implicit_formula(order, var_spr[i]->temp_value, var_spr[i]->value, var_spr[i]->prev_val[0], var_spr[i]->prev_val[1], var_spr[i]->prev_val[2], var_spr[i]->k[0], k_t[num_of_var_species+num_of_var_parameters+num_of_var_compartments+i], var_spr[i]->prev_k[0], var_spr[i]->prev_k[1], dt);
      }

      if(!use_lazy_method || !is_convergence){
        /* calc jacobian by numerical differentiation */
        for(loop=0; loop<sum_num_of_vars; loop++){
          if(loop < num_of_var_species){
            var_sp[loop]->temp_value += delta;
          }else if(loop < num_of_var_species+num_of_var_parameters){
            var_param[loop-num_of_var_species]->temp_value += delta;
          }else if(loop < num_of_var_species+num_of_var_parameters+num_of_var_compartments){
            var_comp[loop-num_of_var_species-num_of_var_parameters]->temp_value += delta;
          }else{
            var_spr[loop-num_of_var_species-num_of_var_parameters-num_of_var_compartments]->temp_value += delta;
          }
          calc_k(var_sp, num_of_var_species, var_param, num_of_var_parameters, var_comp, num_of_var_compartments, var_spr, num_of_var_species_reference, re, num_of_reactions, rule, num_of_rules, cycle, dt, &reverse_time, 0, 0);
          for(i=0; i<num_of_var_species; i++){
            k_next = var_sp[i]->k[0];
            delta_value[i] = calc_implicit_formula(order, var_sp[i]->temp_value, var_sp[i]->value, var_sp[i]->prev_val[0], var_sp[i]->prev_val[1], var_sp[i]->prev_val[2], k_next, k_t[i], var_sp[i]->prev_k[0], var_sp[i]->prev_k[1], dt);
            /* numerical differentiation */
            jacobian[i][loop] = (delta_value[i]-b[i])/delta;
          }
          for(i=0; i<num_of_var_parameters; i++){
            delta_value[num_of_var_species+i] = calc_implicit_formula(order, var_param[i]->temp_value, var_param[i]->value, var_param[i]->prev_val[0], var_param[i]->prev_val[1], var_param[i]->prev_val[2], var_param[i]->k[0], k_t[num_of_var_species+i], var_param[i]->prev_k[0], var_param[i]->prev_k[1], dt);
            /* numerical differentiation */
            jacobian[num_of_var_species+i][loop] = (delta_value[num_of_var_species+i]-b[num_of_var_species+i])/delta;
          }
          for(i=0; i<num_of_var_compartments; i++){
            delta_value[num_of_var_species+num_of_var_parameters+i] = calc_implicit_formula(order, var_comp[i]->temp_value, var_comp[i]->value, var_comp[i]->prev_val[0], var_comp[i]->prev_val[1], var_comp[i]->prev_val[2], var_comp[i]->k[0], k_t[num_of_var_species+num_of_var_parameters+i], var_comp[i]->prev_k[0], var_comp[i]->prev_k[1], dt);
            /* numerical differentiation */
            jacobian[num_of_var_species+num_of_var_parameters+i][loop] = (delta_value[num_of_var_species+num_of_var_parameters+i]-b[num_of_var_species+num_of_var_parameters+i])/delta;
          }
          for(i=0; i<num_of_var_species_reference; i++){
            delta_value[num_of_var_species+num_of_var_parameters+num_of_var_compartments+i] = calc_implicit_formula(order, var_spr[i]->temp_value, var_spr[i]->value, var_spr[i]->prev_val[0], var_spr[i]->prev_val[1], var_spr[i]->prev_val[2], var_spr[i]->k[0], k_t[num_of_var_species+num_of_var_parameters+num_of_var_compartments+i], var_spr[i]->prev_k[0], var_spr[i]->prev_k[1], dt);
            /* numerical differentiation */
            jacobian[num_of_var_species+num_of_var_parameters+num_of_var_compartments+i][loop] = (delta_value[num_of_var_species+num_of_var_parameters+num_of_var_compartments+i]-b[num_of_var_species+num_of_var_parameters+num_of_var_compartments+i])/delta;
          }
          if(loop < num_of_var_species){
            var_sp[loop]->temp_value -= delta;
          }else if(loop < num_of_var_species+num_of_var_parameters){
            var_param[loop-num_of_var_species]->temp_value -= delta;
          }else if(loop < num_of_var_species+num_of_var_parameters+num_of_var_compartments){
            var_comp[loop-num_of_var_species-num_of_var_parameters]->temp_value -= delta;
          }else{
            var_spr[loop-num_of_var_species-num_of_var_parameters-num_of_var_compartments]->temp_value -= delta;
          }
        }
      }

      /* initialize p */
      for(i=0; i<sum_num_of_vars; i++){
        p[i] = i;
      }

      /* LU decomposition */
      error = lu_decomposition(jacobian, p, sum_num_of_vars);
      if(error == 0){/* failure in LU decomposition */
        return NULL;
      }

      /* forward substitution & backward substitution */
      lu_solve(jacobian, p, sum_num_of_vars, b);

      /* calculate next temp value */
      for(i=0; i<sum_num_of_vars; i++){
        if(i < num_of_var_species){
          var_sp[i]->temp_value -= b[i];
        }else if(i < num_of_var_species+num_of_var_parameters){
          var_param[i-num_of_var_species]->temp_value -= b[i];
        }else if(i < num_of_var_species+num_of_var_parameters+num_of_var_compartments){
          var_comp[i-num_of_var_species-num_of_var_parameters]->temp_value -= b[i];
        }else{
          var_spr[i-num_of_var_species-num_of_var_parameters-num_of_var_compartments]->temp_value -= b[i];
        }
      }

      /* convergence judgement */
      if(use_lazy_method){
        is_convergence = 1;
        for(i=0; i<sum_num_of_vars; i++){
          if(fabs(b[i]) > fabs(pre_b[i])){
            is_convergence = 0;
          }
        }
        for(i=0; i<sum_num_of_vars; i++){
          pre_b[i] = b[i];
        }
      }

      /* error judgement */
      flag = 0;
      for(i=0; i<sum_num_of_vars; i++){
        if(fabs(b[i]) > tolerance){
          flag = 1;
        }
      }
    }

    /* calc temp value by assignment */
    for(i=0; i<num_of_all_var_species; i++){
      if(all_var_sp[i]->depending_rule != NULL && all_var_sp[i]->depending_rule->is_assignment){
        all_var_sp[i]->temp_value = calc(all_var_sp[i]->depending_rule->eq, dt, cycle, &reverse_time, 0);
      }
    }
    for(i=0; i<num_of_all_var_parameters; i++){
      if(all_var_param[i]->depending_rule != NULL && all_var_param[i]->depending_rule->is_assignment){
        all_var_param[i]->temp_value = calc(all_var_param[i]->depending_rule->eq, dt, cycle, &reverse_time, 0);
      }
    }
    for(i=0; i<num_of_all_var_compartments; i++){
      if(all_var_comp[i]->depending_rule != NULL && all_var_comp[i]->depending_rule->is_assignment){
        all_var_comp[i]->temp_value = calc(all_var_comp[i]->depending_rule->eq, dt, cycle, &reverse_time, 0);
      }
    }
    for(i=0; i<num_of_all_var_species_reference; i++){
      if(all_var_spr[i]->depending_rule != NULL && all_var_spr[i]->depending_rule->is_assignment){
        all_var_spr[i]->temp_value = calc(all_var_spr[i]->depending_rule->eq, dt, cycle, &reverse_time, 0);
      }
    }

    /* calc temp value algebraic by algebraic */
    if(algEq != NULL){
      if(algEq->num_of_algebraic_variables > 1){
        /* initialize pivot */
        for(i=0; i<algEq->num_of_algebraic_variables; i++){
          alg_pivot[i] = i;
        }
        for(i=0; i<algEq->num_of_algebraic_variables; i++){
          for(j=0; j<algEq->num_of_algebraic_variables; j++){
            coefficient_matrix[i][j] = calc(algEq->coefficient_matrix[i][j], dt, cycle, &reverse_time, 0);
          }
        }
        for(i=0; i<algEq->num_of_algebraic_variables; i++){
          constant_vector[i] = -calc(algEq->constant_vector[i], dt, cycle, &reverse_time, 0);
        }
        /* LU decompostion */
        error = lu_decomposition(coefficient_matrix, alg_pivot, algEq->num_of_algebraic_variables);
        if(error == 0){/* failure in LU decomposition */
          return NULL;
        }
        /* forward substitution & backward substitution */
        lu_solve(coefficient_matrix, alg_pivot, algEq->num_of_algebraic_variables, constant_vector);
        for(i=0; i<algEq->num_of_alg_target_sp; i++){
          algEq->alg_target_species[i]->target_species->temp_value = constant_vector[algEq->alg_target_species[i]->order];
        }    
        for(i=0; i<algEq->num_of_alg_target_param; i++){
          algEq->alg_target_parameter[i]->target_parameter->temp_value = constant_vector[algEq->alg_target_parameter[i]->order];
        }    
        for(i=0; i<algEq->num_of_alg_target_comp; i++){
          /* new code */
          for(j=0; j<algEq->alg_target_compartment[i]->target_compartment->num_of_including_species; j++){
            if(algEq->alg_target_compartment[i]->target_compartment->including_species[j]->is_concentration){
              algEq->alg_target_compartment[i]->target_compartment->including_species[j]->temp_value = algEq->alg_target_compartment[i]->target_compartment->including_species[j]->temp_value*algEq->alg_target_compartment[i]->target_compartment->temp_value/constant_vector[algEq->alg_target_compartment[i]->order];
            }
          }
         /* new code end */
          algEq->alg_target_compartment[i]->target_compartment->temp_value = constant_vector[algEq->alg_target_compartment[i]->order];
        }    
      }else{
        if(algEq->target_species != NULL){
          algEq->target_species->temp_value = -calc(algEq->constant, dt, cycle, &reverse_time, 0)/calc(algEq->coefficient, dt, cycle, &reverse_time, 0);
        }
        if(algEq->target_parameter != NULL){
          algEq->target_parameter->temp_value = -calc(algEq->constant, dt, cycle, &reverse_time, 0)/calc(algEq->coefficient, dt, cycle, &reverse_time, 0);
        }
        if(algEq->target_compartment != NULL){
          /* new code */
          for(i=0; i<algEq->target_compartment->num_of_including_species; i++){
            if(algEq->target_compartment->including_species[i]->is_concentration){
              algEq->target_compartment->including_species[i]->temp_value = algEq->target_compartment->including_species[i]->temp_value*algEq->target_compartment->temp_value/(-calc(algEq->constant, dt, cycle, &reverse_time, 0)/calc(algEq->coefficient, dt, cycle, &reverse_time, 0));
            }
          }
         /* new code end */
          algEq->target_compartment->temp_value = -calc(algEq->constant, dt, cycle, &reverse_time, 0)/calc(algEq->coefficient, dt, cycle, &reverse_time, 0);
        }
      }
    }

    /* preserve prev_value and prev_k for multistep solution */
    for(i=0; i<num_of_var_species; i++){
      var_sp[i]->prev_val[2] = var_sp[i]->prev_val[1];
      var_sp[i]->prev_val[1] = var_sp[i]->prev_val[0];
      var_sp[i]->prev_val[0] = var_sp[i]->value;
      var_sp[i]->prev_k[2] = var_sp[i]->prev_k[1];
      var_sp[i]->prev_k[1] = var_sp[i]->prev_k[0];
      var_sp[i]->prev_k[0] = k_t[i];
    }
    for(i=0; i<num_of_var_parameters; i++){
      var_param[i]->prev_val[2] = var_param[i]->prev_val[1];
      var_param[i]->prev_val[1] = var_param[i]->prev_val[0];
      var_param[i]->prev_val[0] = var_param[i]->value;
      var_param[i]->prev_k[2] = var_param[i]->prev_k[1];
      var_param[i]->prev_k[1] = var_param[i]->prev_k[0];
      var_param[i]->prev_k[0] = k_t[num_of_var_species+i];
    }
    for(i=0; i<num_of_var_compartments; i++){
      var_comp[i]->prev_val[2] = var_comp[i]->prev_val[1];
      var_comp[i]->prev_val[1] = var_comp[i]->prev_val[0];
      var_comp[i]->prev_val[0] = var_comp[i]->value;
      var_comp[i]->prev_k[2] = var_comp[i]->prev_k[1];
      var_comp[i]->prev_k[1] = var_comp[i]->prev_k[0];
      var_comp[i]->prev_k[0] = k_t[num_of_var_species+num_of_var_parameters+i];
    }
    for(i=0; i<num_of_var_species_reference; i++){
      var_spr[i]->prev_val[2] = var_spr[i]->prev_val[1];
      var_spr[i]->prev_val[1] = var_spr[i]->prev_val[0];
      var_spr[i]->prev_val[0] = var_spr[i]->value;
      var_spr[i]->prev_k[2] = var_spr[i]->prev_k[1];
      var_spr[i]->prev_k[1] = var_spr[i]->prev_k[0];
      var_spr[i]->prev_k[0] = k_t[num_of_var_species+num_of_var_parameters+i];
    }

    /* forwarding value */
    forwarding_value(all_var_sp, num_of_all_var_species, all_var_param, num_of_all_var_parameters, all_var_comp, num_of_all_var_compartments, all_var_spr, num_of_all_var_species_reference);
  }
  PRG_TRACE(("Simulation for [%s] Ends!\n", Model_getId(m)));
  if(algEq != NULL){
    for(i=0; i<algEq->num_of_algebraic_variables; i++){
      free(coefficient_matrix[i]);
    }
    free(coefficient_matrix);
    free(constant_vector);
    free(alg_pivot);
  }
  for(i=0; i<sum_num_of_vars; i++){
    free(jacobian[i]);
  }
  free(all_var_sp);
  free(all_var_param);
  free(all_var_comp);
  free(all_var_spr);
  free(var_sp);
  free(var_param);
  free(var_comp);
  free(var_spr);
  /* for implicit */
  free(jacobian);
  return result;
}
Exemplo n.º 8
0
int main(int argc, char *argv[])
{
    int n1;			/* number of samples in input trace	 */
    int n1w;		/* number of samples in time window      */
    int N1w;	       	/* number of time windows		*/
    int n1ws;	       	/* number of samples in window (wo taper)*/
    int n1wf;	       	/* number of samples in first window     */
    int n1wi;	       	/* number of samples in intermed. window */
    int n1taper;	       	/* number of samples in taper		*/
    int n2;	       		/* number of input traces		*/
    int n3;	       		/* number of input sections		*/
    int n2w;	       	/* number of traces in window		*/
    int N2w;	       	/* number of spacial windows		*/
    int n2wu;	       	/* updated number of traces in window	*/
    int nfft;		/* transform length			*/
    int nf;			/* number of frequencies		*/
    int lenf;		/* number of traces for filter		*/
    int i2w,i2,jr,itt;	/* summation indexes			*/
    int i1,i3,i1w,ifq,ir;	/* summation indexes			*/
    int ig,ifv;		/* summation indexes			*/
    bool verb;		/* flag to get advisory messages	*/
    int *ipvt;		/* indices of pivot permutations	*/

    float *info;		/* index of last zero pivot		*/
    float dt;		/* sampling interval in secs		*/
    float df;		/* sample interval in Hz		*/
    float fmin;		/* minimum frequency to process in Hz	*/
    float fmax;		/* maximum frequency to process in Hz	*/
    float taper;		/* length of taper			*/
    float twlen;       	/* time window length 			*/
    float **tracein;      	/* real trace			   	*/
    float **traceout;     	/* real trace			   	*/
    float *traceintw;       /* real trace in time window - input   	*/
    float *traceinttw;      /* real trace in time window - input   	*/
    float *traceotw;       	/* real trace in time window - output   */
    float *traceottw;      	/* real trace in time window - output   */
    float *rauto;       	/* real part of autocorrelation   	*/
    float *iauto;       	/* imaginary part of autocorretation 	*/
    float **cautom;      	/* complex autocorrelation matrix	*/
    float *aav;       	/* advanced autocorrelation vector	*/

    kiss_fft_cpx *cdata;	/* complex transformed trace (window)  	*/
    kiss_fft_cpx **fdata;	/* data - freq. domain          	*/
    kiss_fft_cpx **fdataw;	/* data - freq. domain - in window     	*/
    kiss_fft_cpx *sfv;	/* single frequency vector 	  	*/
    kiss_fft_cpx **ffv;	/* filtered frequency vector 	  	*/
    kiss_fft_cpx *fv;	/* frequency vector		      	*/
    kiss_fft_cpx *sfvout;	/* single frequency output vector      	*/
    kiss_fft_cpx *sfvcj;	/* complex conjugate of sfv	  	*/
    kiss_fft_cpx *acorr;	/* autocorrelation output	  	*/
    kiss_fft_cpx *filter;	/* filter               	  	*/

    kiss_fftr_cfg forw, invs;
    sf_file in, out;

    sf_init(argc,argv);

    in = sf_input("in");
    out = sf_output("out");

    if(!sf_getbool("verb",&verb)) verb=false;
    /* flag to get advisory messages */
	
    if(!sf_histint(in, "n1", &n1))  sf_error("No n1 in input");  if (verb) sf_warning("n1 = %i",n1);
    if(!sf_histfloat(in, "d1", &dt)) sf_error("No d1 in input"); if (verb) sf_warning("dt= %f",dt);
    if(!sf_histint(in,"n2",&n2))   sf_error("No n2 in input");   if (verb) sf_warning("n2= %f",n2);
    if(!sf_histint(in,"n3",&n3))   n3=1; if (verb) sf_warning("n3= %f",n3);

    if(!sf_getfloat("taper",&taper)) taper=.1;
    /* length of taper */
    if (taper==0.0) taper=.004; 

    if(!sf_getfloat("fmin",&fmin)) fmin=1.;    
    /* minimum frequency to process in Hz */
    if (fmin==0.0)  if (verb) sf_warning("using fmin=1 Hz");

    if(!sf_getfloat("fmax",&fmax)) fmax=1./(2*dt); 
    /* maximum frequency to process in Hz */
    if (fmax==0.0) if (verb) sf_warning("using fmax=1/(2*dt) Hz");

    if (!sf_getfloat("twlen", &twlen)) twlen=(float)(n1-1)*dt;
    /* time window length */
    if (twlen<.3) {
	twlen=.3; if (verb) sf_warning("twlen cannot be less than .3s, using .3s");
    }
    /* setting taper and spatial and temporal windows */
    n1taper =roundf(taper/dt);
    n1taper = (n1taper%2 ? n1taper+1 : n1taper); 

    N1w = roundf((n1-1)*dt/twlen);

    if (N1w==1) taper=0.0;

    n1ws = roundf(twlen/dt) + 1;
    n1wf = n1ws + n1taper/2;
    n1wi = n1ws + n1taper;

    if (!sf_getint("n2w", &n2w)) n2w=10;
    /* number of traces in window */
    if (!n2w) {
	n2w = 10;
	if (verb) sf_warning("n2w cannot be zero, using 10 traces");
    }	
    if (verb) sf_warning("n2w = %i",n2w);

    n2wu = n2w;

    if (!sf_getint("lenf", &lenf)) lenf=4; 
    /* number of traces for filter */
    if (!lenf) if (verb) sf_warning("using lenf=4");	
	
    N2w = n2/n2w;

    /* Computute FFT optimization number */
    nfft = 2*kiss_fft_next_fast_size((n1wf+1)/2);

    forw = kiss_fftr_alloc(nfft,0,NULL,NULL);
    invs = kiss_fftr_alloc(nfft,1,NULL,NULL);

    nf = nfft/2 + 1;
    df = 1.0/(nfft*dt);

    /* space allocation */
    cdata 	= (kiss_fft_cpx*) sf_complexalloc(nfft);
    traceintw =sf_floatalloc(nfft);
    traceinttw=sf_floatalloc(nfft);
    fdata   =(kiss_fft_cpx**) sf_complexalloc2(nf,n2);
    fdataw  =(kiss_fft_cpx**) sf_complexalloc2(nf,2*n2w+2*lenf);
    tracein = sf_floatalloc2(n1,n2);
    traceout= sf_floatalloc2(n1,n2);
    sfv   = (kiss_fft_cpx*) sf_complexalloc(2*n2w+2*lenf);
    sfvcj = (kiss_fft_cpx*) sf_complexalloc(2*n2w+2*lenf);
    acorr= (kiss_fft_cpx*) sf_complexalloc(lenf+1);
    rauto  = sf_floatalloc(lenf+1);
    iauto  = sf_floatalloc(lenf+1);
    cautom = sf_floatalloc2(2*lenf,2*lenf);
    aav = sf_floatalloc(2*lenf);
    filter = (kiss_fft_cpx*) sf_complexalloc(2*lenf+1);
    ffv   = (kiss_fft_cpx**) sf_complexalloc2(nf,2*n2w);
    sfvout=(kiss_fft_cpx*) sf_complexalloc(2*n2w);
    fv   =(kiss_fft_cpx*) sf_complexalloc(nfft);
    traceotw = sf_floatalloc(nfft);
    traceottw= sf_floatalloc(nfft);
    ipvt 	= sf_intalloc(4*n2w);
    info	= sf_floatalloc(4*n2w);

    /* zero output file */
    memset((void *) traceout[0], 0, n2*n1*sizeof(float));


    for (i3=0;i3<n3;i3++)
    {
    /* load traces into the zero-offset array and close tmpfile */
    sf_floatread(tracein[0],n1*n2,in);	
	
    /* If dt not set, issue advisory on frequency step df */
    if (dt && verb) sf_warning("df=%f", 1.0/(nfft*dt));

    if (verb) sf_warning("nf=%i, df=%f, nfft=%i, n1taper=%i", nf,df,nfft,n1taper);

    /* loop over time windows */
    for (i1w=0;i1w<N1w;i1w++) {

	if (i1w>0 && i1w<N1w-1) n1w=n1wi; 
	else if (i1w==0) 
	    if (N1w>1) n1w = n1wf;
	    else        n1w = n1;
	else
	    n1w = n1 - n1ws*i1w + n1taper/2;
 
	if (verb) sf_warning("i1w=%i, N1w=%i, n1w=%i, twlen=%f", i1w,N1w,n1w,twlen); 

	/* zero fdata */
	memset((void *) fdata[0], 0, nf*n2*sizeof(kiss_fft_cpx));
     
	/* select data */
	for (i2=0;i2<n2;i2++) {
 
	    if (i1w>0)
		for (i1=0;i1<n1w;i1++)
		    traceintw[i1]=tracein[i2][i1 + i1w*n1ws - n1taper/2];
	    else
		for (i1=0;i1<n1w;i1++)  
		    traceintw[i1]=tracein[i2][i1];	  

	    memset((void *) (traceintw + n1w), 0, (nfft-n1w)*sizeof(float));
	    memset((void *) cdata, 0, nfft*sizeof(kiss_fft_cpx));

	    /* FFT from t to f */
	    for (i1=0;i1<nfft;i1++)
		traceinttw[i1]=(i1%2 ? -traceintw[i1] : traceintw[i1]);
	    kiss_fftr(forw, traceinttw, cdata);

	    /* Store values */    
	    for (ifq = 0; ifq < nf; ifq++) { 
		fdata[i2][ifq] = cdata[nf-1-ifq];
	    }
	}

	/* Loop over space windows */
	for (i2w=0;i2w<N2w;i2w++){

	    /* to take care of a possible incomplete last window */
	    if (n2<i2w*n2w+2*n2w) 
		n2wu = n2 - i2w*n2w;
	    else
		n2wu = n2w;

	    if (verb) {
		sf_warning("i2w=%i, n2=%i, n2w=%i",
			   i2w,n2,n2w);
		sf_warning("n2wu=%i, N2w=%i, lenf=%i",
			   n2wu,N2w,lenf);
	    }

	    /* zero fdataw */
	    for (i2=0;i2<n2w+2*lenf;i2++)
		memset((void *) fdataw[i2], 0, nf*sizeof(kiss_fft_cpx));

	    /* select data */
	    for (i2=0;i2<n2wu+2*lenf;i2++) 
		for (ifq = 0; ifq < nf; ifq++) {

		    if (i2w>0 && i2w<N2w-1)  
			fdataw[i2][ifq] = fdata[i2 + i2w*n2w - lenf][ifq];
		    else if (i2w==0)
			if (i2>=lenf && i2<n2w+lenf) 
			    fdataw[i2][ifq] = fdata[i2 - lenf][ifq];
			else if (i2<lenf) 
			    fdataw[i2][ifq] = fdata[0][ifq];
			else 
			    if (N2w>1) 
				fdataw[i2][ifq] = fdata[i2 - lenf][ifq];
			    else 
				fdataw[i2][ifq] = fdata[n2-1][ifq];
		    else
			if (i2<n2wu+lenf)
			    fdataw[i2][ifq] = fdata[i2 + i2w*n2w - lenf][ifq];
			else 
			    fdataw[i2][ifq] = fdata[n2-1][ifq];
		}

	    /* loop over frequencies */
	    for (ifq=0;ifq<nf;ifq++) {

		if ((float)ifq*df>=fmin && (float)ifq*df<=fmax) {

		    /* Loop over space window */
		    memset((void *) sfv, 0, (n2wu+2*lenf)*sizeof(kiss_fft_cpx));
		    memset((void *) sfvcj, 0, (n2wu+2*lenf)*sizeof(kiss_fft_cpx));

		    for (i2=0;i2<n2wu+2*lenf;i2++) {
	  
			sfv[i2]=fdataw[i2][ifq];
			sfvcj[i2]=sf_conjf(fdataw[i2][ifq]);
		    }

		    memset((void *) acorr, 0, (lenf+1)*sizeof(kiss_fft_cpx));

		    /* complex autocorrelation */
		    cxcor(n2wu,0,sfv,n2wu,0,sfv,lenf+1,0,acorr);

		    /* zeroing files */
		    memset((void *) rauto, 0, (lenf+1)*sizeof(float));
		    memset((void *) iauto, 0, (lenf+1)*sizeof(float));

		    /* taking real and imaginary parts */
		    for (i2=0;i2<lenf+1;i2++) {
			rauto[i2]=acorr[i2].r;
			iauto[i2]=acorr[i2].i;
		    }

		    /* zeroing files */
		    memset((void *) aav, 0, 2*lenf*sizeof(float));
		    memset((void *) filter, 0, (2*lenf+1)*sizeof(kiss_fft_cpx));
		    for (ir=0;ir<2*lenf;ir++) 
			memset((void *) cautom[ir], 0, 2*lenf*sizeof(float));

		    /* matrix problem */
		    for (ir=0;ir<lenf;ir++) 
			for (jr=0;jr<lenf;jr++) { 
			    if (ir>=jr) cautom[ir][jr]=acorr[ir-jr].r;
			    else        cautom[ir][jr]=acorr[jr-ir].r;
			}

		    for (ir=lenf;ir<2*lenf;ir++)
			for (jr=0;jr<lenf;jr++) {
			    if (ir-lenf<jr) cautom[ir][jr]=-acorr[jr-ir+lenf].i;
			    else            cautom[ir][jr]= acorr[ir-jr-lenf].i;
			}

		    for (ir=lenf;ir<2*lenf;ir++)
			for (jr=lenf;jr<2*lenf;jr++)
			    cautom[ir][jr]=cautom[ir-lenf][jr-lenf];

		    for (ir=0;ir<lenf;ir++)
			for (jr=lenf;jr<2*lenf;jr++)
			    cautom[ir][jr]=-cautom[ir+lenf][jr-lenf];

		    for (ig=0;ig<2*lenf;ig++) {
			if (ig<lenf) aav[ig]=acorr[ig+1].r;
			else aav[ig]=acorr[ig-lenf+1].i;
		    }

		    lu_decomposition(2*lenf,cautom,ipvt,info);
		    backward_substitution(2*lenf,cautom,ipvt,aav);
      
		    /* construct filter */
		    for (ifv=0,ig=lenf-1;ifv<lenf;ifv++,ig--) 
			filter[ifv]=sf_conjf(cmplx(aav[ig]/2.,aav[ig+lenf]/2.));

		    for (ifv=lenf+1,ig=0;ifv<2*lenf+1;ifv++,ig++) 
			filter[ifv]=cmplx(aav[ig]/2.,aav[ig+lenf]/2.);
	 
		    memset((void *) sfvout, 0, n2wu*sizeof(kiss_fft_cpx));

		    /* convolution of data with filter */
		    /* output is one sample ahead */
		    cconv(n2wu+2*lenf,-lenf,sfv,2*lenf+1,-lenf,filter,n2wu,0,sfvout); 

		    /* store filtered values */
		    for (i2=0;i2<n2wu;i2++) ffv[i2][ifq]=sfvout[i2];

		}
	    } /* end of frequencies loop */

	    /* loop along space windows */
	    for (i2=0;i2<n2wu;i2++) {
    
		/* select data */
		for (ifq=0,itt=nf-1;ifq<nf;ifq++,itt--)
		    fv[ifq] = ffv[i2][itt]; 

		memset((void *) (fv+nf), 0, (nfft-nf)*sizeof(kiss_fft_cpx));
		memset((void *) traceotw, 0, nfft*sizeof(float));

		/* FFT back from f to t and scaling */
		kiss_fftri(invs,fv,traceotw);
		for (i1=0;i1<SF_MIN(n1,nfft);i1++)
		    traceotw[i1]/=nfft; 
		for (i1=0;i1<SF_MIN(n1,nfft);i1++)
		    traceottw[i1]=(i1%2 ? -traceotw[i1] : traceotw[i1]); 
      
		/*loop along time */
		if (N1w>1) {
		    /* first portion of time window */
		    if (i1w>0) 
			for (i1=0;i1<n1taper;i1++)
			    traceout[i2w*n2w+i2][i1+i1w*n1ws-n1taper/2]+=
				traceottw[i1]*((float)(i1)*dt/taper);
		    else 
			for (i1=0;i1<n1taper;i1++)
			    traceout[i2w*n2w+i2][i1]=traceottw[i1];

		    /* intermediate portion of time window */
		    if (i1w>0) 
			for (i1=n1taper;i1<n1w-n1taper;i1++)
			    traceout[i2w*n2w+i2][i1+i1w*n1ws-n1taper/2]=traceottw[i1];
		    else 
			for (i1=n1taper;i1<n1w-n1taper;i1++)
			    traceout[i2w*n2w+i2][i1]=traceottw[i1];

		    /* last portion of time window */
		    if (i1w>0 && i1w<N1w-1) 
			for (i1=n1w-n1taper;i1<n1w;i1++)
			    traceout[i2w*n2w+i2][i1+i1w*n1ws-n1taper/2]+=
				traceottw[i1]*(1.-((float)(i1-n1w+n1taper))*dt/taper);
		    else if (i1w==N1w-1)
			for (i1=n1w-n1taper;i1<n1w;i1++)
			    traceout[i2w*n2w+i2][i1+i1w*n1ws-n1taper/2]=traceottw[i1];
		    else 
			for (i1=n1w-n1taper;i1<n1w;i1++)
			    traceout[i2w*n2w+i2][i1]+=traceottw[i1]*(1.-((float)(i1-n1w+n1taper))*dt/taper);
		}
		else {
		    for (i1=0;i1<n1;i1++) 
			traceout[i2w*n2w+i2][i1]=traceottw[i1];
		}

	    } /* end loop over space windows */

	} /* end loop over space windows */

    } /* end of time windows loop */
 
    /* Write output data to file */
    sf_floatwrite(traceout[0], n1*n2, out);
    if(verb) sf_warning("I3=%d is done!\n",i3+1);
    }

    /* Free allocated memory */
    free(traceintw);
    free(traceinttw);
    free(fdataw[0]);
    free(fdataw);
    free(sfv);
    free(sfvcj);
    free(acorr);
    free(rauto);
    free(iauto);
    free(cautom[0]);
    free(cautom);
    free(aav);
    free(filter);
    free(sfvout);
    free(fv);
    free(traceotw);
    free(traceottw);
    free(ffv[0]);
    free(ffv);
    free(tracein[0]);
    free(tracein);
    free(traceout[0]);
    free(traceout);

    exit(0);
}
int main(){
    double *a, *l, *u, *lu, *pa;
    int *piv;
    int size = N*N;
    piv = (int *)malloc(sizeof(int)*N);
    a = (double *)malloc(sizeof(int)*size);
    u = (double *)malloc(sizeof(int)*size);
    l = (double *)malloc(sizeof(int)*size);
    lu = (double *)malloc(sizeof(int)*size);
    pa = (double *)malloc(sizeof(int)*size);

    for(int i=0;i<N;i++){
        for(int j=0;j<N;j++){
            IDX(a,N,i,j) = 1.0 / (double)(1 + i + j);
        }
    }
    lu_decomposition(a,l,u,N,piv);

    //construct_P

    double *p;
    p = (double *)calloc(size, sizeof(double));
    for(int i=0; i<N; i++){
        IDX(p,N,where_a(i,piv,N), i) = 1;
    }

    printf("\n");

    //compute PA
    for(int i=0; i<N;i++){
        for(int j=0;j<N;j++){
            IDX(pa,N,i,j) = 0;
            for(int k=0;k<N;k++){
                IDX(pa,N,i,j) += IDX(p,N,i,k) * IDX(a,N,k,j);
            }
        }
    }

    //compute L \times U
    for(int i=0; i<N;i++){
        for(int j=0;j<N;j++){
            IDX(lu,N,i,j) = 0;
            for(int k=0;k<N;k++){
                IDX(lu,N,i,j) += IDX(l,N,i,k) * IDX(u,N,k,j);
            }
        }
    }

    printf("P=\n");
    print_matrix(p,N);
    printf("A=\n");
    print_matrix(a,N);
    printf("L=\n");
    print_matrix(l,N);
    printf("U=\n");
    print_matrix(u,N);
    printf("P time A = \n");
    print_matrix(pa,N);
    printf("L times U =\n");
    print_matrix(lu, N);

    return 0;
}
int lu_decomposition(double *a, double *l, double *u, int m, int *piv){
    if(m==1){
        IDX(l,m,0,0) = 1.0;
        IDX(u,m,0,0) = IDX(a,m,0,0);
        *piv=0;
        return 0;
    }

    //partial_pivoting ;
    double max_a = 0;
    *piv=0;
    for(int i=0;i<m;i++){
        if(fabs(IDX(a,m,i,0)) > max_a){
            max_a = fabs(IDX(a,m,i,0));
            *piv = i;
        }
    }

    for(int i=0;i<m;i++){
        double tem;
        tem = IDX(a,m,0,i);
        IDX(a,m,0,i) = IDX(a,m,*piv,i);
        IDX(a,m,*piv,i) = tem;
    }

    IDX(u,m,0,0) = IDX(a,m,0,0);
    for(int i=1; i<m; i++) IDX(u,m,i,0) = 0.0;
    for(int i=1; i<m; i++) IDX(u,m,0,i) = IDX(a,m,0,i);

    IDX(l,m,0,0) = 1.0;
    for(int i=1; i<m; i++) IDX(l,m,i,0) = IDX(a,m,i,0) / IDX(u,m,0,0);

    int size = (m-1) * (m-1);
    double *sub_a,*sub_l,*sub_u;
    sub_a = (double *) malloc(sizeof(double)*size);
    sub_l = (double *) malloc(sizeof(double)*size);
    sub_u = (double *) malloc(sizeof(double)*size);

    for(int i=1;i<m;i++) for(int j=1;j<m;j++)
            IDX(sub_a,m-1,i-1,j-1) = IDX(a,m,i,j) - IDX(l,m,i,j) - IDX(u,m,0,j);

    lu_decomposition(sub_a,sub_l,sub_u,m-1,piv+1);

     for(int i=1;i<m;i++) for(int j=1;j<m;j++){
         IDX(l,m,i,j) = IDX(sub_l,m-1,i-1,j-1);
         IDX(u,m,i,j) = IDX(sub_u,m-1,i-1,j-1);
     }

     for(int i=1; i<m;i++){
         if(piv[i] != 0){
             double tem = IDX(l,m,i,0);
             IDX(l,m,i,0) = IDX(l,m,i+piv[i],0);
             IDX(l,m,i+piv[i],0) = tem;
         }
     }

     free(sub_a);
     free(sub_u);
     free(sub_l);
     return 0;
}
Exemplo n.º 11
0
int main(){
    int kadai;

    // 課題番号指定
    while(1){
        printf("課題番号: ");
        scanf("%d", &kadai);
        if (kadai < 1 || kadai > 8 || kadai == 7){
            printf("1~8!!\n");
        } else {
            break;
        }
    }

    if (kadai == 1){

        double coefficient_mat[N][N] = {{1, 0, 0}, {3, 1, 0}, {-2, 2, 1}};
        double right_hand_vec[N] = {2, 3, -1};
        double solution_vec[N];

        printf("--------------- L行列 ---------------\n");
        print_array(coefficient_mat);
        printf("------------- 右辺ベクトル ------------\n");
        print_vector(right_hand_vec);

        forward_substitution(coefficient_mat, right_hand_vec, solution_vec);

        printf("------------- 解ベクトル --------------\n");
        print_vector(solution_vec);

    } 
    else if (kadai == 2)
    {

        double coefficient_mat[N][N] = {{2, 1, -1}, {0, 3, 2}, {0, 0, -3}};
        double right_hand_vec[N] = {2, -3, 9};
        double solution_vec[N];

        printf("--------------- U行列 ----------------\n");
        print_array(coefficient_mat);
        printf("------------- 右辺ベクトル -------------\n");
        print_vector(right_hand_vec);

        backward_substitution(coefficient_mat, right_hand_vec, solution_vec);

        printf("-------------- 解ベクトル -------------\n");
        print_vector(solution_vec);

    }
    else if (kadai == 3)
    {

        double coefficient_mat[N][N];
        double l_matrix[N][N] = {{1, 0, 0}, {3, 1, 0}, {-2, 2, 1}};
        double u_matrix[N][N] = {{2, 1, -1}, {0, 3, 2}, {0, 0, -3}};

        mat_mlt(l_matrix, u_matrix, coefficient_mat);

        printf("---------- 係数行列 ----------\n");
        print_array(coefficient_mat);

    }
    else if (kadai == 4)
    {

        double coefficient_mat[N][N] = {{2, 1, -1}, {6, 6, -1}, {-4, 4, 3}};
        double l_matrix[N][N] = {0};
        double u_matrix[N][N] = {0};

        lu_decomposition(coefficient_mat, l_matrix, u_matrix);

        printf("---------- L行列 ----------\n");
        print_array(l_matrix);
        printf("---------- U行列 ----------\n");
        print_array(u_matrix);

    }
    else if (kadai == 5)
    {

        double coefficient_mat[N][N] = {{2, 1, -1}, {6, 6, -1}, {-4, 4, 3}};
        double right_hand_vec[N] = {2, 3, -1};
        double solution_vec[N];

        printf("---------- 係数行列 ----------\n");
        print_array(coefficient_mat);
        printf("---------- 右辺ベクトル ----------\n");
        print_vector(right_hand_vec);

        solute_SLE_with_n_variables(coefficient_mat, right_hand_vec, solution_vec);

        printf("---------- 解ベクトル ----------\n");
        print_vector(solution_vec);

    }
    else if (kadai == 6)
    {

        double h_coefficient_mat[N][N];
        double right_hand_vec[N];
        double solution_vec[N];

        int i, j;
        for (i = 0; i < N; i++){
            for (j = 0; j < N; j++){
                h_coefficient_mat[i][j] = pow(0.5, abs(i-j));
            }
        }
        printf("--------------- 行列H ---------------\n");
        print_array(h_coefficient_mat);

        for (i = 0; i < N; i++){
            right_hand_vec[i] = 3 - pow(2, i-N+1) - pow(2, -i);
        }
        printf("------------- 右辺ベクトル -------------\n");
        print_vector(right_hand_vec);

        solute_SLE_with_n_variables(h_coefficient_mat, right_hand_vec, solution_vec);

        printf("---------- 解ベクトル ----------\n");
        print_vector(solution_vec);

    }
    else if (kadai == 8)
    {

        double h_mat[N][N];
        double h_reverse_mat[N][N] = {0};
        double solution_mat[N][N];

        int i, j;
        for (i = 0; i < N; i++){
            for (j = 0; j < N; j++){
                h_mat[i][j] = pow(0.5, abs(i-j));
            }
        }

        evaluate_reverse_matrix(h_mat, h_reverse_mat);

        mat_mlt(h_mat, h_reverse_mat, solution_mat);
        printf("--------------- 行列H ---------------\n");
        print_array(h_mat);
        printf("------------ 行列Hの逆行列 ------------\n");
        print_array(h_reverse_mat);
        printf("---------- 解行列 ----------\n");
        print_array(solution_mat);
    } 

    return 0;
}