void globalFrictionContact_display(GlobalFrictionContactProblem* problem)
{

  assert(problem);
  int i, n = problem->dimension * problem->numberOfContacts;
  printf("GlobalFrictionContact Display :\n-------------\n");
  printf("dimension :%d \n", problem->dimension);
  printf("numberOfContacts:%d \n", problem->numberOfContacts);
  int m = problem->M->size0;
  if (problem->M)
  {
    printf("M matrix:\n");
    NM_display(problem->M);
  }
  else
    printf("No M matrix:\n");
  if (problem->H)
  {
    printf("H matrix:\n");
    NM_display(problem->H);
  }
  else
    printf("No H matrix:\n");

  if (problem->q)
  {
    printf("q vector:\n");
    for (i = 0; i < m; i++) printf("q[ %i ] = %12.8e\n", i, problem->q[i]);
  }
  else
    printf("No q vector:\n");

  if (problem->b)
  {
    printf("b vector:\n");
    for (i = 0; i < n; i++) printf("b[ %i ] = %12.8e\n", i, problem->b[i]);
  }
  else
    printf("No q vector:\n");

  if (problem->mu)
  {
    printf("mu vector:\n");
    for (i = 0; i < problem->numberOfContacts; i++) printf("mu[ %i ] = %12.8e\n", i, problem->mu[i]);
  }
  else
    printf("No mu vector:\n");

}
/* Alart & Curnier solver for sparse global problem */
void gfc3d_nonsmooth_Newton_AlartCurnier(
  GlobalFrictionContactProblem* problem,
  double *reaction,
  double *velocity,
  double *globalVelocity,
  int *info,
  SolverOptions* options)
{

  assert(problem);
  assert(reaction);
  assert(velocity);
  assert(info);
  assert(options);

  assert(problem->dimension == 3);

  assert(options->iparam);
  assert(options->dparam);

  assert(problem->q);
  assert(problem->mu);
  assert(problem->M);
  assert(problem->H);


  /* M is square */
  assert(problem->M->size0 == problem->M->size1);
  assert(problem->M->size0 == problem->H->size0);



  unsigned int iter = 0;
  unsigned int itermax = options->iparam[SICONOS_IPARAM_MAX_ITER];
  unsigned int erritermax = options->iparam[SICONOS_FRICTION_3D_IPARAM_ERROR_EVALUATION];

  if (erritermax == 0)
  {
    /* output a warning here */
    erritermax = 1;
  }

  assert(itermax > 0);


  double tolerance = options->dparam[SICONOS_DPARAM_TOL];
  assert(tolerance > 0);


  DEBUG_EXPR(NM_display(problem->M););
void secondOrderConeLinearComplementarityProblem_display(SecondOrderConeLinearComplementarityProblem* problem)
{

  assert(problem);



  int i, n ;

  n=problem->n;

  printf("SecondOrderConeLinearComplementarityProblem Display :\n-------------\n");
  printf("nc:%d \n", problem->nc);

  if(problem->M)
  {
    printf("M matrix:\n");
    NM_display(problem->M);
  }
  else
    printf("No M matrix:\n");

  if(problem->q)
  {
    printf("q vector:\n");
    for(i = 0; i < n; i++) printf("q[ %i ] = %12.8e\n", i, problem->q[i]);
  }
  else
    printf("No q vector:\n");

  if(problem->coneIndex)
  {
    printf("coneIndex vector:\n");
    for(i = 0; i < problem->nc+1; i++) printf("coneIndex[ %i ] = %i\n", i, problem->coneIndex[i]);
  }
  else
    printf("No mu vector:\n");
  if(problem->mu)
  {
    printf("mu vector:\n");
    for(i = 0; i < problem->nc; i++) printf("mu[ %i ] = %12.8e\n", i, problem->mu[i]);
  }
  else
    printf("No mu vector:\n");

}
void linearComplementarity_display(LinearComplementarityProblem* problem)
{

  assert(problem);
  int i, n = problem->size;
  printf("LinearComplementarityProblem Display :\n-------------\n");
  printf("size :%d \n", problem->size);
  if (problem->M)
  {
    printf("M matrix:\n");
    NM_display(problem->M);
  }
  else
    printf("No M matrix:\n");

  if (problem->q)
  {
    printf("q vector:\n");
    for (i = 0; i < n; i++) printf("q[ %i ] = %12.8e\n", i, problem->q[i]);
  }
  else
    printf("No q vector:\n");

}
Exemplo n.º 5
0
void ncp_pathsearch(NonlinearComplementarityProblem* problem, double* z, double* F, int *info , SolverOptions* options)
{
/* Main step of the algorithm:
 * - compute jacobians
 * - call modified lemke
*/

  unsigned int n = problem->n;
  unsigned int preAlloc = options->iparam[SICONOS_IPARAM_PREALLOC];
  int itermax = options->iparam[SICONOS_IPARAM_MAX_ITER];

  double merit_norm = 1.0;
  double nn_tol = options->dparam[SICONOS_DPARAM_TOL];
  int nbiter = 0;

  /* declare a LinearComplementarityProblem on the stack*/
  LinearComplementarityProblem lcp_subproblem;
  lcp_subproblem.size = n;


  /* do some allocation if required
   * - nabla_F (used also as M for the LCP subproblem)
   * - q for the LCP subproblem
   *
   * Then fill the LCP subproblem
   */
  if (!preAlloc || (preAlloc && !options->internalSolvers))
  {
    options->internalSolvers = (SolverOptions *) malloc(sizeof(SolverOptions));
    solver_options_set(options->internalSolvers, SICONOS_LCP_PIVOT);
    options->numberOfInternalSolvers = 1;

    SolverOptions * lcp_options = options->internalSolvers;

    /* We always allocation once and for all since we are supposed to solve
     * many LCPs */
    lcp_options->iparam[SICONOS_IPARAM_PREALLOC] = 1;
    /* set the right pivot rule */
    lcp_options->iparam[SICONOS_IPARAM_PIVOT_RULE] = SICONOS_LCP_PIVOT_PATHSEARCH;
    /* set the right stacksize */
    lcp_options->iparam[SICONOS_IPARAM_PATHSEARCH_STACKSIZE] = options->iparam[SICONOS_IPARAM_PATHSEARCH_STACKSIZE];
  }


  assert(problem->nabla_F);
  lcp_subproblem.M = problem->nabla_F;


  if (!preAlloc || (preAlloc && !options->dWork))
  {
    options->dWork = (double *) malloc(4*n*sizeof(double));
  }
  lcp_subproblem.q = options->dWork;
  double* x = &options->dWork[n];
  double* x_plus = &options->dWork[2*n];
  double* r = &options->dWork[3*n];

  NMS_data* data_NMS;
  functions_LSA* functions;

  if (!preAlloc || (preAlloc && !options->solverData))
  {
    options->solverData = malloc(sizeof(pathsearch_data));
    pathsearch_data* solverData = (pathsearch_data*) options->solverData;

    /* do all the allocation */
    solverData->data_NMS = create_NMS_data(n, NM_DENSE, options->iparam, options->dparam);
    solverData->lsa_functions = (functions_LSA*) malloc(sizeof(functions_LSA));
    solverData->data_NMS->set = malloc(sizeof(positive_orthant));

    data_NMS = solverData->data_NMS;
    functions = solverData->lsa_functions;
    /* for use in NMS;  only those 3 functions are called */
    init_lsa_functions(functions, &FB_compute_F_ncp, &ncp_FB);
    functions->compute_H = &FB_compute_H_ncp;

    set_set_id(data_NMS->set, SICONOS_SET_POSITIVE_ORTHANT);

    /* fill ls_data */
    data_NMS->ls_data->compute_F = functions->compute_F;
    data_NMS->ls_data->compute_F_merit = functions->compute_F_merit;
    data_NMS->ls_data->z = NULL; /* XXX to check -- xhub */
    data_NMS->ls_data->zc = NMS_get_generic_workV(data_NMS->workspace, n);
    data_NMS->ls_data->F = NMS_get_F(data_NMS->workspace, n);
    data_NMS->ls_data->F_merit = NMS_get_F_merit(data_NMS->workspace, n);
    data_NMS->ls_data->desc_dir = NMS_get_dir(data_NMS->workspace, n);
    /** \todo this value should be settable by the user with a default value*/
    data_NMS->ls_data->alpha_min = fmin(data_NMS->alpha_min_watchdog, data_NMS->alpha_min_pgrad);
    data_NMS->ls_data->data = (void*)problem;
    data_NMS->ls_data->set = data_NMS->set;
    data_NMS->ls_data->sigma = options->dparam[SICONOS_DPARAM_NMS_SIGMA];
    /* data_NMS->ls_data->searchtype is set in the NMS code */
  }
  else
  {
    pathsearch_data* solverData = (pathsearch_data*) options->solverData;
    data_NMS = solverData->data_NMS;
    functions = solverData->lsa_functions;
  }

  /* initial value for ref_merit */
  problem->compute_F(problem->env, n, z, F);
  functions->compute_F_merit(problem, z, F, data_NMS->ls_data->F_merit);

  data_NMS->ref_merit = .5 * cblas_ddot(n, data_NMS->ls_data->F_merit, 1, data_NMS->ls_data->F_merit, 1);
  data_NMS->merit_bestpoint = data_NMS->ref_merit;
  cblas_dcopy(n, z, 1, NMS_checkpoint_0(data_NMS, n), 1);
  cblas_dcopy(n, z, 1, NMS_checkpoint_T(data_NMS, n), 1);
  cblas_dcopy(n, z, 1, NMS_bestpoint(data_NMS, n), 1);
  /* -------------------- end init ---------------------------*/

  int nms_failed = 0;
  double err = 10*nn_tol;

  /* to check the solution */
  LinearComplementarityProblem lcp_subproblem_check;
  int check_lcp_solution = 1; /* XXX add config for that */

  double normal_norm2_newton_point;

  /* F is already computed here at z */

  while ((err > nn_tol) && (nbiter < itermax) && !nms_failed)
  {
    int force_watchdog_step = 0;
    int force_d_step_merit_check = 0;
    double check_ratio = 0.0;
    nbiter++;
    /* update M, q and r */

    /* First find x */
    ncp_pathsearch_compute_x_from_z(n, z, F, x);
    pos_part(n, x, x_plus); /* update x_plus */

    ncp_pathsearch_update_lcp_data(problem, &lcp_subproblem, n, x_plus, x, r);

    if (check_lcp_solution)
    {
      lcp_subproblem_check.size = n;
      lcp_subproblem_check.M = problem->nabla_F;
      lcp_subproblem_check.q = lcp_subproblem.q;
      //cblas_dcopy(n, x, 1, lcp_subproblem_check.q , 1);
      //prodNumericsMatrix(n, n, -1.0, problem->nabla_F, x_plus, 0.0, lcp_subproblem.q);
    }

    double norm_r2 = cblas_ddot(n, r, 1, r, 1);
    if (norm_r2 < DBL_EPSILON*DBL_EPSILON) /* ||r|| < 1e-15 */
    {
      DEBUG_PRINTF("ncp_pathsearch :: ||r||  = %e < %e; path search procedure was successful!\n", norm_r2, DBL_EPSILON*DBL_EPSILON);
      (*info) = 0;
      ncp_compute_error(n, z, F, nn_tol, &err); /* XXX F should be up-to-date, we should check only CC*/
      break;
    }

    /* end update M, q and r */

    lcp_pivot_covering_vector(&lcp_subproblem, x_plus, x, info, options->internalSolvers, r);

    switch (*info)
    {
      case LCP_PIVOT_SUCCESS:
        DEBUG_PRINT("ncp_pathsearch :: path search procedure was successful!\n");
        if (check_lcp_solution)
        {
          double err_lcp = 0.0;
          cblas_daxpy(n, 1.0, r, 1, lcp_subproblem_check.q, 1);
          lcp_compute_error(&lcp_subproblem_check, x_plus, x, 1e-14, &err_lcp);
          double local_tol = fmax(1e-14, DBL_EPSILON*sqrt(norm_r2));
          printf("ncp_pathsearch :: lcp solved with error = %e; local_tol = %e\n", err_lcp, local_tol);
          //assert(err_lcp < local_tol && "ncp_pathsearch :: lcp solved with very bad precision");
          if (err_lcp > local_tol)
          {
            printf("ncp_pathsearch :: lcp solved with very bad precision\n");
            NM_display(lcp_subproblem.M);
            printf("z r q x_plus\n");
            for (unsigned i = 0; i < n; ++i) printf("%e %e %e %e\n", z[i], r[i], lcp_subproblem.q[i], x_plus[i]);
            options->internalSolvers->iparam[SICONOS_IPARAM_PIVOT_RULE] = 0;
            lcp_pivot(&lcp_subproblem, x_plus, x, info, options->internalSolvers);
            options->internalSolvers->iparam[SICONOS_IPARAM_PIVOT_RULE] = SICONOS_LCP_PIVOT_PATHSEARCH;
            lcp_compute_error(&lcp_subproblem_check, x_plus, x, 1e-14, &err_lcp);
            printf("ncp_pathsearch :: lcp resolved with error = %e; local_tol = %e\n", err_lcp, local_tol);
          }


          /* XXX missing recompute x ?*/
          /* recompute the normal norm */
          problem->compute_F(problem->env, n, x_plus, r);
          cblas_daxpy(n, -1.0, x, 1, r, 1);
          normal_norm2_newton_point = cblas_ddot(n, r, 1, r, 1);
          if (normal_norm2_newton_point > norm_r2)
          {
            printf("ncp_pathsearch :: lcp successfully solved, but the norm of the normal map increased! %e > %e\n", normal_norm2_newton_point, norm_r2);
            //assert(normal_norm2_newton_point <= norm_r2);
          }
          else
          {
            printf("ncp_pathsearch :: lcp successfully solved, norm of the normal map decreased! %e < %e\n", normal_norm2_newton_point, norm_r2);
            //check_ratio = norm_r2/normal_norm2_newton_point;
          }
          if (50*normal_norm2_newton_point < norm_r2)
          {
            force_d_step_merit_check = 1;
          }
          else if (10*normal_norm2_newton_point < norm_r2)
          {
//            check_ratio = sqrt(norm_r2/normal_norm2_newton_point);
          }
        }
        break;
      case LCP_PIVOT_RAY_TERMINATION:
        DEBUG_PRINT("ncp_pathsearch :: ray termination, let's fastened your seat belt!\n");
        break;
      case LCP_PATHSEARCH_LEAVING_T:
        DEBUG_PRINT("ncp_pathsearch :: leaving t, fastened your seat belt!\n");
        DEBUG_PRINTF("ncp_pathsearch :: max t value = %e\n", options->internalSolvers->dparam[2]); /* XXX fix 2 */
        /* try to retry solving the problem */
        /* XXX keep or not ? */
        /* recompute the normal norm */
        problem->compute_F(problem->env, n, x_plus, r);
        cblas_daxpy(n, -1.0, x, 1, r, 1);
        normal_norm2_newton_point = cblas_ddot(n, r, 1, r, 1);
        if (normal_norm2_newton_point > norm_r2)
        {
          printf("ncp_pathsearch :: lcp successfully solved, but the norm of the normal map increased! %e > %e\n", normal_norm2_newton_point, norm_r2);
          //assert(normal_norm2_newton_point <= norm_r2);
        }
        else
        {
          printf("ncp_pathsearch :: lcp successfully solved, norm of the normal map decreased! %e < %e\n", normal_norm2_newton_point, norm_r2);
          check_ratio = 5.0*norm_r2/normal_norm2_newton_point;
        }
        if (options->internalSolvers->dparam[2] > 1e-5) break;
        memset(x_plus, 0, sizeof(double) * n);
        problem->compute_F(problem->env, n, x_plus, r);
        ncp_pathsearch_compute_x_from_z(n, x_plus, r, x);
        ncp_pathsearch_update_lcp_data(problem, &lcp_subproblem, n, x_plus, x, r);
        lcp_pivot_covering_vector(&lcp_subproblem, x_plus, x, info, options->internalSolvers, r);
        if (*info == LCP_PIVOT_SUCCESS)
        {
           DEBUG_PRINT("ncp_pathsearch :: Lemke start worked !\n");
           double err_lcp = 0.0;
           cblas_daxpy(n, 1.0, r, 1, lcp_subproblem_check.q, 1);
           lcp_compute_error(&lcp_subproblem_check, x_plus, x, 1e-14, &err_lcp);
           double local_tol = fmax(1e-14, DBL_EPSILON*sqrt(norm_r2));
           printf("ncp_pathsearch :: lcp solved with error = %e; local_tol = %e\n", err_lcp, local_tol);
           assert(err_lcp < local_tol);
        }
        else
        {
          NM_display(lcp_subproblem.M);
          printf("z r q x_plus\n");
          for (unsigned i = 0; i < n; ++i) printf("%e %e %e %e\n", z[i], r[i], lcp_subproblem.q[i], x_plus[i]);
          DEBUG_PRINT("ncp_pathsearch :: Lemke start did not succeeded !\n");
          lcp_pivot_diagnose_info(*info);
          if (*info == LCP_PATHSEARCH_LEAVING_T)
          {
            DEBUG_PRINTF("ncp_pathsearch :: max t value after Lemke start = %e\n", options->internalSolvers->dparam[2]);
          }
          options->internalSolvers->iparam[SICONOS_IPARAM_PIVOT_RULE] = 0;
          lcp_pivot(&lcp_subproblem, x_plus, x, info, options->internalSolvers);
          options->internalSolvers->iparam[SICONOS_IPARAM_PIVOT_RULE] = SICONOS_LCP_PIVOT_PATHSEARCH;
          double err_lcp = 0.0;
          lcp_compute_error(&lcp_subproblem, x_plus, x, 1e-14, &err_lcp);
          printf("ncp_pathsearch :: lemke start resolved with info = %d; error = %e\n", *info, err_lcp);
          printf("x_plus x_minus\n");
          for (unsigned i = 0; i < n; ++i) printf("%e %e\n", x_plus[i], x[i]);
          /* recompute the normal norm */
          problem->compute_F(problem->env, n, x_plus, r);
          cblas_daxpy(n, -1.0, x, 1, r, 1);
          double normal_norm2_newton_point = cblas_ddot(n, r, 1, r, 1);
          if (normal_norm2_newton_point > norm_r2)
          {
            printf("ncp_pathsearch :: lcp successfully solved, but the norm of the normal map increased! %e > %e\n", normal_norm2_newton_point, norm_r2);
            //assert(normal_norm2_newton_point <= norm_r2);
          }
          else
          {
             printf("ncp_pathsearch :: lcp successfully solved, norm of the normal map decreased! %.*e < %.*e\n", DECIMAL_DIG, normal_norm2_newton_point, DECIMAL_DIG, norm_r2);
          }
          if (100*normal_norm2_newton_point < norm_r2)
          {
            force_d_step_merit_check = 1;
          }
        }
        break;
      case LCP_PIVOT_NUL:
        printf("ncp_pathsearch :: kaboom, kaboom still more work needs to be done\n");
        lcp_pivot_diagnose_info(*info);
//        exit(EXIT_FAILURE);
        force_watchdog_step = 1;
        break;
      case LCP_PATHSEARCH_NON_ENTERING_T:
        DEBUG_PRINT("ncp_pathsearch :: non entering t, something is wrong here. Fix the f****** code!\n");
        assert(0 && "ncp_pathsearch :: non entering t, something is wrong here\n"); 
        force_watchdog_step = 1;
        break;
      default:
        printf("ncp_pathsearch :: unknown code returned by the path search\n");
        exit(EXIT_FAILURE);
    }

    nms_failed = NMS(data_NMS, problem, functions, z, x_plus, force_watchdog_step, force_d_step_merit_check, check_ratio);
    /* at this point z has been updated */

    /* recompute the normal norm */
    problem->compute_F(problem->env, n, z, F);
    functions->compute_F_merit(problem, z, F, data_NMS->ls_data->F_merit);

    /* XXX is this correct ? */
    merit_norm = .5 * cblas_ddot(n, data_NMS->ls_data->F_merit, 1, data_NMS->ls_data->F_merit, 1);

    ncp_compute_error(n, z, F, nn_tol, &err); /* XXX F should be up-to-date, we should check only CC*/
    DEBUG_PRINTF("ncp_pathsearch :: iter = %d, ncp_error = %e; merit_norm^2 = %e\n", nbiter, err, merit_norm);

  }

  options->iparam[1] = nbiter;
  options->dparam[1] = err;
  if (nbiter == itermax)
  {
    *info = 1;
  }
  else if (nms_failed)
  {
    *info = 2;
  }
  else
  {
    *info = 0;
  }

  DEBUG_PRINTF("ncp_pathsearch procedure finished :: info = %d; iter = %d; ncp_error = %e; merit_norm^2 = %e\n", *info, nbiter, err, merit_norm);

  if (!preAlloc)
  {
    freeNumericsMatrix(problem->nabla_F);
    free(problem->nabla_F);
    problem->nabla_F = NULL;
    free(options->dWork);
    options->dWork = NULL;
    solver_options_delete(options->internalSolvers);
    free(options->internalSolvers);
    options->internalSolvers = NULL;
    free_NMS_data(data_NMS);
    free(functions);
    free(options->solverData);
    options->solverData = NULL;
  }
}
static void fc3d_AC_initialize(FrictionContactProblem* problem,
                               FrictionContactProblem* localproblem,
                               SolverOptions * options)
{
  /** In initialize, these operators are "connected" to their corresponding static variables,
   * that will be used to build local problem for each considered contact.
   * Local problem is built during call to update (which depends on the storage type for M).
   */

  DEBUG_PRINTF("fc3d_AC_initialize starts with options->iparam[10] = %i\n",
               options->iparam[SICONOS_FRICTION_3D_NSN_FORMULATION]);

  if (options->iparam[SICONOS_FRICTION_3D_NSN_FORMULATION] ==
      SICONOS_FRICTION_3D_NSN_FORMULATION_ALARTCURNIER_STD )
  {
    Function = &(computeAlartCurnierSTD);
  }
  else if (options->iparam[SICONOS_FRICTION_3D_NSN_FORMULATION] ==
           SICONOS_FRICTION_3D_NSN_FORMULATION_JEANMOREAU_STD )
  {
    Function = &(computeAlartCurnierJeanMoreau);
  }
  else if (options->iparam[SICONOS_FRICTION_3D_NSN_FORMULATION] ==
           SICONOS_FRICTION_3D_NSN_FORMULATION_ALARTCURNIER_GENERATED )
  {
    Function = &(fc3d_AlartCurnierFunctionGenerated);
  }
  else if (options->iparam[SICONOS_FRICTION_3D_NSN_FORMULATION] ==
           SICONOS_FRICTION_3D_NSN_FORMULATION_JEANMOREAU_GENERATED )
  {
    Function = &fc3d_AlartCurnierJeanMoreauFunctionGenerated;;
  }
  else if (options->iparam[SICONOS_FRICTION_3D_NSN_FORMULATION] ==
           SICONOS_FRICTION_3D_NSN_FORMULATION_NULL)
  {
    Function = NULL;
  }

  /* Compute and store default value of rho value */
  int nc = problem->numberOfContacts;

  double avg_rho[3] = {0.0, 0.0, 0.0};

  if (options->solverId == SICONOS_FRICTION_3D_ONECONTACT_NSN ||
      options->solverId == SICONOS_FRICTION_3D_ONECONTACT_NSN_GP)
  {
    if (!options->dWork ||
        options->dWorkSize < 3*nc)
    {
      options->dWork = (double *)realloc(options->dWork,
                                         3*nc * sizeof(double));
      options->dWorkSize = 3*nc ;
    }
  }
  else if (options->solverId == SICONOS_FRICTION_3D_ONECONTACT_NSN_GP_HYBRID)
  {
    if (!options->dWork ||
        options->dWorkSize < 4*nc)
    {
      options->dWork = (double *)realloc(options->dWork,
                                         4*nc * sizeof(double));
      options->dWorkSize = 4*nc ;
    }
  }

  

  double  * rho;
  for (int contact =0; contact <nc ; contact++)
  {
    if (options->solverId == SICONOS_FRICTION_3D_ONECONTACT_NSN ||
        options->solverId == SICONOS_FRICTION_3D_ONECONTACT_NSN_GP)
    {
      rho = &options->dWork[3*contact];
    }
    else if (options->solverId == SICONOS_FRICTION_3D_ONECONTACT_NSN_GP_HYBRID)
    {
      options->dWork[contact] = 1.0; // for PLI algorithm.
      rho = &options->dWork[3*contact+nc];
    }
    numerics_printf("fc3d_AC_initialize "" compute rho for contact = %i",contact);

    if (options->iparam[SICONOS_FRICTION_3D_NSN_RHO_STRATEGY] == SICONOS_FRICTION_3D_NSN_FORMULATION_RHO_STRATEGY_SPLIT_SPECTRAL_NORM_COND)
    {
      fc3d_local_problem_fill_M(problem, localproblem, contact);
      compute_rho_split_spectral_norm_cond(localproblem, rho);
    }
    else if (options->iparam[SICONOS_FRICTION_3D_NSN_RHO_STRATEGY] == SICONOS_FRICTION_3D_NSN_FORMULATION_RHO_STRATEGY_SPLIT_SPECTRAL_NORM)
    {
      fc3d_local_problem_fill_M(problem, localproblem, contact);
      compute_rho_split_spectral_norm(localproblem, rho);
    }
    else if (options->iparam[SICONOS_FRICTION_3D_NSN_RHO_STRATEGY] == SICONOS_FRICTION_3D_NSN_FORMULATION_RHO_STRATEGY_SPECTRAL_NORM)
    {
      fc3d_local_problem_fill_M(problem, localproblem, contact);
      compute_rho_spectral_norm(localproblem, rho);
    }
    else if (options->iparam[SICONOS_FRICTION_3D_NSN_RHO_STRATEGY] == SICONOS_FRICTION_3D_NSN_FORMULATION_RHO_STRATEGY_CONSTANT)
    {
      rho[0]=options->dparam[SICONOS_FRICTION_3D_NSN_RHO];
      rho[1]=options->dparam[SICONOS_FRICTION_3D_NSN_RHO];
      rho[2]=options->dparam[SICONOS_FRICTION_3D_NSN_RHO];
    }
    else if (options->iparam[SICONOS_FRICTION_3D_NSN_RHO_STRATEGY] == SICONOS_FRICTION_3D_NSN_FORMULATION_RHO_STRATEGY_ADAPTIVE)
    {
      numerics_error("fc3d_AC_initialize", "Adaptive strategy for computing rho not yet implemented");
    }
    else
      numerics_error("fc3d_AC_initialize", "unknown strategy for computing rho");


    if (verbose >0)
    {
      avg_rho[0] += rho[0];
      avg_rho[1] += rho[1];
      avg_rho[2] += rho[2];
    }
    numerics_printf("fc3d_AC_initialize""contact = %i, rho[0] = %4.2e, rho[1] = %4.2e, rho[2] = %4.2e", contact, rho[0], rho[1], rho[2]);

    fc3d_local_problem_fill_M(problem, localproblem, contact);
    double m_row_norm = 0.0, sum;
    for (int i =0; i<3; i++ )
    {
      sum =0.0;
      for (int j =0; j<3; j++ )
      {
        sum += fabs(localproblem->M->matrix0[i+j*3]);
      }
      m_row_norm = max(sum, m_row_norm);
    }
    numerics_printf("fc3d_AC_initialize" " inverse of norm of M = %e", 1.0/hypot9(localproblem->M->matrix0) );
    numerics_printf("fc3d_AC_initialize" " inverse of row norm of M = %e", 1.0/m_row_norm );

    DEBUG_EXPR(NM_display(localproblem->M););

  }
Exemplo n.º 7
0
int main(void)
{

  printf("========= Starts Numerics tests for NumericsMatrix ========= \n");

  int i, nmm = 4 ;
  NumericsMatrix ** NMM = (NumericsMatrix **)malloc(nmm * sizeof(NumericsMatrix *)) ;
  NumericsMatrix ** Mread = (NumericsMatrix **)malloc(nmm * sizeof(NumericsMatrix *)) ;


  for (i = 0 ; i < nmm; i++)
  {
    NMM[i] = newNumericsMatrix();
    Mread[i] = newNumericsMatrix();
  }


  int info = test_BuildNumericsMatrix(NMM);

  if (info != 0)
  {
    printf("Construction failed ...\n");
    return info;
  }
  printf("Construction ok ...\n");

  /* Test of various I/O functions */

  for (i = 0 ; i < nmm; i++)
  {

    printf("test on NMM[%i]\n", i);

    NM_display(NMM[i]);
    displayRowbyRow(NMM[i]);
    FILE * foutput = fopen("testprintInfile.dat", "w");
    printInFile(NMM[i], foutput);
    fclose(foutput);
    FILE * finput = fopen("testprintInfile.dat", "r");
    readInFile(NMM[i], finput);
    fclose(finput);
    FILE * finput2 = fopen("testprintInfile.dat", "r");
    newFromFile(Mread[i], finput2);
    fclose(finput2);
    char  filename[50] = "testprintInfileName.dat";
    printInFileName(NMM[i], filename);
    readInFileName(NMM[i], filename);
    printf("end of test on NMM[%i]\n", i);

  }
  for (i = 0 ; i < nmm; i++, i++)
  {
    FILE * foutput2 = fopen("testprintInfileForScilab.dat", "w");
    printInFileForScilab(NMM[i], foutput2);
    fclose(foutput2);
  }



  /* free memory */

  for (i = 0 ; i < nmm; i++)
  {
    freeNumericsMatrix(NMM[i]);
    free(NMM[i]);
    freeNumericsMatrix(Mread[i]);
    free(Mread[i]);
  }

  free(NMM);
  free(Mread);



  printf("========= End Numerics tests for NumericsMatrix ========= \n");
  return info;
}
void mixedLinearComplementarity_display(MixedLinearComplementarityProblem* p)
{
  int n = p->n;
  int m = p->m;
  printf("MLCP DISPLAY:\n-------------\n");
  printf("n :%d m: %d\n", p->n, p->m);


  printf(p->isStorageType1 ? "using (M)\n" : "not using (M)\n");
  printf(p->isStorageType2 ? "using (ABCD)\n" : "not using (ABCD)\n");
  if (p->blocksRows)
  {
    printf("blocks are:\n");
    int NumBlock = 0;
    while (p->blocksRows[NumBlock] < n + m)
    {
      if (p->blocksIsComp[NumBlock])
      {
        printf("->block of complementarity condition (type %d), from line %d, to line %d.\n", p->blocksIsComp[NumBlock], p->blocksRows[NumBlock], p->blocksRows[NumBlock + 1] - 1);
      }
      else
      {
        printf("->block of equality type (type %d), from line %d, to line %d.\n", p->blocksIsComp[NumBlock], p->blocksRows[NumBlock], p->blocksRows[NumBlock + 1] - 1);
      }
      NumBlock++;
    }
  }

  if (p->M)
  {
    printf("M matrix:\n");
    NM_display(p->M);
  }
  else
    printf("No M matrix:\n");

  if (p->q)
  {
    printf("q matrix:\n");
    NM_dense_display(p->q, n + m, 1, 0);
  }
  else
    printf("No q matrix:\n");

  if (p->A)
  {
    printf("A matrix:\n");
    NM_dense_display(p->A, n, n, 0);
  }
  else
  {
    printf("No A matrix:\n");
    if (p->M && !p->M->storageType)
    {
      printf("A matrix from M:\n");
      NM_dense_display(p->M->matrix0, n, n, n + m);
    }
  }
  if (p->B)
  {
    printf("B matrix:\n");
    NM_dense_display(p->B, m, m, 0);
  }
  else
  {
    printf("No B matrix:\n");
    if (p->M && !p->M->storageType)
    {
      printf("B matrix from M:\n");
      NM_dense_display(p->M->matrix0 + n * (n + m) + n, m, m, n + m);
    }
  }

  if (p->C)
  {
    printf("C matrix:\n");
    NM_dense_display(p->C, n, m, 0);
  }
  else
  {
    printf("No C matrix:\n");
    if (p->M && !p->M->storageType)
    {
      printf("C matrix from M:\n");
      NM_dense_display(p->M->matrix0 + n * (n + m), n, m, n + m);
    }
  }

  if (p->D)
  {
    printf("D matrix:\n");
    NM_dense_display(p->D, m, n, 0);
  }
  else
  {
    printf("No D matrix:\n");
    if (p->M && !p->M->storageType)
    {
      printf("D matrix from M:\n");
      NM_dense_display(p->M->matrix0 + n, m, n, n + m);
    }
  }
  if (p->a)
  {
    printf("a matrix:\n");
    NM_dense_display(p->a, n, 1, 0);
  }
  else
    printf("No a matrix:\n");
  if (p->b)
  {
    printf("b matrix:\n");
    NM_dense_display(p->b, m, 1, 0);
  }
  else
    printf("No b matrix:\n");

}