Ejemplo n.º 1
0
void frictionContact2D_enum(FrictionContactProblem* problem, double *reaction, double *velocity, int *info, SolverOptions* options,  NumericsOptions* global_options)
{
  int i;
  // conversion into LCP
  LinearComplementarityProblem* lcp_problem = (LinearComplementarityProblem*)malloc(sizeof(LinearComplementarityProblem));

  FrictionContact2D_tolcp(problem, lcp_problem);
  /* frictionContact_display(problem); */
  /* linearComplementarity_display(lcp_problem); */



  double *zlcp = (double*)malloc(lcp_problem->size * sizeof(double));
  double *wlcp = (double*)malloc(lcp_problem->size * sizeof(double));

  for (i = 0; i < lcp_problem->size; i++)
  {
    zlcp[i] = 0.0;
    wlcp[i] = 0.0;
  }


  /*  FILE * fcheck = fopen("lcp_relay.dat","w"); */
  /*  info = linearComplementarity_printInFile(lcp_problem,fcheck); */

  // Call the lcp_solver

  SolverOptions * lcp_options = options->internalSolvers;

  lcp_enum_init(lcp_problem, lcp_options, 1);
  //}
  * info = linearComplementarity_driver(lcp_problem, zlcp , wlcp, lcp_options, global_options);
  if (options->filterOn > 0)
    lcp_compute_error(lcp_problem, zlcp, wlcp, lcp_options->dparam[0], &(lcp_options->dparam[1]));
  lcp_enum_reset(lcp_problem, lcp_options, 1);

  /*       printf("\n"); */
  int nc = problem->numberOfContacts;
  // Conversion of result
  for (i = 0; i < nc; i++)
  {

    /* printf("Contact number = %i\n",i); */
    reaction[2 * i] = zlcp[3 * i];
    reaction[2 * i + 1] = 1.0 / 2.0 * (zlcp[3 * i + 1] - wlcp[3 * i + 2]);
    /* printf("reaction[ %i]=%12.10e\n", 2*i, reaction[2*i]); */
    /* printf("reaction[ %i]=%12.10e\n", 2*i+1, reaction[2*i+1]); */

    velocity[2 * i] = wlcp[3 * i];
    velocity[2 * i + 1] = wlcp[3 * i + 1] - zlcp[3 * i + 2];
    /* printf("velocity[ %i]=%12.10e\n", 2*i, velocity[2*i]);   */
    /* printf("velocity[ %i]=%12.10e\n", 2*i+1, velocity[2*i+1]); */
  }

  /*        for (i=0; i< lcp_problem->size; i++){  */
  /*     printf("zlcp[ %i]=%12.10e,\t wlcp[ %i]=%12.10e \n", i, zlcp[i],i, wlcp[i]); */
  /*        } */
  /*        printf("\n"); */

  /*        for (i=0; i< problem->size; i++){  */
  /*     printf("z[ %i]=%12.10e,\t w[ %i]=%12.10e\n", i, z[i],i, w[i]); */
  /*        } */


  /*        printf("\n"); */
  double error;
  *info = FrictionContact2D_compute_error(problem, reaction , velocity, options->dparam[0], &error);
  free(zlcp);
  free(wlcp);
  freeLinearComplementarityProblem(lcp_problem);
}
Ejemplo n.º 2
0
void lcp_path(LinearComplementarityProblem* problem, double *z, double *w, int *info , SolverOptions* options)
{
  *info = 1;
#ifdef HAVE_PATHFERRIS
  /* matrix M/vector q of the lcp */
  double * M = problem->M->matrix0;

  double * q = problem->q;
  int nnz, i, j, dim;

  /* size of the LCP */
  int n = problem->size;

  double tol = options->dparam[0];
  MCP_Termination termination;

  nnz = nbNonNulElems(n, M, 1.0e-18);
  int * m_i = (int *)calloc(nnz + 1, sizeof(int));
  int * m_j = (int *)calloc(nnz + 1, sizeof(int));
  double * m_ij = (double *)calloc(nnz + 1, sizeof(double));
  double * lb = (double *)calloc(n + 1, sizeof(double));
  double * ub = (double *)calloc(n + 1, sizeof(double));
  double err, val;


  FortranToPathSparse(n, M, 1.0e-18, m_i, m_j, m_ij);
  for (i = 0; i < n; i++)
  {
    lb[i] = 0.;
    ub[i] = 1.e20;
  }
  SimpleLCP(n, nnz, m_i, m_j, m_ij, q, lb, ub,
            &termination, z);

  if (termination == MCP_Error)
  {
    *info = 1;
    if (verbose > 0)
      printf("PATH : Error in the solution.\n");
  }
  else if (termination == MCP_Solved)
  {
    for (i = 0; i < n; i++)
    {
      val = q[i];
      for (j = 0; j < n; j++)
      {
        val += M[i + j * n] * z[j];
      }
      w[i] = val;
    }
    *info = 0;
    /* **** Criterium convergence **** */
    lcp_compute_error(problem, z, w, tol, &err);

    if (verbose > 0)
      printf("PATH : LCP Solved, error %10.7f.\n", err);
  }
  else
  {
    if (verbose > 0)
      printf("PATH : Other error: %d\n", termination);
  }
  free(m_i);
  free(m_j);
  free(m_ij);
  free(lb);
  free(ub);

#endif /*HAVE_PATHFERRIS*/


  return;
}
Ejemplo n.º 3
0
void lcp_nsgs_SBM(LinearComplementarityProblem* problem, double *z, double *w, int *info, SolverOptions* options)
{
  /* Notes:

     - we suppose that the trivial solution case has been checked
     before, and that all inputs differs from NULL since this function
     is supposed to be called from lcp_driver_global().

     - Input matrix M of the problem is supposed to be sparse-block
       with no null row (ie no rows with all blocks equal to null)
  */
  if (problem->M->matrix1 == NULL)
  {
    fprintf(stderr, "lcp_NSGS_SBM error: wrong storage type for input matrix M of the LCP.\n");
    exit(EXIT_FAILURE);
  }

  /*
    The options for the global "block" solver are defined in options[0].\n
    options[i], for 0<i<numberOfSolvers-1 correspond to local solvers.
   */

  /* Global Solver parameters*/
  int itermax = options[0].iparam[0];
  double tolerance = options[0].dparam[0];

  /* Matrix M/vector q of the LCP */
  SparseBlockStructuredMatrix* blmat = problem->M->matrix1;
  double * q = problem->q;

  /* Number of non-null blocks in blmat */
  int nbOfNonNullBlocks = blmat->nbblocks;
  if (nbOfNonNullBlocks < 1)
  {
    fprintf(stderr, "Numerics::lcp_NSGS_SBM error: empty M matrix (all blocks = NULL).\n");
    exit(EXIT_FAILURE);
  }

  /* Local problem initialization */

  LinearComplementarityProblem * local_problem = (LinearComplementarityProblem *)malloc(sizeof(*local_problem));
  local_problem->M = (NumericsMatrix *)malloc(sizeof(*local_problem->M));
  local_problem->M->storageType = 0; // dense storage
  local_problem->M->matrix0 = NULL;
  local_problem->M->matrix1 = NULL;
  local_problem->M->matrix2 = NULL;
  local_problem->M->internalData = NULL;

  /* Memory allocation for q. Size of q = blsizemax, size of the largest square-block in blmat */
  int blsizemax = blmat->blocksize0[0];
  int k;
  for (unsigned int i = 1 ; i < blmat->blocknumber0 ; i++)
  {
    k = blmat->blocksize0[i] - blmat->blocksize0[i - 1];
    if (k > blsizemax) blsizemax = k;
  }
  local_problem->q = (double*)malloc(blsizemax * sizeof(double));

  /* Current row (of blocks) number */
  unsigned int rowNumber;

  /*****  Gauss-Seidel iterations *****/
  int iter = 0; /* Current iteration number */
  double error = 1.; /* Current error */
  int hasNotConverged = 1;

  /* Output from local solver */
  options[0].iparam[2] = 0;
  options[0].dparam[2] = 0.0;

  if (options->numberOfInternalSolvers < 1)
  {
    numericsError("lcp_nsgs_SBM", "The NSGS_SBM method needs options for the internal solvers, options[0].numberOfInternalSolvers should be >1");
  }



  /*Number of the local solver */
  int localSolverNum = options->numberOfInternalSolvers ;
  SolverOptions * internalSolvers = options->internalSolvers ;

  int pos = 0;
  /* Output from local solver */
  int infoLocal = -1;

  while ((iter < itermax) && (hasNotConverged > 0))
  {
    ++iter;
    /* Loop over the rows of blocks in blmat */
    localSolverNum = 0;
    pos = 0;
    /*       cblas_dcopy(problem->size,w,1,wBackup,1); */
    for (rowNumber = 0; rowNumber < blmat->blocknumber0; ++rowNumber)
    {
      /* Local problem formalization */
      lcp_nsgs_SBM_buildLocalProblem(rowNumber, blmat, local_problem, q, z);
      /* Solve local problem */
      infoLocal = lcp_driver_DenseMatrix(local_problem, &z[pos], &w[pos], &internalSolvers[localSolverNum]);
      pos += local_problem->size;
      /* sum of local number of iterations (output from local_driver)*/
      if (options[localSolverNum].iparam != NULL)
        options[0].iparam[2] += internalSolvers[localSolverNum].iparam[1];
      /* sum of local errors (output from local_driver)*/
      options[0].dparam[2] += internalSolvers[localSolverNum].dparam[1];

      if (infoLocal > 0)
      {
        //free(local_problem->q);
        //free(local_problem->M);
        //free(local_problem);
        /* Number of GS iterations */
        options[0].iparam[1] = iter;
        fprintf(stderr, "lcp_NSGS_SBM error: Warning local LCP solver  at global iteration %d.\n for block-row number %d. Output info equal to %d.\n", iter, rowNumber, infoLocal);
        //exit(EXIT_FAILURE);

        break;

      }

      while (localSolverNum < options->numberOfInternalSolvers - 1)
        localSolverNum++;
    }

    /*       cblas_dcopy(problem->size , problem->q , 1 , w , 1); */
    /*       prod(problem->size,problem->size, 1.0, problem->M,z,1.0,w); */
    /*       cblas_daxpy(problem->size, -1.0, w,1,wBackup, 1); */
    /*       num = cblas_dnrm2(problem->size,wBackup,1); */
    /*       error = num*den; */
    /* Criterium convergence */
    hasNotConverged = lcp_compute_error(problem, z, w, tolerance, &error);
    /*       if(error<tolerance) hasNotConverged = 0; */
  }
  *info = hasNotConverged;
  /* Number of GS iterations */
  options[0].iparam[1] = iter;
  /* Resulting error */
  options[0].dparam[1] = error;

  free(local_problem->q);
  free(local_problem->M);
  free(local_problem);
  /*   free(wBackup); */
}
Ejemplo n.º 4
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;
  }
}